/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/

This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose, 
including commercial applications, and to alter it and redistribute it freely, 
subject to the following restrictions:

1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/


#include "BU_EdgeEdge.h"
#include "BU_Screwing.h"
#include <LinearMath/btPoint3.h>
#include <LinearMath/btPoint3.h>

//#include "BU_IntervalArithmeticPolynomialSolver.h"
#include "BU_AlgebraicPolynomialSolver.h"

#define USE_ALGEBRAIC
#ifdef USE_ALGEBRAIC	
#define BU_Polynomial BU_AlgebraicPolynomialSolver
#else	
#define BU_Polynomial BU_IntervalArithmeticPolynomialSolver
#endif

BU_EdgeEdge::BU_EdgeEdge()
{
}


bool BU_EdgeEdge::GetTimeOfImpact(
								  const BU_Screwing& screwAB,
								  const btPoint3& a,//edge in object A
								  const btVector3& u,
								  const btPoint3& c,//edge in object B
								  const btVector3& v,
								  btScalar &minTime,
								  btScalar &lambda1,
								  btScalar& mu1
								  
								  )
{
	bool hit=false;
	
	btScalar lambda;
	btScalar mu;
	
	const btScalar w=screwAB.GetW();
	const btScalar s=screwAB.GetS();
	
	if (btFuzzyZero(s) &&
		btFuzzyZero(w))
	{
		//no motion, no collision
		return false;
	}
	
	if (btFuzzyZero(w) )
	{
		//pure translation W=0, S <> 0
		//no trig, f(t)=t
		btScalar det = u.y()*v.x()-u.x()*v.y();
		if (!btFuzzyZero(det))
		{		
			lambda = (a.x()*v.y() - c.x() * v.y() - v.x() * a.y() + v.x() * c.y()) / det;
			mu = (u.y() * a.x() - u.y() * c.x() - u.x() * a.y() + u.x() * c.y()) / det;

			if (mu >=0 && mu <= 1 && lambda >= 0 && lambda <= 1)
			{
				// single potential collision is
				btScalar t = (c.z()-a.z()+mu*v.z()-lambda*u.z())/s;
				//if this is on the edge, and time t within [0..1] report hit
				if (t>=0 && t <= minTime)
				{
					hit = true;
					lambda1 = lambda;
					mu1 = mu;
					minTime=t;
				}
			}
			
		} else
		{
			//parallel case, not yet
		}
	} else
	{
		if (btFuzzyZero(s) )
		{
			if (btFuzzyZero(u.z()) )
			{
				if (btFuzzyZero(v.z()) )
				{
					//u.z()=0,v.z()=0
					if (btFuzzyZero(a.z()-c.z()))
					{
						//printf("NOT YET planar problem, 4 vertex=edge cases\n");
						
					} else
					{
						//printf("parallel but distinct planes, no collision\n");
						return false;
					}
					
				} else
				{
					btScalar mu = (a.z() - c.z())/v.z();
					if (0<=mu && mu <= 1)
					{
					//	printf("NOT YET//u.z()=0,v.z()<>0\n");
					} else
					{
						return false;
					}
					
				}
			} else
			{
				//u.z()<>0
				
				if (btFuzzyZero(v.z()) )
				{
					//printf("u.z()<>0,v.z()=0\n");
					lambda =  (c.z() - a.z())/u.z();
					if (0<=lambda && lambda <= 1)
					{
						//printf("u.z()<>0,v.z()=0\n");
						btPoint3 rotPt(a.x()+lambda * u.x(), a.y()+lambda * u.y(),0.f);
						btScalar r2 = rotPt.length2();//px*px + py*py;
						
						//either y=a*x+b, or x = a*x+b...
						//depends on whether value v.x() is zero or not
						btScalar aa;
						btScalar bb;
						
						if (btFuzzyZero(v.x()))
						{
							aa = v.x()/v.y();
							bb= c.x()+  (-c.y() /v.y()) *v.x();
						} else
						{
							//line is c+mu*v;
							//x = c.x()+mu*v.x();
							//mu = ((x-c.x())/v.x());
							//y = c.y()+((x-c.x())/v.x())*v.y();
							//y = c.y()+  (-c.x() /v.x()) *v.y() + (x /v.x())   *v.y();
							//y = a*x+b,where a = v.y()/v.x(), b= c.y()+  (-c.x() /v.x()) *v.y();
							aa = v.y()/v.x();
							bb= c.y()+  (-c.x() /v.x()) *v.y();
						}
						
						btScalar disc = aa*aa*r2 + r2 - bb*bb;
						if (disc <0)
						{
							//edge doesn't intersect the circle (motion of the vertex)
							return false;
						}
						btScalar rad = btSqrt(r2);
						
						if (btFuzzyZero(disc))
						{
							btPoint3 intersectPt;
							
							btScalar mu;
							//intersectionPoint edge with circle;
							if (btFuzzyZero(v.x()))
							{
								intersectPt.setY( (-2*aa*bb)/(2*(aa*aa+1)));
								intersectPt.setX( aa*intersectPt.y()+bb );
								mu = ((intersectPt.y()-c.y())/v.y());
							} else
							{
								intersectPt.setX((-2*aa*bb)/(2*(aa*aa+1)));
								intersectPt.setY(aa*intersectPt.x()+bb);
								mu = ((intersectPt.getX()-c.getX())/v.getX());
								
							}
							
							if (0 <= mu && mu <= 1)
							{
								hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
							}
							//only one solution
						} else
						{
							//two points...
							//intersectionPoint edge with circle;
							btPoint3 intersectPt;
							//intersectionPoint edge with circle;
							if (btFuzzyZero(v.x()))
							{
								btScalar mu;
								
								intersectPt.setY((-2.f*aa*bb+2.f*btSqrt(disc))/(2.f*(aa*aa+1.f)));
								intersectPt.setX(aa*intersectPt.y()+bb);
								mu = ((intersectPt.getY()-c.getY())/v.getY());
								if (0.f <= mu && mu <= 1.f)
								{
									hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
								}
								intersectPt.setY((-2.f*aa*bb-2.f*btSqrt(disc))/(2.f*(aa*aa+1.f)));
								intersectPt.setX(aa*intersectPt.y()+bb);
								mu = ((intersectPt.getY()-c.getY())/v.getY());
								if (0 <= mu && mu <= 1)
								{
									hit = hit || Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
								}
								
							} else
							{
								btScalar mu;
								
								intersectPt.setX((-2.f*aa*bb+2.f*btSqrt(disc))/(2*(aa*aa+1.f)));
								intersectPt.setY(aa*intersectPt.x()+bb);
								mu = ((intersectPt.getX()-c.getX())/v.getX());
								if (0 <= mu && mu <= 1)
								{
									hit = Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
								}
								intersectPt.setX((-2.f*aa*bb-2.f*btSqrt(disc))/(2.f*(aa*aa+1.f)));
								intersectPt.setY(aa*intersectPt.x()+bb);
								mu = ((intersectPt.getX()-c.getX())/v.getX());
								if (0.f <= mu && mu <= 1.f)
								{
									hit = hit || Calc2DRotationPointPoint(rotPt,rad,screwAB.GetW(),intersectPt,minTime);
								}
							}
						}
						
						
						
						//int k=0;
						
					} else
					{
						return false;
					}
					
					
				} else
				{
					//u.z()<>0,v.z()<>0
					//printf("general case with s=0\n");
					hit = GetTimeOfImpactbteralCase(screwAB,a,u,c,v,minTime,lambda,mu);
					if (hit)
					{
						lambda1 = lambda;
						mu1 = mu;
						
					}
				}
			}
			
		} else
		{
			//printf("general case, W<>0,S<>0\n");
			hit = GetTimeOfImpactbteralCase(screwAB,a,u,c,v,minTime,lambda,mu);
			if (hit)
			{
				lambda1 = lambda;
				mu1 = mu;
			}
			
		}
		
		
		//W <> 0,pure rotation
	}
	
	return hit;
}


