/*
		ROUTER PLUGIN
		- GPS ADDITION TO SA-MP
		- Made By Gamer_Z a.k.a. grasmanek94 , Rafal Grasman

		October-2011


		contact: [email protected]

		http://gpb.googlecode.com/
*/

#include "Graph.h"
#include <iostream>
#include <algorithm>
#include <vector>
#include <stack>

Graph::Graph(void)
{
}

Graph::~Graph(void)
{
	while(!mNodes.empty())
	{
		delete mNodes.back();
		mNodes.pop_back();
	}
}

void Graph::addNode(int name, bool exists, Node** NodeID )
{
	Node* pStart = NULL;
	mNodes.push_back(new Node(name,exists));
	std::vector<Node*>::iterator itr;
	itr = mNodes.begin()+mNodes.size()-1;
	pStart = (*itr);
	if(exists == true)pStart->DoesExist_yes();
	*NodeID = pStart;
}

void Graph::connect_oneway(Node* pFirst, Node* pSecond, int moveCost)
{
	if(pFirst != NULL && pSecond != NULL)
	{
		pFirst->createEdge(pSecond, moveCost);
	}
}

#define MAX_NODES (32768)
#define MAX_CONNECTIONS (5)
#include <time.h>

void Graph::findPath_r(Node* pStart, Node* pEnd, std::vector<cell> &road, cell &costMx)
{
	if(pStart == pEnd)
	{
		return;
	}

	std::vector<Node*> openList;
	openList.push_back(pStart);
	Node* pCurrNode = NULL;

	while(!openList.empty())
	{
		//Get best node from open list (lowest F value).
		//Since we sort the list at the end of the previous loop we know
		//the front node is the best
		pCurrNode = openList.front();

		//Exit if we're are the goal
		if(pCurrNode == pEnd)
			break;

		//Remove the node from the open list and place it in the closed
		openList.erase(openList.begin());
		pCurrNode->setClosed(true); //We use a flag instead of a list for speed
		//Test all of the edge nodes from the current node
		std::vector<Edge*>* pEdges = pCurrNode->getEdges();
		Node* pEdgeNode = NULL;
		for(std::vector<Edge*>::iterator i = pEdges->begin(); i != pEdges->end(); ++i)
		{
			pEdgeNode = (*i)->pNode;
			//If it's closed we've already analysed it
			if(!pEdgeNode->getClosed() && pCurrNode->DoesExist() == true)
			{
				if(!inList(pEdgeNode,&openList))
				{
					openList.push_back(pEdgeNode);
					pEdgeNode->setGCost(pCurrNode->getGCost()+(*i)->moveCost);
					pEdgeNode->calcFCost();
					pEdgeNode->setParent(pCurrNode);
				}
				else
				{
					//If this is a better node (lower G cost)
					if(pEdgeNode->getGCost() > pCurrNode->getGCost()+(*i)->moveCost)
					{
						pEdgeNode->setGCost(pCurrNode->getGCost()+(*i)->moveCost);
						pEdgeNode->calcFCost();
						pEdgeNode->setParent(pCurrNode);
					}
				}
			}
		}
		//Place the lowest F cost item in the open list at the top, so we can
		//access it easily next iteration
		std::sort(openList.begin(), openList.end(),  Graph::compareNodes);
	}
	//Make sure we actually found a path
	if(pEnd->getParent() != NULL)
	{
		//Output the path
		//Use a stack because it is LIFO
		std::stack<Node*> path;
		while(pCurrNode != NULL)
		{
			path.push(pCurrNode);
			pCurrNode = pCurrNode->getParent();
		}
		while(!path.empty())
		{
			road.push_back(path.top()->getName());
			costMx += path.top()->getGCost();
			path.pop();
		}
		return;
	}
	return;
}

bool Graph::inList(Node* pNode, std::vector<Node*>* pList)
{
	for(std::vector<Node*>::iterator i = pList->begin(); i != pList->end(); ++i)
	{
		if((*i) == pNode)
		{
			return true;
		}
	}

	return false;
}

bool Graph::compareNodes(Node* pFirst, Node* pSecond)
{
	return pFirst->getFCost() < pSecond->getFCost();
}

void Graph::reset(void)
{
	for(std::vector<Node*>::iterator i = mNodes.begin(); i != mNodes.end(); ++i)
	{
		(*i)->reset();
	}
}