#include <Common/Visualize/hkDebugDisplay.h>

const hkReal hktHullHullAgent::OPTIMAL_SIMPLEX_CHECK_PARAM = .001f;
const hkInt32 hktHullHullAgent::GJK_NUM_ITERATIONS = 10;

hktHullHullAgent::hktHullHullAgent( hktRigidBody* rigidBodyA, hktRigidBody* rigidBodyB )
	:	hktAgent( rigidBodyA, rigidBodyB )
{	
	//HK_ASSERT2( 0, rigidBodyA->getShape()->isType(HKT_SHAPE_CONVEX_HULL), "HullHull agent can only be created between two hulls." );
	//HK_ASSERT2( 0, rigidBodyB->getShape()->isType(HKT_SHAPE_CONVEX_HULL), "HullHull agent can only be created between two hulls." );
}

//allow two contact points
//if contact point exists, then check points in reverse order
//if new point is found, then add
//if 
void hktHullHullAgent::processCollision()
{
	this->m_contacts.clear();
	processCollisionGJK(); 
}

//Will return true/false if distance from origin to closet pt on minkowski object
//is less than m_shell param of hktShape
 bool hktHullHullAgent::processCollisionGJK() {
	 

	hkVector4 nextPoint;
	hkArray<hkVector4> simplex; //will contain 3 pts when finished

	hkVector4 dir = m_rigidBodyB->getPosition(); 
	dir.sub(m_rigidBodyA->getPosition());
	dir.normalize3();
	doSupport(dir, nextPoint); //find starting pt
	simplex.pushBack(nextPoint);
	dir.setMul(nextPoint, -1.0f);

	hkInt32 numIters = 0; 
	while (numIters++ < GJK_NUM_ITERATIONS) { //prevents us locking up

 		doSupport(dir, nextPoint);
		
		if (simplexIsOptimal(simplex, nextPoint, dir)) { //simplex contains? vs next point check
			return solveForContactPoints(simplex, dir);
		}
		else {
			simplex.pushBack(nextPoint);
			if (evolveSimplex(simplex, dir)) { //penetration occurred - go to EPA
				//return solveEPA(simplex, dir);
 			}
		}
	}
	return solveForContactPoints(simplex, dir); //gets here if we cant break out of GJK loop (probably due to optimal simplex checking
}


 bool hktHullHullAgent::simplexIsOptimal(hkArray<hkVector4>& simplex, hkVector4& nextPoint, hkVector4& dir) {
	 hkReal nextPointDist = nextPoint.dot3(dir);

	 for (int i=0; i < simplex.getSize(); i++) {
		 hkReal simplexDist = simplex[i].dot3(dir);
		 if ((nextPointDist - simplexDist) > OPTIMAL_SIMPLEX_CHECK_PARAM) { //nextPoint was sufficiently better than simplex[i]
			 return false; 
		 }
	 }
	 return true;
 }

bool hktHullHullAgent::solveEPA(hkArray<hkVector4>& simplex, hkVector4& dir) {
	//do EPA here
	return true;
}


//Will evolve simplex (by removing points and changing direction)
//Will return true if penetration is found
//Will return false if penetration is not yet found
bool hktHullHullAgent::evolveSimplex(hkArray<hkVector4>& simplex, hkVector4& dir) {
	HK_ASSERT2(0, (simplex.getSize() > 1), "evolveSimplex called with not enough points!");

	if (simplex.getSize() == 2) {
		return evolveSimplexLine(simplex, dir);
	}
	else if (simplex.getSize() == 3) {
		return evolveSimplexTriangle(simplex, dir);
	}
	else {
		HK_ASSERT2(0, false, "Too many points in simplex!");
	}
	return false;
}

bool hktHullHullAgent::evolveSimplexLine(hkArray<hkVector4>& simplex, hkVector4& dir) {
	HK_ASSERT2(0, (simplex.getSize() == 2), "Line called with other than two points!");
	hkVector4 a = simplex[simplex.getSize()-1]; //get last point in array
	//compute A0 = A to 0 = 0-A = -A
	hkVector4 a0 = a;
	a0.mul(-1);

	//we can assume there at least two points now
	hkVector4 b = simplex[0];
	hkVector4 ab;
	ab.setSub(b, a);

	if (ab.dot3(a0) > 0.0f) { //Molly Rocket region 1 for line
		dir = tripleCrossProduct(ab, a0, ab); //calculate perpendicular to existing simplex
		if (isZeroDir(dir)) {
			//this means a0 and ab are aligned, so use the 2D perpendicular trick
			dir.set(-ab(1), ab(0), 0); //just give it some other direction
			dir.normalize3();
		}
	}
	else {
		dir = a0;
		simplex.clear();
		simplex.pushBack(a); 
	}

	return false;
}

