#include "CollisionManager.h"

#include <math.h>
#include "GroundMap.h"

#include "MemoryManager/debugNewOff.h"

using namespace std;

CollisionManager* CollisionManager::instance = NULL;

CollisionManager::CollisionManager(void){
}

CollisionManager::~CollisionManager(void){
	removeAll();
}

CollisionManager& CollisionManager::getInstance(){
	if (instance == NULL){
		instance = new CollisionManager();
	}
	return *instance;
}

void CollisionManager::destroy(){
	delete instance;
	instance = NULL;
}

void CollisionManager::addCollidable(CollisionInfo * info){
	collidables[info] = info;
}

void CollisionManager::removeCollidable(CollisionInfo * info){
	map<void*,CollisionInfo*>::iterator it = collidables.find(info);
	if (it != collidables.end()){
		collidables.erase(it);
	}
}

bool CollisionManager::containCollidable(CollisionInfo * info){
	return collidables.find(info) != collidables.end();
}

void CollisionManager::removeAll(){
	collidables.clear();
}

void CollisionManager::processCollisions(){
	// on parcourt toute les unitée du manager.
	for(map<void*,CollisionInfo*>::iterator currentIt = collidables.begin(); currentIt != collidables.end(); ++currentIt){
		// on ne traite que les unitée en mouvement
		CollisionInfo * current = currentIt->second;
		//if (current->isMoving())
		{
 			Point2D& pos = current->getPosition();
			// on parcourt toutes les autre entité
			for(map<void*,CollisionInfo*>::iterator otherIt = collidables.begin(); otherIt != collidables.end(); ++otherIt){
				CollisionInfo * other = otherIt->second;
				// on entre toujours en collision avec sois même !!!
				if (current == other){
					continue;
				}
				// on calcule la distance entre les entité
				float length = (pos - other->getPosition()).getLength();
				// on additionne les 2 rayons
				float additionRadius = current->getCollisionRadius() + other->getCollisionRadius();
				// on test si les entité entre en collision.
				if (length < additionRadius){
					// il y a collision !!!
					// on ne traite pas de la même maniere les entité immobile de celles en mouvement
					if (other->isMoving()){
						// l'autre entité est en mouvement
						// on test la prioritée entre les 2 entités.
						if (other->getSpeedForCollision() > current->getSpeedForCollision()){
							// on signale les collisions
							other->signalCollision();
							current->signalCollision();
							// l'autre est plus prioritaire
							collide(other,current,true,additionRadius - length);
						}else{
							// on signale les collisions
							other->signalCollision();
							current->signalCollision();
							// l'unitée courrante est plus prioritaire.
							collide(current,other,true,additionRadius - length);
					}
					}else{
						// on signal la collision (mais seulement pour celle qui etait immobile, l'autre n'a pas eut réellement de collision)
						other->signalCollision();
						// l'autre entité est immobile
						collide(current,other,false,additionRadius - length);
					}
				}
			}
		}
	}
}

void CollisionManager::collide(CollisionInfo* first, CollisionInfo * second,bool moving, float interPenetration){
	const float degToRad = 3.14156f/180.0f;
	const float offsetAngle = 0.75f*(3.14156f/2.0f);
	const float offsetFactor = 0.015f;
	float secondAngle = second->getDirectionAngle()*degToRad;
	float firstAngle = first->getDirectionAngle()*degToRad;

	if(!moving)
	{
		// on calcule le vecteur allant de l'entité first vers la second
		Point2D firstToSecond = second->getPosition() - first->getPosition();
		// on calcule la nouvelle position de second sans interpenetration (il n'y a donc plus collision ici)
		Point2D final2 = second->getPosition() + (firstToSecond*(2*interPenetration));
		// on regarde si les 2 entité vont dans le sens inverse ou pas
		if (cosf(firstAngle)*cosf(secondAngle) + sinf(firstAngle)*sinf(secondAngle) < 0){
			// ils vont dans des sens opposé
			// on decale legerement second vers la droite pour qu'il evite l'autre.
			final2.setX(final2.getX() + offsetFactor*cosf(secondAngle	+offsetAngle));
			final2.setY(final2.getY() + offsetFactor*sinf(secondAngle	+offsetAngle));
		}else{
			// ils vont dans le même sens
			// on decale legerement second vers la gauche pour qu'il evite l'autre.
			final2.setX(final2.getX() + offsetFactor*cosf(secondAngle	-offsetAngle));
			final2.setY(final2.getY() + offsetFactor*sinf(secondAngle	-offsetAngle));
		}

		//assignement de la nouvelle position
		ground::Case * c = ground::GroundMap::getInstance().getCase((int)(final2.getX()+0.5f),(int)(final2.getY()+0.5f));
		if(c != NULL && c->isPassable()){
			if (second->canCrossForest() || (c->getGroundType() != ground::FOREST && c->getGroundType() != ground::OREE)){
				second->setPostCollisionPosition(final2);
			}
		}
	}
	else
	{
		// on calcule le vecteur allant de l'entité first vers la second
		Point2D firstToSecond = second->getPosition() - first->getPosition();
		// on calcule la nouvelle position de first sans interpenetration (il n'y a donc plus collision ici)
		Point2D final1 = first->getPosition() - (firstToSecond*(2*interPenetration));
		// on calcule la nouvelle position de second sans interpenetration (il n'y a donc plus collision ici)
		Point2D final2 = second->getPosition() + (firstToSecond*(2*interPenetration));
		// on regarde si les 2 entité vont dans le sens inverse ou pas
		if (cosf(firstAngle)*cosf(secondAngle) + sinf(firstAngle)*sinf(secondAngle) < 0){
			// ils vont dans des sens opposé
			// on decale legerement first vers la droite pour qu'il evite l'autre.
			final1.setX(final1.getX() + offsetFactor*cosf(firstAngle	+offsetAngle));
			final1.setY(final1.getY() + offsetFactor*sinf(firstAngle	+offsetAngle));
			// on decale legerement second vers la droite pour qu'il evite l'autre.
			final2.setX(final2.getX() + offsetFactor*cosf(secondAngle	+offsetAngle));
			final2.setY(final2.getY() + offsetFactor*sinf(secondAngle	+offsetAngle));
		}else{
			// ils vont dans le même sens
			// on decale legerement first vers la droite pour qu'il evite l'autre.
			final1.setX(final1.getX() + offsetFactor*cosf(firstAngle	+offsetAngle));
			final1.setY(final1.getY() + offsetFactor*sinf(firstAngle	+offsetAngle));
			// on decale legerement second vers la gauche pour qu'il evite l'autre.
			final2.setX(final2.getX() + offsetFactor*cosf(secondAngle	-offsetAngle));
			final2.setY(final2.getY() + offsetFactor*sinf(secondAngle	-offsetAngle));
		}

		//assignement de la nouvelle position
		ground::Case * c = ground::GroundMap::getInstance().getCase((int)(final1.getX()+0.5f),(int)(final1.getY()+0.5f));
		if(c != NULL && c->isPassable()){
			if (first->canCrossForest() || (c->getGroundType() != ground::FOREST && c->getGroundType() != ground::OREE)){
				first->setPostCollisionPosition(final1);
			}
		}
		c = ground::GroundMap::getInstance().getCase((int)(final2.getX()+0.5f),(int)(final2.getY()+0.5f));
		if(c != NULL && c->isPassable()){
			if (second->canCrossForest() || (c->getGroundType() != ground::FOREST && c->getGroundType() != ground::OREE)){
				second->setPostCollisionPosition(final2);
			}
		}


	}


}

