#include <stdio.h>
#include <cuda.h>
#include "D3Vector.h"
#define ONE_THIRD .333333333333

#define _DBG_ cerr<<__FILE__<<":"<<__LINE__<<" "
#define _DBG__ cerr<<__FILE__<<":"<<__LINE__<<endl

__global__ void distToRT(trajectory_t *trajectories, int *closestTrajectoryPoints_d, int* wireIndexes_d, float *closestApproach_d, double *s, int NxM, int max)
{  //float *ret, const Wire *wire, const state_t *step, double *s
	//Passed in variables from arrays
	int idx_i = blockIdx.x * blockDim.x + threadIdx.x;
	int idx_j = blockIdx.y * blockDim.y + threadIdx.y;
	Wire *wire = &a_dd[(wireIndexes_d[idx_i + 1])]; //Can't use zero
	trajectory_t trajectory = trajectories[idx_j];
	state_t *step = &trajectory.steps[(closestTrajectoryPoints_d[idx_j + idx_i * NxM])];
	
	//Set up wire directions
	const D3Vector xdir(step->xdir);
	const D3Vector ydir(step->ydir);
	const D3Vector zdir(step->zdir);
	const D3Vector &sdir = wire->sdir;
	const D3Vector &tdir = wire->tdir;
	const D3Vector &udir = wire->udir;
	D3Vector pos_diff = D3Vector(step->origin) - wire->origin;
	
	double A = sdir.Dot(xdir);
	double B = sdir.Dot(ydir);
	double C = sdir.Dot(zdir);
	double D = sdir.Dot(pos_diff);
	double E = tdir.Dot(xdir);
	double F = tdir.Dot(ydir);
	double G = tdir.Dot(zdir);
	double H = tdir.Dot(pos_diff);
	
	//Phi distance:
	double Ro = step->Ro;
	double Ro2 = Ro*Ro;
	double delta_z = D3Vector(step->mom).Dot(step->zdir);
	double delta_phi = D3Vector(step->mom).Dot(step->ydir)/Ro;
	double dz_dphi = delta_z/delta_phi;
	double dz_dphi2=dz_dphi*dz_dphi;
	double Ro_dz_dphi=Ro*dz_dphi;
	double Q=0.25*Ro2*(A*A+E*E);
	double R = -((A*B+E*F)*Ro2 + (A*C+E*G)*Ro_dz_dphi);
	double S= (B*B+F*F)*Ro2+(C*C+G*G)*dz_dphi2+2.0*(B*C+F*G)*Ro_dz_dphi-(A*D+E*H)*Ro;
	double T = 2.0*((B*D+F*H)*Ro + (C*D+G*H)*dz_dphi);
	double U = D*D + H*H;
	double phi = 0.0;
	//Phi check removed - could cause errors. Setting to zero for now.
	if(fabs(Q)>1.0E-6){
		double one_over_fourQ=0.25/Q;
		double a2=3.0*R*one_over_fourQ;
		double a1=2.0*S*one_over_fourQ;
		double a0=T*one_over_fourQ;
		double a2sq=a2*a2;
		double b=ONE_THIRD*(a1-ONE_THIRD*a2sq);
		double c=0.5*(a0-ONE_THIRD*a1*a2)+a2*a2sq/27.0;
		double my_d2=b*b*b+c*c;
		if (my_d2>0){
			double d=sqrt(my_d2);
			double q=cbrt(d-c);
			double p=cbrt(d+c);
			double w0 = q - p;
			phi = w0 - ONE_THIRD*a2;
		}
		else{
	    // Use DeMoivre's theorem to find the cube root of a complex
	    // number.  In this case there are three real solutions.
	    double d=sqrt(-my_d2);
	    c*=-1.;
	    double temp=sqrt(cbrt(c*c+d*d));
	    double theta1=ONE_THIRD*atan2(d,c);
	    double sum_over_2=temp*cos(theta1);
	    double diff_over_2=-temp*sin(theta1);

	    double phi0=-a2/3+2.*sum_over_2;
	    double phi1=-a2/3-sum_over_2+sqrt(3.)*diff_over_2;
	    double phi2=-a2/3-sum_over_2-sqrt(3.)*diff_over_2;

	    double d2_0 = U + phi0*(T + phi0*(S + phi0*(R + phi0*Q)));
	    double d2_1 = U + phi1*(T + phi1*(S + phi1*(R + phi1*Q)));
	    double d2_2 = U + phi2*(T + phi2*(S + phi2*(R + phi2*Q)));
		 if (d2_0<d2_1 && d2_0<d2_2){
	      phi=phi0;
	    }
	    else if (d2_1<d2_0 && d2_1<d2_2){
	      phi=phi1;
	    }
	    else{
	      phi=phi2;
	    }
		}
	}
	if(fabs(Q)<=1.0E-6 || !isfinite(phi)){
		double a = 3.0*R;
		double b = 2.0*S;
		double c = 1.0*T;
		phi = (-b + sqrt(b*b - 4.0*a*c))/(2.0*a); 
	}
	#if 0
	if(finite(phi) && fabs(phi)>2.0E-4){
		if(dist_to_rt_depth>=3){
			_DBG_<<"3 or more recursive calls to DistToRT(). Something is wrong! bailing ..."<<endl;
			//for(int k=0; k<Nswim_steps; k++){
			//	D3Vector &v = swim_steps[k].origin;
			//	_DBG_<<"  "<<k<<": "<<v.X()<<", "<<v.Y()<<", "<<v.Z()<<endl;
			//}
			//exit(-1);
			return std::numeric_limits<double>::quiet_NaN();
		}
		double scale_step = 1.0;
		double s_range = 1.0*scale_step;
		double step_size = 0.02*scale_step;
		int err = InsertSteps(step, phi>0.0 ? +s_range:-s_range, step_size);			// Add new steps near this step by swimming in the direction of phi
		if(!err){
			step=FindClosestSwimStep(wire);									// Find the new closest step
			if(!step)return std::numeric_limits<double>::quiet_NaN();
			dist_to_rt_depth++;
			double doca = DistToRT(wire, step, s);								// re-call ourself with the new step
			dist_to_rt_depth--;
			return doca;
		}
		else{
			if(err<0)return std::numeric_limits<double>::quiet_NaN();
			
			// If InsertSteps() returns an error > 0 then it indicates that it
			// was unable to add additional steps (perhaps because there
			// aren't enough spaces available). In that case, we just go ahead
			// and use the phi we have and make the best estimate possible.
			}
		}
#endif
	
	// It is possible at this point that the value of phi corresponds to
	// a point past the end of the wire. We should check for this here and
	// recalculate, if necessary, the DOCA at the end of the wire. First,
	// calculate h (the vector defined way up above) and dot it into the
	// wire's u-direction to get the position of the DOCA point along the
	// wire.
	double x = -0.5*Ro*phi*phi;
	double y = Ro*phi;
	double z = dz_dphi*phi;
	D3Vector h = pos_diff + x*xdir + y*ydir + z*zdir;
	double u = h.Dot(udir);
	if(fabs(u) > wire->L/2.0){
		// Looks like our DOCA point is past the end of the wire.
		// Find phi corresponding to the end of the wire.
		double L_over_2 = u>0.0 ? wire->L/2.0:-wire->L/2.0;
		double a = -0.5*Ro*udir.Dot(xdir);
		double b = Ro*udir.Dot(ydir) + dz_dphi*udir.Dot(zdir);
		double c = udir.Dot(pos_diff) - L_over_2;
		double twoa=2.0*a;
		double sqroot=sqrt(b*b-4.0*a*c);
		double phi1 = (-b + sqroot)/(twoa); 
		double phi2 = (-b - sqroot)/(twoa);
		phi = fabs(phi1)<fabs(phi2) ? phi1:phi2;
		u=L_over_2;
	}
	//this->last_dist_along_wire = u;

	// Use phi to calculate DOCA
	double d2 = U + phi*(T + phi*(S + phi*(R + phi*Q)));
	double d = sqrt(d2);

	// Calculate distance along track ("s")
	double dz = dz_dphi*phi;
	double Rodphi = Ro*phi;
	double ds = sqrt(dz*dz + Rodphi*Rodphi);
	if(s)*s=step->s + (phi>0.0 ? ds:-ds);
	//if(debug_level>3){
	//	_DBG_<<"distance to rt: "<<*s<<" from step at "<<step->s<<" with ds="<<ds<<" d="<<d<<" dz="<<dz<<" Rodphi="<<Rodphi<<"\n";
	//	_DBG_<<"phi="<<phi<<" U="<<U<<" u="<<u<<"\n";
	//}

	// Remember phi and step so additional info on the point can be obtained
	//this->last_phi = phi;
	//this->last_swim_step = step;
	//this->last_dz_dphi = dz_dphi;
	if(idx_j + idx_i * NxM <= max)
		closestApproach_d[idx_j + idx_i * NxM] = d; //Return distance.
}
/*
int main(void)
{
  float *a_d;
  float a;
  size_t size = sizeof(float);
  cudaMalloc((void **) &a_d, size);
  cudaMemcpy(a_d, &a, sizeof(float), cudaMemcpyHostToDevice);
  distToRT <<<1, 1>>> (a_d, WIRE, STEP, S);
  cudaMemcpy(&a, a_d, sizeof(float), cudaMemcpyDeviceToHost);
  printf("Distance: %f\n", a);
  cudaFree(a_d);
}
*/