#include "MatrixPathFinder.h"

#include <math.h>

#include "MemoryManager/debugNew.h"


using namespace std;
using namespace ground;

MatrixPathFinder::MatrixPathFinder(void){
	width = 0;
	height = 0;
	useStencil = false;
	useIncrementMap = false;
	nbTerminated = 0;
	pathFound = false;

	matrix = NULL;
	incrementMap = NULL;
	stencil = NULL;
	nodes = NULL;
}

MatrixPathFinder::~MatrixPathFinder(void){
	// on nettoie le dernier chemin retourné.
	path.clear();
	// pas de destruction de la matrice. le path finder ne la possede pas.
	matrix = NULL;
	// pas de destruction du masque stencil : le path finder ne le possede pas.
	stencil = NULL;
	// destruction du tableau de nodes.
	for (int i = 0; i < width;i++){
		delete[] nodes[i];
	}
	delete[] nodes;
	nodes = NULL;
}

void MatrixPathFinder::setMatrix(AbstractCase*** matrix, int width, int height){
	this->matrix = matrix;
	this->width = width;
	this->height = height;
	// on detruit le tableau de noeuds precedent si il n'est pas NULL
	if (nodes != NULL){
		for (int i = 0; i < width;i++){
			delete[] nodes[i];
		}
		delete[] nodes;
	}
	// et on crée un nouveau tableau de noeuds
	nodes = new Node*[width];
	for (int i = 0; i < width; i++){
		nodes[i] = new Node[height];
		for (int j = 0; j < height; j++){
			// on initialise chaque noeud
			nodes[i][j].terminated = false;
			nodes[i][j].cases = matrix[i][j];
			nodes[i][j].position.setCoord(i,j);
			nodes[i][j].previous = NULL;
			nodes[i][j].initialised = false;
		}
	}
}

void MatrixPathFinder::setStencil(bool** stencil){
	this->stencil = stencil;
}

void MatrixPathFinder::enableStencil(bool use){
	this->useStencil = use;
}

bool MatrixPathFinder::isStencilEnabled(){
	return useStencil;
}

void MatrixPathFinder::setIncrementMap(float** matrix){
	incrementMap = matrix;
}

void MatrixPathFinder::enableIncrementMap(bool use){
	useIncrementMap = use;
}

bool MatrixPathFinder::isIncrementMapEnabled(){
	return useIncrementMap;
}

bool MatrixPathFinder::haveFoundPath(){
	return pathFound;
}
vector<Point2D> MatrixPathFinder::getPath(){
	return path;
}

