#include "Math/Vector3D.h"
#include "Math/Point3D.h"

#include "Math/Vector2D.h"
#include "Math/Point2D.h"

#include "Math/EulerAngles.h"

#include "Math/Transform3D.h"
#include "Math/Translation3D.h"

#include "Math/Rotation3D.h"
#include "Math/RotationX.h"
#include "Math/RotationY.h"
#include "Math/RotationZ.h"
#include "Math/Quaternion.h"
#include "Math/AxisAngle.h"
#include "Math/EulerAngles.h"
#include "Math/RotationZYX.h"

#include "Math/LorentzRotation.h"

#include "Math/VectorUtil.h"
#ifndef NO_SMATRIX
#include "Math/SMatrix.h"
#endif

#include <vector>

using namespace ROOT::Math;
using namespace ROOT::Math::VectorUtil;



typedef  DisplacementVector3D<Cartesian3D<double>, GlobalCoordinateSystemTag>  GlobalXYZVector; 
typedef  DisplacementVector3D<Cartesian3D<double>, LocalCoordinateSystemTag>   LocalXYZVector; 
typedef  DisplacementVector3D<Polar3D<double>, GlobalCoordinateSystemTag>      GlobalPolar3DVector; 



typedef  PositionVector3D<Cartesian3D<double>, GlobalCoordinateSystemTag>   GlobalXYZPoint; 
typedef  PositionVector3D<Cartesian3D<double>, LocalCoordinateSystemTag>    LocalXYZPoint; 
typedef  PositionVector3D<Polar3D<double>, GlobalCoordinateSystemTag>       GlobalPolar3DPoint; 
typedef  PositionVector3D<Polar3D<double>, LocalCoordinateSystemTag>       LocalPolar3DPoint; 


//#define TEST_COMPILE_ERROR


int compare( double v1, double v2, const std::string & name = "", double scale = 1.0) {
  //  ntest = ntest + 1; 

  // numerical double limit for epsilon 
  double eps = scale* std::numeric_limits<double>::epsilon();
  int iret = 0; 
  double delta = v2 - v1;
  double d = 0;
  if (delta < 0 ) delta = - delta; 
  if (v1 == 0 || v2 == 0) { 
    if  (delta > eps ) { 
      iret = 1; 
    }
  }
  // skip case v1 or v2 is infinity
  else { 
     d = v1; 

    if ( v1 < 0) d = -d; 
    // add also case when delta is small by default
    if ( delta/d  > eps && delta > eps ) 
      iret =  1; 
  }

  if (iret == 0) 
    std::cout << ".";
  else { 
    int pr = std::cout.precision (18);
    std::cout << "\nDiscrepancy in " << name << "() : " << v1 << " != " << v2 << " discr = " << int(delta/d/eps) 
              << "   (Allowed discrepancy is " << eps  << ")\n";
    std::cout.precision (pr);
    //nfail = nfail + 1;
  }
  return iret; 
}

template<class Transform>
bool IsEqual(const Transform & t1, const Transform & t2, unsigned int size)  {
// size should be an enum of the Transform class 
   std::vector<double> x1(size); 
   std::vector<double> x2(size); 
   t1.GetComponents(x1.begin(), x1.end() );
   t2.GetComponents(x2.begin(), x2.end() );
   bool ret = true; 
   unsigned int i = 0; 
   while (ret && i < size) {
      // from TMath::AreEqualRel(x1,x2,2*eps)
      bool areEqual = std::abs(x1[i]-x2[i]) < std::numeric_limits<double>::epsilon() * 
         ( std::abs(x1[i]) + std::abs(x2[i] ) ); 
      ret &= areEqual; 
      i++;
   }
   return ret; 
}

