/*
 * Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com
 *
 * Permission to use, copy, modify, distribute and sell this software
 * and its documentation for any purpose is hereby granted without fee,
 * provided that the above copyright notice appear in all copies.
 * Erwin Coumans makes no representations about the suitability 
 * of this software for any purpose.  
 * It is provided "as is" without express or implied warranty.
*/

#include "BU_CollisionPair.h"
#include "NarrowPhaseCollision/BU_VertexPoly.h"
#include "NarrowPhaseCollision/BU_EdgeEdge.h"
#include "BU_Collidable.h"


#include "BU_MotionStateInterface.h"
#include "CollisionShapes/PolyhedralConvexShape.h"
#include <SimdMinMax.h>
#include "SimdTransformUtil.h"



BU_CollisionPair::BU_CollisionPair(const PolyhedralConvexShape* convexA,const PolyhedralConvexShape* convexB,SimdScalar tolerance)
: m_convexA(convexA),m_convexB(convexB),m_screwing(SimdVector3(0,0,0),SimdVector3(0,0,0)),
m_tolerance(tolerance)
{

}

// if there exists a time-of-impact between any feature_pair (edgeA,edgeB),
// (vertexA,faceB) or (vertexB,faceA) in [0..1], report true and smallest time


/*
bool BU_CollisionPair::GetTimeOfImpact(const SimdVector3& linearMotionA,const SimdQuaternion& angularMotionA,const SimdVector3& linearMotionB,const SimdQuaternion& angularMotionB, SimdScalar& toi,SimdTransform& impactTransA,SimdTransform& impactTransB)

*/