bool BU_EdgeEdge::GetTimeOfImpactbteralCase(
											 const BU_Screwing& screwAB,
											 const btPoint3& a,//edge in object A
											 const btVector3& u,
											 const btPoint3& c,//edge in object B
											 const btVector3& v,
											 btScalar &minTime,
											 btScalar &lambda,
											 btScalar& mu
											 
											 )
{
	bool hit = false;
	
	btScalar coefs[4]={0.f,0.f,0.f,0.f};
	BU_Polynomial polynomialSolver;
	int numroots = 0;
	
	//btScalar eps=1e-15f;
	//btScalar eps2=1e-20f;
	btScalar s=screwAB.GetS();
	btScalar w = screwAB.GetW();
	
	btScalar ax = a.x();
	btScalar ay = a.y();
	btScalar az = a.z();
	btScalar cx = c.x();
	btScalar cy = c.y();
	btScalar cz = c.z();
	btScalar vx = v.x();
	btScalar vy = v.y();
	btScalar vz = v.z();
	btScalar ux = u.x();
	btScalar uy = u.y();
	btScalar uz = u.z();
	
	
	if (!btFuzzyZero(v.z()))
	{
		
		//Maple Autogenerated C code
		btScalar t1,t2,t3,t4,t7,t8,t10;
		btScalar t13,t14,t15,t16,t17,t18,t19,t20;
		btScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
		btScalar t31,t32,t33,t34,t35,t36,t39,t40;
		btScalar t41,t43,t48;
		btScalar t63;
		
		btScalar aa,bb,cc,dd;//the coefficients
		
		t1 = v.y()*s;      t2 = t1*u.x();
		t3 = v.x()*s;
		t4 = t3*u.y();
		t7 = btTan(w/2.0f);
		t8 = 1.0f/t7;
		t10 = 1.0f/v.z();
		aa = (t2-t4)*t8*t10;
		t13 = a.x()*t7;
		t14 = u.z()*v.y();
		t15 = t13*t14;
		t16 = u.x()*v.z();
		t17 = a.y()*t7;
		t18 = t16*t17;
		t19 = u.y()*v.z();
		t20 = t13*t19;
		t21 = v.y()*u.x();
		t22 = c.z()*t7;
		t23 = t21*t22;
		t24 = v.x()*a.z();
		t25 = t7*u.y();
		t26 = t24*t25;
		t27 = c.y()*t7;
		t28 = t16*t27;
		t29 = a.z()*t7;
		t30 = t21*t29;
		t31 = u.z()*v.x();
		t32 = t31*t27;
		t33 = t31*t17;
		t34 = c.x()*t7;
		t35 = t34*t19;
		t36 = t34*t14;
		t39 = v.x()*c.z();
		t40 = t39*t25;
		t41 = 2.0f*t1*u.y()-t15+t18-t20-t23-t26+t28+t30+t32+t33-t35-t36+2.0f*t3*u.x()+t40;
		bb = t41*t8*t10;
		t43 = t7*u.x();
		t48 = u.y()*v.y();
		cc = (-2.0f*t39*t43+2.0f*t24*t43+t4-2.0f*t48*t22+2.0f*t34*t16-2.0f*t31*t13-t2
			-2.0f*t17*t14+2.0f*t19*t27+2.0f*t48*t29)*t8*t10;
		t63 = -t36+t26+t32-t40+t23+t35-t20+t18-t28-t33+t15-t30;
		dd = t63*t8*t10;
		
		coefs[0]=aa;
		coefs[1]=bb;
		coefs[2]=cc;
		coefs[3]=dd;
		
	} else
	{
		
		btScalar t1,t2,t3,t4,t7,t8,t10;
		btScalar t13,t14,t15,t16,t17,t18,t19,t20;
		btScalar t21,t22,t23,t24,t25,t26,t27,t28,t29,t30;
		btScalar t31,t32,t33,t34,t35,t36,t37,t38,t57;
		btScalar p1,p2,p3,p4;

	  t1 = uy*s;
      t2 = t1*vx;
      t3 = ux*s;
      t4 = t3*vy;
      t7 = btTan(w/2.0f);
      t8 = 1/t7;
      t10 = 1/uz;
      t13 = ux*az;
      t14 = t7*vy;
      t15 = t13*t14;
      t16 = ax*t7;
      t17 = uy*vz;
      t18 = t16*t17;
      t19 = cx*t7;
      t20 = t19*t17;
      t21 = vy*uz;
      t22 = t19*t21;
      t23 = ay*t7;
      t24 = vx*uz;
      t25 = t23*t24;
      t26 = uy*cz;
      t27 = t7*vx;
      t28 = t26*t27;
      t29 = t16*t21;
      t30 = cy*t7;
      t31 = ux*vz;
      t32 = t30*t31;
      t33 = ux*cz;
      t34 = t33*t14;
      t35 = t23*t31;
      t36 = t30*t24;
      t37 = uy*az;
      t38 = t37*t27;

	  p4 = (-t2+t4)*t8*t10;
      p3 = 2.0f*t1*vy+t15-t18-t20-t22+t25+t28-t29+t32-t34+t35+t36-t38+2.0f*t3*vx;
      p2 = -2.0f*t33*t27-2.0f*t26*t14-2.0f*t23*t21+2.0f*t37*t14+2.0f*t30*t17+2.0f*t13
*t27+t2-t4+2.0f*t19*t31-2.0f*t16*t24;
      t57 = -t22+t29+t36-t25-t32+t34+t35-t28-t15+t20-t18+t38;
      p1 = t57*t8*t10;

	coefs[0] = p4;
	coefs[1] = p3;
	coefs[2] = p2;
	coefs[1] = p1;
		
	}
	
	numroots = polynomialSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
	
	for (int i=0;i<numroots;i++)
	{
		//btScalar tau = roots[i];//polynomialSolver.GetRoot(i);
		btScalar tau = polynomialSolver.GetRoot(i);
		
		//check whether mu and lambda are in range [0..1]
		
		if (!btFuzzyZero(v.z()))
		{
			btScalar A1=(ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz);
			btScalar B1=((1.f+tau*tau)*(cx*btTan(1.f/2.f*w)*vz+
				vx*az*btTan(1.f/2.f*w)-vx*cz*btTan(1.f/2.f*w)+
				vx*s*tau)/btTan(1.f/2.f*w)/vz)-(ax-ax*tau*tau-2.f*tau*ay);
			lambda = B1/A1;
			
			mu = (a.z()-c.z()+lambda*u.z()+(s*tau)/(btTan(w/2.f)))/v.z();
			
			
			//double check in original equation
			
			btScalar lhs = (a.x()+lambda*u.x())
				*((1.f-tau*tau)/(1.f+tau*tau))-
				(a.y()+lambda*u.y())*((2.f*tau)/(1.f+tau*tau));
			
			lhs = lambda*((ux-ux*tau*tau-2.f*tau*uy)-((1.f+tau*tau)*vx*uz/vz));
			
			btScalar rhs = c.x()+mu*v.x();
			
			rhs = ((1.f+tau*tau)*(cx*btTan(1.f/2.f*w)*vz+vx*az*btTan(1.f/2.f*w)-
				vx*cz*btTan(1.f/2.f*w)+vx*s*tau)/(btTan(1.f/2.f*w)*vz))-
				
				(ax-ax*tau*tau-2.f*tau*ay);
			
			/*btScalar res = coefs[0]*tau*tau*tau+
				coefs[1]*tau*tau+
				coefs[2]*tau+
				coefs[3];*/
			
			//lhs should be rhs !
			
			if (0.<= mu && mu <=1 && 0.<=lambda && lambda <= 1)
			{
				
			} else
			{
				//skip this solution, not really touching
				continue;				
			}
			
		}
		
		btScalar t = 2.f*btAtan(tau)/screwAB.GetW();
		//tau = tan (wt/2) so 2*atan (tau)/w
		if (t>=0.f && t<minTime)
		{
#ifdef STATS_EDGE_EDGE
			printf(" ax = %12.12f\n ay = %12.12f\n az = %12.12f\n",a.x(),a.y(),a.z());
			printf(" ux = %12.12f\n uy = %12.12f\n uz = %12.12f\n",u.x(),u.y(),u.z());
			printf(" cx = %12.12f\n cy = %12.12f\n cz = %12.12f\n",c.x(),c.y(),c.z());
			printf(" vx = %12.12f\n vy = %12.12f\n vz = %12.12f\n",v.x(),v.y(),v.z());
			printf(" s  = %12.12f\n w  = %12.12f\n",       s,     w);
			
			printf(" tau = %12.12f \n lambda = %12.12f \n mu = %f\n",tau,lambda,mu); 
			printf(" ---------------------------------------------\n"); 
			
#endif
			
			//	v,u,a,c,s,w
			
			//	BU_IntervalArithmeticPolynomialSolver iaSolver;
			//	int numroots2 = iaSolver.Solve3Cubic(coefs[0],coefs[1],coefs[2],coefs[3]);
			
			minTime = t;
			hit = true;
		}
	}
	
	return hit;
}