int testVector3D() { 

  int iret = 0;

  std::cout << "testing Vector3D   \t:\t"; 

  // test the vector tags 

  GlobalXYZVector vg(1.,2.,3.); 
  GlobalXYZVector vg2(vg); 

  GlobalPolar3DVector vpg(vg);

  iret |= compare(vpg.R(), vg2.R() );
 
//   std::cout << vg2 << std::endl;

  double r = vg.Dot(vpg); 
  iret |= compare(r, vg.Mag2() );

  GlobalXYZVector vcross = vg.Cross(vpg); 
  iret |= compare(vcross.R(), 0.0,"cross",10 );

//   std::cout << vg.Dot(vpg) << std::endl;
//   std::cout << vg.Cross(vpg) << std::endl;


  
 

  GlobalXYZVector vg3 = vg + vpg;  
  iret |= compare(vg3.R(), 2*vg.R() );

  GlobalXYZVector vg4 = vg - vpg;  
  iret |= compare(vg4.R(), 0.0,"diff",10 );




#ifdef TEST_COMPILE_ERROR
   LocalXYZVector vl; vl = vg; 
  LocalXYZVector vl2(vg2);
  LocalXYZVector vl3(vpg);
  vg.Dot(vl);
  vg.Cross(vl);
  vg3 = vg + vl;
  vg4 = vg - vl;
#endif


  if (iret == 0) std::cout << "\t\t\t\t\tOK\n"; 
  else std::cout << "\t\t\t\tFAILED\n";


  return iret;
}



int testPoint3D() { 

  int iret = 0;

  std::cout << "testing Point3D    \t:\t"; 

  // test the vector tags 

  GlobalXYZPoint pg(1.,2.,3.); 
  GlobalXYZPoint pg2(pg); 

  GlobalPolar3DPoint ppg(pg);

  iret |= compare(ppg.R(), pg2.R() );
  //std::cout << pg2 << std::endl;
  



  GlobalXYZVector vg(pg);

  double r = pg.Dot(vg); 
  iret |= compare(r, pg.Mag2() );

  GlobalPolar3DVector vpg(pg);
  GlobalXYZPoint pcross = pg.Cross(vpg);
  iret |= compare(pcross.R(), 0.0,"cross",10 );

  GlobalPolar3DPoint pg3 = ppg + vg; 
  iret |= compare(pg3.R(), 2*pg.R() );

  GlobalXYZVector vg4 = pg - ppg; 
  iret |= compare(vg4.R(), 0.0,"diff",10 );


#ifdef TEST_COMPILE_ERROR
  LocalXYZPoint pl; pl = pg; 
  LocalXYZVector pl2(pg2);
  LocalXYZVector pl3(ppg);
  pl.Dot(vg);
  pl.Cross(vg);
  pg3 = ppg + pg;
  pg3 = ppg + pl;
  vg4 = pg - pl;
#endif

  // operator - 
  XYZPoint q1(1.,2.,3.);
  XYZPoint q2 = -1.* q1; 
  XYZVector v2 = -XYZVector(q1);
  iret |= compare(XYZVector(q2) == v2,true,"reflection");


  if (iret == 0) std::cout << "\t\t\t\t\tOK\n"; 
  else std::cout << "\t\t\t\tFAILED\n"; 

  return iret;
}



typedef  DisplacementVector2D<Cartesian2D<double>, GlobalCoordinateSystemTag>  GlobalXYVector; 
typedef  DisplacementVector2D<Cartesian2D<double>, LocalCoordinateSystemTag>   LocalXYVector; 
typedef  DisplacementVector2D<Polar2D<double>, GlobalCoordinateSystemTag>      GlobalPolar2DVector; 