bool BU_CollisionPair::calcTimeOfImpact(
					const SimdTransform& fromA,
					const SimdTransform& toA,
					const SimdTransform& fromB,
					const SimdTransform& toB,
					CastResult& result)
{



	
	SimdVector3 linvelA,angvelA;
	SimdVector3 linvelB,angvelB;

	SimdTransformUtil::CalculateVelocity(fromA,toA,1.f,linvelA,angvelA);
	SimdTransformUtil::CalculateVelocity(fromB,toB,1.f,linvelB,angvelB);


	SimdVector3 linearMotionA = toA.getOrigin() - fromA.getOrigin();
	SimdQuaternion angularMotionA(0,0,0,1.f);
	SimdVector3 linearMotionB = toB.getOrigin() - fromB.getOrigin();
	SimdQuaternion angularMotionB(0,0,0,1);
	


	result.m_fraction = 1.f;

	SimdTransform impactTransA;
	SimdTransform impactTransB;

	int index=0;

	SimdScalar toiUnscaled=result.m_fraction;
	const SimdScalar toiUnscaledLimit = result.m_fraction;

	SimdTransform a2w;
	a2w = fromA;
	SimdTransform b2w = fromB;

/* debugging code
	{
		const int numvertsB = m_convexB->GetNumVertices();
		for (int v=0;v<numvertsB;v++)
		{
			SimdPoint3 pt;
			m_convexB->GetVertex(v,pt);
			pt = b2w * pt;
			char buf[1000];

			if (pt.y() < 0.)
			{
				sprintf(buf,"PRE ERROR (%d) %.20E %.20E %.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
				if (debugFile)
					fwrite(buf,1,strlen(buf),debugFile);
			} else
			{
				sprintf(buf,"PRE %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
				if (debugFile)
					fwrite(buf,1,strlen(buf),debugFile);

			}
		}
	}
*/


	SimdTransform b2wp = b2w;
	
	b2wp.setOrigin(b2w.getOrigin() + linearMotionB);
	b2wp.setRotation( b2w.getRotation() + angularMotionB);

	impactTransB = b2wp;
	
	SimdTransform a2wp;
	a2wp.setOrigin(a2w.getOrigin()+ linearMotionA);
	a2wp.setRotation(a2w.getRotation()+angularMotionA);

	impactTransA = a2wp;

	SimdTransform a2winv;
	a2winv = a2w.inverse();

	SimdTransform b2wpinv;
	b2wpinv = b2wp.inverse();

	SimdTransform b2winv;
	b2winv = b2w.inverse();

	SimdTransform a2wpinv;
	a2wpinv = a2wp.inverse();

		//Redon's version with concatenated transforms

	SimdTransform relative;

	relative = b2w * b2wpinv * a2wp * a2winv;

	//relative = a2winv * a2wp  * b2wpinv * b2w;

	SimdQuaternion qrel;
	relative.getBasis().getRotation(qrel);

	SimdVector3 linvel = relative.getOrigin();

	if (linvel.length() < SCREWEPSILON)
	{
		linvel.setValue(0.,0.,0.);
	}
	SimdVector3 angvel;
	angvel[0] = 2.f * asinf (qrel[0]);
	angvel[1] = 2.f * asinf (qrel[1]);
	angvel[2] = 2.f * asinf (qrel[2]);
	
	if (angvel.length() < SCREWEPSILON)
	{
		angvel.setValue(0.f,0.f,0.f);
	}

	//Redon's version with concatenated transforms
	m_screwing = BU_Screwing(linvel,angvel);
	
	SimdTransform w2s;
	m_screwing.LocalMatrix(w2s);

	SimdTransform s2w;
	s2w = w2s.inverse();

	//impactTransA = a2w;
	//impactTransB = b2w;

	bool hit = false;
	
	if (SimdFuzzyZero(m_screwing.GetS()) && SimdFuzzyZero(m_screwing.GetW()))
	{
		//W = 0 , S = 0 , no collision
		//toi = 0;
	/*	
		{
			const int numvertsB = m_convexB->GetNumVertices();
			for (int v=0;v<numvertsB;v++)
			{
				SimdPoint3 pt;
				m_convexB->GetVertex(v,pt);
				pt = impactTransB * pt;
				char buf[1000];
				
				if (pt.y() < 0.)
				{
					sprintf(buf,"EARLY POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
					if (debugFile)
						fwrite(buf,1,strlen(buf),debugFile);
				}
				else
				{
					sprintf(buf,"EARLY POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
					if (debugFile)
						fwrite(buf,1,strlen(buf),debugFile);
				}
			}
		}
	*/	
		
		return false;//don't continue moving within epsilon
	}

#define EDGEEDGE
#ifdef EDGEEDGE

	BU_EdgeEdge edgeEdge;

	//for all edged in A check agains all edges in B
	for (int ea = 0;ea < m_convexA->GetNumEdges();ea++)
	{
		SimdPoint3 pA0,pA1;

		m_convexA->GetEdge(ea,pA0,pA1);

		pA0= a2w * pA0;//in world space
		pA0 = w2s * pA0;//in screwing space

		pA1= a2w * pA1;//in world space
		pA1 = w2s * pA1;//in screwing space

		int numedgesB = m_convexB->GetNumEdges();
		for (int eb = 0; eb < numedgesB;eb++)
		{
			{
				SimdPoint3 pB0,pB1;
				m_convexB->GetEdge(eb,pB0,pB1);

				pB0= b2w * pB0;//in world space
				pB0 = w2s * pB0;//in screwing space

				pB1= b2w * pB1;//in world space
				pB1 = w2s * pB1;//in screwing space


				SimdScalar lambda,mu;
				
				toiUnscaled = 1.;

				SimdVector3 edgeDirA(pA1-pA0);
				SimdVector3 edgeDirB(pB1-pB0);

				if (edgeEdge.GetTimeOfImpact(m_screwing,pA0,edgeDirA,pB0,edgeDirB,toiUnscaled,lambda,mu))
				{
					//printf("edgeedge potential hit\n");
					if (toiUnscaled>=0)
					{
						if (toiUnscaled < toiUnscaledLimit)							
						{
		
							//inside check is already done by checking the mu and gamma !

							SimdPoint3 vtx  = pA0+lambda * (pA1-pA0);
							SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
							
							SimdPoint3 hitptWorld =   s2w * hitpt;
							{

								if (toiUnscaled < result.m_fraction)
									result.m_fraction = toiUnscaled;

								hit = true;

								SimdVector3 hitNormal = edgeDirB.cross(edgeDirA);
								
								hitNormal = m_screwing.InBetweenVector(hitNormal,toiUnscaled);
							

								hitNormal.normalize();
								
								//an approximated normal can be calculated by taking the cross product of both edges
								//take care of the sign !
								
								SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
						
								SimdScalar dist = m_screwing.GetU().dot(hitNormalWorld);
								if (dist > 0)
									hitNormalWorld *= -1;
								
								//todo: this is the wrong point, because b2winv is still at begin of motion
								// not at time-of-impact location!
								//bhitpt = b2winv * hitptWorld;

//								m_manifold.SetContactPoint(BUM_FeatureEdgeEdge,index,ea,eb,hitptWorld,hitNormalWorld);
							}
					
						}
					}
				}
			}

			index++;
		}
	};
#endif //EDGEEDGE

#define VERTEXFACE
#ifdef VERTEXFACE

	// for all vertices in A, for each face in B,do vertex-face
	{
		const int numvertsA = m_convexA->GetNumVertices();
		for (int v=0;v<numvertsA;v++)
		//int v=3;

		{
			SimdPoint3 vtx;
			m_convexA->GetVertex(v,vtx);

			vtx = a2w * vtx;//in world space
			vtx = w2s * vtx;//in screwing space

			const int numplanesB = m_convexB->GetNumPlanes();

			for (int p = 0 ; p < numplanesB; p++)
			//int p=2;
			{

				{
				
					SimdVector3 planeNorm;
					SimdPoint3 planeSupport;

					m_convexB->GetPlane(planeNorm,planeSupport,p);


					planeSupport = b2w * planeSupport;//transform to world space
					SimdVector3 planeNormWorld =  b2w.getBasis() * planeNorm;
				
					planeSupport =  w2s * planeSupport  ; //transform to screwing space
					planeNorm =  w2s.getBasis() * planeNormWorld;

					planeNorm.normalize();

					SimdScalar d = planeSupport.dot(planeNorm);
					
					SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
				
					BU_VertexPoly vtxApolyB;

					toiUnscaled = 1.;

					if ((p==2) && (v==6))
					{
//						printf("%f toiUnscaled\n",toiUnscaled);

					}
					if (vtxApolyB.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,false))
					{
					


						
						if (toiUnscaled >= 0. )
						{
							//not only collect the first point, get every contactpoint, later we have to check the
							//manifold properly!

							if (toiUnscaled <= toiUnscaledLimit)
							{
	//							printf("toiUnscaled %f\n",toiUnscaled );

								SimdPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
								SimdVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled);

								SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
								SimdPoint3 hitptWorld = s2w * hitpt;


								hitpt = b2winv * hitptWorld;
								//vertex has to be 'within' the facet's boundary
								if (m_convexB->IsInside(hitpt,m_tolerance))
								{
//									m_manifold.SetContactPoint(BUM_FeatureVertexFace, index,v,p,hitptWorld,hitNormalWorld);
									
									if (toiUnscaled < result.m_fraction)
										result.m_fraction= toiUnscaled;
									hit = true;

								}
							}
						}
					}
					
				}

				index++;
			}
		}
	}

	//
	// for all vertices in B, for each face in A,do vertex-face
	//copy and pasted from all verts A -> all planes B so potential typos!
	//todo: make this into one method with a kind of 'swapped' logic
	//
	{
		const int numvertsB = m_convexB->GetNumVertices();
		for (int v=0;v<numvertsB;v++)
		//int v=0;

		{
			SimdPoint3 vtx;
			m_convexB->GetVertex(v,vtx);

			vtx = b2w * vtx;//in world space
/*
			
			char buf[1000];

			if (vtx.y() < 0.)
			{
				sprintf(buf,"ERROR !!!!!!!!!\n",v,vtx.x(),vtx.y(),vtx.z());
				if (debugFile)
					fwrite(buf,1,strlen(buf),debugFile);
			}
			sprintf(buf,"vertexWorld(%d) = (%.20E,%.20E,%.20E)\n",v,vtx.x(),vtx.y(),vtx.z());
			if (debugFile)
				fwrite(buf,1,strlen(buf),debugFile);

*/			
			vtx = w2s * vtx;//in screwing space

			const int numplanesA = m_convexA->GetNumPlanes();

			for (int p = 0 ; p < numplanesA; p++)
			//int p=2;
			{

				{
					SimdVector3 planeNorm;
					SimdPoint3 planeSupport;

					m_convexA->GetPlane(planeNorm,planeSupport,p);


					planeSupport = a2w * planeSupport;//transform to world space
					SimdVector3 planeNormWorld =  a2w.getBasis() * planeNorm;
				
					planeSupport =  w2s * planeSupport  ; //transform to screwing space
					planeNorm =  w2s.getBasis() * planeNormWorld;

					planeNorm.normalize();

					SimdScalar d = planeSupport.dot(planeNorm);
					
					SimdVector4 planeEq(planeNorm[0],planeNorm[1],planeNorm[2],d);
				
					BU_VertexPoly vtxBpolyA;

					toiUnscaled = 1.;

					if (vtxBpolyA.GetTimeOfImpact(m_screwing,vtx,planeEq,toiUnscaled,true))
					{
						if (toiUnscaled>=0.)
						{
							if (toiUnscaled < toiUnscaledLimit)
							{
								SimdPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled);
								SimdVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled);
								SimdScalar len =  hitNormal.length()-1;

								//assert( SimdFuzzyZero(len) );

								
								SimdVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
								SimdPoint3 hitptWorld = s2w * hitpt;
								hitpt = a2winv * hitptWorld;
							
							
								//vertex has to be 'within' the facet's boundary
								if (m_convexA->IsInside(hitpt,m_tolerance))
								{
									
//									m_manifold.SetContactPoint(BUM_FeatureFaceVertex,index,p,v,hitptWorld,hitNormalWorld);
									if (toiUnscaled <result.m_fraction)
										result.m_fraction = toiUnscaled;
									hit = true;
								}
							}
						
						}
					
					}
					}

			}
		
			index++;
		}
	}
	

