#include "antsUtilities.h"
#include <algorithm>

#include "itkAddImageFilter.h"
#include "itkBSplineScatteredDataPointSetToImageFilter.h"
#include "itkContinuousIndex.h"
#include "itkGradientMagnitudeRecursiveGaussianImageFilter.h"
#include "itkImageRegionConstIteratorWithIndex.h"
#include "itkImportImageFilter.h"
#include "itkPointSet.h"
#include "itkRescaleIntensityImageFilter.h"
#include "itkTimeProbe.h"
#include "itkVector.h"
#include "itkVectorIndexSelectionCastImageFilter.h"

#include "ReadWriteData.h"

namespace ants
{
template <unsigned int ImageDimension>
int SuperResolution( unsigned int argc, char *argv[] )
{
  typedef float                                   RealType;
  typedef itk::Image<RealType, ImageDimension>    ImageType;

  typedef itk::Vector<RealType, 1>                         ScalarType;
  typedef itk::Image<ScalarType, ImageDimension>           ScalarImageType;
  typedef itk::PointSet<ScalarType, ImageDimension>        PointSetType;
  typedef itk::BSplineScatteredDataPointSetToImageFilter
    <PointSetType, ScalarImageType>                        BSplineFilterType;

  typename ImageType::Pointer domainImage = ITK_NULLPTR;
  ReadImage<ImageType>( domainImage, argv[3] );

  typename BSplineFilterType::Pointer bspliner = BSplineFilterType::New();

  typename PointSetType::Pointer bsplinePoints = PointSetType::New();
  bsplinePoints->Initialize();

  typename BSplineFilterType::WeightsContainerType::Pointer weights =
    BSplineFilterType::WeightsContainerType::New();
  weights->Initialize();

  unsigned int splineOrder = 3;
  typename BSplineFilterType::ArrayType numberOfLevels;
  typename BSplineFilterType::ArrayType ncps;

  bool useGradientWeighting = true;
  RealType gradientSigma = atof( argv[4] );
  if( gradientSigma < 0.0 )
    {
    useGradientWeighting = false;
    }

  std::vector<unsigned int> meshSize = ConvertVector<unsigned int>( std::string( argv[5] ) );
  if ( meshSize.size() == 1 )
    {
    ncps.Fill( meshSize[0] + splineOrder );
    }
  else if ( meshSize.size() == ImageDimension )
    {
    for ( unsigned int d = 0; d < ImageDimension; d++ )
      {
      ncps[d] = meshSize[d] + splineOrder;
      }
    }
  else
    {
    std::cerr << "Invalid ncps format." << std::endl;
    return EXIT_FAILURE;
    }

  std::vector<unsigned int> nlevels = ConvertVector<unsigned int>( std::string( argv[6] ) );
  if ( nlevels.size() == 1 )
    {
    numberOfLevels.Fill( nlevels[0] );
    }
  else if ( nlevels.size() == ImageDimension )
    {
    for ( unsigned int d = 0; d < ImageDimension; d++ )
      {
      numberOfLevels[d] = nlevels[d];
      }
    }
  else
    {
    std::cerr << "Invalid nlevels format." << std::endl;
    return EXIT_FAILURE;
    }

  // Find the average for the offset

  typename ImageType::IndexType domainBeginIndex = domainImage->GetRequestedRegion().GetIndex();
  typename ImageType::IndexType domainEndIndex;
  for( unsigned int d = 0; d < ImageDimension; d++ )
    {
    domainEndIndex[d] = domainBeginIndex[d] + domainImage->GetRequestedRegion().GetSize()[d] - 1;
    }

  RealType averageIntensity = 0.0;
  unsigned int N = 0;
  for( unsigned int n = 7; n < argc; n++ )
    {
    typename ImageType::Pointer inputImage = ITK_NULLPTR;
    ReadImage<ImageType>( inputImage, argv[n] );

    itk::ImageRegionConstIteratorWithIndex<ImageType> It( inputImage, inputImage->GetRequestedRegion() );
    for( It.GoToBegin(); !It.IsAtEnd(); ++It )
      {
      typename ImageType::PointType imagePoint;
      inputImage->TransformIndexToPhysicalPoint( It.GetIndex(), imagePoint );

      itk::ContinuousIndex<RealType, ImageDimension> cidx;
      bool isInside = domainImage->TransformPhysicalPointToContinuousIndex( imagePoint, cidx );

      if( isInside )
        {
        for( unsigned int d = 0; d < ImageDimension; d++ )
          {
          if( cidx[d] <= domainBeginIndex[d] || cidx[d] >= domainEndIndex[d] )
            {
            isInside = false;
            break;
            }
          }
        }

      if( !isInside )
        {
        continue;
        }

      averageIntensity += It.Get();

      N++;
      }
    }
  averageIntensity /= static_cast<RealType>( N );


  typename ScalarImageType::DirectionType identity;
  identity.SetIdentity();

  const typename ScalarImageType::RegionType & bufferedRegion = domainImage->GetBufferedRegion();
  const itk::SizeValueType numberOfPixels = bufferedRegion.GetNumberOfPixels();
  const bool filterHandlesMemory = false;

  typedef itk::ImportImageFilter<RealType, ImageDimension> ImporterType;
  typename ImporterType::Pointer importer = ImporterType::New();
  importer->SetImportPointer( domainImage->GetBufferPointer(), numberOfPixels, filterHandlesMemory );
  importer->SetRegion( domainImage->GetBufferedRegion() );
  importer->SetOrigin( domainImage->GetOrigin() );
  importer->SetSpacing( domainImage->GetSpacing() );
  importer->SetDirection( identity );
  importer->Update();

  const typename ImporterType::OutputImageType * parametricDomainImage = importer->GetOutput();

  N = 0;
  for( unsigned int n = 7; n < argc; n++ )
    {
    typename ImageType::Pointer inputImage = ITK_NULLPTR;
    ReadImage<ImageType>( inputImage, argv[n] );

    typename ImageType::Pointer gradientImage = ITK_NULLPTR;
    if( useGradientWeighting )
      {
      typedef itk::GradientMagnitudeRecursiveGaussianImageFilter<ImageType, ImageType> GradientFilterType;
      typename GradientFilterType::Pointer gradientFilter = GradientFilterType::New();
      gradientFilter->SetSigma( gradientSigma );
      gradientFilter->SetInput( inputImage );

      typedef itk::RescaleIntensityImageFilter<ImageType, ImageType> RescaleFilterType;
      typename RescaleFilterType::Pointer rescaler = RescaleFilterType::New();
      rescaler->SetOutputMinimum( 0.0 );
      rescaler->SetOutputMaximum( 1.0 );
      rescaler->SetInput( gradientFilter->GetOutput() );

      gradientImage = rescaler->GetOutput();
      gradientImage->Update();
      gradientImage->DisconnectPipeline();
      }

    itk::ImageRegionConstIteratorWithIndex<ImageType> It( inputImage, inputImage->GetRequestedRegion() );
    for( It.GoToBegin(); !It.IsAtEnd(); ++It )
      {
      typename ImageType::PointType imagePoint;
      inputImage->TransformIndexToPhysicalPoint( It.GetIndex(), imagePoint );

      itk::ContinuousIndex<RealType, ImageDimension> cidx;
      bool isInside = domainImage->TransformPhysicalPointToContinuousIndex( imagePoint, cidx );

      if( isInside )
        {
        for( unsigned int d = 0; d < ImageDimension; d++ )
          {
          if( cidx[d] <= domainBeginIndex[d] || cidx[d] >= domainEndIndex[d] )
            {
            isInside = false;
            break;
            }
          }
        }

      if( !isInside )
        {
        continue;
        }

      RealType weight = 1.0;
      if( gradientImage.IsNotNull() )
        {
        weight = gradientImage->GetPixel( It.GetIndex() );
        }
      if( weight == 0.0 )
        {
        continue;
        }

//       domainImage->SetPixel( index, 1 );

      parametricDomainImage->TransformContinuousIndexToPhysicalPoint( cidx, imagePoint );

      ScalarType scalar;
      scalar[0] = It.Get() - averageIntensity;

      bsplinePoints->SetPointData( N, scalar );
      bsplinePoints->SetPoint( N, imagePoint );
      weights->InsertElement( N, weight );

      N++;
      }
    }

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

  typename ScalarImageType::PointType parametricOrigin = domainImage->GetOrigin();
  for( unsigned int d = 0; d < ImageDimension; d++ )
    {
    parametricOrigin[d] += (
        domainImage->GetSpacing()[d] *
        domainImage->GetLargestPossibleRegion().GetIndex()[d] );
    }

  bspliner->SetOrigin( parametricOrigin );
  bspliner->SetSpacing( domainImage->GetSpacing() );
  bspliner->SetSize( domainImage->GetRequestedRegion().GetSize() );
  bspliner->SetDirection( domainImage->GetDirection() );
  bspliner->SetGenerateOutputImage( true );
  bspliner->SetNumberOfLevels( numberOfLevels );
  bspliner->SetSplineOrder( splineOrder );
  bspliner->SetNumberOfControlPoints( ncps );
  bspliner->SetInput( bsplinePoints );
  bspliner->SetPointWeights( weights );
  bspliner->Update();

  timer.Stop();

  std::cout << "Elapsed Time:  " << timer.GetMean() << std::endl;

  typedef itk::VectorIndexSelectionCastImageFilter<ScalarImageType, ImageType> SelectorType;
  typename SelectorType::Pointer selector = SelectorType::New();
  selector->SetInput( bspliner->GetOutput() );
  selector->SetIndex( 0 );
  selector->Update();

  typedef itk::AddImageFilter<ImageType> AdderType;
  typename AdderType::Pointer adder = AdderType::New();
  adder->SetInput( selector->GetOutput() );
  adder->SetConstant2( averageIntensity );
  adder->Update();

  WriteImage<ImageType>( adder->GetOutput(), argv[2] );

  return EXIT_SUCCESS;
}

// entry point for the library; parameter 'args' is equivalent to 'argv' in (argc,argv) of commandline parameters to
// 'main()'
int SuperResolution( std::vector<std::string> args, std::ostream* /*out_stream = NULL */ )
{
  // put the arguments coming in as 'args' into standard (argc,argv) format;
  // 'args' doesn't have the command name as first, argument, so add it manually;
  // 'args' may have adjacent arguments concatenated into one argument,
  // which the parser should handle
  args.insert( args.begin(), "SuperResolution" );

  int     argc = args.size();
  char* * argv = new char *[args.size() + 1];
  for( unsigned int i = 0; i < args.size(); ++i )
    {
    // allocate space for the string plus a null character
    argv[i] = new char[args[i].length() + 1];
    std::strncpy( argv[i], args[i].c_str(), args[i].length() );
    // place the null character in the end
    argv[i][args[i].length()] = '\0';
    }
  argv[argc] = ITK_NULLPTR;
  // class to automatically cleanup argv upon destruction
  class Cleanup_argv
  {
public:
    Cleanup_argv( char* * argv_, int argc_plus_one_ ) : argv( argv_ ), argc_plus_one( argc_plus_one_ )
    {
    }

    ~Cleanup_argv()
    {
      for( unsigned int i = 0; i < argc_plus_one; ++i )
        {
        delete[] argv[i];
        }
      delete[] argv;
    }

private:
    char* *      argv;
    unsigned int argc_plus_one;
  };
  Cleanup_argv cleanup_argv( argv, argc + 1 );

  // antscout->set_stream( out_stream );

  if ( argc < 8 )
    {
    std::cerr << argv[0] << " imageDimension outputImage domainImage gradientSigma meshSize numberOfLevels inputImage1 ... inputImageN" << std::endl;
    std::cerr << std::endl;
    std::cerr << "    N.B. The \'gradientSigma\' parameter is used in calculating the gradient magnitude of the input images " << std::endl;
    std::cerr << "       for weighting the voxel points during fitting.  If a negative \'gradient\' sigma is specified then no " << std::endl;
    std::cerr << "       weighting is used." << std::endl;

    if( argc >= 2 &&
        ( std::string( argv[1] ) == std::string("--help") || std::string( argv[1] ) == std::string("-h") ) )
      {
      return EXIT_SUCCESS;
      }
    return EXIT_FAILURE;
    }

  const int ImageDimension = static_cast<int>( atoi( argv[1] ) );

  switch( ImageDimension )
     {
     case 2:
       return SuperResolution<2>( argc, argv );
       break;
     case 3:
       return SuperResolution<3>( argc, argv );
       break;
     case 4:
       return SuperResolution<4>( argc, argv );
       break;
     default:
        std::cerr << "Unsupported dimension" << std::endl;
        exit( EXIT_FAILURE );
     }
  return EXIT_SUCCESS;
}
} // namespace ants

