#include "torsion_angle.h"

#include <stdlib.h>	// for drand48
#include <math.h>	// for M_PI
#include "Transform.h"

const double XRange = 25. ;	// range of coordinates



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

int main()
{
    XYZpoint A(1,0,0), B(0,0,0), C(0,1,0), D(1,1,0);
    
    // The bond is along the Y axis, so as long as A.X>0 and A.Z==0,
    // the torsion angle should be unchanged
    
    A.X = 10.; A.Y= 15;
    
    for (double true_torsion=-M_PI; true_torsion<M_PI; true_torsion += 0.1)
    {
        D.X = cos(true_torsion);
	D.Z = -sin(true_torsion);
	double computed_torsion = torsion_angle(A,B,C,D);
	double computed_torsion_error = computed_torsion - true_torsion;
	if (computed_torsion_error > M_PI)  computed_torsion_error-=2.0*M_PI;
	if (computed_torsion_error < -M_PI) computed_torsion_error+=2.0*M_PI;
	assert (computed_torsion_error*computed_torsion_error < 1.e-10); 
//	cout << "torsion= " << true_torsion << "\tcomputed= "
//	    << torsion_angle(A,B,C,D) << "\n";
    }

    // test constructor for rotation about an axis
    for (int i=0; i<10000; i++)
    {
        A= gen_point();
        B= gen_point();
        C= gen_point();
        D= gen_point();
	XYZpoint B_image,C_image, D_image;
	
	double theta = M_PI * 2.0*(drand48() -0.5);
	
	Transform t(B,C, theta);
	t.trans(B,B_image);
	assert( (B-B_image).mag2() < 1.e-10);
	t.trans(C,C_image);
	assert( (C-C_image).mag2() < 1.e-10);
	t.trans(D,D_image);
        
	double dist2 = sq_dist(B,D);
	double delta_dist2 = sq_dist(B,D_image) - dist2;
	double relative_delta_dist2 = delta_dist2/dist2;
	assert(relative_delta_dist2*relative_delta_dist2 < 1.e-10);

	double delta_bond_angle = bond_angle(B,C,D_image) - bond_angle(B,C,D) ;
	assert( delta_bond_angle*delta_bond_angle < 1.e-10);
	double delta_torsion = torsion_angle(A,B,C,D_image) - torsion_angle(A,B,C,D);
	double torsion_error = delta_torsion - theta;
	if (torsion_error > M_PI)  torsion_error-=2.0*M_PI;
	if (torsion_error < -M_PI) torsion_error+=2.0*M_PI;
	assert (torsion_error*torsion_error < 1.e-7); 
    }

    A.X= -4.289; A.Y= 40.324; A.Z=  21.153;
    B.X= -2.110; B.Y= 40.952; B.Z= 21.933;
    C.X= -1.331; C.Y=  43.757; C.Z=  21.296;
    D.X= -1.734; D.Y=  44.656; D.Z=  20.231;
    cerr << "torsion = " << torsion_angle(A,B,C,D) << "\n";
    
    return 0;
}