//C -S
//S C

bool BU_EdgeEdge::Calc2DRotationPointPoint(const btPoint3& rotPt, btScalar rotRadius, btScalar rotW,const btPoint3& intersectPt,btScalar& minTime)
{
	bool hit = false;
	
	// now calculate the planeEquation for the vertex motion,
	// and check if the intersectionpoint is at the positive side
	btPoint3 rotPt1(btCos(rotW)*rotPt.x()-btSin(rotW)*rotPt.y(),
	btSin(rotW)*rotPt.x()+btCos(rotW)*rotPt.y(),
	0.f);

	btVector3 rotVec = rotPt1-rotPt;
	
	btVector3 planeNormal( -rotVec.y() , rotVec.x() ,0.f);
	
	//btPoint3 pt(a.x(),a.y());//for sake of readability,could write dot directly
	btScalar planeD = planeNormal.dot(rotPt1);
	
	btScalar dist = (planeNormal.dot(intersectPt)-planeD);
	hit = (dist >= -0.001);
	
	//if (hit)
	{
		//		minTime = 0;
		//calculate the time of impact, using the fact of
		//toi = alpha / screwAB.getW();
		// cos (alpha) = adjacent/hypothenuse;
		//adjacent = dotproduct(ipedge,point);
		//hypothenuse = sqrt(r2);
		btScalar adjacent = intersectPt.dot(rotPt)/rotRadius;
		btScalar hypo = rotRadius;
		btScalar alpha = btAcos(adjacent/hypo);
		btScalar t = alpha / rotW;
		if (t >= 0 && t < minTime)
		{
			hit = true;
			minTime = t;
		} else
		{
			hit = false;
		}
		
	}
	return hit;
}

bool BU_EdgeEdge::GetTimeOfImpactVertexEdge(
											const BU_Screwing& screwAB,
											const btPoint3& a,//edge in object A
											const btVector3& u,
											const btPoint3& c,//edge in object B
											const btVector3& v,
											btScalar &minTime,
											btScalar &lamda,
											btScalar& mu
											
											)
{
	return false;
}