void MatrixPathFinder::findPath(Point2D begin, Point2D end){
	// traitement de la validitée des points passé :
	// point de depart dans la matrice
	if (begin.getX() < 0 || begin.getX() >= width || begin.getY() < 0 || begin.getY() >= height){
		//fprintf(stderr,"point de depart d'un chemin non valide :{%d,%d}\n",(int)begin.getX(),(int)begin.getY());
		pathFound = false;
		path.clear();
		return;
	}
	// point d'arrivée dans la matrice
	if (end.getX() < 0 || end.getX() >= width || end.getY() < 0 || end.getY() >= height){
		//fprintf(stderr,"point d'arrivée d'un chemin non valide :{%d,%d}\n",(int)end.getX(),(int)end.getY());
		pathFound = false;
		path.clear();
		return;
	}
	// point de depart passable
	if (!matrix[(int)begin.getX()][(int)begin.getY()]->isPassable(matrix[(int)begin.getX()][(int)begin.getY()])){
		//fprintf(stderr,"point de depart d'un chemin non passable\n",(int)begin.getX(),(int)begin.getY());
		pathFound = false;
		path.clear();
		return;
	}
	// on ne traite pas le cas du point d'arrivée non passable car l'unitée doit pouvoir s'approcher le plus possible
	// de ce point.
	// on arrondi les coordonnée de end pour etre sur d'arriver sur le centre d'une case
	end.setCoord((int)(end.getX()+0.5f),(int)(end.getY()+0.5f));

	// on initialise la matrice de noeuds à traiter.
	for (int i = 0; i < width; i++){
		for (int j = 0; j <height; j++){
			nodes[i][j].terminated = false;
			nodes[i][j].previous = NULL;
			nodes[i][j].initialised = false;
		}
	}
	// on a analysé aucun point... initalisation a 0
	nbTerminated = 0;
	// on initalise les donnée du chemin
	path.clear();
	pathFound = false;
	// on recupere le point de depart.
	Node* depart = &nodes[(int)(begin.getX()+0.5f)][(int)(begin.getY()+0.5f)];
	// on initialise le point de depart
	depart->cost = 0.0f;
	depart->costWithoutHeuristic = 0.0f;
	// on vide la liste des points en attente
	opened.clear();
	// on ajoute le point de depart
	addNodeInOpenedList(depart);
	while (!opened.empty()){
		// on recupere le noeud de plus petit cout.
		Node* current = getLowestCostNode();
		// on a terminée l'analyse quand le point d'arrivée est dans l'ensemble des noeud
		// terminé.
		if (current->position == end){
			// on a trouvé un chemin
			pathFound = true;
			buildPath(current,begin);
			break;
		}else{
			// on continu la recherche
			// on prend tout les point autour du noeud courant
			for(int x=-1;x<=1;x++)
			{
				for(int y=-1;y<=1;y++)
				{
					// on ne veut pas traiter le point courant
					if(!(x==0 && y==0)){
						// on prend en compte le surcout des deplacement en diagonales
						float diagFactor = 1.0f;
						// les cas ou x+y = -2 ou 0 ou 2 sont les cases en diagonales
						if ((x+y) %2 == 0){
							// diagFactor = racine(2) car on est sur une carte de cases carré
							// donc le cout d'un deplacement en diagonale vaut racine(2)*cout autre.
							diagFactor = 1.44f;
						}
						// on genere la position du point à analyser
						int newX = (int)(current->position.getX() + x);
						int newY = (int)(current->position.getY() + y);
						// test pour savoir si on sort des limites de la map
						if(!(newX<0 || newX>=width || newY<0 || newY>=height)){
							// on regarde si la case est passable...
							bool passable;
							if (useStencil){
								// ...en utilisant le stancil
								passable = matrix[newX][newY]->isPassable(current->cases) && stencil[newX][newY];
							}else{
								// ... sans utiliser le stencil.
								passable = matrix[newX][newY]->isPassable(current->cases);
							}
							if (passable){
								// la case est passable
								Node* newNode = &nodes[newX][newY];
								// test si le noeud a deja été entierement traité.
								if (!nodes[newX][newY].terminated){
									// on calcule le cout temporaire
									float cost =	current->costWithoutHeuristic + 
													// cout de la case
													newNode->cases->getCost(current->cases)*diagFactor +
													// heuristique
													heuristic(current->position,
																newNode->position,
																end);
									// on calcule le cout sans heuristique temporaire
									float costWithoutHeuristic = current->costWithoutHeuristic +
										newNode->cases->getCost(current->cases)*diagFactor;
									// si on utilise une increment map, on augmente le cout de la case
									// en fonction de la matrice d'increment.
									if (useIncrementMap){
										cost += incrementMap[newX][newY];
									}
									// le noeud n'a pas encore été traité ?
									if (!newNode->initialised){
										// c'est la premiere fois qu'il est traité.
										// on initialise son pere
										newNode->previous = current;
										// on initialise les couts
										newNode->cost =	cost;
										newNode->costWithoutHeuristic = costWithoutHeuristic;
										// le noeud n'existait pas dans la liste : on l'ajoute
										addNodeInOpenedList(newNode);
										// le noeud est maintenant initilise
										newNode->initialised = true;
									}else{
										// le noeud a dejà été traité : on verifie si on doit le mettre a jour.
										// si le cout de la case est inferieur au cout precedent, on met a jour.
										if (cost < newNode->cost){
											// on met a jour :
											// - le cout
											newNode->cost = cost;
											// - le cout sans heuristique
											newNode->costWithoutHeuristic = costWithoutHeuristic;
											// - le pere de la case.
											newNode->previous = current;
										}
									}
								}
							}
						}
					}
				}
			}
		}
		// on signale que le noeud est terminé
		current->terminated = true;
		// on incremente le nombre de point terminé
		nbTerminated++;
	}
	// on a pas trouvé de chemin, on prend le chemin le plus proche.
	if (!pathFound){
		// recherche du premier point valide sur la droite entre les 2 extreminé du chemin
		float vx = begin.getX() - end.getX();
		float vy = begin.getY() - end.getY();
		// on normalise le vecteur
		float length = sqrt(vx*vx + vy*vy);
		vx /= length;
		vy /= length;
		// le point qui parcourt la droite
		float px = end.getX();
		float py = end.getY();
		int count = 0;
		while (count < length && nodes[(int)px][(int)py].previous == NULL){
			px += vx;
			py += vy;
			count++;
		}
		// ici on a soit trouvé un point valide, soit parcourut tout le vecteur...
		if (count <= length){
			// on a donc trouvé un point valide : on peut reconstruire un chemin
			buildPath(&nodes[(int)px][(int)py],begin);
		}else{
			// rien a faire ici, on a deja le point optimal en restant sur place...
		}
	}
	//printf("MatrixPathFinder::findPath() : nb points traité : %d\n",opened.size() + nbTerminated);
	// on nettoi la liste des noeud en attente de traitement.
	clearOpenedList();
}

