//
// $Id: GeometryXform.cpp,v 1.33 2002/08/25 20:24:27 melanson Exp $
//
// File: GeometryXform.cc
// Purpose: 
// Created: 03-SEP-1997   John Hobbs
// Updated: 10-FEB-2001   Dhiman Chakraborty
//                        Added the yaw-pitch-roll (xyz) convention as an 
//                        option for specifying the Euler angles
//
// $Revision: 1.33 $
//
//

#include "geometry_system/components/GeometryXform.hpp"
#include "geometry_system/components/CartesianCoordinate.hpp"
#include "PhysicsVectors/Rotation.h"
#include "PhysicsVectors/SpaceVector.h"
#include "PhysicsVectors/UnitVector.h"

#include <cassert>
#include <cmath>
#include <limits>
#include <ostream>

using namespace dgs;
using namespace std;

//#ifdef _WIN32
//static double abs(double x)
//{
//  if (x < 0.0)
//  {
//    return -x;
//  }
//  return x;
//}
//#endif

int GeometryXform::_ndecimal=3;
static double _tolerance = 80.0*numeric_limits<double>::epsilon();
//static double _tolerance = sqrt(numeric_limits<double>::epsilon());

// Constructors/Destructors
GeometryXform::GeometryXform(): _cvers("$Revision: 1.33 $"), _GXInv(0)
{
  _origin[0]=0;
  _origin[1]=0;
  _origin[2]=0;
  _rotation[0][0]=1.0;
  _rotation[1][0]=0.0;
  _rotation[2][0]=0.0;
  _rotation[0][1]=0.0;
  _rotation[1][1]=1.0;
  _rotation[2][1]=0.0;
  _rotation[0][2]=0.0;
  _rotation[1][2]=0.0;
  _rotation[2][2]=1.0;
  compute_inverse();
}

GeometryXform::GeometryXform(double dx, double dy, double dz, 
 double phi, double theta,  double psi, EulerConvention cnv): _cvers("$Revision: 1.33 $"), _GXInv(0)
//
// Purpose: The basic user-level constructor taking euler angles in two 
//          different conventions: "x" and "xyz" 
//          (see Goldstein, "Classical Mechanics", Addison Wesley 1980, pp
//          143-148 and pp 608-609 the definitions). This is an active 
//          rotation (Marion/Goldstein is passive), and rotation keeps the 
//          matrix for local_to_global (the inverse transformation).
//
{
  _origin[0]=dx;
  _origin[1]=dy;
  _origin[2]=dz; 
  switch (cnv) {
  case xyz:
    _rotation[0][0]=cos(theta)*cos(phi);
    _rotation[1][0]=cos(theta)*sin(phi);
    _rotation[2][0]=-sin(theta);
    _rotation[0][1]=sin(psi)*sin(theta)*cos(phi) - cos(psi)*sin(phi);
    _rotation[1][1]=sin(psi)*sin(theta)*sin(phi) + cos(psi)*cos(phi);
    _rotation[2][1]=cos(theta)*sin(psi);
    _rotation[0][2]=cos(psi)*sin(theta)*cos(phi) + sin(psi)*sin(phi);
    _rotation[1][2]=cos(psi)*sin(theta)*sin(phi) - sin(psi)*cos(phi);
    _rotation[2][2]=cos(theta)*cos(psi);
    break;
  default:
  case x:
    _rotation[0][0]=cos(psi)*cos(phi) - cos(theta)*sin(phi)*sin(psi);
    _rotation[1][0]=sin(phi)*cos(psi) + cos(theta)*sin(psi)*cos(phi);
    _rotation[2][0]=sin(theta)*sin(psi);
    _rotation[0][1]=-cos(phi)*sin(psi) - cos(theta)*cos(psi)*sin(phi);
    _rotation[1][1]=-sin(psi)*sin(phi) + cos(theta)*cos(phi)*cos(psi);
    _rotation[2][1]=sin(theta)*cos(psi);
    _rotation[0][2]=sin(phi)*sin(theta);
    _rotation[1][2]=-cos(phi)*sin(theta);
    _rotation[2][2]=cos(theta);
    break;
  }
  compute_inverse();
}

GeometryXform::GeometryXform(const SpaceVector& offset,
   const Rotation& rotation): _cvers("$Revision: 1.33 $"), _GXInv(0)
