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

  Program:   Insight Segmentation & Registration Toolkit
  Module:    $RCSfile: itkSplineKernelTransform2Test.cxx,v $
  Language:  C++
  Date:      $Date: 2005/02/08 19:05:55 $
  Version:   $Revision: 1.23 $

  Copyright (c) Insight Software Consortium. All rights reserved.
  See ITKCopyright.txt or http://www.itk.org/HTML/Copyright.htm 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.

=========================================================================*/
#if defined(_MSC_VER)
#pragma warning ( disable : 4786 )
#endif

/**
 * This tests the elastic body spline and thin plate spline 
 * transform classes by warping a unit cube into a cube with side length 3.
 * It performs the test for 2D, 3D, and 4D to ensure that the
 * class works in N dimensions
 *
 * This was modified for the KernelSplineTransform2 set of transforms
 * by Rupert Brooks, March 2007.  The points were shifted so that they
 * actually formed a non-affine transformation, and independently 
 * calculated answers were added as an additional test.
 * Tests of the Jacobian were added.
 */
#include "itkElasticBodySplineKernelTransform2.h"
#include "itkElasticBodyReciprocalSplineKernelTransform2.h"
#include "itkThinPlateSplineKernelTransform2.h"
#include "itkThinPlateR2LogRSplineKernelTransform2.h"
#include "itkVolumeSplineKernelTransform2.h"

using namespace itk;

