/*=========================================================================

  Program:   Advanced Normalization Tools

  Copyright (c) ConsortiumOfANTS. All rights reserved.
  See accompanying COPYING.txt or
 https://github.com/stnava/ANTs/blob/master/ANTSCopyright.txt for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notices for more information.

=========================================================================*/

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

#include "itkDisplacementFieldToBSplineImageFilter.h"
#include "itkGaussianOperator.h"
#include "itkImage.h"
#include "itkImageDuplicator.h"
#include "itkTimeProbe.h"
#include "itkImageRegionIteratorWithIndex.h"
#include "itkImageRegionConstIteratorWithIndex.h"
#include "itkVectorNeighborhoodOperatorImageFilter.h"

namespace ants
{

template <unsigned int ImageDimension>
int SmoothDisplacementField( int argc, char *argv[] )
{

  typedef float RealType;
  typedef itk::Image<RealType, ImageDimension> RealImageType;
  typedef itk::Vector<RealType, ImageDimension> VectorType;
  typedef itk::Image<VectorType, ImageDimension> DisplacementFieldType;

  typename DisplacementFieldType::Pointer field = ITK_NULLPTR;
  ReadImage<DisplacementFieldType>( field, argv[2] );

  typename DisplacementFieldType::Pointer smoothField = DisplacementFieldType::New();

  float elapsedTime = 0.0;

  std::vector<float> var = ConvertVector<float>( std::string( argv[4] ) );
  if( var.size() == 1 )
    {
    float variance = var[0];

    typedef itk::GaussianOperator<float, ImageDimension>                                             GaussianSmoothingOperatorType;
    typedef itk::VectorNeighborhoodOperatorImageFilter<DisplacementFieldType, DisplacementFieldType> GaussianSmoothingSmootherType;

    GaussianSmoothingOperatorType gaussianSmoothingOperator;

    typedef itk::ImageDuplicator<DisplacementFieldType> DuplicatorType;
    typename DuplicatorType::Pointer duplicator = DuplicatorType::New();
    duplicator->SetInputImage( field );
    duplicator->Update();

    smoothField = duplicator->GetOutput();

    itk::TimeProbe timer;
    timer.Start();
    typename GaussianSmoothingSmootherType::Pointer smoother = GaussianSmoothingSmootherType::New();

    for( unsigned int dimension = 0; dimension < ImageDimension; ++dimension )
      {
      // smooth along this dimension
      gaussianSmoothingOperator.SetDirection( dimension );
      gaussianSmoothingOperator.SetVariance( variance );
      gaussianSmoothingOperator.SetMaximumError( 0.001 );
      gaussianSmoothingOperator.SetMaximumKernelWidth( smoothField->GetRequestedRegion().GetSize()[dimension] );
      gaussianSmoothingOperator.CreateDirectional();

      // todo: make sure we only smooth within the buffered region
      smoother->SetOperator( gaussianSmoothingOperator );
      smoother->SetInput( smoothField );
      smoother->Update();

      smoothField = smoother->GetOutput();
      smoothField->Update();
      smoothField->DisconnectPipeline();
      }

    const VectorType zeroVector( 0.0 );

    //make sure boundary does not move
    float weight1 = 1.0;
    if (variance < 0.5)
      {
      weight1 = 1.0 - 1.0 * ( variance / 0.5);
      }
    float weight2 = 1.0 - weight1;

    const typename DisplacementFieldType::RegionType region = field->GetLargestPossibleRegion();
    const typename DisplacementFieldType::SizeType size = region.GetSize();
    const typename DisplacementFieldType::IndexType startIndex = region.GetIndex();

    itk::ImageRegionConstIteratorWithIndex<DisplacementFieldType> fieldIt( field, field->GetLargestPossibleRegion() );
    itk::ImageRegionIteratorWithIndex<DisplacementFieldType> smoothedFieldIt( smoothField, smoothField->GetLargestPossibleRegion() );
    for( fieldIt.GoToBegin(), smoothedFieldIt.GoToBegin(); !fieldIt.IsAtEnd(); ++fieldIt, ++smoothedFieldIt )
      {
      typename DisplacementFieldType::IndexType index = fieldIt.GetIndex();
      bool isOnBoundary = false;
      for ( unsigned int dimension = 0; dimension < ImageDimension; ++dimension )
        {
        if( index[dimension] == startIndex[dimension] || index[dimension] == static_cast<int>( size[dimension] ) - startIndex[dimension] - 1 )
          {
          isOnBoundary = true;
          break;
          }
        }
      if( isOnBoundary )
        {
        smoothedFieldIt.Set( zeroVector );
        }
      else
        {
        smoothedFieldIt.Set( smoothedFieldIt.Get() * weight1 + fieldIt.Get() * weight2 );
        }
      }
    timer.Stop();
    elapsedTime = timer.GetMean();

    }
  else if( var.size() == ImageDimension )
    {
    typedef itk::DisplacementFieldToBSplineImageFilter<DisplacementFieldType> BSplineFilterType;

    unsigned int numberOfLevels = 1;
    if( argc > 5 )
      {
      numberOfLevels = atoi( argv[5] );
      }

    unsigned int splineOrder = 3;
    if( argc > 6 )
      {
      splineOrder = atoi( argv[6] );
      }

    typename BSplineFilterType::ArrayType ncps;
    for( unsigned int d = 0; d < ImageDimension; d++ )
      {
      ncps[d] = var[d] + splineOrder;
      }

    typename BSplineFilterType::Pointer bspliner = BSplineFilterType::New();
    bspliner->SetDisplacementField( field );
    bspliner->SetBSplineDomainFromImage( field );
    bspliner->SetNumberOfControlPoints( ncps );
    bspliner->SetSplineOrder( splineOrder );
    bspliner->SetNumberOfFittingLevels( numberOfLevels );
    bspliner->SetEnforceStationaryBoundary( true );

    if( argc > 7 )
      {
      typename BSplineFilterType::RealImageType::Pointer confidenceImage = ITK_NULLPTR;
      ReadImage<typename BSplineFilterType::RealImageType>( confidenceImage, argv[7] );
      bspliner->SetConfidenceImage( confidenceImage );
      }
    bspliner->SetEstimateInverse( false );

    itk::TimeProbe timer;
    timer.Start();
    bspliner->Update();
    timer.Stop();
    elapsedTime = timer.GetMean();

    smoothField = bspliner->GetOutput();
    smoothField->DisconnectPipeline();
    }
  else
    {
    std::cerr << "Error: unexpected variance format." << std::endl;
    return EXIT_FAILURE;
    }

  typename RealImageType::Pointer rmseImage = RealImageType::New();
  rmseImage->SetOrigin( field->GetOrigin() );
  rmseImage->SetDirection( field->GetDirection() );
  rmseImage->SetSpacing( field->GetSpacing() );
  rmseImage->SetRegions( field->GetLargestPossibleRegion() );
  rmseImage->Allocate();
  rmseImage->FillBuffer( 0.0 );

  itk::ImageRegionConstIterator<DisplacementFieldType> fieldIt( field, field->GetLargestPossibleRegion() );
  itk::ImageRegionConstIterator<DisplacementFieldType> smoothedFieldIt( smoothField, smoothField->GetLargestPossibleRegion() );
  itk::ImageRegionIterator<RealImageType> ItR( rmseImage, rmseImage->GetLargestPossibleRegion() );

  float rmse = 0.0;
  vnl_vector<float> rmse_comp( 2 );
  rmse_comp.fill( 0.0 );
  float N = 0.0;
  for( fieldIt.GoToBegin(), smoothedFieldIt.GoToBegin(), ItR.GoToBegin(); !fieldIt.IsAtEnd(); ++fieldIt, ++smoothedFieldIt, ++ItR )
    {
    ItR.Set( ( fieldIt.Get() - smoothedFieldIt.Get() ).GetSquaredNorm() );
    for( unsigned int d = 0; d < ImageDimension; d++ )
      {
      rmse_comp[d] += vnl_math_sqr( fieldIt.Get()[d] - smoothedFieldIt.Get()[d] );
      }
    rmse += ( fieldIt.Get() - smoothedFieldIt.Get() ).GetSquaredNorm();
    N += 1.0;
    }
  rmse = std::sqrt( rmse / N );

  std::cout << "Elapsed time: " << elapsedTime << std::endl;
  std::cout << "RMSE = " << rmse << std::endl;
  for( unsigned int d = 0; d < ImageDimension; d++ )
    {
    std::cout << "  rmse[" << d << "] = " << std::sqrt( rmse_comp[d] / N ) << std::endl;
    }

  WriteImage<DisplacementFieldType>( smoothField, argv[3] );

  return EXIT_SUCCESS;
}

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

  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 < 5 )
    {
    std::cout << argv[0] << " imageDimension inputField outputField variance_or_mesh_size_base_level "
              << "[numberOfevels=1] [splineOrder=3] [confidenceImage]" << std::endl;
    return EXIT_FAILURE;
    }

  switch( atoi( argv[1] ) )
   {
   case 2:
     return SmoothDisplacementField<2>( argc, argv );
     break;
   case 3:
     return SmoothDisplacementField<3>( argc, argv );
     break;
   case 4:
     return SmoothDisplacementField<4>( argc, argv );
     break;
   default:
      std::cerr << "Unsupported dimension" << std::endl;
      return EXIT_FAILURE;
   }
  return EXIT_SUCCESS;
}

} // namespace ants
