#include "stdafx.h"
#include "AIRoutePlanner.h"
#include "AIGraphH.h"

// plans routes based on shortest path algorithm, and directs navigator to travel a path

unsigned short RPDistance[MAXNODE]; // distance for shortest path routine, the index coorisponds
								  // to the heap index

RPQUEUE RPQueue[MAXNODE];
unsigned short RPQueueStartI;
unsigned short RPQueueEndI;

unsigned short RPStack[MAXNODE];
short RPStackI;

extern NODE Heap[MAXNODE]; 

// the peak stack enum
short RPEnumI;
// the debug enum index
short RPEnumDI;


/************************************************************************
* RPReset(void)
*
* PURPOSE
* To initialize the shortest path algorithm
*************************************************************************/
void RPReset(void)
{
	unsigned short ix;
	for (ix = 0; ix < MAXNODE; ix++)
	{
		RPDistance[ix] = MAXDISTANCE; // max up the distances
		RPQueue[ix].CurrNode = 0; // zero the queue
		RPQueue[ix].PrevNode = 0; // zero the queue
		RPStack[ix] = 0; // zero the stack
	}

	RPQueueStartI = 0; // remove from
	RPQueueEndI = 0; // insert into
	RPStackI = 0;
}

/************************************************************************
* void RPInsertinQueue(unsigned short In)
*
* PURPOSE
* For the shortest path algorithm.  Inserts a node index into a queue
*************************************************************************/
BOOL RPInsertinQueue(unsigned short In, unsigned short Prev)
{
	unsigned short tEnter = RPQueueEndI;
	tEnter++;
	
	if (tEnter == MAXNODE)
		tEnter = 0;

	// queue full, should never really happen
	if (tEnter == RPQueueStartI)
		return FALSE;

	// O.K this is a valid memory space, insert into queue
	RPQueue[RPQueueEndI].CurrNode = In;
	RPQueue[RPQueueEndI].PrevNode = Prev;

	RPQueueEndI = tEnter;

	return TRUE;

}


/************************************************************************
* unsigned short RPRemovefromQueue(void)
*
* PURPOSE
* For the shortest path algorithm.  Removes a node index into a queue
*************************************************************************/
RPQUEUE RPRemovefromQueue(void)
{
	RPQUEUE tReturn;
	// check if queue is empty
	if (RPQueueStartI == RPQueueEndI)
	{
		tReturn.CurrNode = 0;
		tReturn.PrevNode = 0;
		return tReturn;
	}

	tReturn = RPQueue[RPQueueStartI];

	RPQueue[RPQueueStartI].CurrNode = 0; // clear the area
	RPQueue[RPQueueStartI].PrevNode = 0; // clear the area

	RPQueueStartI++;

	if (RPQueueStartI == MAXNODE)
		RPQueueStartI = 0;

	return tReturn;
}

/************************************************************************
* void RPGenerateShortestPath(unsigned short StartingNode, unsigned short EndingNode);
*
* PURPOSE
* Main calling function to generate the shortest path
*************************************************************************/
void RPGenerateShortestPath(unsigned short StartingNode, unsigned short EndingNode)
{
	RPReset();
	// generate the distances
	RPGenerateShortestPath1(StartingNode);
	// generate the stack
	RPGenerateShortestPath2(EndingNode);
}

