#include "collisions.h"

#include <wgd/vector.h>

#include <wgd/game.h>
#include "body.h"
#include "polygon.h"
#include "circle.h"

float EPSILON = 0.0001f;

float NEAR = 5;

using namespace wgd;
using namespace std;

vector<Collisions::Contact> Collisions::contacts;

bool comparePenetration (const Collisions::Contact& a, const Collisions::Contact& b)
{
	return (a.penetration > b.penetration);
}

bool compareSpeed (const Collisions::Contact& a, const Collisions::Contact& b)
{
	if (a.penetration < EPSILON) return false;
	if (b.penetration < EPSILON) return true;
	return (a.approachSpeed > b.approachSpeed);
}

bool notNear (const Collisions::Contact& contact)
{
	return contact.penetration < -NEAR;
}

void Collisions::Contact::recalculate ()
{
	Vector2D p[2];
	
	p[0] = body[0]->getVertex(featureID[0]);
	p[1] = body[1]->getVertex(featureID[1]);
	
	if (isEdge[0]) {
		normal = body[0]->getNormal(featureID[0]);
	} else if (isEdge[1]) {
		normal = -body[1]->getNormal(featureID[1]);
	} else {
		// circle-circle or vertex-circle
		normal = (p[1] - p[0]).normalise();
	}

	p[0] += normal * body[0]->getRadius();
	p[1] -= normal * body[1]->getRadius();
	
	penetration = normal.dot(p[0] - p[1]);
	
	if (isEdge[0]) {
		position = p[1] + normal * penetration / 2;
	} else if (isEdge[1]) {
		position = p[0] - normal * penetration / 2;
	} else {
		position = (p[0] + p[1]) / 2;
	}
	
	offset[0] = position - body[0]->position();
	offset[1] = position - body[1]->position();
}

void Collisions::Contact::init (Body* b1, Body* b2)
{
	body[0] = b1;
	body[1] = b2;
	
	inverseMass[0] = 1 / b1->getMass();
	inverseMass[1] = 1 / b2->getMass();
	
	offset[0] = position - b1->position();
	offset[1] = position - b2->position();
	
	float moi1 = b1->getMomentOfInertia();
	float moi2 = b2->getMomentOfInertia();
	
	angularComponent[0] = (offset[0].cross(normal) / moi1).cross(offset[0]).dot(normal);
	angularComponent[1] = (offset[1].cross(normal) / moi2).cross(offset[1]).dot(normal);
	
	calculateVelocities();
}

void Collisions::Contact::calculateVelocities ()
{
	Vector3D u1 = Vector3D(0, 0, body[0]->getRotationVel()).cross(offset[0]);
	
	u1 += body[0]->getVelocity();
	
	Vector3D u2 = Vector3D(0, 0, body[1]->getRotationVel()).cross(offset[1]);
	
	u2 += body[1]->getVelocity();
	
	approachSpeed = (u1 - u2).dot(normal);
}

template <typename T> inline void swap (T* array) {
	T temp = array[0];
	array[0] = array[1];
	array[1] = temp;
}

void Collisions::Contact::swapBodies () {
	normal *= -1;
	
	swap(inverseMass);
	
	swap(angularComponent);
	
	swap(body);
	
	swap(offset);
	
	swap(isEdge);
	
	swap(featureID);
}
	
void Collisions::draw ()
{
	glColor3f(0, 0, 0);
	
	for (vector<Contact>::iterator it = contacts.begin(); it != contacts.end(); it++)
	{
		float x = it->position.x;
		float y = it->position.y;
		float dx = it->normal.x;
		float dy = it->normal.y;
		glBegin(GL_LINES);
			glVertex3f(x + dx * 10, y + dy * 10, 0);
			glVertex3f(x - dx * 10, y - dy * 10, 0);
			glVertex3f(x - dy * 5, y + dx * 5, 0);
			glVertex3f(x + dy * 5, y - dx * 5, 0);
		glEnd();
	}
}

void Collisions::init ()
{
	#if 0
	for (vector<Contact>::iterator it = contacts.begin(); it != contacts.end(); it++) {
		it->recalculate();
	}
	
	contacts.erase(std::remove_if(contacts.begin(), contacts.end(), notNear), contacts.end());
	#else
	contacts.clear();
	#endif
}

