#include "CherenkovDisplay.h"
#include "HKED_Util.h"
#include <iostream>
using std::cout;
using std::endl;

void CherenkovDisplay::CartesianToPolar(double &mom,double &theta,double &phi,double p[3]){
	mom=sqrt(p[0]*p[0]+p[1]*p[1]+p[2]*p[2]);
	theta=acos(p[2]/mom);
	phi=atan2(p[1],p[0]);
}
void CherenkovDisplay::polarToCartesian(double mom,double theta,double phi,double p[3]){
	p[0]=mom*sin(theta)*cos(phi);
	p[1]=mom*sin(theta)*sin(phi);
	p[2]=mom*cos(theta);
}
double CherenkovDisplay::DotProduct(double a[],double b[],int n)
{
	double sum=0;
	for (int i=0;i<n;i++)
		sum+=a[i]*b[i];
	return sum;
}
bool CherenkovDisplay::FindConeEnd(double pos[3],double line[3],double intersection[3],int &location)
{
	float radius=maxR;

	//cout<<" radius ="<<radius<<endl;

	//cout<<" pos "<<pos[0]<<" "<<pos[1]<<" "<<pos[2]<<endl;
	//cout<<" line "<<line[0]<<" "<<line[1]<<" "<<line[2]<<endl;
	double a[3];
	double b[3];
	a[0]=pos[0];
	a[1]=pos[1];
	a[2]=0.0;
	b[0]=line[0];
	b[1]=line[1];
	b[2]=0.0;
	double ab=DotProduct(a,b,2);
	double bsquared=DotProduct(b,b,2);
	double asquared=DotProduct(a,a,2);
	double firstTerm=-2.0*ab;
	double firstTermUnderSqrt=4.0*ab*ab;
	double secondTermUnderSqrt=4.0*bsquared*(asquared-radius*radius);
	double underSqrt=firstTermUnderSqrt- secondTermUnderSqrt ;
	if(underSqrt<0){
		return kFALSE;
	}
	double top = firstTerm+sqrt(firstTermUnderSqrt- secondTermUnderSqrt );
	double t = top/(2*bsquared);
	intersection[2]=pos[2]+t*line[2];

	location=1;
	if(fabs(intersection[2])>maxZ)
	{
		double Z=maxZ;
		location=0;
		if(intersection[2]<0)
		{
			Z=-maxZ;
			location=2;
		}
		t=(Z-pos[2])/line[2];
	}
	intersection[0]=pos[0]+t*line[0];
	intersection[1]=pos[1]+t*line[1];
	intersection[2]=pos[2]+t*line[2];
	//cout<<" return point "<<intersection[0]<<" "<<intersection[1]<<" "<<intersection[2]<<endl;
	return kTRUE;
}

void CherenkovDisplay::lineTubeIntersect(double cerenkov[3],double pos[3],float cylinderR,float &xc,float &yc)
{
	double lCxy=sqrt(cerenkov[0]*cerenkov[0]+cerenkov[1]*cerenkov[1]);
	double  cx = cerenkov[0]/lCxy;
	double  cy = cerenkov[1]/lCxy;
	double  px=pos[0];
	double  py=pos[1];
	
	float m=cy/cx;
	float Const=py-m*px;
	float a = m*m+1;
	float b = 2*m*Const;
	float c = Const*Const-cylinderR*cylinderR;
	
	
	
	xc = (-b + sqrt(b*b-4*a*c))/(2*a);
	
	yc=m*xc+Const;
}
void         CherenkovDisplay::UnrollView(double position[3],int location)
{
	HKED_Util::UnrollView(position,location,maxR);

	return;
}