//
// Purpose: User-level constructor taking the CLHEP vector and rotation to
//          define the transform.
//
{
  _origin[0] = offset.x();
  _origin[1] = offset.y();
  _origin[2] = offset.z();
  _rotation[0][0] = rotation.xx();
  _rotation[0][1] = rotation.xy();
  _rotation[0][2] = rotation.xz();
  _rotation[1][0] = rotation.yx();
  _rotation[1][1] = rotation.yy();
  _rotation[1][2] = rotation.yz();
  _rotation[2][0] = rotation.zx();
  _rotation[2][1] = rotation.zy();
  _rotation[2][2] = rotation.zz();
  compute_inverse();
}

GeometryXform::GeometryXform(const SpaceVector& offset,
   const SpaceVector& axis, double angle): _cvers("$Revision: 1.33 $"),
  _GXInv(0)
//
// Purpose: User-level constructor taking a vector offset and a vector
//          and angle representing the direction and rotation angle about
//
{
  const UnitVector uaxis(axis);
  double psi=angle,theta=uaxis.theta(),phi=uaxis.phi();
  _origin[0] = offset.x();
  _origin[1] = offset.y();
  _origin[2] = offset.z();
  _rotation[0][0]=cos(psi)*cos(phi) - cos(theta)*sin(phi)*sin(psi);
  _rotation[1][0]=sin(phi)*cos(psi) + cos(theta)*sin(psi)*cos(phi);
  _rotation[2][0]=sin(theta)*sin(psi);
  _rotation[0][1]=-cos(phi)*sin(psi) - cos(theta)*cos(psi)*sin(phi);
  _rotation[1][1]=-sin(psi)*sin(phi) + cos(theta)*cos(phi)*cos(psi);
  _rotation[2][1]=sin(theta)*cos(psi);
  _rotation[0][2]=sin(phi)*sin(theta);
  _rotation[1][2]=-cos(phi)*sin(theta);
  _rotation[2][2]=cos(theta);
  compute_inverse();
}

GeometryXform::GeometryXform(
 const SpaceVector old1, const SpaceVector old2, const SpaceVector old3,
 const SpaceVector new1, const SpaceVector new2, const SpaceVector new3)
{
  SpaceVector pos[3][2]={{old1,new1},{old2,new2},{old3,new3}};
  SpaceVector basis[3][2];
  SpaceVector center[2];
  for(int i=0; i<2;i++)
    {
      center[i]=pos[0][i]+pos[1][i]+pos[2][i];
      center[i]*=(1./3.);
      basis[0][i]=pos[1][i]-pos[0][i];
      basis[0][i]*=(1./basis[0][i].mag());
      SpaceVector tmp=pos[1][i]-pos[2][i];
      basis[1][i]=tmp-(basis[0][i].dot(tmp))*basis[0][i];
      basis[1][i]*=(1./basis[1][i].mag());
      basis[2][i]=basis[0][i].cross(basis[1][i]);
    }
  Rotation rot1(basis[0][0],basis[1][0],basis[2][0]);
  Rotation rot2(basis[0][1],basis[1][1],basis[2][1]);
  GeometryXform x1(center[0],rot1);
  GeometryXform x2(center[1],rot2);
  x1=x1.get_inverse();
  *this=x2.apply(x1);
}


GeometryXform::GeometryXform(const GeometryXform& initializer):
  _cvers("$Revision: 1.33 $")
//
// Purpose: Copy constructor
//
{
  memcpy(_origin,initializer._origin,3*sizeof(double));
  memcpy(_rotation,initializer._rotation,9*sizeof(double));
  memcpy(_inverse,initializer._inverse,9*sizeof(double));
  _GXInv = 0;
}

GeometryXform::GeometryXform(const double* origin, 
 const double rotation[3][3]): _cvers("$Revision: 1.33 $"), _GXInv(0)
//
// Purpose: The private constructor using arrays in the internal format
//
{
  memcpy(_origin,origin,3*sizeof(double));
  memcpy(_rotation,rotation,9*sizeof(double));
  compute_inverse();
}

GeometryXform::~GeometryXform()
//
// Purpose: Destructor.  Be careful to handle the inverse properly.
//
{
  if( _GXInv ) delete _GXInv;
  _GXInv=0;
}


GeometryXform GeometryXform::operator= (const GeometryXform& initializer)
//
// Purpose: 
//
{
  memcpy(_origin,initializer._origin,3*sizeof(double));
  memcpy(_rotation,initializer._rotation,9*sizeof(double));  
  memcpy(_inverse,initializer._inverse,9*sizeof(double));
  _GXInv=0;
  return *this;
}


