#include "GraphPathFinder.h"

// a supprimer
#include "SquareNode.h"

#include "MemoryManager/debugNew.h"


using namespace ground;
using namespace std;

GraphPathFinder::GraphPathFinder(void)
{
	pathFound = false;
	nbNodeTerminated = 0;
	graph = NULL;
	opened.clear();
	path.clear();
}

GraphPathFinder::~GraphPathFinder(void)
{
	path.clear();
}

bool GraphPathFinder::haveFoundPath(){
	return pathFound;
}

std::vector<GraphNode*> GraphPathFinder::getPath(){
	return path;
}

void GraphPathFinder::setGraph(Graph * g){
	graph = g;
}

void GraphPathFinder::findPath(GraphNode* begin,GraphNode* end){
	if (graph == NULL){
		fprintf(stderr,"GraphPathFinder::findPath() :: \n\timpossible d'effectuer un pathfinding sur un graph NULL\n");
		pathFound = false;
		return;
	}
	// initialisation des données du chemin.
	path.clear();
	pathFound = false;
	graph->initialisePathFinding();
	clearOpenedList();
	SquareNode * node = (SquareNode*)begin;
	// on initialise le noeud de depart.
	begin->setCostWithHeuristic(0.0f);
	begin->setCostWithoutHeuristic(0.0f);
	begin->setParent(NULL);
	addNodeInOpenedList(begin);
	// on initialise les variables pour les statistiques.
	nbNodeTerminated = 0;
	while(!opened.empty()){
		GraphNode * current = getLowestCostNode();
		// on test si on est arrivé à la fin :
		if (end->isFinished()){
			// on a trouvé un chemin entre les 2 noeuds...
			pathFound = true;
			// on construit ce chemin.
			buildPath(begin,end);
			break;
		}else{
			// on a pas encore terminée la recherche : on continu
			// on prend tout les points autour du point courant
			vector<GraphArc*>& neighbours = current->getNeighbours();
			for(vector<GraphArc*>::iterator it = neighbours.begin(); it != neighbours.end(); ++it){
				// on recupere l'autre extremitée de l'arc
				GraphNode* node = (*it)->getOtherSide(current);
				// on verifie la validitée du noeud retourné
				if (node != NULL){
					// test si le noeud n'est pas deja entierement traité
					if (!node->isFinished()){
						// le noeud n'est pas terminé
						// on verifie si il est initialisé
						if (node->isInitialised()){
							// le noeud n'est pas initialisé. c'est la premiere fois qu'il est traité
							// on calcule le cout avec heuristique
							float cost = current->getCostWithHeuristic() + 
										(*it)->getCostFrom(*current) + 
										node->heuristic(current,begin,end);
							// et le cout sans heuristique
							float costWithoutHeuristic = current->getCostWithHeuristic() + 
														(*it)->getCostFrom(*current);
							node->setParent(current);
							node->setCostWithHeuristic(cost);
							node->setCostWithoutHeuristic(costWithoutHeuristic);
							addNodeInOpenedList(node);
						}else{
							// le neud a deja été traité.
							// on le supprime de la list
							removeNodefromOpenedList(node);
							// on calcule son cout
							float cost = current->getCostWithHeuristic() + 
										(*it)->getCostFrom(*current) + 
										node->heuristic(current,begin,end);
							// et le cout sans heuristique
							float costWithoutHeuristic = current->getCostWithHeuristic() + 
														(*it)->getCostFrom(*current);
							// on verifie si le chemin courant est moins couteux
							if (cost < node->getCostWithHeuristic()){
								// il est moins couteux : on met à jour
								node->setCostWithHeuristic(cost);
								node->setCostWithoutHeuristic(costWithoutHeuristic);
								node->setParent(current);
							}
							addNodeInOpenedList(node);
						}
					}
				}
			}
		}
		current->setFinished(true);
		nbNodeTerminated++;
	}
	clearOpenedList();
}

GraphNode* GraphPathFinder::getLowestCostNode(){
	// precondition : le list opened ne doit pas etre vide
	// on recupere le premier noeud de la liste
	multimap<float,GraphNode*>::iterator it = opened.begin();
	// on le recupere dans une variable temporaire car le erase sur it invalide sa valeure.
	GraphNode* ret = it->second;
	// on le supprime de la liste
	//printf("lowest cost = %f\n",it->first);
	opened.erase(it);
	// et on retourne le noeud
	return ret;
}

void GraphPathFinder::removeNodefromOpenedList(GraphNode* nodeToRemove){
	float cost = nodeToRemove->getCostWithHeuristic();
	// on recupere le premier noeud ayant le même cout que celui qu'on veut supprimer
	multimap<float,GraphNode*>::iterator it = opened.find(cost);
	// tant que le cout du noeud de l'iterateur est bien le même que celui qu'on veut supprimer
	// ET qu'on est pas sur le noeud à supprimer
	// ET qu'on est pas à la fin de la liste ...
	while(it->first == cost && it->second != nodeToRemove && it != opened.end()){
		// on passe au noeud suivant
		++it;
	}
	// on test si on est bien sur le noeud recherché
	if (it->second == nodeToRemove){
		// on est bien dessus : on le supprime
		opened.erase(it);
	}
}

void GraphPathFinder::addNodeInOpenedList(GraphNode* node){
	opened.insert(std::make_pair<float,GraphNode*>(node->getCostWithHeuristic(),node));
}

void GraphPathFinder::clearOpenedList(){
	opened.clear();
}

void GraphPathFinder::buildPath(GraphNode* begin, GraphNode* end){
	GraphNode* current = end;
	vector<GraphNode*> inversePath;
	// on reconstruit le chemin a l'envers (en partant de la fin pour aller au debut
	while(current->getParent() != NULL && current != begin){
		inversePath.push_back(current);
		current = current->getParent();
	}
	// on ajoute le point de depart
	inversePath.push_back(begin);
	// on reconstruit le chemin.
	for(vector<GraphNode*>::reverse_iterator it = inversePath.rbegin(); it != inversePath.rend(); ++it){
		path.push_back(*it);
	}
}


