void finish_vertex(VertexDescriptorType v, VertexDescriptorType sourceNode, TGraph& g)
  {
    // Construct the region around the vertex
    itk::Index<2> indexToFinish;
    indexToFinish[0] = v[0];
    indexToFinish[1] = v[1];

    itk::ImageRegion<2> region = ITKHelpers::GetRegionInRadiusAroundPixel(indexToFinish, HalfWidth);

    region.Crop(Image->GetLargestPossibleRegion()); // Make sure the region is entirely inside the image

    // Mark all the pixels in this region as filled.
    // It does not matter which image we iterate over, we just want the indices.
    itk::ImageRegionConstIteratorWithIndex<TImage> gridIterator(Image, region);

    while(!gridIterator.IsAtEnd())
    {
      VertexDescriptorType v;
      v[0] = gridIterator.GetIndex()[0];
      v[1] = gridIterator.GetIndex()[1];
      put(FillStatusMap, v, true);
      MaskImage->SetPixel(gridIterator.GetIndex(), MaskImage->GetValidValue());
      ++gridIterator;
    }

    // Additionally, initialize the filled vertices because they may now be valid.
    // This must be done in a separate loop like this because the mask image used to check for boundary pixels is incorrect until the above loop updates it.
    gridIterator.GoToBegin();
    while(!gridIterator.IsAtEnd())
    {
      VertexDescriptorType v;
      v[0] = gridIterator.GetIndex()[0];
      v[1] = gridIterator.GetIndex()[1];
      initialize_vertex(v, g);
      ++gridIterator;
    }

    // Update the priority function.
    this->PriorityFunction->Update(sourceNode, v);

    // Add pixels that are on the new boundary to the queue, and mark other pixels as not in the queue.
    itk::ImageRegionConstIteratorWithIndex<Mask> imageIterator(this->MaskImage, region);

    while(!imageIterator.IsAtEnd())
    {
      VertexDescriptorType v;
      v[0] = imageIterator.GetIndex()[0];
      v[1] = imageIterator.GetIndex()[1];

      // Mark all nodes in the patch around this node as filled (in the FillStatusMap).
      // This makes them ignored if they are still in the boundaryNodeQueue.
      if(ITKHelpers::HasNeighborWithValue(imageIterator.GetIndex(), this->MaskImage, this->MaskImage->GetHoleValue()))
      {
        put(BoundaryStatusMap, v, true);
        this->BoundaryNodeQueue.push(v);
        float priority = this->PriorityFunction->ComputePriority(imageIterator.GetIndex());
        //std::cout << "updated priority: " << priority << std::endl;
        put(this->PriorityMap, v, priority);
      }
      else
      {
        put(this->BoundaryStatusMap, v, false);
      }

      ++imageIterator;
    }

    {
    // Debug only - write the mask to a file
    HelpersOutput::WriteImage(this->MaskImage, Helpers::GetSequentialFileName("debugMask", this->NumberOfFinishedVertices, "png"));
    HelpersOutput::WriteVectorImageAsRGB(this->Image, Helpers::GetSequentialFileName("output", this->NumberOfFinishedVertices, "png"));
    this->NumberOfFinishedVertices++;
    }
  } // finish_vertex
  /** The mask is inpainted with ValidValue in this function. */
  void FinishVertex(VertexDescriptorType targetNode, VertexDescriptorType sourceNode)// __attribute__((optimize(0))) // This supposedly makes this function build in debug mode (-g -O0) when the rest of the program is built in -O3 or similar)
  {
    // Mark this pixel and the area around it as filled, and mark the mask in this region as filled.
    // Determine the new boundary, and setup the nodes in the boundary queue.

//    std::cout << "InpaintingVisitor::FinishVertex()" << std::endl;

    // Construct the region around the vertex
    itk::Index<2> indexToFinish = ITKHelpers::CreateIndex(targetNode);

    itk::ImageRegion<2> regionToFinishFull = ITKHelpers::GetRegionInRadiusAroundPixel(indexToFinish, this->PatchHalfWidth);

    // Copy this region so that we can change (crop) the regionToFinish and still have a copy of the original region
    itk::ImageRegion<2> regionToFinish = regionToFinishFull;

    // Make sure the region is entirely inside the image
    // (because we allow target patches to not be entirely inside the image to handle the case where
    // the hole boundary is near the image boundary)
    regionToFinish.Crop(this->Image->GetLargestPossibleRegion());

    if(this->DebugImages)
    {
      ITKHelpers::WriteImage(this->MaskImage, Helpers::GetSequentialFileName("Mask_Before", this->NumberOfFinishedPatches, "png", 3));
    }

    // Mark all the pixels in this region as filled in the mask.
    ITKHelpers::SetRegionToConstant(this->MaskImage, regionToFinish, this->MaskImage->GetValidValue());

    if(this->DebugImages)
    {
      ITKHelpers::WriteImage(this->MaskImage, Helpers::GetSequentialFileName("Mask_After", this->NumberOfFinishedPatches, "png", 3));

      typename TImage::PixelType red;
      red.Fill(0);
      red[0] = 255;

      typename TImage::PixelType green;
      green.Fill(0);
      green[1] = 255;

      typename TImage::Pointer patchesCopiedImage = TImage::New();
      ITKHelpers::DeepCopy(this->Image, patchesCopiedImage.GetPointer());
      ITKHelpers::OutlineRegion(patchesCopiedImage.GetPointer(), regionToFinish, red);

      itk::Index<2> sourceRegionCenter = ITKHelpers::CreateIndex(sourceNode);
      itk::ImageRegion<2> sourceRegion = ITKHelpers::GetRegionInRadiusAroundPixel(sourceRegionCenter, this->PatchHalfWidth);

      sourceRegion = ITKHelpers::CropRegionAtPosition(sourceRegion, this->Image->GetLargestPossibleRegion(), regionToFinishFull);

      ITKHelpers::OutlineRegion(patchesCopiedImage.GetPointer(), sourceRegion, green);

      ITKHelpers::WriteRGBImage(patchesCopiedImage.GetPointer(), Helpers::GetSequentialFileName("PatchesCopied", this->NumberOfFinishedPatches, "png", 3));
    }

    // Update the priority function. This must be done AFTER the mask is filled,
    // as some of the Priority functors only compute things on the hole boundary, or only
    // use data from the valid region of the image (indicated by valid pixels in the mask).
    this->PriorityFunction->Update(sourceNode, targetNode, this->NumberOfFinishedPatches);

    // Initialize (if requested) all vertices in the newly filled region because they may now be valid source nodes.
    // (You may not want to do this in some cases (i.e. if the descriptors needed cannot be
    // computed on newly filled regions))

    if(this->AllowNewPatches)
    {
      itk::ImageRegionConstIteratorWithIndex<TImage> gridIterator(Image, regionToFinish);
      while(!gridIterator.IsAtEnd())
      {
        VertexDescriptorType v = Helpers::ConvertFrom<VertexDescriptorType, itk::Index<2> >(gridIterator.GetIndex());

        InitializeVertex(v);
        ++gridIterator;
      }
    }

    // Add pixels that are on the new boundary to the queue, and mark other pixels as not in the queue.
    itk::ImageRegionConstIteratorWithIndex<Mask> imageIterator(this->MaskImage, regionToFinish);

    typedef typename TBoundaryNodeQueue::HandleType HandleType;

    while(!imageIterator.IsAtEnd())
    {
      VertexDescriptorType v = Helpers::ConvertFrom<VertexDescriptorType, itk::Index<2> >(imageIterator.GetIndex());

      if(this->MaskImage->HasHoleNeighbor(imageIterator.GetIndex()))
      {
        float priority = this->PriorityFunction->ComputePriority(imageIterator.GetIndex());
        this->BoundaryNodeQueue.push_or_update(v, priority);
      }
      else
      {
        this->BoundaryNodeQueue.mark_as_invalid(v);
      }

      ++imageIterator;
    }

    // std::cout << "FinishVertex after traversing finishing region there are "
    //           << BoostHelpers::CountValidQueueNodes(BoundaryNodeQueue, BoundaryStatusMap)
    //           << " valid nodes in the queue." << std::endl;
    
    // Sometimes pixels that are not in the finishing region that were boundary pixels are no longer
    // boundary pixels after the filling. Check for these.
    // E.g. (H=hole, B=boundary, V=valid, Q=query, F=filled, N=new boundary,
    // R=old boundary pixel that needs to be removed because it is no longer on the boundary)
    // Before filling

    /* V V V B H H H H
     * V V V B H H H H
     * V V V B H H H H
     * V V V B B Q B B
     * V V V V V V V V
     */
    // After filling

    /* V V V B H H H H
     * V V V B H H H H
     * V V V B F F F H
     * V V V B F F F B
     * V V V V F F F V
     */
    // New boundary
    /* V V V B H H H H
     * V V V B H H H H
     * V V V B N N N H
     * V V V R F F N B
     * V V V V F F F V
     */

    // Expand the region
    itk::ImageRegion<2> expandedRegion = ITKHelpers::GetRegionInRadiusAroundPixel(indexToFinish, PatchHalfWidth + 1);
    // Make sure the region is entirely inside the image (to allow for target regions near the image boundary)
    expandedRegion.Crop(this->Image->GetLargestPossibleRegion());
    std::vector<itk::Index<2> > boundaryPixels = ITKHelpers::GetBoundaryPixels(expandedRegion);
    for(unsigned int i = 0; i < boundaryPixels.size(); ++i)
    {
      // the region (the entire image) can be omitted, as this function automatically checks if the pixels are inside the image
      if(!this->MaskImage->HasHoleNeighbor(boundaryPixels[i]))
      {
        VertexDescriptorType v = Helpers::ConvertFrom<VertexDescriptorType, itk::Index<2> >(boundaryPixels[i]);

        put(this->BoundaryNodeQueue.BoundaryStatusMap, v, false);
      }
    }

    // std::cout << "FinishVertex after removing stale nodes outside finishing region there are "
    //               << BoostHelpers::CountValidQueueNodes(BoundaryNodeQueue, BoundaryStatusMap)
    //               << " valid nodes in the queue." << std::endl;

    this->NumberOfFinishedPatches++;

//    std::cout << "Leave InpaintingVisitor::FinishVertex()" << std::endl;
  } // end FinishVertex
  /** The mask is inpainted with ValidValue in this function. */
  void FinishVertex(VertexDescriptorType targetNode, VertexDescriptorType sourceNode) override
  {
    // Mark this pixel and the area around it as filled, and mark the mask in this region as filled.
    // Determine the new boundary, and setup the nodes in the boundary queue.

//    std::cout << "InpaintingVisitor::FinishVertex()" << std::endl;

    // Construct the region around the vertex
    itk::Index<2> indexToFinish = ITKHelpers::CreateIndex(targetNode);

    // Mark this node as having been used as a source node.
    this->UsedNodesSet.insert(indexToFinish);

    itk::ImageRegion<2> regionToFinishFull =
        ITKHelpers::GetRegionInRadiusAroundPixel(indexToFinish, this->PatchHalfWidth);

    // Copy this region so that we can change (crop) the regionToFinish and still have a copy of the original region
    itk::ImageRegion<2> regionToFinish = regionToFinishFull;

    // Make sure the region is entirely inside the image
    // (because we allow target patches to not be entirely inside the image to handle the case where
    // the hole boundary is near the image boundary)
    regionToFinish.Crop(this->FullRegion);

    itk::Index<2> sourceRegionCenter = ITKHelpers::CreateIndex(sourceNode);
    itk::ImageRegion<2> sourceRegion =
        ITKHelpers::GetRegionInRadiusAroundPixel(sourceRegionCenter, this->PatchHalfWidth);

    sourceRegion = ITKHelpers::CropRegionAtPosition(sourceRegion, this->MaskImage->GetLargestPossibleRegion(),
                                                    regionToFinishFull);

    // Mark all pixels that were copied (in the hole region of the source patch) as having been used.
//    std::cout << "InpaintingVisitor::FinishVertex() mark pixels as used" << std::endl;
    itk::ImageRegionConstIteratorWithIndex<SourcePixelMapImageType> sourcePatchIterator(this->SourcePixelMapImage,
                                                                                        sourceRegion);
    itk::ImageRegionConstIteratorWithIndex<Mask> targetPatchIterator(this->MaskImage, regionToFinish);
    while(!sourcePatchIterator.IsAtEnd())
    {
      if(targetPatchIterator.Get() == this->MaskImage->GetHoleValue())
      {
        this->CopiedPixelsImage->SetPixel(sourcePatchIterator.GetIndex(), true); // Mark this pixel as used

        // Save the location from which this pixel came. We want to use the index value in the SourcePixelMapImage,
        // because the value might not equal the current index in the case where new patches are allowed.
        this->SourcePixelMapImage->SetPixel(targetPatchIterator.GetIndex(),
                                            sourcePatchIterator.Get());
      }

      ++sourcePatchIterator;
      ++targetPatchIterator;
    }

    if(this->DebugImages)
    {
      if(this->DebugLevel > 1)
      {
        ITKHelpers::WriteBoolImage(this->CopiedPixelsImage,
                                   Helpers::GetSequentialFileName("CopiedPixels",
                                                                  this->NumberOfFinishedPatches, "png", 3));
        ITKHelpers::WriteImage(this->MaskImage,
                               Helpers::GetSequentialFileName("Mask_Before",
                                                              this->NumberOfFinishedPatches, "png", 3));
      }
    }

    // Mark all the pixels in this region as filled in the mask.
//    std::cout << "InpaintingVisitor::FinishVertex() fill the mask" << std::endl;
    ITKHelpers::SetRegionToConstant(this->MaskImage, regionToFinish,
                                    this->MaskImage->GetValidValue());

    // Write an image of where the source and target patch were in this iteration.
//    if(this->DebugImages && this->Image)
//    {
//      typename TImage::PixelType red;
//      red.Fill(0);
//      red[0] = 255;

//      typename TImage::PixelType green;
//      green.Fill(0);
//      green[1] = 255;

//      typename TImage::Pointer patchesCopiedImage = TImage::New();
//      ITKHelpers::DeepCopy(this->Image, patchesCopiedImage.GetPointer());
//      ITKHelpers::OutlineRegion(patchesCopiedImage.GetPointer(), regionToFinish, red);

//      ITKHelpers::OutlineRegion(patchesCopiedImage.GetPointer(), sourceRegion, green);

//      ITKHelpers::WriteRGBImage(patchesCopiedImage.GetPointer(), Helpers::GetSequentialFileName("PatchesCopied", this->NumberOfFinishedPatches, "png", 3));

//      if(this->DebugLevel > 1)
//      {
//        ITKHelpers::WriteImage(this->MaskImage, Helpers::GetSequentialFileName("Mask_After", this->NumberOfFinishedPatches, "png", 3));
//      }
//    }

    // Update the priority function. This must be done AFTER the mask is filled,
    // as some of the Priority functors only compute things on the hole boundary, or only
    // use data from the valid region of the image (indicated by valid pixels in the mask).
//    std::cout << "InpaintingVisitor::FinishVertex() update priority" << std::endl;
    this->PriorityFunction->Update(sourceNode, targetNode, this->NumberOfFinishedPatches);
//    std::cout << "InpaintingVisitor::FinishVertex() finish update priority" << std::endl;
    // Initialize (if requested) all vertices in the newly filled region because they may now be valid source nodes.
    // (You may not want to do this in some cases (i.e. if the descriptors needed cannot be
    // computed on newly filled regions))

    if(this->AllowNewPatches)
    {
//      std::cout << "Initializing new vertices..." << std::endl;
      itk::ImageRegionConstIteratorWithIndex<Mask> gridIterator(this->MaskImage, regionToFinish);
      while(!gridIterator.IsAtEnd())
      {
        VertexDescriptorType v = Helpers::ConvertFrom<VertexDescriptorType, itk::Index<2> >(gridIterator.GetIndex());

        InitializeVertex(v);
        ++gridIterator;
      }
    }

    // Add pixels that are on the new boundary to the queue, and mark other pixels as not in the queue.
//    std::cout << "InpaintingVisitor::FinishVertex() iterator" << std::endl;
    itk::ImageRegionConstIteratorWithIndex<Mask> imageIterator(this->MaskImage, regionToFinish);
//    std::cout << "InpaintingVisitor::FinishVertex() finish iterator" << std::endl;

    typedef typename TBoundaryNodeQueue::HandleType HandleType;

    // Naive, unparallelizable way
//    while(!imageIterator.IsAtEnd())
//    {
//      VertexDescriptorType v = Helpers::ConvertFrom<VertexDescriptorType, itk::Index<2> >(imageIterator.GetIndex());

//      if(this->MaskImage->HasHoleNeighbor(imageIterator.GetIndex()))
//      {
//        float priority = this->PriorityFunction->ComputePriority(imageIterator.GetIndex());
//        this->BoundaryNodeQueue.push_or_update(v, priority);
//      }
//      else
//      {
//        this->BoundaryNodeQueue.mark_as_invalid(v);
//      }

//      ++imageIterator;
//    }

    // Parallelizable way - collect the pixels that need their priority computed
//    std::cout << "InpaintingVisitor::FinishVertex() Parallelizable way" << std::endl;
    typedef std::vector<itk::Index<2> > IndexVectorType;
    IndexVectorType pixelsToCompute;
    while(!imageIterator.IsAtEnd())
    {
      VertexDescriptorType v = Helpers::ConvertFrom<VertexDescriptorType, itk::Index<2> >(imageIterator.GetIndex());

      if(this->MaskImage->GetPixel(imageIterator.GetIndex()) == this->MaskImage->GetValidValue() &&
         this->MaskImage->HasHoleNeighbor(imageIterator.GetIndex()))
      {
        pixelsToCompute.push_back(imageIterator.GetIndex());
      }
      else
      {
        this->BoundaryNodeQueue->mark_as_invalid(v);
      }

      ++imageIterator;
    }

//    std::cout << "InpaintingVisitor::FinishVertex() update queue" << std::endl;
    #pragma omp parallel for
    for(IndexVectorType::const_iterator pixelIterator = pixelsToCompute.begin();
        pixelIterator < pixelsToCompute.end(); ++pixelIterator)
    {
      VertexDescriptorType v = Helpers::ConvertFrom<VertexDescriptorType, itk::Index<2> >(*pixelIterator);
      float priority = this->PriorityFunction->ComputePriority(*pixelIterator);
      #pragma omp critical // There are weird crashes without this guard
      this->BoundaryNodeQueue->push_or_update(v, priority);
    }

    // std::cout << "FinishVertex after traversing finishing region there are "
    //           << BoostHelpers::CountValidQueueNodes(BoundaryNodeQueue, BoundaryStatusMap)
    //           << " valid nodes in the queue." << std::endl;
    
    // Sometimes pixels that are not in the finishing region that were boundary pixels are no longer
    // boundary pixels after the filling. Check for these.
    // E.g. (H=hole, B=boundary, V=valid, Q=query, F=filled, N=new boundary,
    // R=old boundary pixel that needs to be removed because it is no longer on the boundary)
    // Before filling

    /* V V V B H H H H
     * V V V B H H H H
     * V V V B H H H H
     * V V V B B Q B B
     * V V V V V V V V
     */
    // After filling

    /* V V V B H H H H
     * V V V B H H H H
     * V V V B F F F H
     * V V V B F F F B
     * V V V V F F F V
     */
    // New boundary
    /* V V V B H H H H
     * V V V B H H H H
     * V V V B N N N H
     * V V V R F F N B
     * V V V V F F F V
     */

    // Expand the region
//    std::cout << "InpaintingVisitor::FinishVertex() expand" << std::endl;
    itk::ImageRegion<2> expandedRegion =
        ITKHelpers::GetRegionInRadiusAroundPixel(indexToFinish, PatchHalfWidth + 1);
    // Make sure the region is entirely inside the image (to allow for target regions near the image boundary)
    expandedRegion.Crop(this->FullRegion);
    std::vector<itk::Index<2> > boundaryPixels =
        ITKHelpers::GetBoundaryPixels(expandedRegion);
//    std::cout << "InpaintingVisitor::FinishVertex() boundary pixels" << std::endl;
    for(unsigned int i = 0; i < boundaryPixels.size(); ++i)
    {
      // the region (the entire image) can be omitted, as this function automatically checks if the pixels are inside the image
      if(!this->MaskImage->HasHoleNeighbor(boundaryPixels[i]))
      {
        VertexDescriptorType v =
            Helpers::ConvertFrom<VertexDescriptorType, itk::Index<2> >(boundaryPixels[i]);

        put(this->BoundaryNodeQueue->BoundaryStatusMap, v, false);
      }
    }

    // std::cout << "FinishVertex after removing stale nodes outside finishing region there are "
    //               << BoostHelpers::CountValidQueueNodes(BoundaryNodeQueue, BoundaryStatusMap)
    //               << " valid nodes in the queue." << std::endl;

    this->NumberOfFinishedPatches++;

//    std::cout << "Leave InpaintingVisitor::FinishVertex()" << std::endl;
  } // end FinishVertex