/************************************************************************
* void RPGenerateShortestPath(unsigned short StartingNode)
*
* PURPOSE
* The shortest path algorithm stage 1.  Generates the shortest
* path distances from the starting node, to every other node linked to it.
*************************************************************************/
void RPGenerateShortestPath1(unsigned short StartingNode)
{
	RPQUEUE FromQueue;
	unsigned short CurrNode,PrevNode,DeltaX,DeltaY,tDistance;
	NODE *PrevNodePtr;
	BOOL DoneLink;
	// the distance to itself is zero.
	RPDistance[StartingNode] = 0;

	PrevNodePtr = &Heap[StartingNode];

	// add the first paths into the heap
	if (Heap[StartingNode].LinkE != NULL)
		RPInsertinQueue(Heap[StartingNode].LinkE,StartingNode);
	if (Heap[StartingNode].LinkW != NULL)
		RPInsertinQueue(Heap[StartingNode].LinkW,StartingNode);
	if (Heap[StartingNode].LinkS != NULL)
		RPInsertinQueue(Heap[StartingNode].LinkS,StartingNode);
	if (Heap[StartingNode].LinkN != NULL)
		RPInsertinQueue(Heap[StartingNode].LinkN,StartingNode);

	FromQueue = RPRemovefromQueue();

	// keep on going until the queue is empty
	while (FromQueue.CurrNode != 0)
	{		
		CurrNode = FromQueue.CurrNode;
		PrevNode = FromQueue.PrevNode;

		DoneLink = FALSE;

		while (DoneLink == FALSE)
		{

			if (Heap[CurrNode].NodeType == Landmark)
			{
				// Yes I know one is zero, but this is the fastest way to do it
				DeltaX = abs(Heap[CurrNode].Location.x - Heap[PrevNode].Location.x);
				DeltaY = abs(Heap[CurrNode].Location.y - Heap[PrevNode].Location.y);
				// landmark node, only on path through it, keep on going through the path
				tDistance = RPDistance[PrevNode] + DeltaX + DeltaY;

				// if the distance is greater then exit this search path,
				// else continue
				if (tDistance > RPDistance[CurrNode])
					DoneLink = TRUE;
				else
					RPDistance[CurrNode] = tDistance;				

				// figure out which node to go to next
				if (Heap[CurrNode].LinkE == PrevNode)
				{
					PrevNode = CurrNode;
					CurrNode = Heap[CurrNode].LinkW;
				}
				else if (Heap[CurrNode].LinkW == PrevNode)
				{
					PrevNode = CurrNode;
					CurrNode = Heap[CurrNode].LinkE;
				}
				else if (Heap[CurrNode].LinkN == PrevNode)
				{
					PrevNode = CurrNode;
					CurrNode = Heap[CurrNode].LinkS;
				}
				else if (Heap[CurrNode].LinkS == PrevNode)
				{
					PrevNode = CurrNode;
					CurrNode = Heap[CurrNode].LinkN;
				}
			}
			else if (Heap[CurrNode].NodeType == Decision)
			{
				// the current link is done regardless
				DoneLink = TRUE;

				// Yes I know one is zero, but this is the fastest way to do it
				DeltaX = abs(Heap[CurrNode].Location.x - Heap[PrevNode].Location.x);
				DeltaY = abs(Heap[CurrNode].Location.y - Heap[PrevNode].Location.y);
				// landmark node, only on path through it, keep on going through the path
				tDistance = RPDistance[PrevNode] + DeltaX + DeltaY;

				// if the distance is greater then exit this search path,
				// else add extra nodes to the queue
				if (tDistance > RPDistance[CurrNode])
				{
				}					
				else
				{
					RPDistance[CurrNode] = tDistance;
					// figure out which node to go to next
					if ((Heap[CurrNode].LinkE != PrevNode) && (Heap[CurrNode].LinkE != 0))
						RPInsertinQueue(Heap[CurrNode].LinkE,CurrNode);
					if ((Heap[CurrNode].LinkW != PrevNode)  && (Heap[CurrNode].LinkW != 0))
						RPInsertinQueue(Heap[CurrNode].LinkW,CurrNode);
					if ((Heap[CurrNode].LinkS != PrevNode)  && (Heap[CurrNode].LinkS != 0))
						RPInsertinQueue(Heap[CurrNode].LinkS,CurrNode);
					if ((Heap[CurrNode].LinkN != PrevNode)  && (Heap[CurrNode].LinkN != 0))
						RPInsertinQueue(Heap[CurrNode].LinkN,CurrNode);
				}
			}
			else if (Heap[CurrNode].NodeType == Unknown)
			{
				// add the distance, but then end this link
				DeltaX = abs(Heap[CurrNode].Location.x - Heap[PrevNode].Location.x);
				DeltaY = abs(Heap[CurrNode].Location.y - Heap[PrevNode].Location.y);

				tDistance = RPDistance[PrevNode] + DeltaX + DeltaY;

				if (tDistance < RPDistance[CurrNode])
				{
					RPDistance[CurrNode] = tDistance;
				}

				// the current link is done regardless
				DoneLink = TRUE;
			}
			else
				DoneLink = TRUE;

		}
		FromQueue = RPRemovefromQueue();

	}
}

