/*
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_CollisionPair.h"
#include "NarrowPhaseCollision/BU_VertexPoly.h"
#include "NarrowPhaseCollision/BU_EdgeEdge.h"
#include "BU_Collidable.h"


#include "BU_MotionStateInterface.h"
#include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
#include <btSimdMinMax.h>
#include "LinearMath/btTransformUtil.h"



BU_CollisionPair::BU_CollisionPair(const btPolyhedralConvexShape* convexA,const btPolyhedralConvexShape* convexB,btScalar tolerance)
: m_convexA(convexA),m_convexB(convexB),m_screwing(btVector3(0,0,0),btVector3(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 btVector3& linearMotionA,const btQuaternion& angularMotionA,const btVector3& linearMotionB,const btQuaternion& angularMotionB, btScalar& toi,btTransform& impactTransA,btTransform& impactTransB)

*/

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



	
	btVector3 linvelA,angvelA;
	btVector3 linvelB,angvelB;

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


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


	result.m_fraction = 1.f;

	btTransform impactTransA;
	btTransform impactTransB;

	int index=0;

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

	btTransform a2w;
	a2w = fromA;
	btTransform b2w = fromB;

/* debugging code
	{
		const int numvertsB = m_convexB->getNumVertices();
		for (int v=0;v<numvertsB;v++)
		{
			btPoint3 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);

			}
		}
	}
*/


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

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

	impactTransA = a2wp;

	btTransform a2winv;
	a2winv = a2w.inverse();

	btTransform b2wpinv;
	b2wpinv = b2wp.inverse();

	btTransform b2winv;
	b2winv = b2w.inverse();

	btTransform a2wpinv;
	a2wpinv = a2wp.inverse();

		//Redon's version with concatenated transforms

	btTransform relative;

	relative = b2w * b2wpinv * a2wp * a2winv;

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

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

	btVector3 linvel = relative.getOrigin();

	if (linvel.length() < SCREWEPSILON)
	{
		linvel.setValue(0.,0.,0.);
	}
	btVector3 angvel;
	angvel[0] = 2.f * btAsin (qrel[0]);
	angvel[1] = 2.f * btAsin (qrel[1]);
	angvel[2] = 2.f * btAsin (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);
	
	btTransform w2s;
	m_screwing.LocalMatrix(w2s);

	btTransform s2w;
	s2w = w2s.inverse();

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

	bool hit = false;
	
	if (btFuzzyZero(m_screwing.GetS()) && btFuzzyZero(m_screwing.GetW()))
	{
		//W = 0 , S = 0 , no collision
		//toi = 0;
	/*	
		{
			const int numvertsB = m_convexB->getNumVertices();
			for (int v=0;v<numvertsB;v++)
			{
				btPoint3 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++)
	{
		btPoint3 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++)
		{
			{
				btPoint3 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


				btScalar lambda,mu;
				
				toiUnscaled = 1.;

				btVector3 edgeDirA(pA1-pA0);
				btVector3 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 !

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

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

								hit = true;

								btVector3 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 !
								
								btVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
						
								btScalar 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;

		{
			btPoint3 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;
			{

				{
				
					btVector3 planeNorm;
					btPoint3 planeSupport;

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


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

					planeNorm.normalize();

					btScalar d = planeSupport.dot(planeNorm);
					
					btVector4 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 );

								btPoint3 hitpt = m_screwing.InBetweenPosition(vtx,toiUnscaled);
								btVector3 hitNormal = m_screwing.InBetweenVector(planeNorm ,toiUnscaled);

								btVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
								btPoint3 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;

		{
			btPoint3 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;
			{

				{
					btVector3 planeNorm;
					btPoint3 planeSupport;

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


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

					planeNorm.normalize();

					btScalar d = planeSupport.dot(planeNorm);
					
					btVector4 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)
							{
								btPoint3 hitpt = m_screwing.InBetweenPosition( vtx , -toiUnscaled);
								btVector3 hitNormal = m_screwing.InBetweenVector(-planeNorm ,-toiUnscaled);
								//btScalar len =  hitNormal.length()-1;

								//assert( btFuzzyZero(len) );

								
								btVector3 hitNormalWorld = s2w.getBasis() * hitNormal ;
								btPoint3 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
		{

			//btScalar 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);
			btQuaternion ornB = b2w.getRotation()+angularMotionB*result.m_fraction;
			ornB.normalize();
			impactTransB.setRotation(ornB);

			//now transform A
			btTransform 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
			btQuaternion orn;
			impactTransA.getBasis().getRotation(orn);
			orn.normalize();
			impactTransA.setBasis(btMatrix3x3(orn));
		}
	}

/*
	{
		const int numvertsB = m_convexB->getNumVertices();
		for (int v=0;v<numvertsB;v++)
		{
			btPoint3 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;
}