int testVector2D() { 

  int iret = 0;

  std::cout << "testing Vector2D   \t:\t"; 

  // test the vector tags 

  GlobalXYVector vg(1.,2.); 
  GlobalXYVector vg2(vg); 

  GlobalPolar2DVector vpg(vg);

  iret |= compare(vpg.R(), vg2.R() );
 
//   std::cout << vg2 << std::endl;

  double r = vg.Dot(vpg); 
  iret |= compare(r, vg.Mag2() );

//   std::cout << vg.Dot(vpg) << std::endl;
 

  GlobalXYVector vg3 = vg + vpg;  
  iret |= compare(vg3.R(), 2*vg.R() );

  GlobalXYVector vg4 = vg - vpg;  
  iret |= compare(vg4.R(), 0.0,"diff",10 );


  double angle = 1.;
  vg.Rotate(angle); 
  iret |= compare(vg.Phi(), vpg.Phi() + angle );
  iret |= compare(vg.R(), vpg.R()  );

  GlobalXYZVector v3d(1,2,0);
  GlobalXYZVector vr3d = RotationZ(angle) * v3d; 
  iret |= compare(vg.X(), vr3d.X() );
  iret |= compare(vg.Y(), vr3d.Y()  );
  
  GlobalXYVector vu = vg3.Unit();
  iret |= compare(vu.R(), 1. );
  

#ifdef TEST_COMPILE_ERROR
   LocalXYVector vl; vl = vg; 
  LocalXYVector vl2(vg2);
  LocalXYVector vl3(vpg);
  vg.Dot(vl);
  vg3 = vg + vl;
  vg4 = vg - vl;
#endif


  if (iret == 0) std::cout << "\t\t\t\tOK\n"; 
  else std::cout << "\t\t\tFAILED\n";


  return iret;
}


typedef  PositionVector2D<Cartesian2D<double>, GlobalCoordinateSystemTag>   GlobalXYPoint; 
typedef  PositionVector2D<Cartesian2D<double>, LocalCoordinateSystemTag>    LocalXYPoint; 
typedef  PositionVector2D<Polar2D<double>, GlobalCoordinateSystemTag>       GlobalPolar2DPoint; 
typedef  PositionVector2D<Polar2D<double>, LocalCoordinateSystemTag>       LocalPolar2DPoint; 



int testPoint2D() { 

  int iret = 0;

  std::cout << "testing Point2D    \t:\t"; 

  // test the vector tags 

  GlobalXYPoint pg(1.,2.); 
  GlobalXYPoint pg2(pg); 

  GlobalPolar2DPoint ppg(pg);

  iret |= compare(ppg.R(), pg2.R() );
  //std::cout << pg2 << std::endl;
  



  GlobalXYVector vg(pg);

  double r = pg.Dot(vg); 
  iret |= compare(r, pg.Mag2() );

  GlobalPolar2DVector vpg(pg);

  GlobalPolar2DPoint pg3 = ppg + vg; 
  iret |= compare(pg3.R(), 2*pg.R() );

  GlobalXYVector vg4 = pg - ppg; 
  iret |= compare(vg4.R(), 0.0,"diff",10 );


#ifdef TEST_COMPILE_ERROR
  LocalXYPoint pl; pl = pg; 
  LocalXYVector pl2(pg2);
  LocalXYVector pl3(ppg);
  pl.Dot(vg);
  pl.Cross(vg);
  pg3 = ppg + pg;
  pg3 = ppg + pl;
  vg4 = pg - pl;
#endif

  // operator - 
  XYPoint q1(1.,2.);
  XYPoint q2 = -1.* q1; 
  XYVector v2 = -XYVector(q1);
  iret |= compare(XYVector(q2) == v2,true,"reflection");



  double angle = 1.;
  pg.Rotate(angle); 
  iret |= compare(pg.Phi(), ppg.Phi() + angle );
  iret |= compare(pg.R(), ppg.R()  );

  GlobalXYZVector v3d(1,2,0);
  GlobalXYZVector vr3d = RotationZ(angle) * v3d; 
  iret |= compare(pg.X(), vr3d.X() );
  iret |= compare(pg.Y(), vr3d.Y()  );
  


  if (iret == 0) std::cout << "\t\t\t\tOK\n"; 
  else std::cout << "\t\t\tFAILED\n"; 

  return iret;
}


// missing LV test

