#include "Quaternion.h"
#include "XYZpoint.h"
#include "Transform.h"
#include "Pointlist.h"
#include <time.h>
#include <stdlib.h>

XYZpoint gen_point(void);

void do_test(int nump)
{    
    const int num_tries=200000;
    XYZpoint A;

    Pointlist X(nump), Y(nump),  YC(nump), YO(nump);

    for (int outer=0; outer<num_tries; outer++)
    {   
	X.clear();	Y.clear();
	
	for(int i=0;i<nump;i++)
	{    
	    A = gen_point();
	    X.append_point(A);
	    A = gen_point();
	    Y.append_point(A);
	}
	
	Transform O;
	O.optimal_transform(X,Y);

	O.trans(X,YO);
	
	if (nump==2)
	{    // check that Y and YO are in a straight line.
	    double dist0 = dist(Y[0], YO[0]);
	    double dist1 = dist(Y[1], YO[1]);
	    double distY = dist(Y[0], Y[1]);
	    double distYO= dist(YO[0], YO[1]);
	    double error;
	    if (distY>distYO)
	    {    // YO inside Y:
	    	error= distY-(dist0+distYO+dist1);
	    }
	    else
	    {    // Y inside YO:
	    	error= distYO-(dist0+distY+dist1);
	    }
	    if (error*error > 1.e-08)
	    {   cerr << "Error: on try " << outer << "\n"
		    <<  " error=" <<  error
		    << "\nO=" << O << "\n"
		    << "\nX = " << X
		    << "\nY = " << Y 
		    << "\nYO = " << YO 
		    << "\n" << flush;
	    }
	}
	
	if (nump==3)
	{
	    Transform C;	
	    C.coplanar_trans(X,Y);
	    C.trans(X,YC);

    //	    double Rg;
    //	    cerr << "YC.centroid = " << YC.compute_centroid(Rg) 
    //	    	<<  " Radius of gyration = " << Rg
    //		<< "\n";

	    double dist2 = square_dist(YC,YO);
	    Pointlist zero(nump);
	    zero.append_point(0,0,0, nump);
	    double size =  square_dist(YC,zero);
	    if (dist2 > 2.e-8 * size)
	    {   cerr << "Error: on try " << outer << "\n"
		    << "C=" << C << "\n"
		    << " YC = " << YC 
		    <<  " dist=" << sqrt(square_dist(YC,Y)) << "\n"
		    << "O=" << O << "\n"
		    << " YO = " << YO 
		    <<  " dist=" << sqrt(square_dist(YO,Y)) << "\n"
		    << " Y = " << Y <<"\n"
		    << flush;
		Transform CO(YC,YO);
		Pointlist YC_CO(nump);
		CO.trans(YC,YC_CO);

		cerr << "Tranform from YC to YO = " << CO <<"\n"
    //		    	<< "YC transformed = " << YC_CO << "\n"
		    << flush;

	    }

	}
	Transform inv;
	inverse(inv, O);
	
	Transform prod = O*inv;
	if (prod.rotation().r() <0.999 || prod.offset().magnitude()>0.001)
	{    cerr << "Error: Inverse of " << O
		<< "\n computed to be " << inv
		<< "\n but product is " << prod
		<< "\n" << flush;
	}

    	double matrix[3][3];
	O.get_rotation_matrix(matrix);
	
	Transform FromRot(matrix);
	FromRot.offset() = O.offset();
	prod = FromRot*inv;
	if (prod.rotation().r() <0.999 || prod.offset().magnitude()>0.001)
	{    cerr << "Error: " << O
		<< "\n converted to rotation matrix and back is " << FromRot
		<< "\n but product with inverse is " << prod
		<< "\n" << flush;
	}
	
	

    }

    cerr << "If no previous messages appeared, "
    	<< "then transform probably working for "
	<< nump << " points\n";
}

XYZpoint gen_point(void)
{
  XYZpoint temp;
	
	temp.X = 100*drand48();
	temp.Y = 100*drand48();
	temp.Z = 100*drand48();
	
  return temp;
}

int main(void)
{
    do_test(4);
    do_test(3);
    do_test(2);
}


//CHANGE LOG:
// 6 Oct 2004 Kevin Karplus
//	added crude test for inverse()
// 16 Feb 2005 Kevin Karplus
//	added testing for 2 points as well as 3