//Geometry.cc

// System Headers:
#include <stdlib.h>

// Local Headers:
#include "Geometry.h"
#include <math.h>
//#include <iostream.h>

#define pi 3.141592654
#define rad2deg (180./pi)

// Constructor
Geometry::Geometry ( )       
{
  
}

// Destructor
Geometry::~Geometry ( )       
{
  
}


//Distance between 2 points
double Geometry::DistPP(double A[3], double B[3])
{
	double delta,d;
 	int i;
 	delta=0.;
 	for(i=0;i<=2;i++)
	{
		delta=delta+((A[i]-B[i])*(A[i]-B[i]));
	}
 	d=sqrt(delta);
 	return(d);
}


//Finds direction of a track from A to B
void Geometry::FindDir(double& theta,double& phi,double A[3], double B[3])
{
	double raio=sqrt((B[0]-A[0])*(B[0]-A[0])+(B[1]-A[1])*(B[1]-A[1])+(B[2]-A[2])*(B[2]-A[2]));
	double cos_theta=(B[2]-A[2])/raio;
	theta=acos(cos_theta);
	double sin_theta=sin(theta);
 	double sin_phi =(B[1]-A[1])/(raio*sin_theta);
 	double cos_phi =(B[0]-A[0])/(raio*sin_theta);
 	theta=theta*rad2deg;
 	if((A[0]==B[0])&&(A[1]==B[1]))
	{
   		phi=0.;
 	}
	else
	{
 		if(sin_phi>=0)
		{
			phi=acos(cos_phi)*rad2deg;
		}
  		else
		{
			if(cos_phi>=0)
			{
				phi=360-(acos(cos_phi)*rad2deg);
			}
  			else
			{
				phi=90+(acos(cos_phi)*rad2deg);
			}
		}
 	}
}


//Modulus
double Geometry::Mod(double vec[3])
{
	double modulus=sqrt(vec[0]*vec[0]+vec[1]*vec[1]+vec[2]*vec[2]);
 	return(modulus);
}


//Normalizes a vector
void Geometry::Norm(double versor[3], double vector[3])
{
	double m;
 	int i;
 	m=Mod(vector);
 	if (m!=0.)
	{
  		for(i=0;i<=2;i++)
		{
			versor[i]=vector[i]/m;
		}
	}
}


//Converts zenith & azimuth to a normalized vector
void Geometry::Dir2Vec(double versor[3],double& thetad, double& phid)
{
	int i;
 	double theta=thetad/rad2deg;
 	double phi=phid/rad2deg;
 	versor[0]=cos(phi)*sin(theta);
 	versor[1]=sin(phi)*sin(theta);
 	versor[2]=cos(theta);
 	for(i=0;i<=2;i++)
	{
     		if ((versor[i]<1.E-15)&&(versor[i]>-1.E-15)) 
		{
			versor[i]=0.;
		}
	}
	//cout << "versor=" << endl;
}


//Addiction of vectors
void Geometry::AddVec(double sum[3],double vec1[3], double vec2[3])
{
	int i;
 	for(i=0;i<=2;i++)
	{
		sum[i]=vec1[i]+vec2[i];
	}
}


//Multiplication by scalar
void Geometry::MultScal(double prod[3], double vec1[3], double factor)
{
	int i;
 	for(i=0;i<=2;i++)
	{
		prod[i]=vec1[i]*factor;
	}
}


//Scalar Product
double Geometry::ScalProd(double vec1[3], double vec2[3])
{
	int i;
 	double prod;
 	prod=0;
 	for(i=0;i<=2;i++)
	{
		prod+=vec1[i]*vec2[i];
	}
 	return(prod);
}


//Angle between two vectors
double Geometry::Angle2Vec(double vec1[3], double vec2[3])
{
	double scalar,mod1,mod2,theta;
 	scalar=ScalProd(vec1,vec2);
 	mod1=Mod(vec1);
 	mod2=Mod(vec2);
 	theta=acos(scalar/(mod1*mod2))*rad2deg;
 	return(theta);
}


//Vector Product
double Geometry::VecProd(double prod[3],double vec1[3], double vec2[3])
{
	double scalar,mod1,mod2,theta,cross;
 	prod[0]=(vec1[1]*vec2[2]-vec1[2]*vec2[1]);
 	prod[1]=(vec1[2]*vec2[0]-vec1[0]*vec2[2]);
 	prod[2]=(vec1[0]*vec2[1]-vec1[1]*vec2[0]);
 	scalar=ScalProd(vec1,vec2);
 	mod1=Mod(vec1);
 	mod2=Mod(vec2);
 	theta=acos(scalar/(mod1*mod2));
 	cross=mod1*mod2*sin(theta);
 	return(cross);
}