/************************************************************************
* void RPGenerateShortestPath2(void)
*
* PURPOSE
* The shortest path algorithm stage 2.  Generates the shortest
* path from the previous calculated distances
*************************************************************************/
void RPGenerateShortestPath2(unsigned short EndingNode)
{
	unsigned short CurrDistance,CurrNode,tShortestDistance,tCurrNode;
	// might as well insert it.
	RPInsertinStack(EndingNode);
	CurrDistance = RPDistance[EndingNode];

	// no error condition, should never happen, signifies there is no path from this node
	// to the starting one.
	if (CurrDistance == MAXDISTANCE)
		return;

	CurrNode = EndingNode;

	while (RPDistance[CurrNode] != 0)
	{

		// now traverse the distance graph, to traverse the path.
		tShortestDistance = RPDistance[CurrNode];
		if (Heap[CurrNode].LinkE != NULL)
		{
			if (RPDistance[Heap[CurrNode].LinkE] < tShortestDistance)
			{			
				tCurrNode = Heap[CurrNode].LinkE;
				tShortestDistance = RPDistance[CurrNode];
			}
		}
		if (Heap[CurrNode].LinkW != NULL)
		{
			if (RPDistance[Heap[CurrNode].LinkW] < tShortestDistance)
			{				
				tCurrNode = Heap[CurrNode].LinkW;
				tShortestDistance = RPDistance[CurrNode];
			}
		}
		if (Heap[CurrNode].LinkS != NULL)
		{
			if (RPDistance[Heap[CurrNode].LinkS] < tShortestDistance)
			{
				tCurrNode = Heap[CurrNode].LinkS;
				tShortestDistance = RPDistance[CurrNode];
			}
		}
		if (Heap[CurrNode].LinkN != NULL)
		{
			if (RPDistance[Heap[CurrNode].LinkN] < tShortestDistance)
			{
				tCurrNode = Heap[CurrNode].LinkN;
				tShortestDistance = RPDistance[CurrNode];
			}
		}
		RPInsertinStack(tCurrNode);
		CurrNode = tCurrNode;
		CurrDistance = tShortestDistance;
	}
}

/************************************************************************
* RPInsertinStack(unsigned short In);
*
* PURPOSE
* To instert a node index, into the stack which describes the path of the shortest distance
*************************************************************************/
void RPInsertinStack(unsigned short In)
{
	// no error condition.  In practice should never happen
	if (RPStackI == MAXNODE)
		return;

	RPStack[RPStackI] = In;
	RPStackI++;
}

/************************************************************************
* RPSetupEnum(void)
*
* PURPOSE
* To enumerate the shortest distance stack
* USAGE
* To draw the path, for debugging purposes
*************************************************************************/
void RPSetupEnum(void)
{
	if (RPStackI > 0)
		RPEnumI = RPStackI-1;
	else
		RPEnumI = -1;

}

/************************************************************************
* BOOL RPEnum(unsigned short *Out)
*
* PURPOSE
* To enumerate the shortest distance stack
* USAGE
* To draw the path, for debugging purposes
*************************************************************************/
BOOL RPEnum(unsigned short *Out)
{
	if (RPEnumI == -1)
		return FALSE;

	*Out = RPStack[RPEnumI];
	RPEnumI--;
	return TRUE;	
}

/************************************************************************
* RPSetupEnumD(void)
*
* PURPOSE
* To enumerate the shortest distance stack
* USAGE
* To draw the path, for debugging purposes
*************************************************************************/
void RPSetupEnumD(void)
{
	if (RPStackI >= 0)
	{
		if (RPStack[RPStackI] != 0)
		{
			RPEnumDI = RPStackI;
		}
		else
			RPEnumDI = RPStackI-1;

	}
	else
		RPEnumDI = -1;

}

/************************************************************************
* BOOL RPEnum(unsigned short *Out)
*
* PURPOSE
* To enumerate the shortest distance stack
* USAGE
* To draw the path, for debugging purposes
*************************************************************************/
BOOL RPEnumD(unsigned short *Out)
{
	if (RPEnumDI == -1)
		return FALSE;

	*Out = RPStack[RPEnumDI];
	RPEnumDI--;
	return TRUE;	
}

/************************************************************************
* BOOL RPEnum(unsigned short *Out)
*
* PURPOSE
* Pop a node off the stack
*************************************************************************/
BOOL RPPopStack(unsigned short *Out)
{
	RPStackI--;

	if (RPStackI == -1)
		return FALSE;

	*Out = RPStack[RPStackI];
	
	return TRUE;	
}