int main(int , char* [] )
{

  const double epsilon = 1e-12;
  
  // 2-D case
  int i, j;
  int returnValue=EXIT_SUCCESS;

  typedef ElasticBodySplineKernelTransform2<double, 2>           EBSTransform2DType;
  typedef ElasticBodyReciprocalSplineKernelTransform2<double, 2> EBRSTransform2DType;
  typedef ThinPlateSplineKernelTransform2<double, 2>             TPSTransform2DType;
  typedef ThinPlateR2LogRSplineKernelTransform2<double, 2>       TPR2LRSTransform2DType;
  typedef VolumeSplineKernelTransform2<double, 2>                VSTransform2DType;

  typedef EBSTransform2DType::InputPointType PointType2D;
  typedef EBSTransform2DType::PointsIterator Points2DIteratorType;
  typedef EBSTransform2DType::PointSetType   PointSetType2D;

  PointType2D sourcePoint2D;
  PointType2D targetPoint2D;
  PointType2D mappedPoint2D;

  EBSTransform2DType::Pointer     ebs2D       = EBSTransform2DType::New();
  EBRSTransform2DType::Pointer    ebrs2D      = EBRSTransform2DType::New();
  TPSTransform2DType::Pointer     tps2D       = TPSTransform2DType::New();
  TPR2LRSTransform2DType::Pointer tpr2lrs2D   = TPR2LRSTransform2DType::New();
  VSTransform2DType::Pointer      vs2D        = VSTransform2DType::New();

  // Reserve memory for the number of points
  PointSetType2D::Pointer sourceLandmarks2D = PointSetType2D::New();
  PointSetType2D::Pointer targetLandmarks2D = PointSetType2D::New();

  sourceLandmarks2D->GetPoints()->Reserve( 4 );
  targetLandmarks2D->GetPoints()->Reserve( 4 );

  // Create landmark sets
  Points2DIteratorType source2Dit = sourceLandmarks2D->GetPoints()->Begin();
  Points2DIteratorType target2Dit = targetLandmarks2D->GetPoints()->Begin();
  
  Points2DIteratorType source2Dend = sourceLandmarks2D->GetPoints()->End();

  PointType2D testPoint2D;
  PointType2D answerPoint2D;
  testPoint2D[0]=0.5;
  testPoint2D[1]=0.6;

  EBSTransform2DType::JacobianType mappedJacobian;
  EBSTransform2DType::JacobianType answerJacobian;
  answerJacobian.SetSize(2,8);

  //std::cout.precision(15);


  double temp=0.1;
  double step=0.01;
  unsigned int count=1;
  for (i = 0; i < 2; i++)
    {
    for (j = 0; j < 2; j++)
      {
      sourcePoint2D[0] = (double)j+temp;
	  temp+=step;// we add this little twist so that the 
	          // transform is not simply affine
      sourcePoint2D[1] = (double)i+temp;
	  temp+=count*step;
	  count++;
      source2Dit.Value() = sourcePoint2D;
      targetPoint2D[0] = 3*j;
      targetPoint2D[1] = 3*i;
      target2Dit.Value() = targetPoint2D;
      source2Dit++;
      target2Dit++;
      }
    }

  
  std::cout << "EBS 2D Test:" << std::endl;
  // Poisson's ration = 0.25, Alpha = 12.0 * ( 1 - \nu ) - 1
  ebs2D->SetSourceLandmarks( sourceLandmarks2D );
  ebs2D->SetTargetLandmarks( targetLandmarks2D );
  ebs2D->SetAlpha( 12.0 * ( 1 -  0.25) - 1.0 );
  ebs2D->ComputeWMatrix();


  { // Testing the number of parameters 
  EBSTransform2DType::ParametersType parameters1 = ebs2D->GetParameters();
  const unsigned int numberOfParameters = parameters1.Size();
  if( numberOfParameters != 4 * 2 )
    {
    std::cerr << "Number of parameters was not updated after" << std::endl;
    std::cerr << "invoking SetSourceLandmarks and SetTargetLandmarks" << std::endl;
    std::cerr << "Number of parameters is = " << numberOfParameters << std::endl;
    std::cerr << "While we were expecting = " << 4 * 2 << std::endl;
    returnValue=EXIT_FAILURE;
    }
  }

  source2Dit = sourceLandmarks2D->GetPoints()->Begin();
  target2Dit = targetLandmarks2D->GetPoints()->Begin();
  
  source2Dend = sourceLandmarks2D->GetPoints()->End();
  while( source2Dit != source2Dend )
    {
    sourcePoint2D = source2Dit.Value();
    targetPoint2D = target2Dit.Value();
    mappedPoint2D = ebs2D->TransformPoint(sourcePoint2D);
    std::cout << sourcePoint2D << " : " << targetPoint2D;
    std::cout << " warps to: " << mappedPoint2D << std::endl;
    if( mappedPoint2D.EuclideanDistanceTo( targetPoint2D ) > epsilon )
      {
		  std::cout<<"WARP FAILED"<<std::endl;
          returnValue=EXIT_FAILURE;
      }
    source2Dit++;
    target2Dit++;
    }
  std::cout << std::endl;
  

//-------------------------------------------------------
// this code auto-generated using kernelspline_test.m
// answer point
answerPoint2D[0]=1.096855523423964;
answerPoint2D[1]=1.36705825336609;
// answer jacobian
answerJacobian[0][0]=0.3012249297458245;
answerJacobian[0][1]=0.09194788691083367;
answerJacobian[0][2]=0.2414998437631092;
answerJacobian[0][3]=-0.09029116822775557;
answerJacobian[0][4]=0.3314998437631091;
answerJacobian[0][5]=-0.09029116822775558;
answerJacobian[0][6]=0.125775382727957;
answerJacobian[0][7]=0.08863444954467745;
answerJacobian[1][0]=0.0922634099752082;
answerJacobian[1][1]=0.2971589027521171;
answerJacobian[1][2]=-0.09060100619187117;
answerJacobian[1][3]=0.2454926090091824;
answerJacobian[1][4]=-0.09060100619187121;
answerJacobian[1][5]=0.3354926090091823;
answerJacobian[1][6]=0.0889386024085341;
answerJacobian[1][7]=0.1218558792295182;
// End of Autogenerated code
//-------------------------------------------------------


  mappedPoint2D=ebs2D->TransformPoint(testPoint2D);
  mappedJacobian=ebs2D->GetJacobian(testPoint2D);

  std::cout<<"Point "<< testPoint2D<<" maps to "<<mappedPoint2D<<std::endl;
  std::cout<<"Point "<< testPoint2D<<" has Jacobian:"<<std::endl;
  std::cout<<mappedJacobian<<std::endl;

  mappedJacobian-=answerJacobian;

  if(mappedPoint2D.EuclideanDistanceTo(answerPoint2D)>epsilon)
    {
	  std::cout<<"EBS2D - did not match answer point"<<std::endl;
      returnValue=EXIT_FAILURE;
	} else {
		std::cout<<"EBS2D - point mapped correctly"<<std::endl;
	}
  if(mappedJacobian.array_inf_norm()>epsilon) 
    {
	  std::cout<<"EBS2D - Jacobian did not match answer Jacobian"<<std::endl;
      returnValue=EXIT_FAILURE;
    } else {
		std::cout<<"EBS2D - Jacobian correct"<<std::endl;
	}

 
  std::cout << "EBRS 2D Test:" << std::endl;
  ebrs2D->SetSourceLandmarks( sourceLandmarks2D );
  ebrs2D->SetTargetLandmarks( targetLandmarks2D );
  ebrs2D->SetAlpha( 12.0 * ( 1 -  0.25) - 1.0 );
  ebrs2D->ComputeWMatrix();

  source2Dit = sourceLandmarks2D->GetPoints()->Begin();
  target2Dit = targetLandmarks2D->GetPoints()->Begin();
  
  source2Dend = sourceLandmarks2D->GetPoints()->End();
  while( source2Dit != source2Dend )
    {
    sourcePoint2D = source2Dit.Value();
    targetPoint2D = target2Dit.Value();
    mappedPoint2D = ebrs2D->TransformPoint(sourcePoint2D);
    std::cout << sourcePoint2D << " : " << targetPoint2D;
    std::cout << " warps to: " << mappedPoint2D << std::endl;
    if( mappedPoint2D.EuclideanDistanceTo( targetPoint2D ) > epsilon )
      {
		  std::cout<<"WARP FAILED"<<std::endl;
          returnValue=EXIT_FAILURE;
      }
    source2Dit++;
    target2Dit++;
    }
  std::cout << std::endl;


//-------------------------------------------------------
// this code auto-generated using kernelspline_test.m
// answer point
answerPoint2D[0]=1.098617942820908;
answerPoint2D[1]=1.368711040233936;
// answer jacobian
answerJacobian[0][0]=0.3225697356033854;
answerJacobian[0][1]=0.03799832220981194;
answerJacobian[0][2]=0.2205396290020812;
answerJacobian[0][3]=-0.0373136677555811;
answerJacobian[0][4]=0.310539629002081;
answerJacobian[0][5]=-0.03731366775558112;
answerJacobian[0][6]=0.1463510063924526;
answerJacobian[0][7]=0.03662901330135027;
answerJacobian[1][0]=0.03810900992496408;
answerJacobian[1][1]=0.3207367457472343;
answerJacobian[1][2]=-0.03742236109748725;
answerJacobian[1][3]=0.2223395920139771;
answerJacobian[1][4]=-0.03742236109748725;
answerJacobian[1][5]=0.3123395920139773;
answerJacobian[1][6]=0.03673571227001043;
answerJacobian[1][7]=0.1445840702248112;
// End of Autogenerated code
//-------------------------------------------------------

  mappedPoint2D=ebrs2D->TransformPoint(testPoint2D);
  mappedJacobian=ebrs2D->GetJacobian(testPoint2D);

  std::cout<<"Point "<< testPoint2D<<" maps to "<<mappedPoint2D<<std::endl;
  std::cout<<"Point "<< testPoint2D<<" has Jacobian:"<<std::endl;
  std::cout<<mappedJacobian<<std::endl;

  mappedJacobian-=answerJacobian;

  if(mappedPoint2D.EuclideanDistanceTo(answerPoint2D)>epsilon)
    {
	  std::cout<<"EBRS2D - did not match answer point"<<std::endl;
      returnValue=EXIT_FAILURE;
    } else {
		std::cout<<"EBRS2D - point mapped correctly"<<std::endl;
	}
  if(mappedJacobian.array_inf_norm()>epsilon) 
    {
	  std::cout<<"EBRS2D - Jacobian did not match answer Jacobian"<<std::endl;
      returnValue=EXIT_FAILURE;
    } else {
		std::cout<<"EBRS2D - Jacobian correct"<<std::endl;
	}

 
  std::cout << "TPS 2D Test:" << std::endl;
  tps2D->SetSourceLandmarks( sourceLandmarks2D );
  tps2D->SetTargetLandmarks( targetLandmarks2D );

  tps2D->ComputeWMatrix();

  source2Dit = sourceLandmarks2D->GetPoints()->Begin();
  target2Dit = targetLandmarks2D->GetPoints()->Begin();
  
  source2Dend = sourceLandmarks2D->GetPoints()->End();
  while( source2Dit != source2Dend )
    {
    sourcePoint2D = source2Dit.Value();
    targetPoint2D = target2Dit.Value();
    mappedPoint2D = tps2D->TransformPoint(sourcePoint2D);
    std::cout << sourcePoint2D << " : " << targetPoint2D;
    std::cout << " warps to: " << mappedPoint2D << std::endl;
    if( mappedPoint2D.EuclideanDistanceTo( targetPoint2D ) > epsilon )
      {
		  std::cout<<"WARP FAILED"<<std::endl;
          returnValue=EXIT_FAILURE;
      }
    source2Dit++;
    target2Dit++;
    }
  std::cout << std::endl;

//-------------------------------------------------------
// this code auto-generated using kernelspline_test.m
// answer point
answerPoint2D[0]=1.100747152243849;
answerPoint2D[1]=1.370747152243849;
// answer jacobian
answerJacobian[0][0]=0.3211776834887955;
answerJacobian[0][1]=0;
answerJacobian[0][2]=0.2219065990965883;
answerJacobian[0][3]=0;
answerJacobian[0][4]=0.3119065990965883;
answerJacobian[0][5]=0;
answerJacobian[0][6]=0.145009118318028;
answerJacobian[0][7]=0;
answerJacobian[1][0]=0;
answerJacobian[1][1]=0.3211776834887955;
answerJacobian[1][2]=0;
answerJacobian[1][3]=0.2219065990965883;
answerJacobian[1][4]=0;
answerJacobian[1][5]=0.3119065990965883;
answerJacobian[1][6]=0;
answerJacobian[1][7]=0.145009118318028;
// End of Autogenerated code
//-------------------------------------------------------


  mappedPoint2D=tps2D->TransformPoint(testPoint2D);
  mappedJacobian=tps2D->GetJacobian(testPoint2D);

  std::cout<<"Point "<< testPoint2D<<" maps to "<<mappedPoint2D<<std::endl;
  std::cout<<"Point "<< testPoint2D<<" has Jacobian:"<<std::endl;
  std::cout<<mappedJacobian<<std::endl;

  mappedJacobian-=answerJacobian;

  if(mappedPoint2D.EuclideanDistanceTo(answerPoint2D)>epsilon)
    {
	  std::cout<<"TPS2D - did not match answer point"<<std::endl;
      returnValue=EXIT_FAILURE;
    } else {
		std::cout<<"TPS2D - point mapped correctly"<<std::endl;
	}
  if(mappedJacobian.array_inf_norm()>epsilon) 
    {
		std::cout<<"TPS2D - Jacobian did not match answer Jacobian"<<std::endl;
        returnValue=EXIT_FAILURE;
    } else {
		std::cout<<"TPS2D - Jacobian correct"<<std::endl;
	}


	//TPS R2LogR
  std::cout << "TPS R2LogR 2D Test:" << std::endl;
  tpr2lrs2D->SetSourceLandmarks( sourceLandmarks2D );
  tpr2lrs2D->SetTargetLandmarks( targetLandmarks2D );

  tpr2lrs2D->ComputeWMatrix();

  source2Dit = sourceLandmarks2D->GetPoints()->Begin();
  target2Dit = targetLandmarks2D->GetPoints()->Begin();
  
  source2Dend = sourceLandmarks2D->GetPoints()->End();
  while( source2Dit != source2Dend )
    {
    sourcePoint2D = source2Dit.Value();
    targetPoint2D = target2Dit.Value();
    mappedPoint2D = tpr2lrs2D->TransformPoint(sourcePoint2D);
    std::cout << sourcePoint2D << " : " << targetPoint2D;
    std::cout << " warps to: " << mappedPoint2D << std::endl;
    if( mappedPoint2D.EuclideanDistanceTo( targetPoint2D ) > epsilon )
      {
		  std::cout<<"WARP FAILED"<<std::endl;
          returnValue=EXIT_FAILURE;
      }
    source2Dit++;
    target2Dit++;
    }
  std::cout << std::endl;


//-------------------------------------------------------
// this code auto-generated using kernelspline_test.m
// answer point
answerPoint2D[0]=1.1015247673811;
answerPoint2D[1]=1.3715247673811;
// answer jacobian
answerJacobian[0][0]=0.306791803449643;
answerJacobian[0][1]=0;
answerJacobian[0][2]=0.2360332740899902;
answerJacobian[0][3]=0;
answerJacobian[0][4]=0.3260332740899903;
answerJacobian[0][5]=0;
answerJacobian[0][6]=0.1311416483703766;
answerJacobian[0][7]=0;
answerJacobian[1][0]=0;
answerJacobian[1][1]=0.306791803449643;
answerJacobian[1][2]=0;
answerJacobian[1][3]=0.2360332740899902;
answerJacobian[1][4]=0;
answerJacobian[1][5]=0.3260332740899903;
answerJacobian[1][6]=0;
answerJacobian[1][7]=0.1311416483703766;
// End of Autogenerated code
//-------------------------------------------------------

  mappedPoint2D=tpr2lrs2D->TransformPoint(testPoint2D);
  mappedJacobian=tpr2lrs2D->GetJacobian(testPoint2D);

  std::cout<<"Point "<< testPoint2D<<" maps to "<<mappedPoint2D<<std::endl;
  std::cout<<"Point "<< testPoint2D<<" has Jacobian:"<<std::endl;
  std::cout<<mappedJacobian<<std::endl;

  mappedJacobian-=answerJacobian;

  if(mappedPoint2D.EuclideanDistanceTo(answerPoint2D)>epsilon)
    {
	  std::cout<<"TPR2LRS2D - did not match answer point"<<std::endl;
      returnValue=EXIT_FAILURE;
    } else {
		std::cout<<"TPR2LRS2D - point mapped correctly"<<std::endl;
	}
  if(mappedJacobian.array_inf_norm()>epsilon) 
    {
		std::cout<<"TPR2LRS2D - Jacobian did not match answer Jacobian"<<std::endl;
        returnValue=EXIT_FAILURE;
    } else {
		std::cout<<"TPR2LRS2D - Jacobian correct"<<std::endl;
	}

	//Volume Spline
  std::cout << "Volume Spline 2D Test:" << std::endl;
  vs2D->SetSourceLandmarks( sourceLandmarks2D );
  vs2D->SetTargetLandmarks( targetLandmarks2D );

  vs2D->ComputeWMatrix();

  source2Dit = sourceLandmarks2D->GetPoints()->Begin();
  target2Dit = targetLandmarks2D->GetPoints()->Begin();
  
  source2Dend = sourceLandmarks2D->GetPoints()->End();
  while( source2Dit != source2Dend )
    {
    sourcePoint2D = source2Dit.Value();
    targetPoint2D = target2Dit.Value();
    mappedPoint2D = vs2D->TransformPoint(sourcePoint2D);
    std::cout << sourcePoint2D << " : " << targetPoint2D;
    std::cout << " warps to: " << mappedPoint2D << std::endl;
    if( mappedPoint2D.EuclideanDistanceTo( targetPoint2D ) > epsilon )
      {
		  std::cout<<"WARP FAILED"<<std::endl;
          returnValue=EXIT_FAILURE;
      }
    source2Dit++;
    target2Dit++;
    }
  std::cout << std::endl;

//-------------------------------------------------------
// this code auto-generated using kernelspline_test.m
// answer point
answerPoint2D[0]=1.102198400856157;
answerPoint2D[1]=1.372198400856157;
// answer jacobian
answerJacobian[0][0]=0.2943295841610964;
answerJacobian[0][1]=0;
answerJacobian[0][2]=0.2482709488868513;
answerJacobian[0][3]=0;
answerJacobian[0][4]=0.3382709488868512;
answerJacobian[0][5]=0;
answerJacobian[0][6]=0.119128518065201;
answerJacobian[0][7]=0;
answerJacobian[1][0]=0;
answerJacobian[1][1]=0.2943295841610964;
answerJacobian[1][2]=0;
answerJacobian[1][3]=0.2482709488868513;
answerJacobian[1][4]=0;
answerJacobian[1][5]=0.3382709488868512;
answerJacobian[1][6]=0;
answerJacobian[1][7]=0.119128518065201;
// End of Autogenerated code
//-------------------------------------------------------


  mappedPoint2D=vs2D->TransformPoint(testPoint2D);
  mappedJacobian=vs2D->GetJacobian(testPoint2D);

  std::cout<<"Point "<< testPoint2D<<" maps to "<<mappedPoint2D<<std::endl;
  std::cout<<"Point "<< testPoint2D<<" has Jacobian:"<<std::endl;
  std::cout<<mappedJacobian<<std::endl;

  mappedJacobian-=answerJacobian;

  if(mappedPoint2D.EuclideanDistanceTo(answerPoint2D)>epsilon)
    {
	  std::cout<<"VS2D - did not match answer point"<<std::endl;
      returnValue=EXIT_FAILURE;
    }  else {
		std::cout<<"VS2D - point mapped correctly"<<std::endl;
	}
  if(mappedJacobian.array_inf_norm()>epsilon) 
    {
		std::cout<<"VS2D - Jacobian did not match answer Jacobian"<<std::endl;
        returnValue=EXIT_FAILURE;
    } else {
		std::cout<<"VS2D - Jacobian correct"<<std::endl;
	}



   // 3-D case
  int k;
  typedef ElasticBodySplineKernelTransform2<double, 3> EBSTransform3DType;
  typedef ThinPlateSplineKernelTransform2<double, 3>   TPSTransform3DType;

  typedef EBSTransform3DType::InputPointType PointType3D;
  typedef EBSTransform3DType::PointsIterator Points3DIteratorType;

  PointType3D sourcePoint3D;
  PointType3D targetPoint3D;
  PointType3D mappedPoint3D;

  PointType3D testPoint3D;
  PointType3D answerPoint3D;
  testPoint3D[0]=0.5;
  testPoint3D[1]=0.6;
  testPoint3D[2]=0.7;

  answerJacobian.SetSize(3,24);
  mappedJacobian.SetSize(3,24);


  EBSTransform3DType::Pointer ebs3D = EBSTransform3DType::New();
  TPSTransform3DType::Pointer tps3D = TPSTransform3DType::New();

 
  // Reserve memory for the number of points
  ebs3D->GetTargetLandmarks()->GetPoints()->Reserve( 8 );  
  tps3D->GetTargetLandmarks()->GetPoints()->Reserve( 8 );
  ebs3D->GetSourceLandmarks()->GetPoints()->Reserve( 8 );  
  tps3D->GetSourceLandmarks()->GetPoints()->Reserve( 8 );


  // Create landmark sets
  Points3DIteratorType ebs3Ds = ebs3D->GetSourceLandmarks()->GetPoints()->Begin();
  Points3DIteratorType ebs3Dt = ebs3D->GetTargetLandmarks()->GetPoints()->Begin();
  Points3DIteratorType tps3Ds = tps3D->GetSourceLandmarks()->GetPoints()->Begin();
  Points3DIteratorType tps3Dt = tps3D->GetTargetLandmarks()->GetPoints()->Begin();

  Points3DIteratorType ebs3DsEnd  = ebs3D->GetSourceLandmarks()->GetPoints()->End();
  Points3DIteratorType tps3DsEnd  = tps3D->GetSourceLandmarks()->GetPoints()->End();
  
  temp=0.1;
  count=1;
  for (i = 0; i < 2; i++)
    {
    for (j = 0; j < 2; j++)
      {
      for (k = 0; k < 2; k++)
        {
        sourcePoint3D[0] = (double)k+temp;
		temp+=step;
        sourcePoint3D[1] = (double)j+temp;
		temp+=step;
        sourcePoint3D[2] = (double)i+temp;
		temp+=count*step;
		count++;
        ebs3Ds.Value() = sourcePoint3D;
        tps3Ds.Value() = sourcePoint3D;
        targetPoint3D[0] = 3*k;
        targetPoint3D[1] = 3*j;
        targetPoint3D[2] = 3*i;
        ebs3Dt.Value() = targetPoint3D;
        tps3Dt.Value() = targetPoint3D;
        ebs3Ds++;
        ebs3Dt++;
        tps3Ds++;
        tps3Dt++;
        }
      }
    }



  std::cout << "EBS 3D Test:" << std::endl;
  // Poisson's ration = 0.25, Alpha = 12.0 * ( 1 - \nu ) - 1
  ebs3D->SetAlpha( 12.0 * ( 1 -  0.25) - 1.0 );
  ebs3D->ComputeWMatrix();
  
  ebs3Ds     = ebs3D->GetSourceLandmarks()->GetPoints()->Begin();
  ebs3Dt     = ebs3D->GetTargetLandmarks()->GetPoints()->Begin();
  ebs3DsEnd  = ebs3D->GetSourceLandmarks()->GetPoints()->End();

  while( ebs3Ds != ebs3DsEnd )
  {
    sourcePoint3D = ebs3Ds.Value();
    targetPoint3D = ebs3Dt.Value();
    mappedPoint3D = ebs3D->TransformPoint(sourcePoint3D);
    std::cout << sourcePoint3D << " : " << targetPoint3D;
    std::cout << " warps to: " << mappedPoint3D << std::endl;
    if( mappedPoint3D.EuclideanDistanceTo( targetPoint3D ) > epsilon )
    {
		  std::cout<<"WARP FAILED"<<std::endl;
          returnValue=EXIT_FAILURE;
    }
    ebs3Ds++;
    ebs3Dt++;
  }
  std::cout << std::endl;
  
//-------------------------------------------------------
// this code auto-generated using kernelspline_test.m
// answer point
answerPoint3D[0]=0.8298914835618;
answerPoint3D[1]=1.092099893723852;
answerPoint3D[2]=1.359047493505423;
// answer jacobian
answerJacobian[0][0]=0.08681132347297593;
answerJacobian[0][1]=0.06140088277078758;
answerJacobian[0][2]=0.0481237582544775;
answerJacobian[0][3]=0.1055686120471957;
answerJacobian[0][4]=-0.06920172730220132;
answerJacobian[0][5]=-0.05615558047769212;
answerJacobian[0][6]=0.2123664175619031;
answerJacobian[0][7]=-0.0786587377089629;
answerJacobian[0][8]=0.04242082299700239;
answerJacobian[0][9]=0.1366349974532398;
answerJacobian[0][10]=0.08689006033547078;
answerJacobian[0][11]=-0.0328313239247974;
answerJacobian[0][12]=0.3480587941311403;
answerJacobian[0][13]=0.03789549664857983;
answerJacobian[0][14]=-0.07558683764332194;
answerJacobian[0][15]=0.09094262088400223;
answerJacobian[0][16]=-0.0296641740220718;
answerJacobian[0][17]=0.08517633671552682;
answerJacobian[0][18]=0.07414481536929535;
answerJacobian[0][19]=-0.02020716361531008;
answerJacobian[0][20]=-0.01340006675916738;
answerJacobian[0][21]=-0.0545275809197534;
answerJacobian[0][22]=0.01154536289370781;
answerJacobian[0][23]=0.002252890837972005;
answerJacobian[1][0]=0.06217165184368056;
answerJacobian[1][1]=0.08303536887982188;
answerJacobian[1][2]=0.04753088874197084;
answerJacobian[1][3]=-0.07196555079145839;
answerJacobian[1][4]=0.1240672026760996;
answerJacobian[1][5]=0.03765662204407402;
answerJacobian[1][6]=-0.07649164941284505;
answerJacobian[1][7]=0.2016904035405691;
answerJacobian[1][8]=-0.05704335801533843;
answerJacobian[1][9]=0.08669198784191923;
answerJacobian[1][10]=0.1329067519980397;
answerJacobian[1][11]=-0.0242836172544835;
answerJacobian[1][12]=0.03734621862688681;
answerJacobian[1][13]=0.3466815134290009;
answerJacobian[1][14]=-0.06813422307400771;
answerJacobian[1][15]=-0.02714588019781228;
answerJacobian[1][16]=0.07791564210960789;
answerJacobian[1][17]=-0.01319275219581426;
answerJacobian[1][18]=-0.02261978157642564;
answerJacobian[1][19]=0.0902924412451378;
answerJacobian[1][20]=0.08150722786359835;
answerJacobian[1][21]=0.01201300366605504;
answerJacobian[1][22]=-0.0565893238782762;
answerJacobian[1][23]=-0.004040788109999569;
answerJacobian[2][0]=0.05173570687270318;
answerJacobian[2][1]=0.05046547442057022;
answerJacobian[2][2]=0.07648465167814611;
answerJacobian[2][3]=-0.06030150332341503;
answerJacobian[2][4]=0.03701014018881024;
answerJacobian[2][5]=0.1228797370047635;
answerJacobian[2][6]=0.04143653012785353;
answerJacobian[2][7]=-0.06078665652260412;
answerJacobian[2][8]=0.2090709119015415;
answerJacobian[2][9]=-0.0312221197707637;
answerJacobian[2][10]=-0.02268904580109349;
answerJacobian[2][11]=0.1329003420550142;
answerJacobian[2][12]=-0.07522511037348423;
answerJacobian[2][13]=-0.06773241037075602;
answerJacobian[2][14]=0.3541267923091464;
answerJacobian[2][15]=0.08543952073057412;
answerJacobian[2][16]=-0.01574329195294158;
answerJacobian[2][17]=0.07784446164740921;
answerJacobian[2][18]=-0.01629851272069446;
answerJacobian[2][19]=0.08205350475847263;
answerJacobian[2][20]=0.08165328675063122;
answerJacobian[2][21]=0.004435488457226383;
answerJacobian[2][22]=-0.002577714720457938;
answerJacobian[2][23]=-0.0549601833466522;
// End of Autogenerated code
//-------------------------------------------------------

  mappedPoint3D=ebs3D->TransformPoint(testPoint3D);
  mappedJacobian=ebs3D->GetJacobian(testPoint3D);

  std::cout<<"Point "<< testPoint3D<<" maps to "<<mappedPoint3D<<std::endl;
  std::cout<<"Point "<< testPoint3D<<" has Jacobian:"<<std::endl;
  std::cout<<mappedJacobian<<std::endl;

  mappedJacobian-=answerJacobian;

  if(mappedPoint3D.EuclideanDistanceTo(answerPoint3D)>epsilon)
    {
	  std::cout<<"EBS3D - did not match answer point"<<std::endl;
      returnValue=EXIT_FAILURE;
    }  else {
		std::cout<<"EBS3D - point mapped correctly"<<std::endl;
	}
  if(mappedJacobian.array_inf_norm()>epsilon) 
    {
		std::cout<<"EBS3D - Jacobian did not match answer Jacobian"<<std::endl;
        returnValue=EXIT_FAILURE;
    } else {
		std::cout<<"EBS3D - Jacobian correct"<<std::endl;
	}


  std::cout << "TPS 3D Test:" << std::endl;

  tps3D->ComputeWMatrix();

  tps3Ds = tps3D->GetSourceLandmarks()->GetPoints()->Begin();
  tps3Dt = tps3D->GetTargetLandmarks()->GetPoints()->Begin();
  tps3DsEnd  = tps3D->GetSourceLandmarks()->GetPoints()->End();

  while( tps3Ds != tps3DsEnd )
  {
    sourcePoint3D = tps3Ds.Value();
    targetPoint3D = tps3Dt.Value();
    mappedPoint3D = tps3D->TransformPoint(sourcePoint3D);
    std::cout << sourcePoint3D << " : " << targetPoint3D;
    std::cout << " warps to: " << mappedPoint3D << std::endl;
    if( mappedPoint3D.EuclideanDistanceTo( targetPoint3D ) > epsilon )
    {
		  std::cout<<"WARP FAILED"<<std::endl;
          returnValue=EXIT_FAILURE;
    }
    tps3Ds++;
    tps3Dt++;
  }
  std::cout << std::endl;

  std::cout << "Get/Set Parameters test " << std::endl;
  TPSTransform3DType::ParametersType parameters1 = tps3D->GetParameters();
  tps3D->SetParameters( parameters1 );
  TPSTransform3DType::ParametersType parameters2 = tps3D->GetParameters();
  const unsigned int numberOfParameters = parameters1.Size();
  const double tolerance = 1e-7;
  for(unsigned int pr = 0; pr < numberOfParameters; pr++)
    {
    if( vnl_math_abs( parameters1[pr] - parameters2[pr] ) > tolerance )
      {
      std::cout << "Parameters were not correctly recovered " << std::endl;
      returnValue=EXIT_FAILURE;
      }
    }
  std::cout << "Get/Set Parameters Passed" << std::endl << std::endl;

//-------------------------------------------------------
// this code auto-generated using kernelspline_test.m
// answer point
answerPoint3D[0]=0.8228482486199771;
answerPoint3D[1]=1.092848248619979;
answerPoint3D[2]=1.362848248619977;
// answer jacobian
answerJacobian[0][0]=0.1418686158951991;
answerJacobian[0][1]=0;
answerJacobian[0][2]=0;
answerJacobian[0][3]=0.1155416535588348;
answerJacobian[0][4]=0;
answerJacobian[0][5]=0;
answerJacobian[0][6]=0.1938041099202675;
answerJacobian[0][7]=0;
answerJacobian[0][8]=0;
answerJacobian[0][9]=0.09450287108570613;
answerJacobian[0][10]=0;
answerJacobian[0][11]=0;
answerJacobian[0][12]=0.2980903905383016;
answerJacobian[0][13]=0;
answerJacobian[0][14]=0;
answerJacobian[0][15]=0.08021659046767217;
answerJacobian[0][16]=0;
answerJacobian[0][17]=0;
answerJacobian[0][18]=0.09195413410623965;
answerJacobian[0][19]=0;
answerJacobian[0][20]=0;
answerJacobian[0][21]=-0.01597836557222046;
answerJacobian[0][22]=0;
answerJacobian[0][23]=0;
answerJacobian[1][0]=0;
answerJacobian[1][1]=0.1418686158951991;
answerJacobian[1][2]=0;
answerJacobian[1][3]=0;
answerJacobian[1][4]=0.1155416535588348;
answerJacobian[1][5]=0;
answerJacobian[1][6]=0;
answerJacobian[1][7]=0.1938041099202675;
answerJacobian[1][8]=0;
answerJacobian[1][9]=0;
answerJacobian[1][10]=0.09450287108570613;
answerJacobian[1][11]=0;
answerJacobian[1][12]=0;
answerJacobian[1][13]=0.2980903905383016;
answerJacobian[1][14]=0;
answerJacobian[1][15]=0;
answerJacobian[1][16]=0.08021659046767217;
answerJacobian[1][17]=0;
answerJacobian[1][18]=0;
answerJacobian[1][19]=0.09195413410623965;
answerJacobian[1][20]=0;
answerJacobian[1][21]=0;
answerJacobian[1][22]=-0.01597836557222046;
answerJacobian[1][23]=0;
answerJacobian[2][0]=0;
answerJacobian[2][1]=0;
answerJacobian[2][2]=0.1418686158951991;
answerJacobian[2][3]=0;
answerJacobian[2][4]=0;
answerJacobian[2][5]=0.1155416535588348;
answerJacobian[2][6]=0;
answerJacobian[2][7]=0;
answerJacobian[2][8]=0.1938041099202675;
answerJacobian[2][9]=0;
answerJacobian[2][10]=0;
answerJacobian[2][11]=0.09450287108570613;
answerJacobian[2][12]=0;
answerJacobian[2][13]=0;
answerJacobian[2][14]=0.2980903905383016;
answerJacobian[2][15]=0;
answerJacobian[2][16]=0;
answerJacobian[2][17]=0.08021659046767217;
answerJacobian[2][18]=0;
answerJacobian[2][19]=0;
answerJacobian[2][20]=0.09195413410623965;
answerJacobian[2][21]=0;
answerJacobian[2][22]=0;
answerJacobian[2][23]=-0.01597836557222046;
// End of Autogenerated code
//-------------------------------------------------------



  mappedPoint3D=tps3D->TransformPoint(testPoint3D);
  mappedJacobian=tps3D->GetJacobian(testPoint3D);

  std::cout<<"Point "<< testPoint3D<<" maps to "<<mappedPoint3D<<std::endl;
  std::cout<<"Point "<< testPoint3D<<" has Jacobian:"<<std::endl;
  std::cout<<mappedJacobian<<std::endl;

  mappedJacobian-=answerJacobian;

  if(mappedPoint3D.EuclideanDistanceTo(answerPoint3D)>epsilon)
    {
	  std::cout<<"TPS3D - did not match answer point"<<std::endl;
      returnValue=EXIT_FAILURE;
    }  else {
		std::cout<<"TPS3D - point mapped correctly"<<std::endl;
	}
  if(mappedJacobian.array_inf_norm()>epsilon) 
    {
		std::cout<<"TPS3D - Jacobian did not match answer Jacobian"<<std::endl;
        returnValue=EXIT_FAILURE;
    } else {
		std::cout<<"TPS3D - Jacobian correct"<<std::endl;
	}

  // 4-D case
  int l;
  typedef ElasticBodySplineKernelTransform2<double, 4> EBSTransform4DType;
  typedef ThinPlateSplineKernelTransform2<double, 4>   TPSTransform4DType;
  
  typedef EBSTransform4DType::InputPointType PointType4D;
  typedef EBSTransform4DType::PointsIterator Points4DIteratorType;

  PointType4D sourcePoint4D;
  PointType4D targetPoint4D;
  PointType4D mappedPoint4D;

  EBSTransform4DType::Pointer ebs4D = EBSTransform4DType::New();
  TPSTransform4DType::Pointer tps4D = TPSTransform4DType::New();

  // Reserve memory for the number of points
  ebs4D->GetTargetLandmarks()->GetPoints()->Reserve( 16 );  
  tps4D->GetTargetLandmarks()->GetPoints()->Reserve( 16 );
  
  ebs4D->GetSourceLandmarks()->GetPoints()->Reserve( 16 );  
  tps4D->GetSourceLandmarks()->GetPoints()->Reserve( 16 );

  // Create landmark sets
  Points4DIteratorType ebs4Ds = ebs4D->GetSourceLandmarks()->GetPoints()->Begin();
  Points4DIteratorType ebs4Dt = ebs4D->GetTargetLandmarks()->GetPoints()->Begin();
  Points4DIteratorType tps4Ds = tps4D->GetSourceLandmarks()->GetPoints()->Begin();
  Points4DIteratorType tps4Dt = tps4D->GetTargetLandmarks()->GetPoints()->Begin();
  
  Points4DIteratorType ebs4DsEnd  = ebs4D->GetSourceLandmarks()->GetPoints()->End();
  Points4DIteratorType tps4DsEnd  = tps4D->GetSourceLandmarks()->GetPoints()->End();
 
  temp=0.1;
  for (i = 0; i < 2; i++)
    {
    for (j = 0; j < 2; j++)
      {
      for (k = 0; k < 2; k++)
        {
        for (l = 0; l < 2; l++)
          {
          sourcePoint4D[0] = (double)l+temp;
		  temp+=0.01;
          sourcePoint4D[1] = (double)k+temp;
		  temp+=0.01;
          sourcePoint4D[2] = (double)j+temp;
		  temp+=0.01;
          sourcePoint4D[3] = (double)i+temp;
		  temp+=0.01;
          ebs4Ds.Value() = sourcePoint4D;
          tps4Ds.Value() = sourcePoint4D;
          targetPoint4D[0] = 3*l;
          targetPoint4D[1] = 3*k;
          targetPoint4D[2] = 3*j;
          targetPoint4D[3] = 3*i;
          ebs4Dt.Value() = targetPoint4D;
          tps4Dt.Value() = targetPoint4D;
          ebs4Ds++;
          ebs4Dt++;
          tps4Ds++;
          tps4Dt++;
          }
        }
      }
    }
  std::cout << "EBS 4D Test:" << std::endl;
  // Poisson's ration = 0.25, Alpha = 12.0 * ( 1 - \nu ) - 1
  ebs4D->SetAlpha( 12.0 * ( 1 -  0.25) - 1.0 );
  ebs4D->ComputeWMatrix();

  ebs4Ds = ebs4D->GetSourceLandmarks()->GetPoints()->Begin();
  ebs4Dt = ebs4D->GetTargetLandmarks()->GetPoints()->Begin();
  ebs4DsEnd  = ebs4D->GetSourceLandmarks()->GetPoints()->End();

  while( ebs4Ds != ebs4DsEnd )
  {
    sourcePoint4D = ebs4Ds.Value();
    targetPoint4D = ebs4Dt.Value();
    mappedPoint4D = ebs4D->TransformPoint(sourcePoint4D);
    std::cout << sourcePoint4D << " : " << targetPoint4D;
    std::cout << " warps to: " << mappedPoint4D << std::endl;
    if( mappedPoint4D.EuclideanDistanceTo( targetPoint4D ) > epsilon )
    {
		  std::cout<<"WARP FAILED"<<std::endl;
          returnValue=EXIT_FAILURE;
    }
    ebs4Ds++;
    ebs4Dt++;
  }
  std::cout << std::endl;

  std::cout << "TPS 4D Test:" << std::endl;
  tps4D->ComputeWMatrix();

  tps4Ds = tps4D->GetSourceLandmarks()->GetPoints()->Begin();
  tps4Dt = tps4D->GetTargetLandmarks()->GetPoints()->Begin();
  tps4DsEnd  = tps4D->GetSourceLandmarks()->GetPoints()->End();
  while( tps4Ds != tps4DsEnd )
  {
    sourcePoint4D = tps4Ds.Value();
    targetPoint4D = tps4Dt.Value();
    mappedPoint4D = tps4D->TransformPoint(sourcePoint4D);
    std::cout << sourcePoint4D << " : " << targetPoint4D;
    std::cout << " warps to: " << mappedPoint4D << std::endl;
    if( mappedPoint4D.EuclideanDistanceTo( targetPoint4D ) > epsilon )
    {
		  std::cout<<"WARP FAILED"<<std::endl;
          returnValue=EXIT_FAILURE;
    }
    tps4Ds++;
    tps4Dt++;
  }
  std::cout << std::endl;

  std::cout << ebs2D << std::endl;

  if(returnValue==EXIT_SUCCESS) {
	  std::cout<<"ALL TESTS PASSED"<<std::endl;
  } else {
	  std::cout<<"WARNING: SOME TESTS FAILED"<<std::endl;
  }

  std::cout << "TEST DONE" << std::endl;

  return returnValue;

}