#include "Salamander.h"
#include "Collision.h"

#include "Contact.h"
#include "Polygon.h"
#include "Line.h"
#include "World.h"

#define ALLOWED_PENETRATION 0.01f
#define BIAS_FACTOR 0.8f

namespace Salamander
{

Collision::Collision(Body* body1, Body* body2)
{
	body1_ = (body1 < body2) ? body1 : body2;
	body2_ = (body1 < body2) ? body2 : body1;

	generateContacts();
}

Collision::~Collision()
{
	contacts_.clear();
}

std::vector<Contact> Collision::getContacts()
{
	return contacts_;
}

// update existing contacts instead of creating new ones, therefore
// utilising existing impulses
void Collision::setContacts(std::vector<Contact> contacts)
{
	std::vector<Contact> mergedContacts;
	mergedContacts.resize(contacts.size());

	for (unsigned int i = 0; i < contacts.size(); ++i)
	{
		Contact newContact = contacts[i];
		int k = -1;
		for (unsigned int j = 0; j < contacts_.size(); ++j)
		{
			Contact oldContact = contacts_[j];
			if(newContact.edgePair_ == oldContact.edgePair_)
			{
				k = j;
				break;
			}
		}

		if (k > -1)
		{
			Contact c = mergedContacts[i];
			Contact oldContact = contacts_[k];
			c = newContact;
				c.accImpulseNormal_ = oldContact.accImpulseNormal_;	
				c.accImpulseTangent_ = oldContact.accImpulseTangent_;
				c.accImpulseTranslationBias_ = oldContact.accImpulseTranslationBias_;
			mergedContacts[i] = c;
		}
		else
		{
			mergedContacts[i] = contacts[i];
		}
	}

	contacts_ = mergedContacts;
}

unsigned int Collision::getNumContacts()
{
	return contacts_.size();
}

void Collision::performPrecomputation(double inv_ms)
{	
	// precompute data for contact points to save on extra processing during iterations
	for(ContactIterator contact = contacts_.begin();
		contact != contacts_.end();
		++contact)
	{
		// compute radius for later
		Vec2 r1 = (*contact).translation_ - body1_->getTranslation();
		Vec2 r2 = (*contact).translation_ - body2_->getTranslation();
		(*contact).radius1_ = r1;
		(*contact).radius2_ = r2;

		// precompute normal mass
		double rn1 = r1 * (*contact).normal_;
		double rn2 = r2 * (*contact).normal_;
		double inverseNormalMass = body1_->inverseMass_ + body2_->inverseMass_;
		inverseNormalMass += body1_->inverseMassMomentOfInertia_ * ((r1 * r1) - rn1 * rn1) + 
							 body2_->inverseMassMomentOfInertia_ * ((r2 * r2) - rn2 * rn2);
		(*contact).massNormal_ = 1.0f / inverseNormalMass;

		// tangent mass
		double rt1 = r1 * (*contact).tangent_;
		double rt2 = r2 * (*contact).tangent_;
		double inverseTangentMass = body1_->inverseMass_ + body2_->inverseMass_;
		inverseTangentMass += body1_->inverseMassMomentOfInertia_ * ((r1 * r1) - rt1 * rt1) + 
							  body2_->inverseMassMomentOfInertia_ * ((r2 * r2) - rt2 * rt2);
		(*contact).massTangent_ = 1.0f / inverseTangentMass;

		// bias
		(*contact).bias_ = BIAS_FACTOR * inv_ms * std::min<double>(0.0f, (*contact).separation_ + ALLOWED_PENETRATION);

		// Apply normal and friction impulse
		Vec2 accumulatedImpulse = (*contact).normal_ * (*contact).accImpulseNormal_ + 
								 (*contact).tangent_ * (*contact).accImpulseTangent_;

		body1_->velocity_ -= accumulatedImpulse * body1_->inverseMass_;
		body1_->angularVelocity_ -= body1_->inverseMassMomentOfInertia_ * (r1 ^ accumulatedImpulse);

		body2_->velocity_ += accumulatedImpulse * body2_->inverseMass_;
		body2_->angularVelocity_ += body2_->inverseMassMomentOfInertia_ * (r2 ^ accumulatedImpulse);

	}
}

void Collision::performIteration()
{
	for(ContactIterator contact = contacts_.begin();
		contact != contacts_.end();
		++contact)
	{
		// Relative velocity at contact
		Vec2 relativeVelocity = body2_->velocity_ + ((*contact).radius2_ ^ -body2_->angularVelocity_) -
								body1_->velocity_ - ((*contact).radius1_ ^ -body1_->angularVelocity_);

		// Compute normal impulse
		double normalVelocity = relativeVelocity * (*contact).normal_;
		double diffNormalImpulse = -normalVelocity * (*contact).massNormal_;
		
		// Clamp the accumulated impulse
		double initialNormalImpulse = (*contact).accImpulseNormal_;
		(*contact).accImpulseNormal_ = std::min<double>(initialNormalImpulse + diffNormalImpulse, 0.0f);
		diffNormalImpulse = (*contact).accImpulseNormal_ - initialNormalImpulse;

		// Apply contact impulse
		Vec2 normalImpulse = (*contact).normal_ * diffNormalImpulse;

		body1_->velocity_ -= normalImpulse * body1_->inverseMass_;
		body1_->angularVelocity_ -= body1_->inverseMassMomentOfInertia_ * ((*contact).radius1_ ^ normalImpulse);
		body2_->velocity_ += normalImpulse * body2_->inverseMass_;
		body2_->angularVelocity_ += body2_->inverseMassMomentOfInertia_ * ((*contact).radius2_ ^ normalImpulse);

		// Compute bias impulse
		relativeVelocity = body2_->biasedVelocity_ + ((*contact).radius2_ ^ -body2_->biasedAngularVelocity_) - 
						   body1_->biasedVelocity_ - ((*contact).radius1_ ^ -body1_->biasedAngularVelocity_);
		double normalVelocityBias = relativeVelocity * (*contact).normal_;

		double diffNormalImpulseBias = (*contact).massNormal_ * (-normalVelocityBias + (*contact).bias_);
		double initialNormalImpulseBias = (*contact).accImpulseTranslationBias_;
		(*contact).accImpulseTranslationBias_ = std::min<double>(initialNormalImpulseBias + diffNormalImpulseBias, 0.0f);
		diffNormalImpulseBias = (*contact).accImpulseTranslationBias_ - initialNormalImpulseBias;

		Vec2 impulseBias = (*contact).normal_* -diffNormalImpulseBias * 0.01f;

		body1_->biasedVelocity_ -= impulseBias * -body1_->inverseMass_;
		body1_->biasedAngularVelocity_ -= body1_->inverseMassMomentOfInertia_ * ((*contact).radius1_ ^ impulseBias);

		body2_->biasedVelocity_ += impulseBias * -body2_->inverseMass_;
		body2_->biasedAngularVelocity_ += body2_->inverseMassMomentOfInertia_ * ((*contact).radius2_ ^ impulseBias);

		// Relative velocity at contact
		relativeVelocity = body2_->velocity_ + ((*contact).radius2_ ^ -body2_->angularVelocity_) - 
						   body1_->velocity_ - ((*contact).radius1_ ^ -body1_->angularVelocity_);

		double tangentVelocity = relativeVelocity * (*contact).tangent_;
		double diffTangentImpulse = (*contact).massTangent_ * -tangentVelocity;

		// Clamp accumulated friction impulse
		double maxTangentImpulse = (*contact).friction_ * (*contact).accImpulseNormal_;
		double oldTangentImpulse = (*contact).accImpulseTangent_;
		(*contact).accImpulseTangent_ = std::max<double>(std::min(oldTangentImpulse + diffTangentImpulse, -maxTangentImpulse), maxTangentImpulse);
		diffTangentImpulse = (*contact).accImpulseTangent_ - oldTangentImpulse;
	
		// Apply tangent impulse
		Vec2 tangentImpulse = (*contact).tangent_ * diffTangentImpulse;

		body1_->velocity_ -= tangentImpulse * body1_->inverseMass_;
		body1_->angularVelocity_ -= body1_->inverseMassMomentOfInertia_ * ((*contact).radius1_ ^ tangentImpulse);

		body2_->velocity_ += tangentImpulse * body2_->inverseMass_;
		body2_->angularVelocity_ += body2_->inverseMassMomentOfInertia_ * ((*contact).radius2_ ^ tangentImpulse);
	}
	
}

// TODO PHYS : nicely
void Collision::performPostcomputation()
{
	// composites can split up, so check for excess forces
	for(ContactIterator contact = contacts_.begin();
			contact != contacts_.end();
			++contact)
	{
		Vec2 accumulatedImpulse = (*contact).normal_ * (*contact).accImpulseNormal_ + 
							      (*contact).tangent_ * (*contact).accImpulseTangent_;
	}
}

// the intersection test
void Collision::generateContacts()
{
	// don't test if both bodies are inactive
	// todo :
	if(body1_->getMass() == DBL_MAX && body2_->getMass() == DBL_MAX)
	{
		contacts_.clear();
		return;
	}

	// bounding collision test
	double distanceSquared = (body1_->getTranslation() - body2_->getTranslation()).lengthSquared();
	double totalRadius = body1_->radius() + body2_->radius();
	
	if(distanceSquared > totalRadius * totalRadius)
	{
		contacts_.clear();
		return;
	}

	// close contact
	generateBodyContacts();


}

void Collision::generateBodyContacts()
{
	// figure out which collision test to use, and then use it
	if(body1_->shapeId() == Polygon::TypeId && body2_->shapeId() == Polygon::TypeId)
	{
		generatePolygonPolygonContacts();
	}
	else if((body1_->shapeId() == Polygon::TypeId && body2_->shapeId() == Line::TypeId) ||
			(body2_->shapeId() == Polygon::TypeId && body1_->shapeId() == Line::TypeId))
	{
		generatePolygonLineContacts();
	}
	else
	{
		// todo : unsupported collision type
	}
}

void Collision::generatePolygonPolygonContacts()
{
	Polygon* polygon1 = (Polygon*)body1_->getShape();
	Polygon* polygon2 = (Polygon*)body2_->getShape();

	// find contact points as per the separating axis theorm
	Contact contact;
	Vec2 overlap1; 
	Vec2 overlap2;
	double separation = 0.0f;
	double d = DBL_MAX;
	double lastSeparation;

	// data for the physics properties we need
	Polygon* contactLine = 0;
	Polygon* incidentLine = 0;
	unsigned int contactIndex = 0;

	// Step 1 - test only edges facing toward each other
	Vec2 centreDirection = body2_->getTranslation() - body1_->getTranslation();
	centreDirection.normalise();

	for(unsigned int v = 0; v < polygon1->getNumVertices(); v++) {	
	//if((polygon1->normal(v) ^ centreDirection) >= 0.0f) // todo : optimisation
	{
		// Step 2 - project the points of both polygons onto the axis of separation
		polygon1->projectPoints(polygon1->normal(v), overlap1);
		polygon2->projectPoints(polygon1->normal(v), overlap2);

		// Step 3 - check that the bounds overlap. If they do not overlap, then the objects
		//			do not intersect and we can return
		lastSeparation = contact.separation_;
		if(contact.updateSeparation(overlap1, overlap2, separation))
		{
			if(separation >= 0) return;
			
			// step 3.1 - if it was a smaller separation, then we update
			//			  regardless

			// step 3.2 - project one of the vertices from the face onto the same vector as the SAT. 
			//			  We choose the contact face as the one that is closest to the intersection
			//			  (SAT does not distinguish between parallel lines)
			
			double dcentre = abs(~polygon1->normal(v) ^ polygon1->getVertex(v));
			double intcentre = 0.0f;
			if(overlap1.x_ + overlap1.y_ < overlap2.x_ + overlap2.y_)
			{
				intcentre = abs(overlap1.y_ + overlap2.x_) * 0.5f;
			}
			else
			{
				intcentre = abs(overlap2.y_ + overlap1.x_) * 0.5f;
			}

			double dfromintersect = abs(intcentre - dcentre);
			if(separation > lastSeparation || (lastSeparation == separation && dfromintersect < d))
			{
				d = dfromintersect;

				// Step 4 - record our intersection face
				contactLine = polygon1;
				incidentLine = polygon2;
				contactIndex = v;
			}

		}

	}
	}

	for(unsigned int v = 0; v < polygon2->getNumVertices(); v++) {
	//if((polygon2->normal(v) ^ centreDirection) <= 0.0f)
	{
		polygon1->projectPoints(polygon2->normal(v), overlap1);
		polygon2->projectPoints(polygon2->normal(v), overlap2);
		
		lastSeparation = contact.separation_;
		if(contact.updateSeparation(overlap1, overlap2, separation))
		{
			if(separation >= 0) return;
			
			double dcentre = abs(~polygon2->normal(v) ^ polygon2->getVertex(v));
			double intcentre = 0.0f;
			if(overlap1.x_ + overlap1.y_ < overlap2.x_ + overlap2.y_)
			{
				intcentre = abs(overlap1.y_ + overlap2.x_) * 0.5f;
			}
			else
			{
				intcentre = abs(overlap2.y_ + overlap1.x_) * 0.5f;
			}

			double dfromintersect = abs(intcentre - dcentre);
			if(separation > lastSeparation || (lastSeparation == separation && dfromintersect < d))
			{
				d = dfromintersect;
				contactLine = polygon2;
				incidentLine = polygon1;
				contactIndex = v;
			}


		}
	}
	}

	// Step 5 - global collision data we need to pass out
	contact.normal_ = contactLine->normal(contactIndex);
	if(contactLine == polygon2)
		contact.normal_ = -contact.normal_;

	contact.tangent_ = ~contact.normal_;
	contact.accImpulseNormal_ = 0.0f;
	contact.accImpulseTangent_ = 0.0f;
	contact.accImpulseTranslationBias_ = 0.0f;

	contact.edgePair_.body1_ = body1_;
	contact.edgePair_.body2_ = body2_;

	contact.friction_ = sqrt(body1_->getFriction() * body2_->getFriction());

	// todo : debug only
	contactBody_ = (contactLine == polygon1) ? body1_ : body2_;
	incidentBody_ = (incidentLine == polygon1) ? body1_ : body2_;
	contactFace_ = contactIndex;

	// Step 6 - find the contact points we use to apply inertia
	Vec2 edgeOverlap;
	unsigned int nextIndex = (contactIndex + 1) % contactLine->getNumVertices();
	edgeOverlap.x_ = contactLine->getVertex(contactIndex) ^ contactLine->normal(contactIndex);
	edgeOverlap.y_ = contactLine->getVertex(nextIndex) ^ contactLine->normal(contactIndex);

	bool currentVertexInside = false;
	bool lastVertexInside = false;
	Vec2 relativeVertex = incidentLine->getVertex(incidentLine->getNumVertices() - 1) - contactLine->getVertex(contactIndex);
	if((relativeVertex ^ contactLine->edge(contactIndex)) <= 0.0f)
		if(contactLine->isPointInside(incidentLine->getVertex(incidentLine->getNumVertices() - 1)))
			lastVertexInside = true;

	for(unsigned int i = 0; i < incidentLine->getNumVertices(); i++)
	{
		// find all points that lie behind our collision face
		currentVertexInside = false;
		relativeVertex = incidentLine->getVertex(i) - contactLine->getVertex(contactIndex);
		if((relativeVertex ^ contactLine->edge(contactIndex)) <= 0.0f)
		{
			// todo : now clip them (or do we test that they lie within the polygon?)
			if(contactLine->isPointInside(incidentLine->getVertex(i)))
			{
				currentVertexInside = true;
				contact.translation_ = incidentLine->getVertex(i);
				contacts_.push_back(contact);
			}
		}

		// todo : we shouldn't need to test all of these, so optimize it
		// test against the two edges adjoining the collision face
		for(unsigned int j = 0; j < contactLine->getNumVertices(); j ++)
		//for(int j = -1; j <= 1; j+=2)
		{
			//int jm = (int)contactIndex + j;
			//if(jm < 0)										jm += contactLine->getNumVertices();
			//if(jm >= (int)contactLine->getNumVertices()) jm -= contactLine->getNumVertices();
			int jm = j;

			// if we changed from inside to outside or vica versa, clip the edge
			//if(currentVertexInside != lastVertexInside)
			{
				//int im = (int)i - 1;
				//if(im < 0) im += incidentLine->getNumVertices();
				int im = i;

				Vec2 p1 = incidentLine->getVertex(im);
				Vec2 e1 = incidentLine->edge(im);
				Vec2 p2 = contactLine->getVertex(jm);
				Vec2 e2 = contactLine->edge(jm);
				Vec2 d = p2 - p1;

				double t = -1.0f;
				double s = -1.0f;

				double denominator = (e1.y_ * e2.x_ - e2.y_ * e1.x_);

				if(denominator != 0.0f)
				{
					// intersection point
					t = (d.y_ * e1.x_ - d.x_ * e1.y_) / denominator;
					s = (d.y_ * e2.x_ - d.x_ * e2.y_) / denominator;
				}

				// we only have an intersection if it lies within the line segment
				if(t >= 0.0f && t <= 1.0f && s >= 0.0f && s <= 1.0f)
				{
					contact.edgePair_.edge1_ = (polygon1 == contactLine ? contactIndex : i);
					contact.edgePair_.edge2_ = (polygon2 == contactLine ? contactIndex : i);
					contact.translation_ = p2 + e2 * t;
					contacts_.push_back(contact);
				}


			}
		}

		lastVertexInside = currentVertexInside;
	}

	if(!contacts_.size())
		std::cout << "intersection but no contacts" << std::endl;

	
}

void Collision::generatePolygonLineContacts()
{
	incidentBody_ = body1_->shapeId() == Polygon::TypeId ? body1_ : body2_;
	contactBody_  = body1_->shapeId() == Line::TypeId    ? body1_ : body2_;
	
	Polygon* polygon = (Polygon*)incidentBody_->getShape();
	Line* line = (Line*)contactBody_->getShape();

	// contacts are created when points from the polygon lie behind the line
	Contact contact;

	contact.accImpulseNormal_ = 0.0f;
	contact.accImpulseTangent_ = 0.0f;
	contact.accImpulseTranslationBias_ = 0.0f;

	contact.edgePair_.body1_ = body1_;
	contact.edgePair_.body2_ = body2_;

	contact.friction_ = sqrt(body1_->getFriction() * body2_->getFriction());

	// todo : worrying hack
	double flip = World::gravity_.y_ < 0.0f ? 1.0f : -1.0f;

	// test against every edge in the line
	for(unsigned int vl = 0; vl < line->getNumVertices() - 1; vl++)	
	{
		// Step 2 - test the vertices of the polygon in respect to the line
		for(unsigned int vp = 0; vp < polygon->getNumVertices(); vp++)
		{
			// if the vertex is behind the face
			Vec2 relativeLowerVertex = polygon->getVertex(vp) - line->getVertex(vl);
			if(relativeLowerVertex * line->normal(vl) > 0.0f)
			{
				// and within the bounds of the face. This is done by checking that the point is bound by the normals of the adjacent faces
				// so that we don't get gaps in the interior when using concave objects
				unsigned int lowerNormalIndex = std::max(int(vl) - 1, 0);
				unsigned int upperNormalIndex = std::min(int(vl) + 1, int(line->getNumVertices()) - 2);
				Vec2 relativeUpperVertex = polygon->getVertex(vp) - line->getVertex(vl + 1);
				
				if(relativeLowerVertex * ~line->normal(lowerNormalIndex) > 0.0f &&
					relativeUpperVertex * ~line->normal(upperNormalIndex) < 0.0f)
				{

					// project our point onto the line, and find the separation
					contact.separation_ = relativeLowerVertex ^ ~line->normal(vl);

					// give the line depth
					if(-contact.separation_ < line->getDepth())
					{
						contactFace_ = vl;
						incidentFace_ = vp;

						contact.normal_ = line->normal(vl) * flip;
						contact.tangent_ = ~contact.normal_;
						contact.translation_ = polygon->getVertex(vp);
						contacts_.push_back(contact);
					}
				}

			}
		}
	}


}


}