int testRotations3D() {  

  int iret=0; 
  std::cout << "testing 3D Rotations\t:\t"; 


  Rotation3D rot = RotationZ(1.) * RotationY(2) * RotationX(3); 
  GlobalXYZVector vg(1.,2.,3); 
  GlobalXYZPoint  pg(1.,2.,3); 
  GlobalPolar3DVector vpg(vg);  

  //  GlobalXYZVector vg2 = rot.operator()<Cartesian3D,GlobalCoordinateSystemTag, GlobalCoordinateSystemTag> (vg); 
  GlobalXYZVector vg2 = rot(vg); 
  iret |= compare(vg2.R(), vg.R(),"rot3D" );

  GlobalXYZPoint pg2 = rot(pg); 
  iret |= compare(pg2.X(), vg2.X(),"x diff");
  iret |= compare(pg2.Y(), vg2.Y(),"y diff");
  iret |= compare(pg2.Z(), vg2.Z(),"z diff");


  Quaternion qrot(rot); 

  pg2 = qrot(pg); 
  iret |= compare(pg2.X(), vg2.X(),"x diff",10);
  iret |= compare(pg2.Y(), vg2.Y(),"y diff",10);
  iret |= compare(pg2.Z(), vg2.Z(),"z diff",10);

  GlobalPolar3DVector vpg2 = qrot * vpg; 

  iret |= compare(vpg2.X(), vg2.X(),"x diff",10 );
  iret |= compare(vpg2.Y(), vg2.Y(),"y diff",10 );
  iret |= compare(vpg2.Z(), vg2.Z(),"z diff",10 );

  AxisAngle arot(rot); 
  pg2 = arot(pg); 
  iret |= compare(pg2.X(), vg2.X(),"x diff",10 );
  iret |= compare(pg2.Y(), vg2.Y(),"y diff",10 );
  iret |= compare(pg2.Z(), vg2.Z(),"z diff",10 );

  vpg2 = arot (vpg);
  iret |= compare(vpg2.X(), vg2.X(),"x diff",10 );
  iret |= compare(vpg2.Y(), vg2.Y(),"y diff",10 );
  iret |= compare(vpg2.Z(), vg2.Z(),"z diff",10 );
  
  EulerAngles erot(rot); 

  vpg2 = erot (vpg); 
  iret |= compare(vpg2.X(), vg2.X(),"x diff",10 );
  iret |= compare(vpg2.Y(), vg2.Y(),"y diff",10 );
  iret |= compare(vpg2.Z(), vg2.Z(),"z diff",10 );

  GlobalXYZVector vrx = RotationX(3) * vg; 
  GlobalXYZVector vry = RotationY(2) * vrx; 
  vpg2 = RotationZ(1) * GlobalPolar3DVector (vry); 
  iret |= compare(vpg2.X(), vg2.X(),"x diff",10 );
  iret |= compare(vpg2.Y(), vg2.Y(),"y diff",10 );
  iret |= compare(vpg2.Z(), vg2.Z(),"z diff",10 );

  // test Get/SetComponents
  XYZVector v1,v2,v3; 
  rot.GetComponents(v1,v2,v3);
  const Rotation3D rot2(v1,v2,v3); 
  //rot2.SetComponents(v1,v2,v3);
  double r1[9],r2[9]; 
  rot.GetComponents(r1,r1+9);
  rot2.GetComponents(r2,r2+9);
  for (int i = 0; i < 9; ++i) {
     iret |= compare(r1[i],r2[i],"Get/SetComponents"); 
  }
  // operator == fails for numerical precision
  //iret |= compare( (rot2==rot),true,"Get/SetComponens");

  // test get/set with a matrix
#ifndef NO_SMATRIX
  SMatrix<double,3> mat; 
  rot2.GetRotationMatrix(mat);
  rot.SetRotationMatrix(mat); 
  iret |= compare( (rot2==rot),true,"Get/SetRotMatrix");
#endif

  //test inversion
  Rotation3D rotInv = rot.Inverse();
  rot.Invert(); // invert in place
  bool comp = (rotInv == rot ); 
  iret |= compare(comp,true,"inversion");

  // rotation and scaling of points
  XYZPoint q1(1.,2,3); double a = 3;
  XYZPoint qr1 =  rot( a * q1);
  XYZPoint qr2 =  a * rot( q1);
  iret |= compare(qr1.X(), qr2.X(),"x diff",10 );
  iret |= compare(qr1.Y(), qr2.Y(),"y diff",10 );
  iret |= compare(qr1.Z(), qr2.Z(),"z diff",10 );


  if (iret == 0) std::cout << "\tOK\n"; 
  else std::cout << "\t FAILED\n";

  return iret; 
}


