#include "itkBSplineScatteredDataPointSetToImageFilter.h"
#include "itkPointSet.h"
#include <math.h>

const unsigned VectorDim=3;

typedef itk::Vector < double , VectorDim > VectorType ;
typedef itk::Image < VectorType , 1 > SplineCurveImageType ;
typedef itk::PointSet < VectorType , 1 > SplineCurvePointSetType ;
typedef itk::BSplineScatteredDataPointSetToImageFilter
    < SplineCurvePointSetType , SplineCurveImageType > SplineFilterType ;


VectorType getpointattt(double tt){
  VectorType pointval;
  double phistart=0.2;
  double phirange=0.4;
  double phi=phistart+tt*phirange;
  double theta=0.6;
  pointval[0] = cos(theta);
  pointval[1] = sin(theta)*cos(phi);
  pointval[2] = sin(theta)*sin(phi);
  return pointval;
}

int main (void) {
  SplineFilterType::Pointer spline = SplineFilterType::New();
  SplineCurvePointSetType::Pointer datapoints = SplineCurvePointSetType::New();

  SplineCurveImageType::SpacingType spacing ;
  SplineCurveImageType::SizeType size ;
  SplineCurveImageType::PointType origin ;

  unsigned numberofinputpoints = 100;

  spacing.Fill(1.0/(1000));
  size.Fill ( 1000+1 );
  origin.Fill ( 0.0 );
  double pointspacing = 1.0/(numberofinputpoints-1);


  double tstep=1.0/(numberofinputpoints-1);
  for (unsigned ii=0 ; ii<numberofinputpoints ; ii++ ) {
    VectorType midpointval;
    
    double tt=tstep*ii;
    midpointval=getpointattt(tt);

    double mag=0;
    for ( unsigned element=0 ; element<3 ; element++) {
      mag += midpointval[element]*midpointval[element];
    }
    mag=sqrt(mag);

    SplineCurvePointSetType::PointType thispointparam;
    thispointparam[0] = tt;
    datapoints->SetPoint(ii, thispointparam);
    datapoints->SetPointData(ii,midpointval);
  }

  SplineFilterType::ArrayType ncontrol ;
  ncontrol[0]=20;
  SplineFilterType::ArrayType closedim;
  closedim[0]= 0;
  int splineorder=2;
  spline->SetGenerateOutputImage(false);
  spline->SetOrigin ( origin );
  spline->SetSize ( size );
  spline->SetSpacing ( spacing );
  spline->SetInput ( datapoints );
  spline->SetSplineOrder ( splineorder );
  spline->SetNumberOfControlPoints ( ncontrol );
  spline->SetNumberOfLevels(1);
  spline->SetCloseDimension ( closedim );
  try {
    spline->Update();
  }
  catch (itk::ExceptionObject &err) {
    std::cerr << "Failure in spline fitting" << std::endl;
    std::cerr << spline << std::endl;
    std::cerr << err << std::endl;
    return 0 ;
  }

  for (unsigned ii=0 ; ii<=20; ii++) {
    std::cout<<"Point "<< ii << std::endl;
    
    double tt=ii/20.0;
    VectorType loc = getpointattt(tt);
    VectorType splineloc;
    SplineCurvePointSetType::PointType ttpoint;
    ttpoint[0] = tt;
    
    spline->Evaluate(ttpoint, splineloc);
    double splinemag=0, mag=0;
    for (unsigned el=0; el<3; el++ ) {
      splinemag+=splineloc[el]*splineloc[el];
      mag+=loc[el]*loc[el];
    }
    splinemag=sqrt(splinemag);
    mag=sqrt(mag);
    std::cout << tt << " "
	      << loc
	      << " mag " << mag << " "
	      << splineloc
	      << " mag " << splinemag
	      << std::endl;
  }

  std::cout << "Control points " << spline->GetCurrentNumberOfControlPoints()
	    << std::endl;

  int showcontrolpoints=1;
  if (showcontrolpoints) {
    SplineCurveImageType::Pointer phiLattice = spline->GetPhiLattice();
    SplineCurveImageType::RegionType::SizeType latticesize = phiLattice->GetLargestPossibleRegion().GetSize();
    for ( unsigned ii=0; ii< latticesize[0] ; ii++) {
      VectorType controlloc;
      SplineCurveImageType::IndexType index;
      index[0]=ii;
      controlloc=phiLattice->GetPixel(index);
      std::cout << controlloc << std::endl;
    }
  }

  return 0;
};