//
// $Id: coordinates_t.cpp,v 1.8 2000/04/04 13:22:05 hobbs Exp $
//
// File: GeometryXform_test.cc
// Purpose: 
// Created: 03-SEP-1997   John Hobbs
//
// $Revision: 1.8 $
//
//

// Turn off the double->float initialization warning
#ifdef NT_MSCVPP
#pragma warning (disable: 4305)
#endif

// Include files
#include <limits>
#include <iostream>
#include <iomanip>

#include <geometry_system/components/GeometryXform.hpp>
#include <geometry_system/components/CartesianCoordinate.hpp>
#include <geometry_system/components/CylindricalCoordinate.hpp>
#include <geometry_system/components/SphericalCoordinate.hpp>
#include <PhysicsVectors/Rotation.h>

using namespace std;
using namespace dgs;

bool equal(const double a, const double b) {
  double maxdif = sqrt(std::numeric_limits<double>::epsilon());
  double num = 2.0*(b-a);
  double den = b + a;
  double dif;
  dif = fabs(num);
  if( den!=0.0 ) dif = fabs(num/den);
  //if( !(dif<=maxdif) ) 
  //  cout << "Equal: a = " << a 
  //       << "       b = " << b
  //       << "     dif = " << dif << endl;
  return dif <= maxdif;
}

int main() 
{
  int error_code=0;
  const float pi4=0.785;
  CartesianCoordinate x(0.0,1.0,2.0);
  GeometryXform dx(1.0,0.0,0.0,0.0,0.0,0.0),dphi(0.0,0.0,0.0,pi4,0.0,0.0);
  GeometryXform dxphi;
  dxphi=dx.apply(dphi);
  cout << "*** Testing coordinates and transformations ***" << endl;
  cout << "    point is " << endl << x << endl;
  cout << "    translation is " << dx << endl;
  cout << "    rotation is " << dphi << endl;
  cout << "    translation then rotation is " << dxphi << endl;
  cout << "    After translation point is " << endl << dx.apply(x) << endl;
  cout << "    After rotation point " << endl << dphi.apply(x) << endl;
  cout << "    After t+r point " << endl << dxphi.apply(x) << endl << endl;

  CartesianCoordinate xc(1.0,1.0,1.0);
  CylindricalCoordinate xcyl=xc;
  SphericalCoordinate xsph=xc;
  cout << "*** Testing coordinate systems ***" << endl;
  cout << "    Point is " << endl << xc << endl;
  cout << "    Point is " << endl << xcyl << endl;
  cout << "    Point is " << endl << xsph << endl;

  // Check inverse of geometry xform applied to vectors and transforms.
  // Use complex test transform to avoid possibility of luckily choosing
  // a preferred axis
  CartesianCoordinate simple(1.0,1.0,1.0),gpnone;
  //double phi=0.345,theta=-0.821,psi=-0.2345;
  double phi=0.345,theta=0.821,psi=0.2345;
  GeometryXform fancy(5.0,-3.0,10.0,phi,theta,psi),none;

  if( (simple==simple) && (simple!=gpnone) )
    cout << "Passes coordinate '==' test" << endl;
  else {
    cout << "*** Fails coordinate '==' test *** " << endl;
    error_code += 1;
  }

  if( (fancy==fancy) && (fancy!=none) ) 
    cout << "Passes GeometryXform '==' test" << endl;
  else {
    cout << "*** Fails GeometryXform '==' test *** " << endl;
    error_code += 2;
  }

  double rphi,rtheta,rpsi;
  fancy.get_Euler(rphi,rtheta,rpsi);
  if( equal(rphi,phi) && equal(rtheta,theta) && equal(rpsi,psi) ) 
    cout << "Passes Euler angle get tests" << endl;
  else {
    cout << "*** Fails euler angle get tests ***" << endl;
    cout << "  Originals (phi,theta,psi): " 
	 << phi << " " << theta << " " << psi << endl;
    cout << "  Retrived (phi,theta,psi): " 
	 << rphi << " " << rtheta << " " << rpsi << endl;
    error_code += 4;
  }
  
  cout << "Testing get_inverse() method" << endl;
  cout << "Original xform: " << fancy << endl;
  cout << "  Its inverse:  " << fancy.get_inverse() << endl;

  CartesianCoordinate simple_xformed = fancy.apply(simple);
  CartesianCoordinate simple_again = fancy.invert(simple_xformed);
  double dr = ::distance(simple,simple_again);
  double dmax = 4.0*numeric_limits<double>::epsilon();
  if( dr>dmax ) {
    cout << "Inverse transform failed for coordinates" << endl;
    cout << "     Initial Point = " << simple << endl;
    cout << "     Transform = " << fancy << endl;
    cout << "     Transformed point = " << simple_xformed << endl;
    cout << "     Inverted transform of transformed point = " << 
      fancy.invert(simple_xformed) << endl;
    cout << "     Distance between points: " << dr << endl;
    cout << "     Machine epsilon: " << numeric_limits<double>::epsilon() << endl;
    error_code += 8;
  }
  else cout << "Passes inverting transform of coordinates" << endl;

  GeometryXform first(-1.0,3.4,0.5,0.573,-0.08975,0.087125);
  GeometryXform first_fancy = first.apply(fancy);
  GeometryXform first_again=first_fancy.invert(fancy);
  if( first_again != first ) {
    cout << "Inverse transform failed for GeometryXform" << endl;
    cout << "     Initial transform = " << first << endl;
    cout << "     transformed by = " << fancy << endl;
    cout << "     to get " << first_fancy << endl;
    cout << "     Inverted back to initial = " << first_again << endl;
    error_code += 16;
  }
  else cout << "Passes inverting GeometryXform" << endl;

  // Check the Euler angles returned by get_rotation.get_X() against the
  // expection
  psi=0.123;
  phi=0.707;
  theta=-0.685;
  GeometryXform echeck(0.0,0.0,0.0,phi,theta,psi);
  rpsi=echeck.get_rotation().getPsi();
  rtheta=echeck.get_rotation().getTheta();
  rphi=echeck.get_rotation().getPhi();
  if( !equal(rpsi,-phi) || !equal(rtheta,-theta) || !equal(rphi,-psi) ) {
    cout << setiosflags(ios::fixed) << setw(19) << setprecision(16);
    cout << "Euler angle retrieval via class Rotation failed" << endl;
    cout << "  Originals (phi,theta,psi): " 
	 << phi << " " << theta << " " << psi << endl;
    cout << "  Retrived (phi,theta,psi): " 
	 << -rpsi << " " << -rtheta << " " << -rphi << endl;
    error_code += 32;
  }
  else cout << "Passes Euler angle retrieval" << endl;

  if( !error_code ) cout << "OK coordinate_tests" << endl;
  else cout << "FAIL coordinate_tests" << endl;
  return error_code;
}