#include "ActionGroupMove.h"
#include "GroundMap.h"
#include "DebugAI.h"
#include "RenderableEntity.h"

#include "MemoryManager/debugNew.h"

using namespace std;
using namespace ground;
using namespace RenderSystem;

ActionGroupMove::ActionGroupMove(Unit * u,unitEnumeration::UnitState state,ground::GroupPathFinder * group,
								 std::vector<Point2D>& centerPath, ground::pathFinding::PathFinderMode pathFinderMode)
	: UnitAction(u,state)
{
	this->group = group;
	if(group->containUnit(u)){
		offset = group->getOffsetMap()[u];
	}else{
		offset.setCoord(0.0f,0.0f);
	}
	this->pathFinderMode = pathFinderMode;
	// on definit l'unité sur laquel on va travailler
	ground::GroundMap::getInstance().setCurrentUnit(unit);
	// on empile la position de l'unité pour etre sur d'avoir toujours un point
	// valide dans le vecteur
	path.push_back(unit->getPosition());
	// pour chaque case du chemin
	for(vector<Point2D>::iterator it = centerPath.begin(); it != centerPath.end(); ++it){
		Point2D pos = (*it) + offset;
		// on test si la case courante est passable
		float posX = pos.getX();
		float posY = pos.getY();
		if (posX < 0.0f){
			posX = 0.0f;
		}
		if (posY < 0.0f){
			posY = 0.0f;
		}
		if ((int)(posX+0.5f) >= GroundMap::getInstance().getWidth()){
			posX = GroundMap::getInstance().getWidth()-0.51f;
		}
		if ((int)(posY+0.5f) >= GroundMap::getInstance().getHeight()){
			posY = GroundMap::getInstance().getHeight()-0.51f;
		}
		pos.setCoord(posX,posY);
		if (GroundMap::getInstance().getCase((int)(pos.getX()+0.5f),(int)(pos.getY()+0.5f))->isPassable()){
			// elle est passable
			path.push_back(pos);
		}else{
			// elle n'est pas passable, il faut calculer la prochaine case valide et calculer le detour
			Point2D nextPos = pos;
			// le compteur de detour
			int invalideCaseNumber = 0;
			// tant qu'on est pas arrivé à la fin du chemin et que les cases ne sont pas passables
			while(it != centerPath.end()){
				invalideCaseNumber++;
				nextPos = (*it) + offset;
				float posX = nextPos.getX();
				if ((int)(posX + 0.5f) >= GroundMap::getInstance().getWidth()){
					posX = GroundMap::getInstance().getWidth() - 1;
				}
				if ((int)(posX+0.5f) < 0.0f){
					posX = 0.0f;
				}
				float posY = nextPos.getY();
				if ((int)(posY + 0.5f) >= GroundMap::getInstance().getHeight()){
					posY = GroundMap::getInstance().getHeight() - 1;
				}
				if ((int)(posY+0.5f) < 0.0f){
					posY = 0.0f;
				}
				nextPos.setCoord(posX,posY);
				if(GroundMap::getInstance().getCase((int)(nextPos.getX()+0.5f),(int)(nextPos.getY()+0.5f))->isPassable()){
					break;
				}
				++it;
			}
			// ici, soit on a trouvé une case valide, soit on a terminé le chemin sans trouver d'autre case passable
			if (it == centerPath.end()){
				// on doit sortir de la boucle car sinon on va réincrementer it qui ne sera plus valide...
				break;
			}
			if(GroundMap::getInstance().getCase((int)(nextPos.getX()+0.5f),(int)(nextPos.getY()+0.5f))->isPassable()){
				// on a trouvé une case passable
				// on calcule le nouveau chemin
				MatrixPathFinder& pathFinder = GroundMap::getInstance().getMatrixPathFinder();
				if (path.empty()){
					pathFinder.findPath(unit->getPosition(),nextPos);
				}else{
					pathFinder.findPath(*path.rbegin(),nextPos);
				}
				// on ajoute le chemin calcule au chemin en cours d'analyse
				vector<Point2D>& newPath = pathFinder.getPath();
				if (!newPath.empty()){
					// on supprime la premiere case qui est deja dans le vecteur
					newPath.erase(newPath.begin());
					// le detour réel est la taille du nouveau chemin moins la taille du chemin qu'on
					// aurais du parcourir
					for(vector<Point2D>::iterator jt = newPath.begin(); jt != newPath.end(); ++jt){
						path.push_back(*jt);
					}
				}
			}
		}
	}
	ground::GroundMap::getInstance().setCurrentUnit(NULL);
}

ActionGroupMove::~ActionGroupMove(void)
{
	// on supprime l'unité du groupe de deplacement
	group->removeUnit(unit);
}

