#include <iostream>
#include <cmath>
#include <float.h>
#include "geometry_system/components/absGeometryDistortion.hpp"
#include "spacegeom/SpacePoint.h"

using namespace dgs;
using namespace std;

class testDistortion : public absGeometryDistortion
{
public:
  testDistortion(){};
  SpacePoint * distort(const SpacePoint& point_on_surface) const {
    double r=point_on_surface.rxy();
    double phi=point_on_surface.phi();
    double z=point_on_surface.z();
    return new CylindricalPoint(r+1.,phi+0.1,z);
  };
  /// remove destortion 
  SpacePoint * undo(const SpacePoint& distorted_point) const
    { 
      double r=distorted_point.rxy();
      double phi=distorted_point.phi();
      double z=distorted_point.z();
      return new CylindricalPoint(r-1.,phi-0.1,z);
    };
  /// make a copy (user is responsible for deleteing this object)
  absGeometryDistortion * clone() const
    {
      return new testDistortion();
    };
  /// print out
  void printme(std::ostream& os) const
    {
      os<<"Test distortion r->r+1., phi -> phi+0.1"<<std::endl;
    }; 
};

int main()
{
  testDistortion dist;
  absGeometryDistortion * Dolly=dist.clone();
  CylindricalPoint original(1.,0.,0.);
  SpacePoint * distored=Dolly->distort(original);
  std::cout<<*Dolly<<std::endl;
  std::cout<<"Original "<<original<<std::endl<<" Distorted"<<*distored<<std::endl;
  SpacePoint * backToOriginal=Dolly->undo(*distored);
  std::cout<<"Back to original "<<*backToOriginal<<std::endl;
  if(fabs(backToOriginal->rxy()-original.rxy())<FLT_EPSILON*10. &&
     fabs(backToOriginal->phi()-original.phi())<FLT_EPSILON*10. &&
     fabs(distored->rxy()-original.rxy()-1.)<FLT_EPSILON*10.
     ) return 0;
  return 1;
}