// Accessors

GeometryXform& GeometryXform::get_inverse() const
//
// Purpose: Return a reference to the inverse of this Xform.  The construction/
//          destruction of the inverse is handled by this class.
//
{
  if( !_GXInv ) build_managed_inverse();
  return *_GXInv;
}

const SpaceXform& GeometryXform::inverse() const
//
// Purpose: Complete the SpaceXform interface
//
{
  if( !_GXInv ) build_managed_inverse();
  return *_GXInv;
}

SpaceVector GeometryXform::get_offset() const
//
// Purpose: Get the offset portion of the transformation
//
{
  return SpaceVector(_origin[0],_origin[1],_origin[2]);
}

Rotation GeometryXform::get_rotation() const
//
// Purpose: Return the rotation matrix as a class Rotation.
//
{
  double xx = _rotation[0][0];
  double xy = _rotation[0][1];
  double xz = _rotation[0][2];
  double yx = _rotation[1][0];
  double yy = _rotation[1][1];
  double yz = _rotation[1][2];
  double zx = _rotation[2][0];
  double zy = _rotation[2][1];
  double zz = _rotation[2][2];
  return Rotation(ZMpvRep3x3(xx,xy,xz,yx,yy,yz,zx,zy,zz));
}

Rotation GeometryXform::get_inverse_rotation() const
//
// Purpose: Return the inverse matrix as a class Rotation.
//
{
  double xx = _inverse[0][0];
  double xy = _inverse[0][1];
  double xz = _inverse[0][2];
  double yx = _inverse[1][0];
  double yy = _inverse[1][1];
  double yz = _inverse[1][2];
  double zx = _inverse[2][0];
  double zy = _inverse[2][1];
  double zz = _inverse[2][2];
  return Rotation(ZMpvRep3x3(xx,xy,xz,yx,yy,yz,zx,zy,zz));
}

UnitVector GeometryXform::get_axis() const 
//
// Purpose: Get the direction of the axis of rotation
//
{
  double phi,psi,theta;
  get_Euler(phi,theta,psi);

  double e1=cos(0.5*(phi-psi))*sin(theta/2.0);
  double e2=sin(0.5*(phi-psi))*sin(theta/2.0);
  double e3=sin(0.5*(phi+psi))*cos(theta/2.0);
  if (e1==0 && e2==0 && e3==0) e3 = 1.0;// Mo rotation?
  return UnitVector(e1,e2,e3);
}

double GeometryXform::get_angle() const
//
// Purpose: Get the rotation angle
//
{
  double theta,phi,psi;
  get_Euler(phi,theta,psi);
  double e0 = cos(0.5*(phi+psi))*cos(theta/2.0);
  return 2.0*acos(e0);
}

GeometryXform GeometryXform::apply(const GeometryXform& add_this) const
//
// Purpose: Generate a transformation which is the current transformation(this)
//          changed by the amount given in add_this
// States:
//
{
  double offset[3]={0.0,0.0,0.0},rotmat[3][3];
  int i; // Here 'cause of MSVC5.0 bug

  // Rotate the new piece into my own coordinate system,
  for( i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) 
    offset[i] += _rotation[i][j]*add_this._origin[j];

  // add this to my reference point to get the new origin.
  for( i=0 ; i<3 ; i++ ) offset[i] += _origin[i];

  // and finally rotate this by the amount specified by the user.
  for( i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) {
    rotmat[i][j] = 0.0;
    for( int k=0 ; k<3 ; k++ ) rotmat[i][j] += 
			     _rotation[i][k]*add_this._rotation[k][j];
  }
 
  return( GeometryXform(offset,rotmat) );
}

SpacePoint GeometryXform::apply(const SpacePoint& lpt) const
//
// Purpose: Return a point transformed by this transformation.   This is
//          the ``local_to_global'' transformation.
//
{
  double point[3],org_point[3]={lpt.x(),lpt.y(),lpt.z()};
  int i; // Here 'cause of MSVC5.0 bug

  // First rotate,
  for( i=0 ; i<3 ; i++ ) {
    point[i]=0.0;
    for( int j=0 ; j<3 ; j++ ) point[i] += _rotation[i][j]*org_point[j];
  }

  // then offset.
  for( i=0 ; i<3 ; i++ ) point[i] += _origin[i];
  return( CartesianCoordinate(point[0],point[1],point[2]) );
}

