#include "SquareNode.h"

#include <math.h>
#ifdef WIN32
#include <windows.h>
#endif
#include <GL/glut.h>
#include <GL/GL.h>
#include "Renderer.h"

#include "MemoryManager/debugNew.h"


using namespace ground;
using namespace std;

SquareNode::SquareNode(void)
{
	debugColor[0] = (rand()%255)/255.0f;
	debugColor[1] = (rand()%255)/255.0f;
	debugColor[2] = (rand()%255)/255.0f;
	size = 0.0f;
}

SquareNode::~SquareNode(void)
{
	while(!neighbour.empty()){
		SquareArc *arc = (SquareArc*)&(*neighbour.back());
		arc->detachExtremity();
		delete arc;
	}
}

std::vector<GraphArc*>& SquareNode::getNeighbours(){
	return neighbour;
}

void SquareNode::addArc(SquareArc* arc){
	neighbour.push_back(arc);
}

void SquareNode::removeArc(SquareArc* arc){
	GraphArc * pointer = arc;
	// on parcourt le vecteur
	for (vector<GraphArc*>::iterator it = neighbour.begin(); it != neighbour.end(); ++it){
		GraphArc * itArc = *it;
		// on a trouvé notre arc
		if (pointer == itArc){
			// on retire l'arc de la list.
			neighbour.erase(it);
			// et on sort immediatement.
			return;
		}
	}
}


SquareArc *  SquareNode::addNeighbour(SquareNode * newNeighbour){
	// on n'a pas deja ce neud dans notre list (pas de multi arc dans ce graph)
	if (!haveNeighbour(newNeighbour)){
		// on crée un nouvel arc
		SquareArc* newArc = new SquareArc(this,newNeighbour);
		// on l'ajoute a notre list de voisins
		addArc(newArc);
		// et on l'ajoute aussi à la list du nouveau voisin.
		newNeighbour->addArc(newArc);
		// on retourne le nouvel arc crée
		return newArc;
	}
	// le noeud est deja voisin : on retourn NULL
	return NULL;
}

bool SquareNode::haveNeighbour(SquareNode * node){
	// on parcourt tout les arc voisin
	for(vector<GraphArc*>::iterator it = neighbour.begin(); it != neighbour.end(); ++it){
		// on recupere les 2 extremités de l'arc
		SquareNode * gn1 = static_cast<SquareNode *>( &(*it)->getFirtNode() );
		SquareNode * gn2 = static_cast<SquareNode *>( &(*it)->getSecondNode() );
		// on regarde si le noeud passé en parametre est une des extremitée
		if (gn1 == node || gn2 == node){
			// c'est une extremitée : on retourne vrai
			return true;
		}
	}
	// on a jamais trouvé le noeud dans les voisin ? c'est donc qu'il n'est
	// pas voisin...
	return false;
}

void SquareNode::removeNeighbour(SquareNode * node){
	// on parcourt tout les arc voisin
	for(vector<GraphArc*>::iterator it = neighbour.begin(); it != neighbour.end(); ++it){
		// on recupere les 2 extremités de l'arc
		SquareNode * sn1 = static_cast<SquareNode *>( &(*it)->getFirtNode() );
		SquareNode * sn2 = static_cast<SquareNode *>( &(*it)->getSecondNode() );
		// on regarde si le noeud passé en parametre est une des extremitée
		if (sn1 == node || sn2 == node){
			// c'est une extremitée 
			SquareArc * arc = static_cast<SquareArc*>( (*it) );
			// on supprime l'arc des 2 extremité
			sn1->removeArc(arc);
			sn2->removeArc(arc);
			// puis on libere la memoire
			delete arc;
			// l'iterateur est invalidé !!!
			// on sort de la methode.
			return;
		}
	}
}

void SquareNode::setPosition(const Point2D& pos){
	position.setCoord(pos.getX(),pos.getY());
}

Point2D& SquareNode::getPosition(){
	return position;
}

void SquareNode::setSize(unsigned int s){
	size = s;
}

unsigned int SquareNode::getSize(){
	return size;
}

void SquareNode::setGroundType(GroundType type){
	this->type = type;
}

GroundType SquareNode::getGroundType(){
	return type;
}

float SquareNode::heuristic(GraphNode* previous, GraphNode* begin, GraphNode* end){
	//return 0.0f;
	// distance euclidienne
	SquareNode * prev = static_cast<SquareNode*>(previous);
	SquareNode * start = static_cast<SquareNode*>(begin);
	SquareNode * last = static_cast<SquareNode*>(end);
	float v1 = position.getX() - last->getPosition().getX();
	float v2 = position.getY() - last->getPosition().getY();
	float result =  sqrt(v1*v1 + v2*v2);
	return result;
}

float* SquareNode::getDebugColor(){
	return debugColor;
}

void SquareNode::drawDebug(int caseSize){
	// on affiche le centre de la case
	glColor3fv(debugColor);
	float posX = position.getX() * caseSize;
	float posY = RenderSystem::Renderer::getInstance().getElevation(position)+ 0.5f;
	float posZ = position.getY() * caseSize;
	float halfSize = ((size+1) / 2.0f)*caseSize;
	/*glPushMatrix();
		glTranslatef(posX,posY,posZ);
		glutSolidSphere(1,6,6);
	glPopMatrix();*/
	glBegin(GL_LINE_LOOP);
	float hauteur = RenderSystem::Renderer::getInstance().getElevationWorldCoord(posX - halfSize,posZ - halfSize)+ 0.5f;
	glVertex3f(posX - halfSize,hauteur,posZ - halfSize);
	hauteur = RenderSystem::Renderer::getInstance().getElevationWorldCoord(posX + halfSize,posZ - halfSize)+ 0.5f;
	glVertex3f(posX + halfSize,hauteur,posZ - halfSize);
	hauteur = RenderSystem::Renderer::getInstance().getElevationWorldCoord(posX + halfSize,posZ + halfSize)+ 0.5f;
	glVertex3f(posX + halfSize,hauteur,posZ + halfSize);
	hauteur = RenderSystem::Renderer::getInstance().getElevationWorldCoord(posX - halfSize,posZ + halfSize)+ 0.5f;
	glVertex3f(posX - halfSize,hauteur,posZ + halfSize);
	glEnd();
	glBegin(GL_LINES);
	for (vector<GraphArc*>::iterator it = neighbour.begin(); it != neighbour.end(); ++it){
		glColor3fv(debugColor);
		glVertex3f(posX,posY,posZ);
		SquareArc * arc = static_cast<SquareArc*>(*it);
		SquareNode* voisin = static_cast<SquareNode*>(arc->getOtherSide(this));
		hauteur = RenderSystem::Renderer::getInstance().getElevation(voisin->position)+ 0.5f;
		glColor3fv(voisin->debugColor);
		glVertex3f(voisin->position.getX()*caseSize,hauteur,voisin->position.getY()*caseSize);
	}
	glEnd();
	
}