int testTransform3D() { 


  std::cout << "testing 3D Transform\t:\t"; 
  int iret = 0; 

  EulerAngles r(1.,2.,3.);

  GlobalPolar3DVector v(1.,2.,3.);
  GlobalXYZVector w(v);

  Transform3D t1( v );
  GlobalXYZPoint pg; 
  t1.Transform( LocalXYZPoint(), pg ); 
  iret |= compare(pg.X(), v.X(),"x diff",10 );
  iret |= compare(pg.Y(), v.Y(),"y diff",10 );
  iret |= compare(pg.Z(), v.Z(),"z diff",10 );


  Transform3D t2( r, v );

  GlobalPolar3DVector vr = r.Inverse()*v; 

//   std::cout << GlobalXYZVector(v) << std::endl; 
//   std::cout << GlobalXYZVector(vr) << std::endl; 
//   std::cout << GlobalXYZVector (r(v)) << std::endl; 
//   std::cout << GlobalXYZVector (r(vr)) << std::endl; 
//   std::cout << vr << std::endl; 
//   std::cout << r(vr) << std::endl; 

  

//   std::cout << r << std::endl; 
//   std::cout << r.Inverse() << std::endl; 
//   std::cout << r * r.Inverse() << std::endl; 
//   std::cout << Rotation3D(r) * Rotation3D(r.Inverse()) << std::endl; 
//   std::cout << Rotation3D(r) * Rotation3D(r).Inverse() << std::endl; 


  // test Translation3D

  Translation3D tr1(v);
  Translation3D tr2(v.X(),v.Y(),v.Z());
  iret |= compare(tr1 ==tr2, 1,"eq transl",1 );

  Translation3D tr3 = tr1 * tr1.Inverse(); 
  GlobalPolar3DVector vp2 = tr3 * v;
  iret |= compare(vp2.X(), v.X(),"x diff",10 );
  iret |= compare(vp2.Y(), v.Y(),"y diff",10 );
  iret |= compare(vp2.Z(), v.Z(),"z diff",10 );


  Transform3D t2b = tr1 * Rotation3D(r);
  // this above fails on Windows - use a comparison with tolerance
  // 12 is size of Transform3D internal vector
  iret |= compare( IsEqual(t2,t2b,12), true,"eq1 transf",1 );
  //iret |= compare(t2 ==t2b, 1,"eq1 transf",1 );
  Transform3D t2c( r, tr1); 
  iret |= compare( IsEqual(t2,t2c,12), true,"eq2 transf",1 );
  //iret |= compare(t2 ==t2c, 1,"eq2 transf",1 );


  Transform3D t3 =  Rotation3D(r) * Translation3D(vr); 

  Rotation3D rrr;
  XYZVector vvv;
  t2b.GetDecomposition(rrr,vvv);
  iret |= compare(Rotation3D(r) ==rrr, 1,"eq transf rot",1 );
  iret |= compare( tr1.Vect() == vvv, 1,"eq transf vec",1 );
//   if (iret) std::cout << vvv << std::endl;
//   if (iret) std::cout << Translation3D(vr) << std::endl;

  Translation3D ttt;
  t2b.GetDecomposition(rrr,ttt);
  iret |= compare( tr1 == ttt, 1,"eq transf trans",1 );
//   if (iret) std::cout << ttt << std::endl;

  EulerAngles err2; 
  GlobalPolar3DVector vvv2;
  t2b.GetDecomposition(err2,vvv2);
  iret |= compare( r.Phi(), err2.Phi(),"transf rot phi",4 );
  iret |= compare( r.Theta(), err2.Theta(),"transf rot theta",1 );
  iret |= compare( r.Psi(), err2.Psi(),"transf rot psi",1 );

  iret |= compare( v == vvv2, 1,"eq transf g vec",1 );

  // create from other rotations
  RotationZYX rzyx(r); 
  Transform3D t4( rzyx);
  iret |= compare( t4.Rotation() == Rotation3D(rzyx), 1,"eq trans rzyx",1 );

  Transform3D trf2 = tr1 * r; 
  iret |= compare( trf2 == t2b, 1,"trasl * e rot",1 );
  Transform3D trf3 = r * Translation3D(vr); 
  //iret |= compare( trf3 == t3, 1,"e rot * transl",1 );
  // this above fails on i686-slc5-gcc43-opt - use a comparison with tolerance
  iret |= compare( IsEqual(trf3,t3,12), true,"e rot * transl",1 );

  Transform3D t5(rzyx, v);
  Transform3D trf5 = Translation3D(v) * rzyx; 
  //iret |= compare( trf5 == t5, 1,"trasl * rzyx",1 );
  iret |= compare( IsEqual(trf5,t5,12), true,"trasl * rzyx",1 );

  Transform3D t6(rzyx, rzyx * Translation3D(v).Vect() );
  Transform3D trf6 = rzyx * Translation3D(v); 
  iret |= compare( trf6 == t6, 1,"rzyx * transl",1 );
  if (iret) std::cout << t6 << "\n---\n" << trf6 << std::endl;



  Transform3D trf7 = t4 * Translation3D(v);
  //iret |= compare( trf7 == trf6, 1,"tranf * transl",1 );
  iret |= compare( IsEqual(trf7,trf6,12), true,"tranf * transl",1 );
  Transform3D trf8 = Translation3D(v) * t4;
  iret |= compare( trf8 == trf5, 1,"trans * transf",1 );

  Transform3D trf9 = Transform3D(v) * rzyx;
  iret |= compare( trf9 == trf5, 1,"tranf * rzyx",1 );
  Transform3D trf10 = rzyx * Transform3D(v);
  iret |= compare( trf10 == trf6, 1,"rzyx * transf",1 );
  Transform3D trf11 = Rotation3D(rzyx) * Transform3D(v);
  iret |= compare( trf11 == trf10, 1,"r3d * transf",1 );

  RotationZYX rrr2 = trf10.Rotation<RotationZYX>(); 
  //iret |= compare( rzyx == rrr2, 1,"gen Rotation()",1 );
  iret |= compare( rzyx.Phi() , rrr2.Phi(),"gen Rotation() Phi",1 );
  iret |= compare( rzyx.Theta(), rrr2.Theta(),"gen Rotation() Theta",10 );
  iret |= compare( rzyx.Psi(), rrr2.Psi(),"gen Rotation() Psi",1 );
  if (iret) std::cout << rzyx << "\n---\n" << rrr2 << std::endl;


  //std::cout << t2 << std::endl; 
  //std::cout << t3 << std::endl; 

  XYZPoint p1(-1.,2.,-3);
  
  XYZPoint p2 = t2 (p1); 
  Polar3DPoint p3 = t3 ( Polar3DPoint(p1) ); 
  iret |= compare(p3.X(), p2.X(),"x diff",10 );
  iret |= compare(p3.Y(), p2.Y(),"y diff",10 );
  iret |= compare(p3.Z(), p2.Z(),"z diff",10 );

  GlobalXYZVector v1(1.,2.,3.);
  LocalXYZVector v2;  t2.Transform (v1, v2); 
  GlobalPolar3DVector v3;  t3.Transform ( GlobalPolar3DVector(v1), v3 ); 

  iret |= compare(v3.X(), v2.X(),"x diff",10 );
  iret |= compare(v3.Y(), v2.Y(),"y diff",10 );
  iret |= compare(v3.Z(), v2.Z(),"z diff",10 );

  XYZPoint q1(1,2,3);
  XYZPoint q2(-1,-2,-3);
  XYZPoint q3 = q1 +  XYZVector(q2);
  //std::cout << q3 << std::endl; 
  XYZPoint qt3 = t3(q3); 
  //std::cout << qt3 << std::endl; 
  XYZPoint qt1 = t3(q1);
  XYZVector vt2 = t3( XYZVector(q2) );
  XYZPoint qt4 = qt1 + vt2; 
  iret |= compare(qt3.X(), qt4.X(),"x diff",10 );
  iret |= compare(qt3.Y(), qt4.Y(),"y diff",10 );
  iret |= compare(qt3.Z(),  qt4.Z(),"z diff",10 );
     //std::cout << qt4 << std::endl; 

  // this fails
//  double a = 3;
  //XYZPoint q4 = a*q1;
//   std::cout << t3( a * q1) << std::endl;
//   std::cout << a * t3(q1) << std::endl;

  // test get/set with a matrix
#ifndef NO_SMATRIX
  SMatrix<double,3,4> mat; 
  t3.GetTransformMatrix(mat);
  Transform3D t3b;  t3b.SetTransformMatrix(mat); 
  iret |= compare( (t3==t3b),true,"Get/SetTransformMatrix");

  // test LR 
  Boost b(0.2,0.4,0.8); 
  LorentzRotation lr(b); 
  SMatrix<double,4> mat4; 
  lr.GetRotationMatrix(mat4);
  LorentzRotation lr2;  lr2.SetRotationMatrix(mat4); 
  iret |= compare( (lr==lr2),true,"Get/SetLRotMatrix");
#endif


  if (iret == 0) std::cout << "OK\n"; 
  else std::cout << "\t\t\tFAILED\n"; 

  return iret; 
}