SpacePointVector GeometryXform::apply(const SpacePointVector& lpv) const
//
// Purpose: Return a point transformed by this transformation
//
{
  CartesianCoordinate point;
  double vec[3];
  double gpt[3]={lpv.v_x(),lpv.v_y(),lpv.v_z()};
  int i; // Here 'cause of MSVC5.0 bug

  // First transform the point portion as usual
  point = apply((SpacePoint)lpv);

  // Then rotate the vector part
  for( i=0 ; i<3 ; i++ ) {
    vec[i]=0.0;
    for( int j=0 ; j<3 ; j++ ) vec[i] += _rotation[i][j]*gpt[j];
  }

  return CartesianPointVector(point.x(),point.y(),point.z(),
			      vec[0],vec[1],vec[2]);
}

GeometryXform GeometryXform::invert(const GeometryXform& remove_this) const
//
// Purpose: Define an inversion operation such that if 
//
//        GeometryXform C = B.apply(GeometryXform A)
//
//   then
//
//        B = C.invert(A)
//
{
  int i;

  // Remove (invert) the coordinate rotation
  double rotmat[3][3];
  for( i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) {
    rotmat[i][j] = 0.0;
    for( int k=0 ; k<3 ; k++ ) 
      rotmat[i][j] += _rotation[i][k]*remove_this._inverse[k][j];
  }

  // Undo the offset correction (Inverse of rotmat = transpose of rotmat)
  double origin[3];
  for( i=0 ; i<3 ; i++ ) {
    origin[i] = _origin[i];
    for( int j=0 ; j<3 ; j++ )origin[i] -= rotmat[i][j]*remove_this._origin[j];
  }

  return( GeometryXform(origin,rotmat) );
}

SpacePoint GeometryXform::invert(const SpacePoint& gpt) const
//
// Purpose: Define an inversion operation such that if 
//
//        SpacePoint C = B.apply(SpacePoint A)
//
//   then
//
//        A = B.invert(C)
//
{

  // Undo the offset.
  double no_shift[3];
  double tmp[3] = {gpt.x(),gpt.y(),gpt.z()};
  int i;
  for( i=0 ; i<3 ; i++ ) no_shift[i] = tmp[i]-_origin[i];

  // then undo the rotatation
  double new_point[3];
  for( i=0 ; i<3 ; i++ ) {
    new_point[i] = 0.0;
    for( int j=0 ; j<3 ; j++ ) new_point[i] += _inverse[i][j]*no_shift[j];
  }

  return CartesianCoordinate(new_point[0],new_point[1],new_point[2]);
}

GeometryXform GeometryXform::diff(const GeometryXform& base) const
//
// Purpose: Compute a transform A such that if
//
//     C = B.apply(A);
//
//   then
//
//     A = C.diff(B)
//
{
  // Remove (invert) the coordinate rotation
  double rotmat[3][3];
  int i;
  for( i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) {
    rotmat[i][j] = 0.0;
    for( int k=0 ; k<3 ; k++ ) 
      rotmat[i][j] += base._inverse[i][k]*_rotation[k][j];
  }

  // Undo the offset correction
  double origin[3],delta[3];
  for( i=0 ; i<3 ; i++ ) delta[i] = _origin[i] - base._origin[i];
  for( i=0 ; i<3 ; i++ ) {
    origin[i]=0.0;
    for( int j=0 ; j<3 ; j++ ) origin[i] += base._inverse[i][j]*delta[j];
  }

  return( GeometryXform(origin,rotmat) );
}

bool dgs::operator==(const GeometryXform& lhs, const GeometryXform& rhs)
//
// Purpose: Are transformations identical?
//
{
  double delta;
  int i;

  // Offset term
  for( i=0 ; i<3 ; i++ ) {
    if( lhs._origin[i] != rhs._origin[i] ) {
      if(  lhs._origin[i]!=0.0 &&  rhs._origin[i]!=0.0 ) {
	double diff=abs(lhs._origin[i]-rhs._origin[i]);
	double sum=(lhs._origin[i]+rhs._origin[i]);
	delta = diff/sum;
      }
      else {
	if( lhs._origin[i]!=0.0 ) delta=lhs._origin[i];
	else delta=rhs._origin[i];
      }
      if( delta<0.0 ) delta = -delta;
      if( delta >= _tolerance ) return false;  // Error return
    }      
  }

  // Rotation term
  for( i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) {
    if( lhs._rotation[i][j] != rhs._rotation[i][j] ) {
      if( lhs._rotation[i][j]!=0.0 && rhs._rotation[i][j]!=0.0 ) {
	double diff=abs(lhs._rotation[i][j]-rhs._rotation[i][j]);
	double sum=(lhs._rotation[i][j]+rhs._rotation[i][j]);
	delta = diff/sum;
      }
      else {
	if( lhs._rotation[i][j]!=0.0 ) delta=lhs._rotation[i][j];
	else delta=rhs._rotation[i][j];
      }
      if( delta<0.0 ) delta = -delta;
      if( delta >= _tolerance ) return false;
    }      
  }

  // If I get this far, all terms are indeed equal
  return true;
}