//Defines a track
void Geometry::Track(double track[6],double A[3], double vec[3])
{
	int i;
 	double vector[3];
 	for(i=0;i<=2;i++)
	{
		track[i]=A[i];
	}
// 	Dir2Vec(vector,theta,phi);
 	for(i=3;i<=5;i++)
	{
		track[i]=vec[i-3];
	}
 	//cout << endl<< "Track = (" << track[0] << "," << track[1] << "," << track[2] << ") ";
 	//cout << "+ lambda * (" << track[3] << "," << track[4] << "," << track[5] << ")" << endl;
}


//Intersection of tracks
void Geometry::IntTrack(double A[3], double tr1[6], double tr2[6])
{
	int i;
	//double O[3]={3*0.};
 	double u[3],v[3],w[3];
 	double l0,l1,l2,m0,m1,m2,n0,n1,n2;
 	double det,detl,detm;
 	double lambda,mu;
 	double cross;

 	for(i=0;i<=2;i++)
	{
     		v[i]=tr1[i+3];
     		w[i]=tr2[i+3];
	}

 	l0=v[0], l1=v[1],l2=v[2]; 
 	m0=-w[0],m1=-w[1],m2=-w[2];
 	n0=tr2[0]-tr1[0],n1=tr2[1]-tr1[1],n2=tr2[2]-tr1[2]; 
 
 	cross=VecProd(u,v,w);
 	//cout << endl << cross << endl;
 	if(cross<1.e-10)
	{
   		for(i=0;i<=2;i++)
		{
			A[i]=tr1[i];
		}  
 	}
	else
	{
 		det=(l0*m1-l1*m0),detl=(n0*m1-n1*m0),detm=(l0*n1-l1*n0);
 		if(det==0.)
		{
			det=(l0*m2-l2*m0),detl=(n0*m2-n2*m0),detm=(l0*n2-l2*n0);
		}  
 		if(det==0.)
		{
			det=(l2*m1-l1*m2),detl=(n2*m1-n1*m2),detm=(l2*n1-l1*n2);
		}
 		if(det==0.)
		{
   			//cout << endl << "There is no common point" << endl;
   			exit(8);
 		}
		else
		{
   			lambda=detl/det,mu=detm/det;
   			for(i=0;i<=2;i++)
			{
     				A[i]=tr1[i]+lambda*v[i];
   			}
		}
	}
}


//Distance of a point to a track
double Geometry::DistPT(double P[3], double track[6])
{
	int i;
 	double A[3],AP[3],vec1[3],vec2[3];
 	double dst,md;
 	for(i=0;i<=2;i++)
	{
		A[i]=track[i];
	}
 	for(i=3;i<=5;i++)
	{
		vec1[i-3]=track[i];
	}
 	MultScal(vec2,A,-1.);
 	AddVec(AP,P,vec2);
 	VecProd(vec2,AP,vec1);
 	dst=Mod(vec2);
 	md=Mod(vec1);
 	dst/=md;
 	return(dst);
}


//Find plane (from 3 points)
void Geometry::Plane(double plan[9], double A[3],double B[3], double C[3])
{
	int i;
 	//double v[3],w[3],vec[3];
 	//double a0,a1,a2,a3;
 	for(i=0;i<=2;i++) plan[i]=A[i];
	for(i=3;i<=5;i++) plan[i]=B[i-3];
 	for(i=6;i<=8;i++) plan[i]=C[i-6];
 	
	/*MultScal(vec,A,-1.);
 	AddVec(v,B,vec); //v=AB
 	AddVec(w,C,vec); //w=AC
 	cout << "Plane = (" << plan[0] << "," << plan[1] << "," << plan[2] << ") + ";
 	cout << "lambda * (" << plan[3] << "," << plan[4] << "," << plan[5] << ") + ";
 	cout << "mu * (" << plan[6] << "," << plan[7] << "," << plan[8] << ")" << endl;*/
 	/*eq. paramétrica
	a0=plan[4]*plan[8]-plan[5]*plan[7];
 	a1=plan[5]*plan[6]-plan[3]*plan[8];
 	a2=plan[3]*plan[7]-plan[6]*plan[4];
 	a3=-plan[0]*a0-plan[1]*a1-plan[2]*a2;
 	cout << a0 << "x+" << a1 << "y+" << a2 << "z+" << a3 << "=0" << endl;*/
}