int testVectorUtil() { 

  std::cout << "testing VectorUtil  \t:\t"; 
   int iret = 0;

   // test new perp functions
   XYZVector v(1.,2.,3.); 

   XYZVector vx = ProjVector(v,XYZVector(3,0,0) );
   iret |= compare(vx.X(), v.X(),"x",1 );
   iret |= compare(vx.Y(), 0,"y",1 );
   iret |= compare(vx.Z(), 0,"z",1 );

   XYZVector vpx = PerpVector(v,XYZVector(2,0,0) );
   iret |= compare(vpx.X(), 0,"x",1 );
   iret |= compare(vpx.Y(), v.Y(),"y",1 );
   iret |= compare(vpx.Z(), v.Z(), "z",1 );

   double perpy = Perp(v, XYZVector(0,2,0) );  
   iret |= compare(perpy, std::sqrt( v.Mag2() - v.y()*v.y()),"perpy" );

   XYZPoint  u(1,1,1); 
   XYZPoint  un = u/u.R(); 
   

   XYZVector vl = ProjVector(v,u);
   XYZVector vl2 = XYZVector(un) * ( v.Dot(un ) ); 

   iret |= compare(vl.X(), vl2.X(),"x",1 );
   iret |= compare(vl.Y(), vl2.Y(),"y",1 );
   iret |= compare(vl.Z(), vl2.Z(),"z",1 );

   XYZVector vp = PerpVector(v,u);
   XYZVector vp2 = v - XYZVector ( un * ( v.Dot(un ) ) ); 
   iret |= compare(vp.X(), vp2.X(),"x",10 );
   iret |= compare(vp.Y(), vp2.Y(),"y",10 );
   iret |= compare(vp.Z(), vp2.Z(),"z",10 );

   double perp = Perp(v,u);
   iret |= compare(perp, vp.R(),"perp",1 );
   double perp2 = Perp2(v,u);
   iret |= compare(perp2, vp.Mag2(),"perp2",1 );

  if (iret == 0) std::cout << "\t\t\t\tOK\n"; 
  else std::cout << "\t\t\t\t\t\tFAILED\n"; 
  return iret; 

}

int testGenVector() { 

  int iret = 0; 
  iret |= testVector3D(); 
  iret |= testPoint3D(); 

  iret |= testVector2D(); 
  iret |= testPoint2D(); 

  iret |= testRotations3D(); 

  iret |= testTransform3D();

  iret |= testVectorUtil();


  if (iret !=0) std::cout << "\nTest GenVector FAILED!!!!!!!!!\n";
  return iret; 

}

int main() { 
   int ret = testGenVector();
   if (ret)  std::cerr << "test FAILED !!! " << std::endl; 
   else   std::cout << "test OK " << std::endl;
   return ret;
}