bool dgs::operator !=(const GeometryXform& lhs, const GeometryXform& rhs)
//
// Purpose: Are transformations different?
//
{
  return !(lhs==rhs);
}

bool dgs::operator <(const GeometryXform& lhs, const GeometryXform& rhs) {
  return lhs[2]<rhs[2];
}

ostream& dgs::operator <<(ostream& os, const GeometryXform& xf)
//
// Purpose: Formatted dump of a GeometryXform
//
{
  // Set the number of decimal places to print and format to fixed, 
  // saving the original values
  int old=os.precision(xf._ndecimal);
  std::ios_base::fmtflags old_flags = os.setf(ios::fixed,ios::floatfield);
  os << "GeometryXform: Offset = (" << xf._origin[0] << "," 
     << xf._origin[1] << "," << xf._origin[2] << "), Rotation: (";
  for( int i=0 ; i<3 ; i++ ) {
    os << "(";
    for( int j=0 ; j<3 ; j++ ) {
      os << xf._rotation[i][j] ;
      if( j<2 ) os << ",";
    }
    os << ")";
    if( i<2 ) os << ",";
  }
  os << ")";
  // Restore the original number of decimal places and floating point format.
  old=os.precision(old);
  old_flags=os.flags(old_flags);
  return os;
}

void GeometryXform::get_Euler(double &phi, double &theta, double &psi, EulerConvention cnv) const
//
// Purpose: extract the Euler angles
//
{
  switch (cnv) {
  case xyz:  // Roll, pitch, yaw
    theta=asin(-_rotation[2][0]);
    if (abs(_rotation[2][0]) != 1.0 ) {
      phi=atan2(_rotation[1][0],_rotation[0][0]);
      psi=atan2(_rotation[2][1],_rotation[2][2]); 
    }
    else {// degenerate, only angle is either phi-psi or phi+psi.
      // put it all in phi, set psi to zero.
      phi = atan2(-_rotation[0][1],_rotation[1][1]);
      psi = 0;
    }
    break;
  default:
  case x:    // Usual Euler angles
    theta=acos(_rotation[2][2]);
    if (abs(_rotation[2][2]) != 1.0 ) {
      phi=atan2(_rotation[0][2],-_rotation[1][2]);
      psi=atan2(_rotation[2][0],_rotation[2][1]); 
    }
    else {// degenerate, only angle is either phi-psi or phi+psi.
      // put it all in phi, set psi to zero.
      phi = atan2(_rotation[1][0],_rotation[0][0]);
      psi = 0;
    }
    break;
  }
}

void GeometryXform::compute_inverse()
//
// Purpose: Duh
//
{
  
  // Do not compute the inverse -- these are orthonormal
  _inverse[0][0] = _rotation[0][0];
  _inverse[1][0] = _rotation[0][1];
  _inverse[2][0] = _rotation[0][2];
  _inverse[0][1] = _rotation[1][0];
  _inverse[1][1] = _rotation[1][1];
  _inverse[2][1] = _rotation[1][2];
  _inverse[0][2] = _rotation[2][0];
  _inverse[1][2] = _rotation[2][1];
  _inverse[2][2] = _rotation[2][2];

}

void GeometryXform::activate()
//
// Purpose: Fill the necessary non-persistant attributes
//
{
  compute_inverse();
}

void GeometryXform::build_managed_inverse() const
//
// Purpose: Build the locally managed inverse
//
{
  if( !_GXInv ) {
    double offset[3] = {0.0,0.0,0.0};
    for( int i=0 ; i<3 ; i++ ) for( int j=0 ; j<3 ; j++ ) 
      offset[i] -= _inverse[i][j]*_origin[j];
    _GXInv = new GeometryXform(offset,_inverse);
  }
}