void Collisions::resolve ()
{
	int limit = contacts.size() * (int) WGD["iterations"];
	
	vector<Contact>::iterator here = min_element(contacts.begin(), contacts.end(), compareSpeed);
	
	for (int i = 0; i < limit; i++)
	{
		Contact& data = *here;
		
		if (data.approachSpeed < EPSILON) { break; }
		if (data.penetration < EPSILON) { break; }
		
		resolveVelocities(data);
		
		updateVelocities(here);
	}
	
	float moveLinear[2];
	float moveAngular[2];
	
	here = min_element(contacts.begin(), contacts.end(), comparePenetration);
	
	for (int i = 0; i < limit; i++)
	{
		Contact& data = *here;
		
		if (data.penetration < EPSILON) { break; }
		
		resolvePenetration(data, moveLinear, moveAngular);
		
		updatePenetrations(here, moveLinear, moveAngular);
	}
}

void Collisions::resolveVelocities (Contact& data)
{
	Body* body1 = data.body[0];
	Body* body2 = data.body[1];
	
	float approaching = data.approachSpeed;
	
	float restitution = (approaching < 0.1f) ? 0.0f : (float) WGD["restitution"];
	
	float velAcc = 0;
	
	if (data.inverseMass[0] == 0)
	{
		float t = Game::frameTime() * (float) WGD["speed"];
		
		velAcc = Vector2D(0, WGD["g"]).dot(data.normal) * t;
	}
	else if (data.inverseMass[1] == 0)
	{
		float t = Game::frameTime() * (float) WGD["speed"];
		
		velAcc = Vector2D(0, WGD["g"]).dot(data.normal) * -t;
	}
	
	if (velAcc > approaching) { velAcc = approaching; }
	
	float separating = restitution * (approaching - velAcc);
	
	float velocityPerImpulse = data.inverseMass[0] + data.inverseMass[1] + data.angularComponent[0] + data.angularComponent[1];
	
	float velocityChange = approaching + separating;
	
	float impulseNormal = velocityChange / velocityPerImpulse;
	
	body1->impulse(-data.normal * impulseNormal, data.position);
	body2->impulse(data.normal * impulseNormal, data.position);
	
	Vector3D u1 = Vector3D(0, 0, data.body[0]->getRotationVel()).cross(data.offset[0]);
	
	u1 += data.body[0]->getVelocity();
	
	Vector3D u2 = Vector3D(0, 0, data.body[1]->getRotationVel()).cross(data.offset[1]);
	
	u2 += data.body[1]->getVelocity();
	
	Vector2D tangent(-data.normal.y, data.normal.x);
	
	float vTangent = tangent.dot(u2 - u1);
	
	float frictionCoefficient = WGD["friction"];
	
	float moi1 = body1->getMomentOfInertia();
	float moi2 = body2->getMomentOfInertia();
	
	velocityPerImpulse = data.inverseMass[0] + data.inverseMass[1] + (data.offset[0].cross(tangent) / moi1).cross(data.offset[0]).dot(tangent) + (data.offset[1].cross(tangent) / moi2).cross(data.offset[1]).dot(tangent);
	
	float impulseTangent = vTangent / velocityPerImpulse;
	
	float frictionMax = frictionCoefficient * impulseNormal;
	
	if (impulseTangent > frictionMax) {
		impulseTangent = frictionMax;
	}
	
	body1->impulse(tangent * impulseTangent, data.position);
	body2->impulse(-tangent * impulseTangent, data.position);
}

void Collisions::updateVelocities (vector<Contact>::iterator& it1)
{
	for (vector<Contact>::iterator it2 = contacts.begin(); it2 != contacts.end(); it2++)
	{
		if (it1->body[0] == it2->body[0] || it1->body[0] == it2->body[1] || it1->body[1] == it2->body[0] || it1->body[1] == it2->body[1])
		{
			it2->calculateVelocities();
		}
	}
	
	it1 = min_element(contacts.begin(), contacts.end(), compareSpeed);
}

