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

  Program:   Insight Segmentation & Registration Toolkit
  Module:    $RCSfile: itkDeformableSuperellipseShapeSignedDistanceFunctionTest.cxx,v $
  Language:  C++
  Date:      $Date: 2006/08/10 17:38:46 $
  Version:   $Revision: 1.1.1.1 $

  Copyright (c) 2002 Insight 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.

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

#include "itkDeformableSuperellipseShapeSignedDistanceFunction.h"

#include "vnl/vnl_sample.h"
#include "vnl/vnl_math.h"
#include "itkImageRegionIterator.h"
#include "itkImageFileWriter.h"
#include "itkImage.h"

/**
 * This module tests the functionality of the 
 * DeformableSuperellipseShapeSignedDistanceFunction class.
 *
 * The parameters are randomly generated. The signed distance is 
 * evaluated at all image points and compared to expected values. 
 * The test fails if the evaluated results is not within a certain tolerance 
 * of the expected results.
 */
int itkDeformableSuperellipseShapeSignedDistanceFunctionTest( int, char *[])
{
  typedef double CoordRep;
  const unsigned int Dimension    = 2;
  const unsigned int ImageWidth   = 400;
  const unsigned int ImageHeight  = 300;


  // define a DeformableSuperellipse shape function
  typedef itk::DeformableSuperellipseShapeSignedDistanceFunction<
    CoordRep, Dimension> ShapeFunction;
  ShapeFunction::Pointer shape = ShapeFunction::New();


  // set up the parameters
  unsigned int numberOfShapeParameters  = shape->GetNumberOfShapeParameters();
  unsigned int numberOfPoseParameters   = shape->GetNumberOfPoseParameters();
  unsigned int numberOfParameters = 
    numberOfShapeParameters  + numberOfPoseParameters;
  ShapeFunction::ParametersType parameters(numberOfParameters);
  ShapeFunction::ParametersType shapeParameters(numberOfShapeParameters);
  ShapeFunction::ParametersType poseParameters(numberOfPoseParameters);

  // set up the random number generator
  vnl_sample_reseed();

  parameters[0] = shapeParameters[0] = vnl_sample_uniform(0.5, 1.5);  // x/y
  parameters[1] = shapeParameters[1] = vnl_sample_uniform(0.5, 1.5);  // squareness
  parameters[2] = shapeParameters[2] = vnl_sample_uniform(-0.5, 0.5); // tapering
  parameters[3] = shapeParameters[3] = vnl_sample_uniform(-0.5, 0.5); // bending

  parameters[4] = poseParameters[0] = vnl_sample_uniform(50, 80);     // scale (y)
  parameters[5] = poseParameters[1] = vnl_sample_uniform(-1, 1);      // rotation angle in radian
  parameters[6] = poseParameters[2] = vnl_sample_uniform(-50, 50);    // translation x
  parameters[7] = poseParameters[3] = vnl_sample_uniform(-50, 50);    // translation y
  
  shape->SetParameters(parameters);


  // we must initialize the function before use
  shape->Initialize();


 // print out private information
 shape->Print( std::cout );

 std::cout << "Transform: "  << shape->GetTransform()  << std::endl;
 std::cout << "Parameters: " << shape->GetParameters() << std::endl;


 // prepare for image creation 
  typedef itk::Image<unsigned char, 2> ImageType;
  ImageType::SizeType imageSize = {{ImageWidth, ImageHeight}};

  double origin[ImageType::ImageDimension];
  origin[0] = -200;
  origin[1] = -150;

  ImageType::IndexType startIndex;
  startIndex.Fill(0);

  ImageType::RegionType  region;
  region.SetSize(imageSize);
  region.SetIndex(startIndex);


  // set up the image
  ImageType::Pointer image = ImageType::New();
  image->SetRegions(region);
  image->Allocate();
  image->SetOrigin(origin);

  typedef itk::ImageRegionIterator<ImageType> ImageIterator;
  ImageIterator imageIt(image, image->GetBufferedRegion());

  // check DeformableSuperellipse shape signed distance calculation
  ImageType::IndexType      index;
  ShapeFunction::PointType  point, outPoint;
  ShapeFunction::OutputType output, expected;

  typedef ShapeFunction::TransformType TransformType;
  TransformType::Pointer transform = TransformType::New();

  transform->SetScale(poseParameters[0]);
  transform->SetAngle(poseParameters[1]);

  TransformType::OutputVectorType translation(2);
  translation[0] = poseParameters[2];
  translation[1] = poseParameters[3];
  transform->SetTranslation(translation);

  double sy_b, sy_b_y0, gamma, r, t;
  double theta, cos_theta, sin_theta, sign_sin, sign_cos;

  for(imageIt.GoToBegin(); !imageIt.IsAtEnd(); ++imageIt)
    {
    // from index to physical point
    index = imageIt.GetIndex(); 
    image->TransformIndexToPhysicalPoint(index, point);

    output = shape->Evaluate(point);

    // inverse transform
    point = transform->BackTransform(point);

	  // inverse bending
    if (vnl_math_abs(parameters[3]) > 1e-5)
      {
		  sy_b = 1.0/parameters[3];
		  sy_b_y0 = vnl_math_sgn(parameters[3]) * (sy_b - point[1]);
		  gamma = vcl_atan2(point[0], sy_b_y0);

		  r = vnl_math_sgn(parameters[3]) * 
        vcl_sqrt(point[0]*point[0] + sy_b_y0*sy_b_y0);

		  point[0] = r * gamma;
		  point[1] = sy_b - r;
	    }

    // inverse tapering
    t = parameters[2] * point[1] + 1;
    point[0] = point[0] / (t==0 ? vnl_math::eps : t);

    // signed distance
    ShapeFunction::OutputType a = point[0] / parameters[0];
    ShapeFunction::OutputType b = point[1];
    ShapeFunction::OutputType p = 1.0 / parameters[1];

    expected = vcl_pow( (vcl_pow(a*a, p) + vcl_pow(b*b, p)), parameters[1]) - 1;

    // compare
    if(vnl_math_abs( output - expected ) > 1e-9)
      {
      std::cout << "output value is: " << output << std::endl;
      std::cout << "But expected value is: " << expected << std::endl;
      return EXIT_FAILURE;
      }

    // output image
//    if(output < 0)  { imageIt.Set(50); }
//    else  { imageIt.Set(150); }

    imageIt.Set( static_cast<unsigned char>( output ) );
    }


  // check DeformableSuperellipse shape boundary calculation
	unsigned int  nPtsOut=100;

	for (unsigned int i=0; i<nPtsOut; i++) 
    {
    theta = 2*vnl_math::pi*i/nPtsOut;

	  // sampling
	  cos_theta = vcl_cos(theta); 
    sign_cos = vnl_math_sgn(cos_theta) * 
      vcl_pow(vcl_abs(cos_theta), parameters[1]);

	  sin_theta = vcl_sin(theta); 
    sign_sin = vnl_math_sgn(sin_theta) * 
      vcl_pow(vcl_abs(sin_theta), parameters[1]);

	  point[0] = parameters[0]*sign_cos;
	  point[1] = sign_sin;

    // tapering
    point[0] = (parameters[2] * point[1] + 1) * point[0]; 

    // bending
	  if (vnl_math_abs(parameters[3]) > 1e-5)
      {
		  sy_b = 1.0 / parameters[3];
		  sy_b_y0 = sy_b - point[1];
		  gamma = point[0] / sy_b_y0;

		  point[0] = sy_b_y0 * vcl_sin(gamma);
		  point[1] = sy_b - sy_b_y0 * vcl_cos(gamma);
	    }

    // transform
    point = transform->TransformPoint(point);

    // compare
    outPoint = shape->Evaluate(theta);

    if(vnl_math_abs( point[0] - outPoint[0] ) > 1e-9 || 
      vnl_math_abs( point[1] - outPoint[1] ) > 1e-9)
      {
      std::cout << "output point is: " << outPoint << std::endl;
      std::cout << "But expected point is: " << point << std::endl;
      return EXIT_FAILURE;
      }

    // output image
    image->TransformPhysicalPointToIndex(shape->Evaluate(theta), index);
    image->SetPixel(index, 255);
	  }

  // write out resulting image
  typedef itk::ImageFileWriter<ImageType> WriterType;
  WriterType::Pointer writer = WriterType::New();

  writer->SetFileName( "foo.mhd" );
  writer->SetInput(image);
  writer->Update();

  std::cout << "Test passed. " << std::endl;
  return EXIT_SUCCESS;
}