MatrixPathFinder::Node * MatrixPathFinder::getLowestCostNode(){
	// on recupere le premier noeud de la liste
	multimap<float,Node*>::iterator it = opened.begin();
	// on le recupere car le erase invalide la valeure contenue dans it
	Node * ret = it->second;
	// on le supprime de la liste
	opened.erase(it);
	// et on le retourne pour etre traiter.
	return ret;

}

void MatrixPathFinder::addNodeInOpenedList(Node * node){
	// on inserte le noeud dans la map en fonction de son cout
	opened.insert(std::make_pair<float,Node*>(node->cost,node));
}

float MatrixPathFinder::heuristic(Point2D& current, Point2D& position, Point2D& destination){
	float currentToPosition[2];
	float currentToDestination[2];
	// on crée les 2 vecteurs
	currentToPosition[0] = position.getX() - current.getX();
	currentToPosition[1] = position.getY() - current.getY();
	currentToDestination[0] = destination.getX() - current.getX();
	currentToDestination[1] = destination.getY() - current.getY();
	// on normalise le premier
	float tempLength = sqrt(currentToPosition[0]*currentToPosition[0] + currentToPosition[1]*currentToPosition[1]);
	currentToPosition[0]/=tempLength;
	currentToPosition[1]/=tempLength;
	// on normalise le second. length represente aussi la distance euclidienne du chemin restant à parcourir
	float length = sqrt(currentToDestination[0]*currentToDestination[0] + currentToDestination[1]*currentToDestination[1]);
	currentToDestination[0]/=length;
	currentToDestination[1]/=length;

	// le produit scalaire entre les deux vecteurs normalisé. 
	float scalaire = 1.0f-((currentToPosition[0] * currentToDestination[0] + currentToPosition[1] * currentToDestination[1]));
	int v1 = (int)(abs(destination.getX() - position.getX()));
	int v2 = (int)(abs(destination.getY() - position.getY()));
	// on calcule le min de v1 et v2... pas de fonction standard en C pour effectuer ce genre
	// de calculs basiques... c'est naze
	int min;
	if (v1 <= v2){
		min = v1;
	}else{
		min = v2;
	}
	// min represente le deplacement en diagonale... il faut donc lui multiplier le cout par sqrt(2)
	float result = (min*1.44f) + fabs((float)(v1 - v2));
	return result + scalaire*2.0f;
}

void MatrixPathFinder::buildPath(Node * last, Point2D& start){
	// vecteur stockant le chemin a partir du point d'arrivée jusqu'au point de depart
	// (donc a l'envers)
	vector<Point2D> inversePath;
	Node * current = last;
	// on reconstruit le chemin a partir des noeuds.
	while(current->previous != NULL && !(current->position == start)){
		inversePath.push_back(current->position);
		current = current->previous;
	}
	// on ajoute le point de depart
	inversePath.push_back(current->position);
	// on parcourt le vecteur a l'envers pour reconstruire le chemin dans le bon sens
	for(MPFLIST<Point2D>::reverse_iterator it = inversePath.rbegin(); it != inversePath.rend(); ++it){
		path.push_back(*it);
	}
}

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