void ActionGroupMove::execute(void)
{
	
	// on test si tout le monde est arrive
	if (path.empty()){
		if (group->moveFinished()){
			// tout le monde est arrive, on peut supprimer l'action.
			dispose();
			// on supprime l'unité du groupe de mouvement, ainsi, quand toutes les unités se seront
			// supprimée du groupe, le manager de groupe pourra detruire le groupe.
			group->removeUnit(unit);
		}else{
			// l'unité est arrivé mais d'autres unité du groupe ne sont pas arrivé
			// on attend
			unit->pushAction(this);
			unit->wait(12);
		}
		return;
	}
	/**
	 * on restaure l'etat de l'unité
	 */
	unit->setState(this->unitState);
	unit->allowDust(true);
	// on doit bouger : on recupere la vitesse du plus lent du groupe
	float speed = std::min(group->getMinUnitSpeed(),unit->getSpeed());
	ground::GroundMap::getInstance().setCurrentUnit(unit);
	moveUnit(speed);
	ground::GroundMap::getInstance().setCurrentUnit(NULL);
}

void ActionGroupMove::moveUnit(float speed){
	Point2D position = unit->getPosition();
	Point2D destination = *path.begin();
	float tmpX = destination.getX()-position.getX();
	float tmpY = destination.getY()-position.getY();
	ground::Case * c = ground::GroundMap::getInstance().getCase(destination.getX()+0.5f,destination.getY() + 0.5f);
	if (abs(tmpX)>=2 || abs(tmpY)>=2 || !c->isPassable()){
		// on est trop loin de la prochaine case, on doit recalculer un chemin
		if (!path.empty()){
			// on avait pas fini de parcourir le chemin
			// on supprime toute les cases non passable au debut du chemin
			this->removeInvalidePathStart();
			if (!path.empty()){
				// on calcule un chemin qui va de la position actuelle à la prochaine case passable
				GroundMap::getInstance().setPathFinderMode(pathFinderMode);
				GroundMap::getInstance().computePath(position,*path.begin());
				// puis on ajoute ce morceau de chemin au chemin precedent
				std::vector<Point2D> newPath = GroundMap::getInstance().getLastPath();
				for (std::vector<Point2D>::iterator it = path.begin(); it != path.end(); ++it){
					newPath.push_back(*it);
				}
				path = newPath;
			}
		}else{
			path.clear();
		}
		unit->pushAction(this);
		return;
	}
	float x = destination.getX()-position.getX();
	float y = destination.getY()-position.getY();
	float distance = sqrt( x*x + y*y  );
	float arriveX;
 	float arriveY;
	
	if (!(position == destination)){
		unit->setOrientation(position.getAngle(destination));
	}

	if(distance > unit->getRadius()) {
		// on ne pourra pas atteindre le point mais on s'en rapproche
		arriveX = (x / distance) * speed;	
		arriveY = (y / distance) * speed ;
		unit->pushAction(this);
		unit->setPosition(Point2D(position.getX() + arriveX,position.getY() + arriveY));
		// gestion de l'animation
		if(unit->getCurrentAnimation() != RenderableEntity::WALK ) {
			unit->setAnimation(RenderableEntity::WALK, unit->isMounted());
		}
	} else {
		// on est arrivé sur la nouvelle case : on la consomme
		path.erase(path.begin());
		if (!path.empty()){
			// on continu à parcourire le chemin
			unit->pushAction(this);
			// on supprime toutes les cases non passables du debut du chemin.
			removeInvalidePathStart();
			// gestion de l'animation
			if(unit->getCurrentAnimation() != RenderableEntity::WALK ) {
				unit->setAnimation(RenderableEntity::WALK, unit->isMounted());
			}
		}else{
			// on attend une demi seconde
			unit->pushAction(this);
			unit->wait(12);
			// et on signale qu'on est arrivé
			group->signalArriving(unit);
		}
	}

}

void ActionGroupMove::removeInvalidePathStart(){
	if (!path.empty()){
		// on recupere la premiere case
		ground::Case * c = ground::GroundMap::getInstance().getCase((*path.begin()).getX()+0.5f,(*path.begin()).getY()+0.5f);
		// tant qu'on a des cases dans le chemin et que la premiere cases n'est pas passable
		while (!c->isPassable() && !path.empty()){
			// on supprime la premiere case
			path.erase(path.begin());
			if (!path.empty()){
				// et on recupere la nouvelle premiere case.
				c = ground::GroundMap::getInstance().getCase((*path.begin()).getX()+0.5f,(*path.begin()).getY()+0.5f);
			}
		}
	}
}