bool CherenkovDisplay::createCherenkov(THKCherenkov* cone, THKCherenkov2D* cone2D,
	TString Name,Color_t colour,double Momentum,double theta,double phi,float Mass,double pos[3],bool dotted)
{
	//cout<<" max R = "<<maxR<<endl;
	if(Mass<=0.0)return false;

	cone2D->SetValues(pos,Momentum,theta,phi);
	
	//?	double length=pScale*Momentum;
	float length=1.0;
	double px = Momentum*cos(phi)*sin(theta);
	double py = Momentum*sin(phi)*sin(theta);
	double pz = Momentum*cos(theta);
	double E = sqrt(Momentum*Momentum+Mass*Mass);
	const double n{1.34};
	//if(E <= Momentum*n)return false;
	double Beta = Momentum/E;
	//cout<<" Momentum "<<Momentum<<" E "<<E<<" beta "<<Beta<<endl;
	double cosAlpha=1/(n*Beta);
	if(abs(cosAlpha)>1.0)return false;

	
	/*
	Construct a vector of points showing the cherenkov circle where it
	impacts with the detector. An alternative is to draw the cones themselves
	- great for publicity but not very practical
	*/
	
	cone->SetElementTitle(Name);
	cone->SetMainColor(colour);	
	cone->SetLineWidth(2.0);
	cone->SetLineStyle(1);
	
	cone2D->SetElementTitle(Name);
	cone2D->SetMainColor(colour);	
	cone2D->SetLineWidth(2.0);
	cone2D->SetLineStyle(1);
	
	double azim=0;
	int nPoints=250;
	double delta  = 6.283/nPoints;
	double alpha=acos(cosAlpha);
	if(fabs(cosAlpha)>1.0)
	{
			//cout<<" cos Alpha is illegal "<<cosAlpha<<" return "<<endl;
		return false;
	}
	double firstX=0.0;
	double firstY=0.0;
	double firstZ=0.0;
	double firstX2D=0.0;
	double firstY2D=0.0;
	double firstZ2D=0.0;
	double oldX2D=0.0;
	double oldY2D=0.0;
	double oldZ2D=0.0;
	double oldX{0.0};
	double oldY{0.0};
	double oldZ{0.0};
	int oldlocation=-1;
	maxTube=-1;
	nextMaxTube=-1;
	nextNextMaxTube=-1;
	double endPoint[3];
	
	
	bool first=true;
	bool thisOne{true};
	do
	{
		/*
		Point x1,y1,z1 is a point on the Cherenkov cone, in the frame which
		has Z along the track direction.
		The cone is at an angle alpha w.r.t. the direction of the track.
		*/
		double x1=length*sin(alpha)*cos(azim);
		double y1=length*sin(alpha)*sin(azim);
		double z1=length*cos(alpha);

		if(std::isnan(x1))
		{
			//cout<<" CherenkovDisplay::createCherenkov : point in track frame is "<<x1<<" "<<y1<<" "<<z1<<endl;
			//cout<<" length = "<<length<<" alpha "<<alpha<<endl;
			return false;
		}
		/*
		Rotate into the detector frame
		*/
		Double_t u1 = px/Momentum;
		Double_t u2 = py/Momentum;
		Double_t u3 = pz/Momentum;
		
		Double_t up = u1*u1 + u2*u2;
		double x2=0.0;
		double y2=0.0;
		double z2=0.0;
		if (up) {
			up = TMath::Sqrt(up);
			Double_t PX = x1,  PY = y1,  PZ = z1;
			x2= (u1*u3*PX - u2*PY + u1*up*PZ)/up;
			y2 = (u2*u3*PX + u1*PY + u2*up*PZ)/up;
			z2 = (u3*u3*PX -    PX + u3*up*PZ)/up;
		}
		else if (u3 < 0.) 
			{ x2 = -x1; z2 = -y1; }      // phi=0  teta=pi
		else {};
		
		/*
		Cherenkov photon  starts at pos[...], in direction cerenkov and ends at endPoint
		*/
		double cerenkov[3];
		copyPointsToArray(x2,y2,z2,cerenkov);
		
		float step=delta;
		int location;
		//cout<<" look for cone end for pos "<<pos[0]<<" "<<pos[1]<<" "<<pos[2]<<"  and angle "<<cerenkov[0]<<" "<<cerenkov[1]<<" "<<cerenkov[2]<<endl;
		if(FindConeEnd(pos,cerenkov,endPoint,location))
		{
			//cout<<" FOUND CONE END"<<endl;
			float rTest=sqrt(endPoint[0]*endPoint[0]+endPoint[1]*endPoint[1]);
			//cout<<" rTest "<<rTest<<endl;

			if(rTest<=maxR){
				//cout<<" Cone next point is "<<endPoint[0]<<" "<<endPoint[1]<<" "<<endPoint[2]<<endl;
				if(!first){
					if(thisOne){
						cone->AddLine(oldX,oldY,oldZ,endPoint[0],endPoint[1],endPoint[2]);
					//SetNextPoint(endPoint[0],endPoint[1],endPoint[2]);
					}
				}
				if(first){
					copyPointsFromArray(endPoint,firstX,firstY,firstZ);
				}
				//oldX=endPoint[0];
				//oldY=endPoint[1];
				copyPointsFromArray(endPoint,oldX,oldY,oldZ);
				oldZ=endPoint[2];

				UnrollView(endPoint,location);
				//cone2D->SetNextPoint(endPoint[0],endPoint[1],endPoint[2]);
				if(!first){
					length=sqrt(
						(oldX2D-endPoint[0])*(oldX2D-endPoint[0])+
						(oldY2D-endPoint[1])*(oldY2D-endPoint[1])+
						(oldZ2D-endPoint[2])*(oldZ2D-endPoint[2]));
					if(oldlocation==location && length < 500)
					{

						if(thisOne){
							cone2D->AddLine(oldX2D,oldY2D,oldZ2D,endPoint[0],endPoint[1],endPoint[2]);
						}
						if(dotted)
							thisOne=!thisOne;
					}
				}
				copyPointsFromArray(endPoint,oldX2D,oldY2D,oldZ2D);
				oldlocation=location;
				if(first==true){
					first=false;
					copyPointsFromArray(endPoint,firstX2D,firstY2D,firstZ2D);
				}
			}
			float testz=endPoint[2];
			if((maxZ-abs(testz)<500)&&(maxR-rTest)<500)
			{
				step=delta/10.0;
			}
		}
		azim+=step;
		
	}
	while(azim<6.283);
	if(!first){
		//cout<<" Cone next point is "<<firstX<<" "<<firstY<<" "<<firstZ<<endl;
		if(thisOne)
		{
		//	cone->SetNextPoint(firstX,firstY,firstZ);
			//cout<<"join the end to the start "<<oldX<<" "<<oldY<<" "<<oldZ<<" "<<firstX<<" "<<firstY<<" "<<firstZ<<endl;
			cone->AddLine(oldX,oldY,oldZ,firstX,firstY,firstZ);
			cone2D->AddLine(endPoint[0],endPoint[1],endPoint[2],firstX2D,firstY2D,firstZ2D);
		}
	}
	return true;
}