#include <stdio.h>
#include <cuda.h>

#define ONE_THIRD .333333333333

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

//----------------
// GPU_DistToRT
//----------------
__device__ float GPU_DistToRT(const Wire *wire, const step_t *step)
{
	//Set up wire directions
	const float3 &xdir = step->xdir;
	const float3 &ydir = step->ydir;
	const float3 &zdir = step->zdir;
	const float3 &sdir = wire->sdir;
	const float3 &tdir = wire->tdir;
	const float3 &udir = wire->udir;
	float3 pos_diff = step->pos - wire->origin;
	
	double A = dot(sdir, xdir);
	double B = dot(sdir, ydir);
	double C = dot(sdir, zdir);
	double D = dot(sdir, pos_diff);
	double E = dot(tdir, xdir);
	double F = dot(tdir, ydir);
	double G = dot(tdir, zdir);
	double H = dot(tdir, pos_diff);
	
	//Phi distance:
	double Ro = step->Ro;
	double Ro2 = Ro*Ro;
	double delta_z = dot(step->mom, step->zdir);
	double delta_phi = dot(step->mom, 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;
	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); 
	}
	
	// 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;
	float3 h = pos_diff + x*xdir + y*ydir + z*zdir;
	double u = dot(h, 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*dot(udir, xdir);
		double b = Ro*dot(udir, ydir) + dz_dphi*dot(udir, zdir);
		double c = dot(udir, 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;
	}

	// 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);

	return d; //Return distance.
}