#include "Salamander.h"
#include "World.h"

#include "Joint.h"
#include "Collision.h"
#include "CollisionKey.h"

namespace Salamander
{

Vec2 World::gravity_ = Vec2(0.0f, 98.1f);

World::World(unsigned int iterations)
{
	iterations_ = iterations;
}

// free memory occupied by bodies in our list
World::~World()
{
	while(!bodies_.empty())
	{
		delete bodies_.back();
		bodies_.pop_back();
	}
}

void World::timeStep(double ms)
{
	double inv_ms = ms > 0.0f ? 1.0f / ms : 0.0f;

	// update the vertices in the objects
	updateBodyMatrices();

	// generate the contacts
	findCollisions();

	// perfom the physics!
	updateBodyForces(ms);
	performPrecomputations(inv_ms);
	performIterations();
	performPostcomputations();
	updateBodyVelocities(ms);
}

void World::updateBodyMatrices()
{
	// apply rotation/translation matrices to the bodies
	for(BodyIterator body = bodies_.begin();
		body != bodies_.end();
		++body)
	{
		(*body)->updateMatrix();
	}
}

// generate collision data for objects
void World::findCollisions()
{
	// test every object against every other object
	for(BodyIterator body1 = bodies_.begin();
		body1 != bodies_.end();
		++body1)
	{
		for(BodyIterator body2 = bodies_.begin();
		body2 < body1;
		++body2)
		{

			// perform collisions, tracking only objects with contact points
			// (note : slightly faster allocating to the heap than the stack, but we do it so often that
			// this is important)
			Collision newCollision(*body1, *body2);
			CollisionKey collisionKey(*body1, *body2);
		
			if (newCollision.getNumContacts())
			{
				CollisionIterator existingCollision = collisions_.find(collisionKey);
				if (existingCollision == collisions_.end())
				{
					// we didn't have a collision here last frame
					collisions_.insert(CollisionPair(collisionKey, newCollision));
				}
				else
				{
					// update the existing collision
					existingCollision->second.setContacts(newCollision.getContacts());
				}
			}
			else
			{
				// we had a collision last frame but don't any more
				collisions_.erase(collisionKey);
			}

		}
	}

}

void World::updateBodyForces(double ms)
{
	// Integrate forces.
	for(BodyIterator body = bodies_.begin();
		body != bodies_.end();
		++body)
	{
		(*body)->integrateForces(ms);
	}
}

void World::performPrecomputations(double inv_ms)
{
	for(CollisionIterator collision = collisions_.begin();
		collision != collisions_.end();
		++collision)
	{
		(*collision).second.performPrecomputation(inv_ms);
	}

	for(JointIterator joint = joints_.begin();
		joint != joints_.end();
		++joint)
	{
		(*joint)->performPrecomputation(inv_ms);
	}
}

void World::performIterations()
{
	for (unsigned int i = 0; i < iterations_; ++i)
	{
		for(CollisionIterator collision = collisions_.begin();
			collision != collisions_.end();
			++collision)
		{
			(*collision).second.performIteration();
		}

		for(JointIterator joint = joints_.begin();
			joint != joints_.end();
			++joint)
		{
			(*joint)->performIteration();
		}

	}
}

void World::performPostcomputations()
{
	for(CollisionIterator collision = collisions_.begin();
		collision != collisions_.end();
		++collision)
	{
		(*collision).second.performPostcomputation();
	}

}

void World::updateBodyVelocities(double ms)
{
	// Integrate forces.
	for(BodyIterator body = bodies_.begin();
		body != bodies_.end();
		++body)
	{
		(*body)->integrateVelocities(ms);
	}
}

void World::addBody(Body* body)
{
	bodies_.push_back(body);
	body->world_ = this;
}

Body* World::getBody(unsigned int i)
{
	return bodies_.at(i);
}

// return true if the body was found and removed
bool World::removeBody(Body* body)
{
	for (BodyIterator item = bodies_.begin();
		item != bodies_.end();
		++item)
	{
		if(*item == body)
		{
			bodies_.erase(item);
			body->world_ = NULL;

			// remove any collisions this object was in
			collisions_.clear();

			return true;
		}
	}

	return false;
}

unsigned int World::getNumBodies()
{
	return bodies_.size();
}

void World::addJoint(Joint* joint)
{
	joints_.push_back(joint);
}

Joint* World::getJoint(unsigned int i)
{
	return joints_.at(i);
}

// return true if the joint was found and removed
bool World::removeJoint(Joint* joint)
{
	for (JointIterator item = joints_.begin();
		item != joints_.end();
		++item)
	{
		if(*item == joint)
		{
			joints_.erase(item);
			return true;
		}
	}

	return false;
}

unsigned int World::getNumJoints()
{
	return joints_.size();
}

// cast a ray from location along the line given by direction
// and pick the fist body. If direction = 0, then just pick a
// body at location.

Pick World::pickBody(Vec2 location, Vec2 direction)
{
	Pick pick;
	Vec2 directionUnit = direction; directionUnit.normalise();

	// pick bodies until the closest one is found
	for(BodyIterator body = bodies_.begin();
		body != bodies_.end();
		++body)
	{
		// if we're picking a single point, special case
		if(direction == Vec2())
		{
			if((location - (*body)->getTranslation()).lengthSquared() < (*body)->radiusSquared())
			{
				if((*body)->isPointInside(location)) pick.body = *body;
				if(pick.body) return pick;
			}
		}
		else
		{
			// full pick
			Vec2 relativeVertex = (*body)->getTranslation() - location;
			if(abs(relativeVertex ^ directionUnit) < (*body)->radius())
			{
				// we intersect the body bound
				Pick localPick = (*body)->getRayIntersection(location, direction);
				if((localPick.distance < pick.distance && localPick.body != NULL) || pick.body == NULL)
				{
					pick = localPick;
				}
			}

		}
		
	}

	// didn't find one
	return pick;
}

// not only find the model, but remove it from the composite and the world
Body* World::pickUpBody(Vec2 location)
{
	Pick pick = pickBody(location);

	if(pick.body)
	{
		removeBody(pick.body);
	}

	return pick.body;
}

}