void Collisions::resolvePenetration (Contact& data, float* moveLinear, float* moveAngular)
{
	Body* body1 = data.body[0];
	Body* body2 = data.body[1];
	
	moveLinear[0] = 0;
	moveLinear[1] = 0;
	moveAngular[0] = 0;
	moveAngular[1] = 0;
	
	float penetration = data.penetration;
	
	#if 0
	
	float unitMovement = data.inverseMass[0] + data.inverseMass[1] + data.angularComponent[0] + data.angularComponent[1];
	
	moveLinear[0] = -penetration * data.inverseMass[0] / unitMovement;
	moveLinear[1] = penetration * data.inverseMass[1] / unitMovement;
	
	moveAngular[0] = -penetration * data.angularComponent[0] / unitMovement;
	moveAngular[1] = penetration * data.angularComponent[1] / unitMovement;
	
	const float maxAngular = 0;
	
	if (moveAngular[0] < -maxAngular)
	{
		moveLinear[0] += moveAngular[0] + maxAngular;
		moveAngular[0] = -maxAngular;
	}
	
	if (moveAngular[1] > maxAngular)
	{
		moveLinear[1] += moveAngular[1] - maxAngular;
		moveAngular[1] = maxAngular;
	}
	
	body1->move(data.normal * moveLinear[0]);
	body2->move(data.normal * moveLinear[1]);
	
	if (moveAngular[0]) {
		Vector3D impulsePerMove1 = data.offset[0].cross(data.normal) / body1->getMomentOfInertia();
		Vector3D rotation1 = impulsePerMove1 / data.angularComponent[0] * moveAngular[0];
		moveAngular[0] = rotation1.z;
		body1->rotate(rotation1);
	}
	
	if (moveAngular[1]) {
		Vector3D impulsePerMove2 = data.offset[1].cross(data.normal) / body2->getMomentOfInertia();
		Vector3D rotation2 = impulsePerMove2 / data.angularComponent[1] * moveAngular[1];
		moveAngular[1] = rotation2.z;
		body2->rotate(rotation2);
	}
	
	#else
	
	if (data.inverseMass[0] == 0) {
		moveLinear[1] = penetration;
	} else if (data.inverseMass[1] == 0) {
		moveLinear[0] = -penetration;
	} else {
		float totalMass = 1 / data.inverseMass[0] + 1 / data.inverseMass[1];
		moveLinear[0] = -penetration / data.inverseMass[1] / totalMass;
		moveLinear[1] = penetration / data.inverseMass[0] / totalMass;
	}
	
	body1->move(data.normal * moveLinear[0]);
	body2->move(data.normal * moveLinear[1]);
	
	#endif
	
	if (penetration != penetration) { std::cerr << "Welcome to bugland!\n"; }
}

void Collisions::updatePenetrations (vector<Contact>::iterator& it1, float* moveLinear, float* moveAngular)
{
	for (vector<Contact>::iterator it2 = contacts.begin(); it2 != contacts.end(); it2++)
	{
		Vector3D change;
		
		//std::cerr << it2->penetration << "\n";
		
		if (it1->body[0] == it2->body[0])
		{
			change = moveLinear[0] * it1->normal;
			
			change += Vector3D(0, 0, moveAngular[0]).cross(it2->offset[0]);
			
			it2->penetration += change.dot(it2->normal);
		}
		else if (it1->body[0] == it2->body[1])
		{
			change = moveLinear[0] * it1->normal;
			
			change -= Vector3D(0, 0, moveAngular[0]).cross(it2->offset[1]);
			
			it2->penetration -= change.dot(it2->normal);
		}
		
		/*std::cerr << "Old body 0 changes new body 0: " << (it1->body[0] == it2->body[0]) << "\n";
		std::cerr << "Old body 0 changes new body 1: " << (it1->body[0] == it2->body[1]) << "\n";
		
		std::cerr << moveLinear[0] << ", " << moveAngular[0] << "\n";
		
		std::cerr << change.dot(it2->normal) << "\n";*/
		
		if (it1->body[1] == it2->body[0])
		{
			change = moveLinear[1] * it1->normal;
			
			change -= Vector3D(0, 0, moveAngular[1]).cross(it2->offset[0]);
			
			it2->penetration += change.dot(it2->normal);
		}
		else if (it1->body[1] == it2->body[1])
		{
			change = moveLinear[1] * it1->normal;
			
			change += Vector3D(0, 0, moveAngular[1]).cross(it2->offset[1]);
			
			it2->penetration -= change.dot(it2->normal);
		}
		
		/*std::cerr << "Old body 1 changes new body 0: " << (it1->body[1] == it2->body[0]) << "\n";
		std::cerr << "Old body 1 changes new body 1: " << (it1->body[1] == it2->body[1]) << "\n";
		
		std::cerr << moveLinear[1] << ", " << moveAngular[1] << "\n";
		
		std::cerr << change.dot(it2->normal) << "\n";
		
		std::cerr << it2->normal << "\n";*/
	}
	
	it1 = min_element(contacts.begin(), contacts.end(), comparePenetration);
}

float min (const Vector2D& axis, Body* body, Vector2D& point, int& index)
{
	point = body->getExtremePoint(-axis, index);
	
	return axis.dot(point);
}

float max (const Vector2D& axis, Body* body, Vector2D& point, int& index)
{
	point = body->getExtremePoint(axis, index);
	
	return axis.dot(point);
}

