#include "itkCSVNumericObjectFileWriter.h"
#include "antsUtilities.h"
#include "antsAllocImage.h"
#include "itkantsRegistrationHelper.h"
#include "itkCSVArray2DFileReader.h"
#include "itkAffineTransform.h"
#include "itkCompositeTransform.h"
#include "itkDisplacementFieldTransform.h"
#include "itkIdentityTransform.h"
#include "itkImageFileReader.h"
#include "itkImageFileWriter.h"
#include "itkMatrixOffsetTransformBase.h"
#include "itkResampleImageFilter.h"
#include "itkTransformFactory.h"
#include "itkTransformFileReader.h"

#include "itkBSplineInterpolateImageFunction.h"
#include "itkLinearInterpolateImageFunction.h"
#include "itkGaussianInterpolateImageFunction.h"
#include "itkNearestNeighborInterpolateImageFunction.h"
#include "itkWindowedSincInterpolateImageFunction.h"
#include "itkLabelImageGaussianInterpolateImageFunction.h"

namespace ants
{
template <unsigned int Dimension, class RealType>
int antsApplyTransformsToPoints( itk::ants::CommandLineParser::Pointer & parser )
{
  typedef vnl_matrix<RealType> MatrixType;
  MatrixType points_out;
  MatrixType points_in;
  typedef itk::Image<RealType, 2>                       ImageType;
  typedef itk::CSVArray2DFileReader<RealType>              ReaderType;
  typedef itk::CSVArray2DDataObject<RealType>              DataFrameObjectType;
  typedef typename DataFrameObjectType::StringVectorType StringVectorType;
  StringVectorType colheadernames;
  typename ImageType::Pointer pointimage = ITK_NULLPTR;

  /**
   * Input object option - for now, we're limiting this to images.
   */
  typename itk::ants::CommandLineParser::OptionType::Pointer inputOption = parser->GetOption( "input" );
  typename itk::ants::CommandLineParser::OptionType::Pointer outputOption = parser->GetOption( "output" );
  if( inputOption && inputOption->GetNumberOfFunctions() > 0 )
    {
    std::string ext =
      itksys::SystemTools::GetFilenameExtension(  ( inputOption->GetFunction( 0 )->GetName() ).c_str()  );
    if( strcmp(ext.c_str(), ".csv") == 0 )
      {
      typename ReaderType::Pointer reader = ReaderType::New();
      reader->SetFileName(  ( inputOption->GetFunction( 0 )->GetName() ).c_str()  );
      reader->SetFieldDelimiterCharacter( ',' );
      reader->SetStringDelimiterCharacter( '"' );
      reader->HasColumnHeadersOn();
      reader->HasRowHeadersOff();
      //    reader->UseStringDelimiterCharacterOff();
      try
        {
        reader->Update();
        }
      catch( itk::ExceptionObject& exp )
        {
        std::cerr << "Exception caught!" << std::endl;
        std::cerr << exp << std::endl;
        }
      typename DataFrameObjectType::Pointer dfo = reader->GetOutput();
      colheadernames = dfo->GetColumnHeaders();
      if( colheadernames.size() < Dimension )
        {
        std::cerr
          <<
          "Input csv file must have column names such as x,y,z,t,label - where there are a minimum of N-Spatial-Dimensions names e.g. x,y in 2D.  ***Or pass in a 2D mha (meta format) binary image file."
          << std::endl;
        return EXIT_FAILURE;
        }
      points_in = dfo->GetMatrix();
      points_out.set_size( points_in.rows(),  points_in.cols() );
      }
    else if( strcmp(ext.c_str(), ".mha") == 0 )
      {
      std::string fn1 = inputOption->GetFunction( 0 )->GetName();
      ReadImage<ImageType>( pointimage, fn1.c_str() );
      typename ImageType::IndexType ind;
      ind.Fill(0);
      typename ImageType::SizeType sz;
      sz.Fill(0);
      sz = pointimage->GetLargestPossibleRegion().GetSize();
      points_in.set_size( sz[0],  sz[1] );
      points_out.set_size( points_in.rows(),  points_in.cols() );
      for ( unsigned int d = 0; d < sz[0]; d++ )
        for ( unsigned int dd = 0; dd < sz[1]; dd++ )
          {
          ind[0] = d;
          ind[1] = dd;
          points_in( d, dd ) = pointimage->GetPixel( ind );
          }
      }
    else
      {
      std::cerr << "An input csv or mha file is required." << std::endl;
      return EXIT_FAILURE;
      }

    if(  points_in.cols() < Dimension )
      {
      std::cerr << "The number of columns in the input point set is fewer than " << Dimension << " Exiting."
                << std::endl;
      return EXIT_FAILURE;
      }

    if( outputOption && outputOption->GetNumberOfFunctions() > 0 )
      {
      if( outputOption->GetFunction( 0 )->GetNumberOfParameters() > 1 &&
          parser->Convert<unsigned int>( outputOption->GetFunction( 0 )->GetParameter( 1 ) ) == 0 )
        {
        std::cerr << "An input csv file is required." << std::endl;
        return EXIT_FAILURE;
        }
      }

    /**
     * Transform option
     */
    // Register the matrix offset transform base class to the
    // transform factory for compatibility with the current ANTs.
    typedef itk::MatrixOffsetTransformBase<RealType, Dimension, Dimension> MatrixOffsetTransformType;
    itk::TransformFactory<MatrixOffsetTransformType>::RegisterTransform();
    typedef itk::MatrixOffsetTransformBase<RealType, Dimension, Dimension> MatrixOffsetTransformType;
    itk::TransformFactory<MatrixOffsetTransformType>::RegisterTransform();

    /**
     * Load an identity transform in case no transforms are loaded.
     */
    // Register the matrix offset transform base class to the
    // transform factory for compatibility with the current ANTs.
    typedef itk::AffineTransform<RealType, Dimension> AffineTransformType;
    typename AffineTransformType::Pointer aff =
      AffineTransformType::New();
    aff->SetIdentity();

    typedef itk::CompositeTransform<RealType, Dimension> CompositeTransformType;
    typename CompositeTransformType::InputPointType point_in;
    typename CompositeTransformType::OutputPointType point_out;
    typename itk::ants::CommandLineParser::OptionType::Pointer
      transformOption = parser->GetOption( "transform" );

    std::vector<bool> isDerivedTransform;
    typename CompositeTransformType::Pointer compositeTransform =
      GetCompositeTransformFromParserOption<RealType, Dimension>( parser, transformOption, isDerivedTransform );

    if ( compositeTransform->GetNumberOfTransforms() == 0 )
      compositeTransform->AddTransform( aff );

    if( compositeTransform.IsNull() )
      {
      return EXIT_FAILURE;
      }
    for( unsigned int pointct = 0; pointct < points_in.rows(); pointct++ )
      {
      point_in.Fill( 0 );
      point_out.Fill( 0 );
      for( unsigned int p = 0; p < Dimension; p++ )
        {
        point_in[p] = points_in( pointct, p );
        }
      point_out = compositeTransform->TransformPoint( point_in );
      for( unsigned int p = 0; p < Dimension; p++ )
        {
        points_out( pointct, p ) = point_out[p];
        }
      for( unsigned int p = Dimension; p < points_in.cols(); p++ )
        {
        points_out( pointct, p ) = points_in( pointct, p );
        }
      }
    /**
     * output
     */
    if( outputOption && outputOption->GetNumberOfFunctions() > 0 )
      {
      std::string outputFileName = "";
      if( outputOption->GetFunction( 0 )->GetNumberOfParameters() > 1 &&
          parser->Convert<unsigned int>( outputOption->GetFunction( 0 )->GetParameter( 1 ) ) == 0 )
        {
        outputFileName = outputOption->GetFunction( 0 )->GetParameter( 0 );
        }
      else
        {
        outputFileName = outputOption->GetFunction( 0 )->GetName();
        }
      std::string exto =
        itksys::SystemTools::GetFilenameExtension( outputFileName.c_str() );

      if( strcmp(exto.c_str(), ".csv" ) == 0 )
        {
        StringVectorType ColumnHeaders = colheadernames;
        typedef itk::CSVNumericObjectFileWriter<RealType, 1, 1> WriterType;
        typename WriterType::Pointer writer = WriterType::New();
        writer->SetFileName( outputFileName );
        writer->SetInput( &points_out );
        writer->SetColumnHeaders( ColumnHeaders );
        try
          {
          writer->Write();
          }
        catch( itk::ExceptionObject& exp )
          {
          std::cerr << "Exception caught!" << std::endl;
          std::cerr << exp << std::endl;
          return EXIT_FAILURE;
          }
        }
      if( ( strcmp(exto.c_str(), ".mha" ) == 0 ) &&
          (  ! pointimage.IsNull() ) )
        {
        typename ImageType::IndexType ind;
        ind.Fill(0);
        typename ImageType::SizeType sz;
        sz.Fill(0);
        sz = pointimage->GetLargestPossibleRegion().GetSize();
        if ( sz[0] != points_out.rows() ||
             sz[1] != points_out.cols() )
          {
          std::cout << " the size of points_out must match the input pointimage" << std::endl;
          return EXIT_FAILURE;
          }
        for ( unsigned int d = 0; d < sz[0]; d++ )
          for ( unsigned int dd = 0; dd < sz[1]; dd++ )
            {
            ind[0] = d;
            ind[1] = dd;
            pointimage->SetPixel( ind , points_out( d, dd ) );
            }
        WriteImage<ImageType>(pointimage, outputFileName.c_str() );
        }
      }
    }

  return EXIT_SUCCESS;
}

static void antsApplyTransformsToPointsInitializeCommandLineOptions( itk::ants::CommandLineParser *parser )
{

    {
    std::string description =
      std::string( "This option forces the points to be treated as a specified-" )
      + std::string( "dimensionality." );

    OptionType::Pointer option = OptionType::New();
    option->SetLongName( "dimensionality" );
    option->SetShortName( 'd' );
    option->SetUsageOption( 0, "2/3" );
    option->SetDescription( description );
    parser->AddOption( option );
    }

    {
    std::string description =
      std::string( "use-double-precision" );
    OptionType::Pointer option = OptionType::New();
    option->SetLongName( "precision" );
    option->SetShortName( 'p' );
    option->SetUsageOption( 0, "0/1" );
    option->SetDescription( description );
    parser->AddOption( option );
    }

    {
    std::string description =
      std::string( "Currently, the only input supported is a csv file with " )
      + std::string( "columns including x,y,z,t (all 4) column headers. " )
      + std::string( "if you dont have 4D data, still supply 4D filling in extra places with zero. " )
      + std::string( "The points should be defined in physical space. " )
      + std::string( "Points are transformed in the OPPOSITE direction of images, therefore " )
      + std::string( "you should pass the inverse of what is needed to warp the images. " )
      + std::string( "Eg if the image is warped by  Affine.mat, you should pass the inverse of Affine.mat " )
      + std::string( "to transform points defined in the same space as the image. " )
      + std::string( "If in doubt how to convert coordinates from your files to the space " )
      + std::string( "required by antsApplyTransformsToPoints try creating/drawing a simple " )
      + std::string( "label volume with only one voxel set to 1 and all others set to 0. " )
      + std::string( "Write down the voxel coordinates. Then use ImageMaths LabelStats to find " )
      + std::string( "out what coordinates for this voxel antsApplyTransformsToPoints is " )
      + std::string( "expecting.  ITK uses a LPS coordinate system.  See http://sourceforge.net/p/advants/discussion/840261/thread/2a1e9307/" )
      + std::string(" ***Or pass in a 2D mha (meta format) binary image file.");

    OptionType::Pointer option = OptionType::New();
    option->SetLongName( "input" );
    option->SetShortName( 'i' );
    option->SetUsageOption( 0, "inputFileName" );
    option->SetDescription( description );
    parser->AddOption( option );
    }

    {
    std::string description =
      std::string( "One can output the warped points to a csv file.");

    OptionType::Pointer option = OptionType::New();
    option->SetLongName( "output" );
    option->SetShortName( 'o' );
    option->SetUsageOption( 0, "warpedOutputFileName" );
    option->SetDescription( description );
    parser->AddOption( option );
    }

    {
    std::string description =
      std::string( "Several transform options are supported including all " )
      + std::string( "those defined in the ITK library in addition to " )
      + std::string( "a deformation field transform.  The ordering of " )
      + std::string( "the transformations follows the ordering specified " )
      + std::string( "on the command line.  An identity transform is pushed " )
      + std::string( "onto the transformation stack. Each new transform " )
      + std::string( "encountered on the command line is also pushed onto " )
      + std::string( "the transformation stack. Then, to warp the input object, " )
      + std::string( "each point comprising the input object is warped first " )
      + std::string( "according to the last transform pushed onto the stack " )
      + std::string( "followed by the second to last transform, etc. until " )
      + std::string( "the last transform encountered which is the identity " )
      + std::string( "transform. " )
      + std::string( "Also, it should be noted that the inverse transform can " )
      + std::string( "be accommodated with the usual caveat that such an inverse " )
      + std::string( "must be defined by the specified transform class " );

    OptionType::Pointer option = OptionType::New();
    option->SetLongName( "transform" );
    option->SetShortName( 't' );
    option->SetUsageOption( 0, "transformFileName" );
    option->SetUsageOption( 1, "[transformFileName,useInverse]" );
    option->SetDescription( description );
    parser->AddOption( option );
    }

    {
    std::string description = std::string( "Print the help menu (short version)." );

    OptionType::Pointer option = OptionType::New();
    option->SetShortName( 'h' );
    option->SetDescription( description );
    option->AddFunction( std::string( "0" ) );
    parser->AddOption( option );
    }

    {
    std::string description = std::string( "Print the help menu." );

    OptionType::Pointer option = OptionType::New();
    option->SetLongName( "help" );
    option->SetDescription( description );
    option->AddFunction( std::string( "0" ) );
    parser->AddOption( option );
    }
}

// entry point for the library; parameter 'args' is equivalent to 'argv' in (argc,argv) of commandline parameters to
// 'main()'
int antsApplyTransformsToPoints( 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(), "antsApplyTransformsToPoints" );
  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 );

  itk::ants::CommandLineParser::Pointer parser =
    itk::ants::CommandLineParser::New();

  parser->SetCommand( argv[0] );

  std::string examplestring = std::string(
      "reads in a csv file with the first D columns defining the spatial location where the spatial location is defined in physical coordinates.    the csv file should have a header row.   here is an example")
    + std::string("\n") + std::string("cat chicken-3.csv ") + std::string("x,y,z,t,label,comment") + std::string("\n")
    + std::string("82.5,116.5,0,0,1,this is the breast") + std::string("\n") + std::string(
      "137.5,35.5,0,0,2,this is the beak") + std::string("\n") + std::string(
      "antsApplyTransformsToPoints -d 2 -i chicken-3.csv -o test.csv -t [chicken3to4.mat ,1 ]") + std::string("\n")
    + std::string("cat test.csv ") + std::string("\n") + std::string("x,y,z,t,label,comment") + std::string("\n")
    + std::string("10.8945447481644,162.082675013049,0,0,1,nan") + std::string("\n") + std::string(
      "7.5367085472988,52.099713111629,0,0,2,nan") + std::string("\n") + std::string(
      "the nan appears in the last column until the ITK CSV I/O can handle mixed numeric / string types.  if your input is fully numeric, all is well.");

  std::string mhastring = std::string("\n\n**** We now can also read / write .mha files.") + std::string("\n") + std::string("This is a simple binary format (Meta format - look it up!) that is much faster to read/write than csv format.\n Note: To write a mha file, you must also pass an mha file as input.\n");

  std::string commandDescription =
    std::string( "antsApplyTransformsToPoints, applied to an input image, transforms it " )
    + std::string( "according to a reference image and a transform " )
    + std::string( "(or a set of transforms).  " ) + examplestring +
    mhastring;

  parser->SetCommandDescription( commandDescription );
  antsApplyTransformsToPointsInitializeCommandLineOptions( parser );

  if( parser->Parse( argc, argv ) == EXIT_FAILURE )
    {
    return EXIT_FAILURE;
    }

  if( argc < 2 || ( parser->GetOption( "help" ) &&
                    ( parser->Convert<bool>( parser->GetOption( "help" )->GetFunction()->GetName() ) ) ) )
    {
    parser->PrintMenu( std::cout, 5, false );
    if( argc < 2 )
      {
      return EXIT_FAILURE;
      }
    return EXIT_SUCCESS;
    }
  else if( parser->GetOption( 'h' ) &&
           ( parser->Convert<bool>( parser->GetOption( 'h' )->GetFunction()->GetName() ) ) )
    {
    parser->PrintMenu( std::cout, 5, true );
    return EXIT_SUCCESS;
    }

  unsigned int                                      dimension = 3;
  itk::ants::CommandLineParser::OptionType::Pointer dimOption =
    parser->GetOption( "dimensionality" );
  if( dimOption && dimOption->GetNumberOfFunctions() > 0 )
    {
    dimension = parser->Convert<unsigned int>( dimOption->GetFunction( 0 )->GetName() );
    }
  else
    {
    std::cerr << "No -d ( dimensionality ) option is specified.  Exiting." << std::endl;
    return EXIT_FAILURE;
    }

  itk::ants::CommandLineParser::OptionType::Pointer precOption =
    parser->GetOption( "precision" );
  unsigned int myprecision = 0;
  if( precOption && precOption->GetNumberOfFunctions() > 0 )
    {
    myprecision = parser->Convert<unsigned int>( precOption->GetFunction( 0 )->GetName() );
    }

  if ( myprecision == 1 )
    {
    switch( dimension )
      {
      case 2:
        {
        antsApplyTransformsToPoints<2,double>( parser );
        }
        break;
      case 3:
        {
        antsApplyTransformsToPoints<3,double>( parser );
        }
        break;
      case 4:
        {
        antsApplyTransformsToPoints<4,double>( parser );
        }
        break;
      default:
        std::cerr << "Unsupported dimension" << std::endl;
        return EXIT_FAILURE;
      }
    }
  else
    {
    switch( dimension )
      {
      case 2:
        {
        antsApplyTransformsToPoints<2,float>( parser );
        }
        break;
      case 3:
        {
        antsApplyTransformsToPoints<3,float>( parser );
        }
        break;
      case 4:
        {
        antsApplyTransformsToPoints<4,float>( parser );
        }
        break;
      default:
        std::cerr << "Unsupported dimension" << std::endl;
        return EXIT_FAILURE;
      }
    }
  return EXIT_SUCCESS;
}

} // namespace ants