bool hktHullHullAgent::evolveSimplexTriangle(hkArray<hkVector4>& simplex, hkVector4& dir) {
	int INDEX_B = 1;
	int INDEX_C = 0;
	HK_ASSERT2(0, (simplex.getSize() == 3), "Triangle called with other than three points!");

	hkVector4 a = simplex[simplex.getSize()-1]; //get last point added to array
	//compute A0 = 0-A = -A
	hkVector4 a0; a0.setMul(a, -1.0f);
	hkVector4 ab; ab.setSub(simplex[INDEX_B], a);
	hkVector4 ac; ac.setSub(simplex[INDEX_C], a);
	hkVector4 abc(0.0f,0.0f, 1.0f); //easiest way to keep triangle normal correct in 2D
	abc.setCross(ab, ac);
	hkVector4 test; 
	
	test.setCross(abc, ac);
	if (test.dot3(a0) > 0.0f) {
		if (ac.dot3(a0) > 0.0f) { //Molly Rocket Region 1
			simplex.removeAt(INDEX_B);
			dir = tripleCrossProduct(ac, a0, ac);
			HK_ASSERT2(0, !isZeroDir(dir), "Zero direction vector found!");
			return false;
		}
		else {
			//STAR CASE
			if (ab.dot3(a0) > 0.0f) { //Molly Rocket Region 4
				simplex.removeAt(INDEX_C);
				dir = tripleCrossProduct(ab, a0, ab);
				HK_ASSERT2(0, !isZeroDir(dir), "Zero direction vector found!");
				return false;
			}
			else { //Molly Rocket Region 5
				simplex.clear();
				simplex.pushBack(a);
				dir = a0;
				HK_ASSERT2(0, !isZeroDir(dir), "Zero direction vector found!");
				return false;
			}
			//END STAR CASE
		}
	}
	else {
		test.setCross(ab, abc);
		if (test.dot3(a0) > 0.0f) {
			//STAR CASE
			if (ab.dot3(a0) > 0.0f) { //Molly Rocket Region 4
				simplex.removeAt(INDEX_C);
				dir = tripleCrossProduct(ab,a0,ab);
				HK_ASSERT2(0, !isZeroDir(dir), "Zero direction vector found!");
				return false;
			}
			else { //Molly Rocket Region 5
				simplex.clear();
				simplex.pushBack(a);
				dir = a0;
				HK_ASSERT2(0, !isZeroDir(dir), "Zero direction vector found!");
				return false;
			}
			//END STAR CASE
		}
		else {
			//Molly Rocket Region 2 and 3 (inside triangle for 2D)
			return true;
		}
	}
}

hkReal hktHullHullAgent::getMaxShell() {
	return ((m_rigidBodyA->getShell() > m_rigidBodyB->getShell()) ? m_rigidBodyA->getShell() : m_rigidBodyB->getShell());
}

bool hktHullHullAgent::isZeroDir(const hkVector4& dir)  {
	hkVector4 myDir = dir;
	myDir.normalize3();
	return !myDir.isNormalized3();
}
	

hkVector4 hktHullHullAgent::tripleCrossProduct(const hkVector4& a, const hkVector4& b, const hkVector4& c) {
	//cross product gets singularity if origin is along direction axis
	hkVector4 product = a;
	product.setCross(a,b);
	product.setCross(product, c);
	return product;
}


void hktHullHullAgent::doSupport(const hkVector4& dir, hkVector4& support) {
	hkVector4 aPt(0,0,0);
	hkVector4 bPt(0,0,0);
	hkVector4 myDir = dir;
	doPartialSupport(m_rigidBodyA, myDir, aPt);
	myDir = dir;
	myDir.mul(-1);
	doPartialSupport(m_rigidBodyB, myDir, bPt);
	support.setSub(aPt, bPt);
}

void hktHullHullAgent::doPartialSupport(const hktRigidBody *rigidBody, const hkVector4& dir, hkVector4& partialSupport) {
	const hktShape *shape = dynamic_cast<const hktShape *>(rigidBody->getShape());
	hkTransform toWorld;
	rigidBody->getTransform(toWorld);
	hkVector4 myDir = dir;
	myDir.setRotatedInverseDir(toWorld.getRotation(), myDir); //rotates vector to box coord
	myDir.normalize3();
	shape->getSupportingVertex(myDir, partialSupport); //will return these in box coord
	partialSupport.setTransformedPos(toWorld, partialSupport);
}

void hktHullHullAgent::doPartialSupport(const hktRigidBody *rigidBody, const hkVector4& dir, hkArray<hkVector4>& partialSupport) {
	const hktShape *box = dynamic_cast<const hktShape *>(rigidBody->getShape());
	hkTransform toWorld;
	rigidBody->getTransform(toWorld);
	hkVector4 myDir = dir;
	myDir.setRotatedInverseDir(toWorld.getRotation(), myDir); //rotates vector to box coord
	myDir.normalize3();
	box->getSupportingVertexes(myDir, partialSupport); //will return these in box coord
	for (int i=0; i < partialSupport.getSize(); i++){
		partialSupport[i].setTransformedPos(toWorld, partialSupport[i]);
	}
}