//Intersection of Track and Plane
void Geometry::IntTrPl(double A[3],double track[6], double plan[9])
{
	/*int i;
 	double det,detx,dety,detz;
 	double xp,yp,zp;
 	double angle,normal[3],u[3];
 	double a0,a1,a2,a3; 
 	a0=plan[4]*plan[8]-plan[5]*plan[7];
 	a1=plan[5]*plan[6]-plan[3]*plan[8];
 	a2=plan[3]*plan[7]-plan[6]*plan[4];
 	a3=-plan[0]*a0-plan[1]*a1-plan[2]*a2;
 	det=track[3]*a0+track[4]*a1+track[5]*a2;
 	if (det==0.) 
	{
   		//cout << "There is no common point" << endl;
 	}
	else
	{
   		normal[0]=a0;
   		normal[1]=a1;
   		normal[2]=a2;
   		for(i=0;i<=2;i++)
		{
			u[i]=track[i+3];
		}
   		angle=Angle2Vec(normal,u);
   		angle-=90;
   		//cout << "Angle between track and plane = " << angle << endl;
  		detx=track[0]*(a1*track[4]+a2*track[5])-track[3]*(a3+a1*track[1]+a2*track[2]);
   		dety=track[1]*(a0*track[3]+a2*track[5])-track[4]*(a3+a0*track[0]+a2*track[2]);
   		detz=track[2]*(a0*track[3]+a1*track[4])-track[5]*(a3+a0*track[0]+a1*track[1]);
   		xp=detx/det,yp=dety/det,zp=detz/det;
   		A[0]=xp,A[1]=yp,A[2]=zp;
   		//cout << "Intersection at (" << xp << "," << yp << "," << zp << ")" << endl;
	}*/
	cout << endl << "Plane = (" << plan[0] << "," << plan[1] << "," << plan[2] << ") + ";
 	cout << "lambda*(" << plan[3] << "," << plan[4] << "," << plan[5] << ") + ";
 	cout << "mu*(" << plan[6] << "," << plan[7] << "," << plan[8] << ")" << endl;
 	cout << "Track = (" << track[0] << "," << track[1] << "," << track[2] << ") + " ;
 	cout << "lambda*(" << track[3] << "," << track[4] << "," << track[5] << ")" << endl;
	
	double P01[3],P02[3],P0R[3],AB[3],w[3];
	double lambda,s;
	for(int i=0;i<=2;i++)
		{
			P01[i]=plan[i]-plan[i+3];
			P02[i]=plan[i]-plan[i+6];
			P0R[i]=track[i]-plan[i];
			AB[i]=track[i+3];
		}
	/*cout << "P01=(" << P01[0] << "," << P01[1] << "," << P01[2] << ")" << endl;
	cout << "P02=(" << P02[0] << "," << P02[1] << "," << P02[2] << ")" << endl;
	cout << "P0R=(" << P0R[0] << "," << P0R[1] << "," << P0R[2] << ")" << endl;
	cout << "AB=(" << AB[0] << "," << AB[1] << "," << AB[2] << ")" << endl;*/
	VecProd(w,P01,P02);
	s=ScalProd(AB,w);
	if(s==0) {
		cout << "There is no common point" << endl;
	}else{
		lambda=-ScalProd(w,P0R)/s;
		for(int j=0;j<=2;j++) A[j]=track[j]+lambda*AB[j];	
	}
}


//Definition of a Sphere
void Geometry::Sphere(double sphere[4],double O[3], double& radius)
{
	int i;
 	for(i=0;i<=2;i++)
	{
		sphere[i]=O[i];
	}
 	sphere[3]=radius;
}


//Intersection of Track and Sphere
void Geometry::IntTrSph(double track[6],double sphere[4])
{
	float a,b,c,delta,t1,t2;
	//cout << " Started at (" << track[0] << "," << track[1] << "," << track[2] << ")"  << endl;
 	a=track[3]*track[3]+track[4]*track[4]+track[5]*track[5];
 	b=(2*(track[3]*(track[0]-sphere[0])+track[4]*(track[1]-sphere[1])+track[5]*(track[2]-sphere[2])));
 	c=(track[0]-sphere[0])*(track[0]-sphere[0])+(track[1]-sphere[1])*(track[1]-sphere[1])+(track[2]-sphere[2])*(track[2]-sphere[2])-sphere[3]*sphere[3];
 	delta=(b*b)-(4.*(a*c));
 	if (delta<0) 
	{
		cout << "There is no common point" << endl;
	}
  	else
	{
  		t1=(-b+sqrt(delta))/(2*a);
  		t2=(-b-sqrt(delta))/(2*a);
  		//cout << "t1=" << t1 << " " << "t2=" << t2 << endl;
  		track[0]=track[0]+t1*track[3];
  		track[1]=track[1]+t1*track[4];
  		track[2]=track[2]+t1*track[5]; 		
		//cout << "Intersections at (" << track[0] << "," << track[1] << "," << track[2] << ")"  << endl;
  		//cout << " and (" << track[0]+t2*track[3] << "," << track[1]+t2*track[4] << "," << track[2]+t2*track[5] << ")" << endl;

 	}
}