bool Collisions::check (Body* b1, Body* b2)
{
	float inf = Infinity;
	
	if (b1->getDensity() == inf && b2->getDensity() == inf)
	{
		return false;
	}
	
	int count1 = b1->getVertexCount();
	int count2 = b2->getVertexCount();
	
	if (count1 == 0 || count2 == 0) { return false; }
	
	if (! checkBounds(b1, b2))
	{
		return false;
	}
	
	Contact contact;
	
	contact.penetration = inf;
	
	// for each edge of body 1, check closest vertex of body 2
	
	for (int i = 0; i < count1; i++)
	{
		Vector2D axis = b1->getNormal(i);
		
		if (axis == Vector2D()) { continue; }
		
		float max1 = axis.dot(b1->getVertex(i)) + b1->getRadius();
		
		int j;
		
		Vector2D point;
		
		float min2 = min(axis, b2, point, j);
		
		float overlap = max1 - min2;
		
		if (overlap < -NEAR) { return false; }
		
		if (overlap < contact.penetration)
		{
			contact.penetration = overlap;
			contact.normal = axis;
			contact.position = point;
			contact.isEdge[0] = true;
			contact.isEdge[1] = false;
			contact.featureID[0] = i;
			contact.featureID[1] = j;
		}
	}
	
	// for each edge of body 2, check closest vertex of body 1
	
	for (int i = 0; i < count2; i++)
	{
		Vector2D axis = -b2->getNormal(i);
		
		if (axis == Vector2D()) { continue; }
		
		int j;
		
		Vector2D point;
		
		float max1 = max(axis, b1, point, j);
		
		float min2 = axis.dot(b2->getVertex(i)) - b2->getRadius();
		
		float overlap = max1 - min2;
		
		if (overlap < -NEAR) { return false; }
		
		if (overlap < contact.penetration)
		{
			contact.penetration = overlap;
			contact.normal = axis;
			contact.position = point;
			contact.isEdge[0] = false;
			contact.isEdge[1] = true;
			contact.featureID[0] = j;
			contact.featureID[1] = i;
		}
	}
	
	float r = b1->getRadius() + b2->getRadius();
	
	if (r > 0)
	{
		for (int i = 0; i < count1; i++)
		{
			for (int j = 0; j < count2; j++)
			{
				Vector2D axis = b2->getVertex(j) - b1->getVertex(i);
				
				axis = axis.normalise();
				
				// OPTIMISE
				
				Vector2D point;
				
				int i2, j2;
				
				float max1 = max(axis, b1, point, i2);
				float min2 = min(axis, b2, point, j2);
				
				float overlap = max1 - min2;
				
				if (overlap < -NEAR) { return false; }
				
				if (overlap < contact.penetration)
				{
					contact.penetration = overlap;
					contact.normal = axis;
					contact.position = b1->getVertex(i2) + axis * (b1->getRadius() - overlap / 2);
					contact.isEdge[0] = false;
					contact.isEdge[1] = false;
					contact.featureID[0] = i2;
					contact.featureID[1] = j2;
				}
			}
		}
	}
	
	contact.init(b1, b2);
	
	contacts.push_back(contact);
	
	return true;
}

bool Collisions::checkBounds (Body* b1, Body* b2)
{
	return (b1->getMaxX() >= b2->getMinX() - NEAR
		&& b1->getMinX() <= b2->getMaxX() + NEAR
		&& b1->getMaxY() >= b2->getMinY() - NEAR
		&& b1->getMinY() <= b2->getMaxY() + NEAR);
}

bool Collisions::check (Circle* c1, Circle* c2)
{
	Vector2D v = c2->position() - c1->position();
	
	float distSq = v.lengthSquared();
	
	float r = c1->getRadius() + c2->getRadius() + NEAR;
	
	// distSq != distSq tests if distSq is NaN
	if (distSq != distSq || distSq > r * r)
	{
		return false;
	}
	
	r -= NEAR;
	
	float dist = sqrt(distSq);
	
	if (v == Vector2D()) {
		v = Vector2D(1, 0);
	}
	
	Contact contact;
	
	contact.normal = v.normalise();
	contact.penetration = r - dist;
	contact.position = c1->position() + contact.normal * (c1->getRadius() - (r - dist) / 2);
	contact.isEdge[0] = false;
	contact.isEdge[1] = false;
	contact.featureID[0] = 0;
	contact.featureID[1] = 0;
	
	contact.init(c1, c2);
	
	contacts.push_back(contact);
	
	return true;
}