#endif// VERTEXFACE

	//the manifold now consists of all points/normals generated by feature-pairs that have a time-of-impact within this frame
	//in addition there are contact points from previous frames
	//we have to cleanup the manifold, using an additional epsilon/tolerance
	//as long as the distance from the contactpoint (in worldspace) to both objects is within this epsilon we keep the point
	//else throw it away
	

	if (hit)
	{

		//try to avoid numerical drift on close contact
		
		if (result.m_fraction < 0.00001)
		{
//			printf("toiUnscaledMin< 0.00001\n");
			impactTransA = a2w;
			impactTransB = b2w;

		} else
		{

			//SimdScalar vel = linearMotionB.length();
			
			//todo: check this margin
			result.m_fraction *= 0.99f;

			//move B to new position
			impactTransB.setOrigin(b2w.getOrigin()+ result.m_fraction*linearMotionB);
			SimdQuaternion ornB = b2w.getRotation()+angularMotionB*result.m_fraction;
			ornB.normalize();
			impactTransB.setRotation(ornB);

			//now transform A
			SimdTransform a2s,a2b;
			a2s.mult( w2s , a2w);
			a2s= m_screwing.InBetweenTransform(a2s,result.m_fraction);
			a2s.multInverseLeft(w2s,a2s);
			a2b.multInverseLeft(b2w, a2s);

			//transform by motion B
			impactTransA.mult(impactTransB, a2b);
			//normalize rotation
			SimdQuaternion orn;
			impactTransA.getBasis().getRotation(orn);
			orn.normalize();
			impactTransA.setBasis(SimdMatrix3x3(orn));
		}
	}

/*
	{
		const int numvertsB = m_convexB->GetNumVertices();
		for (int v=0;v<numvertsB;v++)
		{
			SimdPoint3 pt;
			m_convexB->GetVertex(v,pt);
			pt = impactTransB * pt;
			char buf[1000];

			if (pt.y() < 0.)
			{
				sprintf(buf,"POST ERROR (%d) %.20E,%.20E,%.20E!!!!!!!!!\n",v,pt.x(),pt.y(),pt.z());
				if (debugFile)
					fwrite(buf,1,strlen(buf),debugFile);
			}
			else
			{
				sprintf(buf,"POST %d = %.20E,%.20E,%.20E\n",v,pt.x(),pt.y(),pt.z());
				if (debugFile)
					fwrite(buf,1,strlen(buf),debugFile);
			}
		}
	}
*/
	return hit;
}