// Calculate patch addressing
void tetPointFieldDecomposer::
tetPolyPatchFieldDecomposer::calcPatchAddressing() const
{
    if (directPatchAddressingPtr_)
    {
        FatalErrorIn
        (
            "void tetPointFieldDecomposer::"
            "tetPolyPatchFieldDecomposer::calcPatchAddressing() const"
        )   << "addressing already calculated"
            << abort(FatalError);
    }

    // Allocate the addressing
    directPatchAddressingPtr_ = new labelList(targetPatch().size(), -1);
    labelList& addr = *directPatchAddressingPtr_;

    // Algorithm:
    // Go to the source patch, create a lookup list the size of all
    // points in the mesh and then gather the points for the current
    // patch.
    labelList pointLookup
        (sourcePatch().boundaryMesh().mesh().nPoints(), -1);

    const labelList& sourcePatchPoints = sourcePatch().meshPoints();

    forAll (sourcePatchPoints, pointI)
    {
        pointLookup[sourcePatchPoints[pointI]] = pointI;
    }

    // Gather the information
    
    const labelList& targetPatchPoints = targetPatch().meshPoints();

    forAll (targetPatchPoints, pointI)
    {
        addr[pointI] =
            pointLookup[directAddressing_[targetPatchPoints[pointI]]];
    }

    if (addr.size() && min(addr) < 0)
    {
        FatalErrorIn
        (
            "void tetPointFieldDecomposer::"
            "tetPolyPatchFieldDecomposer::calcPatchAddressing() const"
        )   << "error in addressing"
            << abort(FatalError);
    }
}
void Vector()
{
  typedef itk::Image<unsigned char, 2 >  ChannelType;
  const unsigned int NumberOfChannels = 3;
  typedef itk::Image<itk::CovariantVector<unsigned char, NumberOfChannels>, 2 >  ImageType;

  ImageType::Pointer image = ImageType::New();
  itk::Index<2> corner = {{0,0}};
  itk::Size<2> imageSize = {{500,500}};
  itk::ImageRegion<2> fullRegion(corner, imageSize);
  image->SetRegions(fullRegion);
  image->Allocate();

  for(unsigned int i = 0; i < NumberOfChannels; ++i)
  {
    itk::RandomImageSource<ChannelType>::Pointer randomImageSource =
      itk::RandomImageSource<ChannelType>::New();
    randomImageSource->SetNumberOfThreads(1); // to produce non-random results
    randomImageSource->SetSize(imageSize);
    randomImageSource->Update();

    ITKHelpers::SetChannel(image.GetPointer(), i, randomImageSource->GetOutput());
  }

  itk::Size<2> patchSize = {{21,21}};

  // There is nothing magic about these particular patches
  itk::Index<2> targetCorner = {{319, 302}};
  itk::ImageRegion<2> targetRegion(targetCorner, patchSize);

  itk::Index<2> sourceCorner = {{341, 300}};
  itk::ImageRegion<2> sourceRegion(sourceCorner, patchSize);

  Mask::Pointer mask = Mask::New();
  mask->SetRegions(fullRegion);
  mask->Allocate();
  ITKHelpers::SetImageToConstant(mask.GetPointer(), mask->GetValidValue());

  typedef SumSquaredPixelDifference<ImageType::PixelType> PixelDifferenceType;

  typedef ImagePatchPixelDescriptor<ImageType> PatchType;

  ImagePatchDifference<PatchType, PixelDifferenceType> imagePatchDifference;

  PatchType targetPatch(image, mask, targetRegion);
  PatchType sourcePatch(image, mask, sourceRegion);

  float difference = imagePatchDifference(targetPatch, sourcePatch);

  std::cout << "GMHDifference: " << difference << std::endl;

}
void Scalar()
{
  typedef itk::Image< unsigned char, 2 >  ImageType;

  itk::Size<2> imageSize = {{500,500}};

  itk::RandomImageSource<ImageType>::Pointer randomImageSource =
    itk::RandomImageSource<ImageType>::New();
  randomImageSource->SetNumberOfThreads(1); // to produce non-random results
  randomImageSource->SetSize(imageSize);
  randomImageSource->Update();

  ImageType* image = randomImageSource->GetOutput();

  itk::Size<2> patchSize = {{21,21}};

  // There is nothing magic about these particular patches
  itk::Index<2> targetCorner = {{319, 302}};
  itk::ImageRegion<2> targetRegion(targetCorner, patchSize);

  itk::Index<2> sourceCorner = {{341, 300}};
  itk::ImageRegion<2> sourceRegion(sourceCorner, patchSize);

  Mask::Pointer mask = Mask::New();
  mask->SetRegions(randomImageSource->GetOutput()->GetLargestPossibleRegion());
  mask->Allocate();
  ITKHelpers::SetImageToConstant(mask.GetPointer(), mask->GetValidValue());

  typedef SumSquaredPixelDifference<ImageType::PixelType> PixelDifferenceType;

  typedef ImagePatchPixelDescriptor<ImageType> PatchType;

  ImagePatchDifference<PatchType, PixelDifferenceType> imagePatchDifference;

  PatchType targetPatch(image, mask, targetRegion);
  PatchType sourcePatch(image, mask, sourceRegion);

  float difference = imagePatchDifference(targetPatch, sourcePatch);

  std::cout << "Difference: " << difference << std::endl;

}
int main(int argc, char *argv[])
{
  unsigned int t = time(NULL);
  srand(t);

  itk::Size<2> size;
  size.Fill(100);

  itk::Index<2> index;
  index.Fill(0);

  itk::ImageRegion<2> region(index, size);
/*
  // Generate a random image (this method doesn't work with VectorImage)
  itk::RandomImageSource<FloatVectorImageType>::Pointer imageSource =
    itk::RandomImageSource<FloatVectorImageType>::New();
  imageSource->SetNumberOfThreads(1); // to produce non-random results
  imageSource->SetSize(size);
  imageSource->SetMin(0);
  imageSource->SetMax(100);
  imageSource->Update();
  FloatVectorImageType::Pointer image = imageSource->GetOutput();
*/
  // Generate a random image
  FloatVectorImageType::Pointer image = FloatVectorImageType::New();
  image->SetRegions(region);
  image->SetNumberOfComponentsPerPixel(3);
  image->Allocate();

  {
  itk::ImageRegionIterator<FloatVectorImageType> imageIterator(image, image->GetLargestPossibleRegion());

  while(!imageIterator.IsAtEnd())
    {
    FloatVectorImageType::PixelType pixel;
    pixel.SetSize(3);
    pixel[0] = drand48();
    pixel[1] = drand48();
    pixel[2] = drand48();
    imageIterator.Set(pixel);
    ++imageIterator;
    }
  }

  // Generate a random membership image
  IntImageType::Pointer membershipImage = IntImageType::New();
  membershipImage->SetRegions(region);
  membershipImage->Allocate();
  membershipImage->FillBuffer(0);

  {
  itk::ImageRegionIterator<IntImageType> membershipImageIterator(membershipImage, membershipImage->GetLargestPossibleRegion());

  while(!membershipImageIterator.IsAtEnd())
    {
    IntImageType::PixelType pixel;
    pixel = rand() / 1000;
    membershipImageIterator.Set(pixel);
    ++membershipImageIterator;
    }
  }

  // Write the image
  itk::ImageFileWriter<FloatVectorImageType>::Pointer imageWriter =
    itk::ImageFileWriter<FloatVectorImageType>::New();
  imageWriter->SetFileName("image.mha");
  imageWriter->SetInput(image);
  imageWriter->Update();

//   // Generate a random mask
//   itk::RandomImageSource<Mask>::Pointer maskSource = itk::RandomImageSource<Mask>::New();
//   maskSource->SetNumberOfThreads(1); // to produce non-random results
//   maskSource->SetSize(size);
//   maskSource->SetMin(0);
//   maskSource->SetMax(255);
//   maskSource->Update();
//
//   // Threshold the mask
//   //typedef itk::ThresholdImageFilter <UnsignedCharImageType> ThresholdImageFilterType;
//   typedef itk::BinaryThresholdImageFilter <Mask, Mask> ThresholdImageFilterType;
//   ThresholdImageFilterType::Pointer thresholdFilter = ThresholdImageFilterType::New();
//   thresholdFilter->SetInput(maskSource->GetOutput());
//   thresholdFilter->SetLowerThreshold(0);
//   thresholdFilter->SetUpperThreshold(122);
//   thresholdFilter->SetOutsideValue(1);
//   thresholdFilter->SetInsideValue(0);
//   thresholdFilter->Update();
//   Mask::Pointer mask = thresholdFilter->GetOutput();

  std::cout << "Creating mask..." << std::endl;
  Mask::Pointer mask = Mask::New();
  mask->SetRegions(region);
  mask->Allocate();

  {
  itk::ImageRegionIterator<Mask> maskIterator(mask, mask->GetLargestPossibleRegion());

  while(!maskIterator.IsAtEnd())
    {
    int randomNumber = rand()%10;
    //std::cout << "randomNumber: " << randomNumber << std::endl;
    if(randomNumber > 5)
      {
      maskIterator.Set(mask->GetHoleValue());
      }
    else
      {
      maskIterator.Set(mask->GetValidValue());
      }
    ++maskIterator;
    }
  }
  std::cout << "Writing mask..." << std::endl;
  // Write the mask
  itk::ImageFileWriter<Mask>::Pointer maskWriter = itk::ImageFileWriter<Mask>::New();
  maskWriter->SetFileName("mask.png");
  maskWriter->SetInput(mask);
  maskWriter->Update();

  std::cout << "Creating source patches..." << std::endl;
  unsigned int patchRadius = 10;
  // Create source patches
  itk::ImageRegionConstIterator<FloatVectorImageType> imageIterator(image, image->GetLargestPossibleRegion());
  std::vector<Patch> sourcePatches;
  while(!imageIterator.IsAtEnd())
    {
    itk::Index<2> currentPixel = imageIterator.GetIndex();
    itk::ImageRegion<2> region = Helpers::GetRegionInRadiusAroundPixel(currentPixel, patchRadius);
    if(image->GetLargestPossibleRegion().IsInside(region))
      {
      sourcePatches.push_back(Patch(region));
      }
    ++imageIterator;
    }
  std::cout << "Source patches: " << sourcePatches.size() << std::endl;
  itk::Size<2> targetSize;
  targetSize.Fill(patchRadius * 2 + 1);

  itk::Index<2> targetIndex;
  targetIndex.Fill(3);

  itk::ImageRegion<2> targetRegion(targetIndex, targetSize);
  Patch targetPatch(targetRegion);

  CandidatePairs pairs(targetPatch);
  pairs.AddPairFromPatch(targetPatch);

  itk::ImageRegion<2> adjacentRegion = targetRegion;
  itk::Index<2> adjacentIndex;
  adjacentIndex[0] = targetIndex[0] + 1;
  adjacentIndex[1] = targetIndex[1] + 1;
  adjacentRegion.SetIndex(adjacentIndex);
  Patch adjacentPatch(adjacentRegion);
  pairs.AddPairFromPatch(adjacentPatch);
  //pairs.AddPairFromPatch(sourcePatches[0]);

  SelfPatchCompare patchCompare;
  patchCompare.SetPairs(&pairs);
  patchCompare.SetImage(image);
  patchCompare.SetMask(mask);
  patchCompare.SetNumberOfComponentsPerPixel(3);
  patchCompare.SetMembershipImage(membershipImage);

  patchCompare.FunctionsToCompute.push_back(boost::bind(&SelfPatchCompare::SetPatchMembershipDifference,&patchCompare,_1));
  patchCompare.ComputeAllSourceDifferences();

  std::cout << "pairs: " << pairs.size() << std::endl;
  for(unsigned int i = 0; i < pairs.size(); ++i)
    {
    std::cout << "MembershipDifference: " << pairs[i].DifferenceMap[PatchPair::MembershipDifference] << std::endl;
    }

  //unsigned int bestMatchSourcePatchId = patchCompare.FindBestPatch();
  //std::cout << "bestMatchSourcePatchId: " << bestMatchSourcePatchId << std::endl;
/*
  unsigned int patchId = 1;
  float slowPatchDifference = patchCompare.SlowDifference(sourcePatches[patchId]);
  std::cout << "slowPatchDifference: " << slowPatchDifference << std::endl;

  float fastPatchDifference = patchCompare.PatchDifference(sourcePatches[patchId]);
  std::cout << "fastPatchDifference: " << fastPatchDifference << std::endl;

  unsigned int iterations = 1e6;

  itk::TimeProbe slowTimer;
  slowTimer.Start();

  for(unsigned int i = 0; i < iterations; ++i)
    {
    float slowPatchDifference = patchCompare.SlowDifference(sourcePatches[patchId]);
    }

  slowTimer.Stop();
  std::cout << "Slow Total: " << slowTimer.GetTotal() << std::endl;

  itk::TimeProbe fastTimer;
  fastTimer.Start();

  for(unsigned int i = 0; i < iterations; ++i)
    {
    float fastPatchDifference = patchCompare.PatchDifference(sourcePatches[patchId]);
    }

  fastTimer.Stop();
  std::cout << "Fast Total: " << fastTimer.GetTotal() << std::endl;*/

  return EXIT_SUCCESS;
}
void FullPatchScalarComparison()
{
  std::cout << "FullPatchScalarComparison()" << std::endl;
  FloatScalarImageType::Pointer scalarImage = FloatScalarImageType::New();
  Testing::GetBlankImage<FloatScalarImageType>(scalarImage);

  // Make the left half of the image 0, and the right half 5
  itk::ImageRegionIterator<FloatScalarImageType> imageIterator(scalarImage, scalarImage->GetLargestPossibleRegion());

  while(!imageIterator.IsAtEnd())
    {
    if(imageIterator.GetIndex()[0] < static_cast<int>(scalarImage->GetLargestPossibleRegion().GetSize()[0]/2))
      {
      imageIterator.Set(0);
      }
    else
      {
      imageIterator.Set(5);
      }

    ++imageIterator;
    }

  itk::Size<2> patchSize;
  patchSize.Fill(10);

  std::cout << "Full patch different test." << std::endl;
  // Full patch is different
  {
  itk::Index<2> sourceCorner;
  sourceCorner.Fill(0);
  itk::ImageRegion<2> sourceRegion(sourceCorner, patchSize);
  ImagePatchPixelDescriptor<FloatScalarImageType> sourcePatch(scalarImage, sourceRegion, true);

  itk::Index<2> targetCorner;
  targetCorner.Fill(scalarImage->GetLargestPossibleRegion().GetSize()[0]/2 + 4); // No magic about 4, just want a patch on the right side of the image
  itk::ImageRegion<2> targetRegion(targetCorner, patchSize);
  ImagePatchPixelDescriptor<FloatScalarImageType> targetPatch(scalarImage, targetRegion, true);
  std::cout << "targetPatch: " << targetPatch << std::endl;

  PatchPair<FloatScalarImageType> patchPair(&sourcePatch, targetPatch);
  PatchDifferencePixelWiseSum<FloatScalarImageType, PixelDifference> scalar_patchDifferencePixelWiseSum;
  scalar_patchDifferencePixelWiseSum.SetImage(scalarImage);
  float difference = scalar_patchDifferencePixelWiseSum.Difference(patchPair);

  std::cout << "Number of pixels: " << targetPatch.GetRegion().GetNumberOfPixels() << std::endl;

  float correctDifference = targetPatch.GetRegion().GetNumberOfPixels() * 5;

  if(difference != correctDifference)
    {
    std::stringstream ss;
    ss << "Difference " << difference << " does not match correctDifference " << correctDifference;
    throw std::runtime_error(ss.str());
    }
  }

  std::cout << "Identical patch test." << std::endl;
  // Patches are identical
  {
  itk::Index<2> sourceCorner;
  sourceCorner.Fill(0);
  itk::ImageRegion<2> sourceRegion(sourceCorner, patchSize);
  ImagePatchPixelDescriptor<FloatScalarImageType> sourcePatch(scalarImage, sourceRegion, true);

  itk::Index<2> targetCorner;
  targetCorner.Fill(10); // No magic about 10, just want a patch not at (0,0) but still fully on the left side of the image
  itk::ImageRegion<2> targetRegion(targetCorner, patchSize);
  ImagePatchPixelDescriptor<FloatScalarImageType> targetPatch(scalarImage, targetRegion, true);

  PatchPair<FloatScalarImageType> patchPair(&sourcePatch, targetPatch);
  PatchDifferencePixelWiseSum<FloatScalarImageType, PixelDifference> scalar_patchDifferencePixelWiseSum;
  scalar_patchDifferencePixelWiseSum.SetImage(scalarImage);
  float difference = scalar_patchDifferencePixelWiseSum.Difference(patchPair);

  float correctDifference = 0;
  if(difference != correctDifference)
    {
    std::stringstream ss;
    ss << "Difference " << difference << " does not match correctDifference " << correctDifference;
    throw std::runtime_error(ss.str());
    }
  }
}
void PartialPatchVectorComparison()
{
  std::cout << "PartialPatchVectorComparison()" << std::endl;

  const unsigned int dimension = 3;
  FloatVectorImageType::Pointer vectorImage = FloatVectorImageType::New();
  Testing::GetBlankImage<FloatVectorImageType>(vectorImage, dimension);

  // Make the left half of the image (0,0,0) and the right half (5,6,7)
  itk::ImageRegionIterator<FloatVectorImageType> imageIterator(vectorImage, vectorImage->GetLargestPossibleRegion());

  itk::VariableLengthVector<float> leftHalfPixel;
  leftHalfPixel.SetSize(dimension);
  leftHalfPixel.Fill(0);

  itk::VariableLengthVector<float> rightHalfPixel;
  rightHalfPixel.SetSize(dimension);
  rightHalfPixel[0] = 5;
  rightHalfPixel[1] = 6;
  rightHalfPixel[2] = 7;

  while(!imageIterator.IsAtEnd())
    {
    if(imageIterator.GetIndex()[0] < static_cast<int>(vectorImage->GetLargestPossibleRegion().GetSize()[0]/2))
      {
      imageIterator.Set(leftHalfPixel);
      }
    else
      {
      imageIterator.Set(rightHalfPixel);
      }

    ++imageIterator;
    }

  itk::Size<2> patchSize;
  patchSize.Fill(10);

  // Full patches differ
  std::cout << "Full patch different test." << std::endl;
  {
  itk::Index<2> sourceCorner;
  sourceCorner.Fill(0);
  itk::ImageRegion<2> sourceRegion(sourceCorner, patchSize);
  ImagePatchPixelDescriptor<FloatVectorImageType> sourcePatch(vectorImage, sourceRegion, true);

  itk::Index<2> targetCorner;
  targetCorner.Fill(vectorImage->GetLargestPossibleRegion().GetSize()[0]/2 + 4); // No magic about 4, just want a patch on the right side of the image
  itk::ImageRegion<2> targetRegion(targetCorner, patchSize);
  ImagePatchPixelDescriptor<FloatVectorImageType> targetPatch(vectorImage, targetRegion, true);

  PatchPair<FloatVectorImageType> patchPair(&sourcePatch, targetPatch);
  PatchDifferencePixelWiseSum<FloatVectorImageType, PixelDifference> vector_patchDifferencePixelWiseSum;
  vector_patchDifferencePixelWiseSum.SetImage(vectorImage);
  float difference = vector_patchDifferencePixelWiseSum.Difference(patchPair);

  float correctDifference = targetRegion.GetNumberOfPixels() * 18; // 18 = 5+6+7, the sum of the elements of 'rightHalfPixel'
  if(difference != correctDifference)
    {
    std::stringstream ss;
    ss << "Difference " << difference << " does not match correctDifference " << correctDifference;
    throw std::runtime_error(ss.str());
    }
  }

  // Full patches identical
  std::cout << "Identical patch test." << std::endl;
  {
  itk::Index<2> sourceCorner;
  sourceCorner.Fill(5);
  itk::ImageRegion<2> sourceRegion(sourceCorner, patchSize);
  ImagePatchPixelDescriptor<FloatVectorImageType> sourcePatch(vectorImage, sourceRegion, true);

  itk::Index<2> targetCorner;
  targetCorner.Fill(5);
  itk::ImageRegion<2> targetRegion(targetCorner, patchSize);
  ImagePatchPixelDescriptor<FloatVectorImageType> targetPatch(vectorImage, targetRegion, true);

  PatchPair<FloatVectorImageType> patchPair(&sourcePatch, targetPatch);
  PatchDifferencePixelWiseSum<FloatVectorImageType, PixelDifference> vector_patchDifferencePixelWiseSum;
  vector_patchDifferencePixelWiseSum.SetImage(vectorImage);
  float difference = vector_patchDifferencePixelWiseSum.Difference(patchPair);

  float correctDifference = 0;
  if(difference != correctDifference)
    {
    std::stringstream ss;
    ss << "Difference " << difference << " does not match correctDifference " << correctDifference;
    throw std::runtime_error(ss.str());
    }
  }
}