#include "stdafx.h"
#include "AIGraph.h"
#include "ChildView.h"
#include "AICloseLoop.h"
#include "AIUtils.h"
#include "DefaultGraph.h"

NODE Heap[MAXNODE]; // the "heap" so to speak
unsigned short FreeHeapCount; // counter of how many free nodes there are
unsigned short FreeHeapIndex; // the index of the next free node

NodePtr CurrPtr; // current node 
NodePtr CurrDecisionPtr; // current decision node
NodePtr CloseLoopPtr; // a pointer to the fist node in the loop
char CloseLoopDirection; // because decision nodes don't store approaching direction
// information this variable has to do it.  For the close loop check

short CloseLoopMarker; // stores which loop it is on
GPOINT LastCloseLoopOffset;

unsigned short EnumIndex;

#ifdef CHEATPOSITION
	extern CChildView *GlobalPtr;	
#endif
		

/************************************************************************
* void GInitialize(void)
*
* PURPOSE
* To initialize the graph, should only be done in the start
*************************************************************************/
void GInitialize(void)
{
	unsigned short ix;

	// initialize the "heap"
	for (ix = 0; ix < MAXNODE; ix++)
	{
		Heap[ix].NodeType = Empty;
		Heap[ix].LinkN = ix+1;
		Heap[ix].Index = ix;
	}
	// the last one has a null link
	Heap[ix-1].LinkN = NULL;
	Heap[0].NodeType = NA; // the 0 can't be avialable
	FreeHeapCount = MAXNODE-1;
	CurrPtr = NULL; // 0 is not avialable so this means that is it starting
	CurrDecisionPtr = NULL; // 0 = not valid
	CloseLoopPtr = NULL;
	FreeHeapIndex = 1; // because 0 is not valid

	EnumIndex = 0;
}


/************************************************************************
* void GInitNode(NODE *Node);
*
* PURPOSE
* To initialize a node to nulls, used before adding information to the node
*************************************************************************/
void GInitNode(NODE *Node)
{
	Node->hasCloseLoopMarker = FALSE;
	Node->LandMarkE = RoCNone; 
	Node->LandMarkN = RoCNone;
	Node->LandMarkS = RoCNone;
	Node->LandMarkW = RoCNone;
	Node->LinkE = NULL;
	Node->LinkN = NULL;
	Node->LinkS = NULL;
	Node->LinkW = NULL;
	Node->Location.x = 0;
	Node->Location.y = 0;
	Node->Special = SPCNone;
}

/************************************************************************
* unsigned short GGetNewNode(void);
*
* PURPOSE
* Returns a new node index, 0 if none left.
*************************************************************************/
unsigned short GGetNewNode(void)
{
	unsigned short tIndex = FreeHeapIndex;
	if (FreeHeapCount == 0)
		tIndex = 0;

	FreeHeapCount--;
	// get the new index to be "free"
	FreeHeapIndex = Heap[FreeHeapIndex].LinkN;
	// intialize the node to all null so to speak
	GInitNode(&Heap[tIndex]);
	return tIndex;
}

/************************************************************************
* GDeleteNode(NODE *NodePtr)
*
* PURPOSE
* Deletes a node an adds it back to the heap
*************************************************************************/
void GDeleteNode(NODE *NodePtr)
{
	if (NodePtr == NULL)
	{
		return;
	}

	if (NodePtr->LinkE != NULL)
	{
		if (Heap[NodePtr->LinkE].LinkW == NodePtr->Index)
		{
			Heap[NodePtr->LinkE].LinkW = NULL;
		}
	}
	if (NodePtr->LinkW != NULL)
	{
		if (Heap[NodePtr->LinkW].LinkE == NodePtr->Index)
		{
			Heap[NodePtr->LinkW].LinkE = NULL;
		}
	}
	if (NodePtr->LinkS != NULL)
	{
		if (Heap[NodePtr->LinkS].LinkN == NodePtr->Index)
		{
			Heap[NodePtr->LinkS].LinkN = NULL;
		}
	}
	if (NodePtr->LinkN != NULL)
	{
		if (Heap[NodePtr->LinkN].LinkS == NodePtr->Index)
		{
			Heap[NodePtr->LinkN].LinkS = NULL;
		}
	}

	// now clear the node
	GInitNode(NodePtr);

	// add the node to the heap
	FreeHeapCount++;
	// add it to the front of the heap, because it is much easier to do this
	// then adding it to the back.
	NodePtr->LinkN = Heap[FreeHeapIndex].Index;
	FreeHeapIndex = NodePtr->Index;
	// delete any references if applicable
}

/************************************************************************
* BOOL AddLandmarkNode(char Direction, LANDMARK Landmark, short RoC, GPOINT Displacement)
*
* PURPOSE
* To Add a landmark node
* Returns false if out of heap pointers
* ALGORITHM
* 1) Obtain a new node
* 2) Add the Landmark informatio to the new node;
* 3) Add the new position to the new node;
* 4) Add links from the old node to/from the new one.
*************************************************************************/
BOOL GAddLandmarkNode(char TDirection, char LDirection, LANDMARK LandmarkType, short RoC, short Displacement)
{
	unsigned short NewIndex;
	NodePtr newNode,oldNode;
	short *LandmarkPtr;
	BOOL Reverse;

	if (FreeHeapCount == 0)
		return FALSE;

	
	if (CurrPtr == NULL)
		CurrPtr = CurrDecisionPtr;

	// error
	if (CurrPtr == NULL)
		return FALSE;	


	// 1) get a new node index;
	NewIndex = GGetNewNode();
	newNode = &Heap[NewIndex];
	newNode->NodeType = Landmark;


	oldNode = CurrPtr;


	// add information to node

	// 2) first get the pointer of the landmark indicator
//	if (((TDirection & North) != 0) || ((TDirection & East) != 0))
//	{
		if ((LDirection & North) != 0)
		{
			LandmarkPtr = &newNode->LandMarkN;
		}
		else if ((LDirection & East) != 0)
		{
			LandmarkPtr = &newNode->LandMarkE;
		}
		else if ((LDirection & South) != 0)
		{
			LandmarkPtr = &newNode->LandMarkS;
		}
		else if ((LDirection & West) != 0)
		{
			LandmarkPtr = &newNode->LandMarkW;
		}
		else
			return FALSE;
		Reverse = FALSE;
/*	} // the calling module generates the right vector no matter what.
	else // either south or west reverse landmark direction
	{
		char tempo = TDirection & East;
		if ((LDirection & North) != 0)
		{
			LandmarkPtr = &newNode->LandMarkS;
		}
		else if ((LDirection & East) != 0)
		{
			LandmarkPtr = &newNode->LandMarkW;
		}
		else if ((LDirection & South) != 0)
		{
			LandmarkPtr = &newNode->LandMarkN;
		}
		else if ((LDirection & West) != 0)
		{
			LandmarkPtr = &newNode->LandMarkE;
		}
		else
			return FALSE;
		
		Reverse = TRUE;
	}*/

	// now add the landmark information
	if (Reverse == FALSE)
	{
		if (LandmarkType == LostDetection)
			*LandmarkPtr = RoCLostDetection;
		else if (LandmarkType == GainDetection)
			*LandmarkPtr = RoCGainDetection;
		else
			*LandmarkPtr = RoC;
	}
	else // reverse == true
	{
		if (LandmarkType == LostDetection)
			*LandmarkPtr = RoCGainDetection;
		else if (LandmarkType == GainDetection)
			*LandmarkPtr = RoCLostDetection;
		else
			*LandmarkPtr = -RoC;
	}

	// 3) now adds the nodes position
	if (CurrDecisionPtr == NULL)
	{		
		newNode->Location = oldNode->Location;		
	}
	else
	{
		newNode->Location = CurrDecisionPtr->Location;		
	}
	if ((TDirection & North) != 0)
	{
		newNode->Location.y -= Displacement;
	}
	else if ((TDirection & East) != 0)
	{
		newNode->Location.x += Displacement;
	}
	else if ((TDirection & South) != 0)
	{
		newNode->Location.y += Displacement;
	}
	else if ((TDirection & West) != 0)
	{
		newNode->Location.x -= Displacement;
	}

	#ifdef CHEATPOSITION
		newNode->Location = GlobalPtr->GlobalGetPosition();		
	#endif


	// 4) link this node to the last one
	if (CurrDecisionPtr == NULL)
	{
		// simple add, no decision node to look at
		GLinkNodes(oldNode,newNode,TDirection);
	}
	else
	{
		// slightly more complicated, because of a decision node, need to update decision 
		// information
		oldNode = CurrDecisionPtr;
		// link the old node
		GLinkNodes(oldNode,newNode,TDirection);	

		CurrDecisionPtr = NULL;

	}
	CurrPtr = newNode;
	// add it to the stack, for loop closing
	CLAddtoStack(CurrPtr->Index);
	return TRUE;
}

/************************************************************************
* BOOL GLinkNodes(NODE *Node1, NODE *Node2, char Direction);
*
* PURPOSE
* To link two nodes together
* ALGORITHM
* 1) If there isn't any nodes already linked in the direction link the two together
* 2) If there are nodes linked together, break the links.
*************************************************************************/
void GLinkNodes(NODE *oldNode, NODE *newNode, char TDirection)
{
	NODE *tNodePtr;
	char tempDirection;
	BOOL isFourPointsJ;


	// special case, the close loop markers have to be the same otherwise make a dummy
	// node
	if ((oldNode->NodeType == Decision) && (newNode->NodeType == Landmark))
	{
		// not equal, will have to clone old node
		if (oldNode->hasCloseLoopMarker != CloseLoopMarker)
		{
			tNodePtr = oldNode;

			// reverse the direction for the clone node function
			if (TDirection == North)
				tempDirection = South;
			else if (TDirection == South)
				tempDirection = North;
			else if (TDirection == East)
				tempDirection = West;
			else if (TDirection == West)
				tempDirection = East;
			oldNode = GCloneDecisionNode(oldNode,tempDirection,25, TRUE ,&isFourPointsJ);
		
			if (isFourPointsJ == TRUE)
			{
				// Readjust the landmark node, because the 4 point junction forced the decision
				// node to align to the other 3 decision points, and the landmark node may be in the wrong place.
				if (TDirection == North)
					newNode->Location.y = oldNode->Location.y - 25;
				else if (TDirection == South)			
					newNode->Location.y = oldNode->Location.y + 25;
				else if (TDirection == East)			
					newNode->Location.x = oldNode->Location.x + 25;
				else if (TDirection == West)			
					newNode->Location.x = oldNode->Location.x - 25;

			}
			else
			{

				// make its location a standard 25 ticks away from the newnode
				if (TDirection == North)			
					oldNode->Location.y = newNode->Location.y + 25;
				else if (TDirection == South)			
					oldNode->Location.y = newNode->Location.y - 25;
				else if (TDirection == East)			
					oldNode->Location.x = newNode->Location.x - 25;
				else if (TDirection == West)			
					oldNode->Location.x = newNode->Location.x + 25;
			}

			LastCloseLoopOffset.x = tNodePtr->Location.x - oldNode->Location.x;
			LastCloseLoopOffset.y = tNodePtr->Location.y - oldNode->Location.y;

			
			// reset close loop list
			CLCreateCloseLoopList();

		}
	}


	if ((TDirection & North) != 0)
	{
		if (oldNode->LinkN == NULL)
		{
			oldNode->LinkN = newNode->Index;
			newNode->LinkS = oldNode->Index;
		}
		else if ((oldNode->NodeType == Decision) && (newNode->NodeType == Decision)
			&& (Heap[oldNode->LinkS].NodeType == Unknown))
		{		
			// this happens when closing a loop, the unknown node should be deleted
			GDeleteNode(&Heap[oldNode->LinkS]);
			oldNode->LinkN = newNode->Index;
			newNode->LinkS = oldNode->Index;
	
		}
		else 
		{
			// have to also link the node that is broken
			tNodePtr = &Heap[oldNode->LinkN];
			oldNode->LinkN = newNode->Index;
			newNode->LinkS = oldNode->Index;
			tNodePtr->LinkS = newNode->Index;
			newNode->LinkN = tNodePtr->Index;
		}
		// adjust the decision nodes, remaining possibilites.
		if (oldNode->NodeType == Decision)
		{
			oldNode->LandMarkN = SNRConnection;
		}
	}
	else if ((TDirection & East) != 0)
	{
		if (oldNode->LinkE == NULL)
		{
			oldNode->LinkE = newNode->Index;
			newNode->LinkW = oldNode->Index;
		}
		else if ((oldNode->NodeType == Decision) && (newNode->NodeType == Decision)
			&& (Heap[oldNode->LinkW].NodeType == Unknown))
		{		
			// this happens when closing a loop, the unknown node should be deleted
			GDeleteNode(&Heap[oldNode->LinkW]);
			oldNode->LinkE = newNode->Index;
			newNode->LinkW = oldNode->Index;
	
		}
		else 
		{
			// have to also link the node that is broken
			tNodePtr = &Heap[oldNode->LinkE];
			oldNode->LinkE = newNode->Index;
			newNode->LinkW = oldNode->Index;
			tNodePtr->LinkW = newNode->Index;
			newNode->LinkE = tNodePtr->Index;
		}
		// adjust the decision nodes, remaining possibilites.
		if (oldNode->NodeType == Decision)
		{
			oldNode->LandMarkE = SNRConnection;
		}
	}
	else if ((TDirection & South) != 0)
	{
		if (oldNode->LinkS == NULL)
		{
			oldNode->LinkS = newNode->Index;
			newNode->LinkN = oldNode->Index;
		}
		else if ((oldNode->NodeType == Decision) && (newNode->NodeType == Decision)
			&& (Heap[oldNode->LinkN].NodeType == Unknown))
		{		
			// this happens when closing a loop, the unknown node should be deleted
			GDeleteNode(&Heap[oldNode->LinkN]);
			oldNode->LinkS = newNode->Index;
			newNode->LinkN = oldNode->Index;
	
		}
		else 
		{
			// have to also link the node that is broken
			tNodePtr = &Heap[oldNode->LinkS];
			oldNode->LinkS = newNode->Index;
			newNode->LinkN = oldNode->Index;
			tNodePtr->LinkN = newNode->Index;
			newNode->LinkS = tNodePtr->Index;
		}
		// adjust the decision nodes, remaining possibilites.
		if (oldNode->NodeType == Decision)
		{
			oldNode->LandMarkS = SNRConnection;
		}
	}
	else if ((TDirection & West) != 0)
	{
		if (oldNode->LinkW == NULL)
		{
			oldNode->LinkW = newNode->Index;
			newNode->LinkE = oldNode->Index;
		}
		else if ((oldNode->NodeType == Decision) && (newNode->NodeType == Decision)
			&& (Heap[oldNode->LinkE].NodeType == Unknown))
		{		
			// this happens when closing a loop, the unknown node should be deleted
			GDeleteNode(&Heap[oldNode->LinkE]);
			oldNode->LinkW = newNode->Index;
			newNode->LinkE = oldNode->Index;
	
		}
		else 
		{
			// have to also link the node that is broken
			tNodePtr = &Heap[oldNode->LinkW];
			oldNode->LinkW = newNode->Index;
			newNode->LinkE = oldNode->Index;
			tNodePtr->LinkE = newNode->Index;
			newNode->LinkW = tNodePtr->Index;
		}
		// adjust the decision nodes, remaining possibilites.
		if (oldNode->NodeType == Decision)
		{
			oldNode->LandMarkW = SNRConnection;
		}
	}
}

/************************************************************************
* BOOL AddSpecialNode(SPECIAL SpecialType, char Direction);
*
* PURPOSE
* To Add a special node decision node
* Returns false if out of heap pointers
* NOTES
* Current types support,
* Intial node (the first one)
*************************************************************************/
BOOL GAddSpecialNode(SPECIAL SpecialType, char Direction)
{

	if (SpecialType == StartingPoint)
	{
	
		if (CurrPtr == NULL)
		{			 
			CurrPtr = &Heap[GGetNewNode()];
			CurrPtr->Special = StartingPoint;
			CurrPtr->Location.x = 0;
			CurrPtr->Location.y = 0;
			#ifdef CHEATPOSITION
				GlobalPtr->GlobalSetZero();
			#endif
			CurrPtr->hasCloseLoopMarker = TRUE;
			CurrPtr->NodeType = Special;

		}
		else
			return FALSE;
	}

	return FALSE;

}
/************************************************************************
* BOOL GAddDecisionNode(char Possibilities, short Possibilities[4], GPOINT Location, BOOL isWallDecisionNode)
*
* PURPOSE
* To Add a decision node
* Returns false if out of heap pointers
* ALGORITHM
* 1) Get a node node pointer
* 2) Configure it as a decision node
* 3) Add decision node index to decision node index;
* INPUT
* Possibilities 0 is north
*				1 is east
*				2 is south
*				3 is west
* BOOL CloseLoopNode, if this variable is true, then a link
* to this node is generated, so it can be compared, with input nodes, to see if
* a loop has been closed
* NOTE
* The decision node is linked when the next landmark node is created.  This means
* that two decision nodes can't be linked together, a landmark has to be inbetween
*************************************************************************/
BOOL GAddDecisionNode(char TDirection, short Possibilities[4], short Location, BOOL isWallDecisionNode)
{
	unsigned short NewIndex;
	NodePtr newNode,oldNode;

	// pick the correct ptr
	if (CurrDecisionPtr == NULL)
		oldNode = CurrPtr;
	else
		oldNode = CurrDecisionPtr;

	if (FreeHeapCount == 0)
		return FALSE;

	// 1) get a new node index;
	NewIndex = GGetNewNode();
	newNode = &Heap[NewIndex];

	if (isWallDecisionNode == TRUE)
		newNode->Special = AtWallDecision;

	newNode->NodeType = Decision;
	newNode->hasCloseLoopMarker = CloseLoopMarker; // null the marker

	// special case, happens if this is the first node entered
	if (oldNode == NULL)
	{
		newNode->Location.y = 0;
		newNode->Location.x = 0;
	}
	else
	{


		// add its location
		newNode->Location = oldNode->Location;
		if ((TDirection & North) != 0)
		{
			newNode->Location.y -= Location;
		}
		else if ((TDirection & East) != 0)
		{
			newNode->Location.x += Location;
		}
		else if ((TDirection & South) != 0)
		{
			newNode->Location.y += Location;
		}
		else if ((TDirection & West) != 0)
		{
			newNode->Location.x -= Location;
		}
	}

	#ifdef CHEATPOSITION
		newNode->Location = GlobalPtr->GlobalGetPosition();
	#endif

	// fill in the possibilites information
	newNode->LandMarkN = Possibilities[0];
	newNode->LandMarkE = Possibilities[1];
	newNode->LandMarkS = Possibilities[2];
	newNode->LandMarkW = Possibilities[3];

	if (newNode->LandMarkN == SNRUnknown)
	{
		GAddUnknownNode(North,newNode);
	}
	if (newNode->LandMarkE == SNRUnknown)
	{
		GAddUnknownNode(East,newNode);
	}
	if (newNode->LandMarkS == SNRUnknown)
	{
		GAddUnknownNode(South,newNode);
	}
	if (newNode->LandMarkW == SNRUnknown)
	{
		GAddUnknownNode(West,newNode);
	}


	// in any of them are an unknown add a unknown node link in that direction
	// starting case
	if (oldNode != NULL)
	{

		// like it with the past node
		if ((TDirection & North) != 0)
		{
			oldNode->LinkN = newNode->Index;
			newNode->LinkS = oldNode->Index;
			newNode->LandMarkS = FALSE;
		}
		else if ((TDirection & East) != 0)
		{
			oldNode->LinkE = newNode->Index;
			newNode->LinkW = oldNode->Index;
			newNode->LandMarkW = FALSE;
		}
		else if ((TDirection & South) != 0)
		{
			oldNode->LinkS = newNode->Index;
			newNode->LinkN = oldNode->Index;
			newNode->LandMarkN = FALSE;
		}
		else if ((TDirection & West) != 0)
		{
			oldNode->LinkW = newNode->Index;
			newNode->LinkE = oldNode->Index;
			newNode->LandMarkE = FALSE;
		}
	}

	// link it globally
	CurrDecisionPtr = newNode;
	// add it to the stack, for loop closing
	CLAddtoStack(CurrDecisionPtr->Index);

	return TRUE;

}

/************************************************************************
* void GAddPossibilities(tNodePtr,Possibilites);
*
* PURPOSE
* used in the clone node routine, to still
* input unknown area information
*************************************************************************/
void GAddPossibilities(NODE *tNodePtr,short Possibilities[4])
{
	if (tNodePtr == NULL)
		return;
	if (tNodePtr->NodeType != Decision)
		return;

	// fill in the possibilites information
	
	tNodePtr->LandMarkN = Possibilities[0];
	tNodePtr->LandMarkE = Possibilities[1];
	tNodePtr->LandMarkS = Possibilities[2];
	tNodePtr->LandMarkW = Possibilities[3];

	if ((tNodePtr->LandMarkN == SNRUnknown) && (tNodePtr->LinkN == NULL))
	{
		GAddUnknownNode(North,tNodePtr);
	}
	if ((tNodePtr->LandMarkE == SNRUnknown) && (tNodePtr->LinkE == NULL))
	{
		GAddUnknownNode(East,tNodePtr);
	}
	if ((tNodePtr->LandMarkS == SNRUnknown) && (tNodePtr->LinkS == NULL))
	{
		GAddUnknownNode(South,tNodePtr);
	}
	if ((tNodePtr->LandMarkW == SNRUnknown) && (tNodePtr->LinkW == NULL))
	{
		GAddUnknownNode(West,tNodePtr);
	}
}



/************************************************************************
* void GAddUnknownNode(char Direction, NODE *DecisionNode)
*
* PURPOSE
* Appends an unknown node to a decision node, given a direction and the 
* decision node;
*************************************************************************/
BOOL GAddUnknownNode(char Direction, NODE *DecisionNode)
{
	NodePtr newNode;
	unsigned short NewIndex;

	if (FreeHeapCount == 0)
		return FALSE;

	// 1) get a new node index;
	NewIndex = GGetNewNode();
	newNode = &Heap[NewIndex];

	newNode->NodeType = Unknown;

	newNode->Location = DecisionNode->Location;
	if (Direction == North)
	{
		newNode->Location.y -= UNKNOWNDISTANCECONSTANT;
		newNode->LinkS = DecisionNode->Index;
		DecisionNode->LinkN = newNode->Index;
	}
	if (Direction == East)
	{
		newNode->Location.x += UNKNOWNDISTANCECONSTANT;
		newNode->LinkW = DecisionNode->Index;
		DecisionNode->LinkE = newNode->Index;
	}
	if (Direction == South)
	{
		newNode->Location.y += UNKNOWNDISTANCECONSTANT;
		newNode->LinkN = DecisionNode->Index;
		DecisionNode->LinkS = newNode->Index;
	}
	if (Direction == West)
	{
		newNode->Location.x -= UNKNOWNDISTANCECONSTANT;
		newNode->LinkE = DecisionNode->Index;
		DecisionNode->LinkW = newNode->Index;
	}



	return TRUE;
}

/************************************************************************
* BOOL GIsLoopClosed(char TDirection, short Possibilities[4], short Location)
*
* PURPOSE
* Checks if the decision node about to be placed on the graph, matches
* up with the one designated as the close loop decision node;
* Return true if it does match up, in terms of traveling directions and possibilities
* NOTE
* Later one if neccessary the location (x,y) could also be used to check if
* the loop is closed.
* Could also use some fuzzy logic too, like if 2/3 cases match then close loop
*************************************************************************/
BOOL GIsLoopClosed(char TDirection, short Possibilities[4], short Location)
{
	GPOINT Position;

	if (CurrDecisionPtr != NULL)
		Position = CurrDecisionPtr->Location;
	else if (CurrPtr != NULL)
		Position = CurrPtr->Location;
	else
		return FALSE;

	if (TDirection == North)
		Position.y -= Location;
	else if (TDirection == South)
		Position.y += Location;
	else if (TDirection == East)
		Position.x += Location;
	else if (TDirection == West)
		Position.x -= Location;

	return CLIsLoopClosed(TDirection,Possibilities,Position);
		
}

/************************************************************************
* void GSetupEnum(void)
*
* PURPOSE
* Sets up a graph enumuration (an output off all of its information).
*************************************************************************/
void GSetupEnum(void)
{
	EnumIndex = 1;
}; 

/************************************************************************
* void GRollBackEnum(void)
*
* PURPOSE
* Rolls back the enum by one in case the message was lost
*************************************************************************/
void GRollBackEnum(void)
{
	if (EnumIndex > 1)
		EnumIndex--;	
}

/************************************************************************
* BOOL BOOL GEnumGraph(char *ptr);;
*
* PURPOSE
* To enumerate the graph.  If returns true the information is valid, if false
* the enum is complete.
*************************************************************************/
BOOL GEnumGraph(char *ptr)
{
	// note differences in two byte standards
	unsigned short ix;

	if (EnumIndex == 0)
		return FALSE;

	for (ix = EnumIndex; (ix != 0 && ix < MAXNODE); ix++)
	{
		if (Heap[ix].NodeType != Empty)
		{
			// output information			
			GEnumAppend(ptr, Heap[ix].Index);
			ptr+=2;
			GEnumAppend(ptr, Heap[ix].hasCloseLoopMarker);
			ptr+=2;
			GEnumAppend(ptr, Heap[ix].LandMarkE);
			ptr+=2;
			GEnumAppend(ptr, Heap[ix].LandMarkN);
			ptr+=2;
			GEnumAppend(ptr, Heap[ix].LandMarkS);
			ptr+=2;
			GEnumAppend(ptr, Heap[ix].LandMarkW);
			ptr+=2;
			GEnumAppend(ptr, Heap[ix].LinkE);
			ptr+=2;
			GEnumAppend(ptr, Heap[ix].LinkN);
			ptr+=2;
			GEnumAppend(ptr, Heap[ix].LinkS);
			ptr+=2;
			GEnumAppend(ptr, Heap[ix].LinkW);
			ptr+=2;
			GEnumAppend(ptr, Heap[ix].Location.x);
			ptr+=2;
			GEnumAppend(ptr, Heap[ix].Location.y);
			ptr+=2;
			GEnumAppend(ptr, Heap[ix].NodeType);
			ptr+=2;
			GEnumAppend(ptr, Heap[ix].Special);

			EnumIndex = ix+1;
			return TRUE;
		}
	}

	return FALSE;	
}

/************************************************************************
* void  GEnumAppend(char *ptr, short Input)
*
* PURPOSE
* Copy a short byte wise into a char array
*************************************************************************/
void GEnumAppend(char *ptr, short Input) // file serverices
{
	unsigned char *ptr1 = &(unsigned char &)(Input);

	// copy over the two bytes
	*ptr = *ptr1;
	ptr++;
	ptr1++;
	*ptr = *ptr1;
	ptr++;
	ptr1++;

}

/************************************************************************
* void  GEnumAppend(char *ptr, short *Input)
*
* PURPOSE
* Extract a short for a char array
*************************************************************************/
void GEnumAppendR(char *ptr, short *Input)
{
	unsigned char *ptr1 = &(unsigned char &)(*Input);
	
	// copy over the two bytes
	*ptr1 = *ptr;
	ptr++;
	ptr1++;
	*ptr1 = *ptr;
}

/************************************************************************
* void  GEnumAppend(char *ptr, short *Input)
*
* PURPOSE
* Extract a short for a char array
*************************************************************************/
void GEnumAppendRU(char *ptr, unsigned short *Input)
{
	unsigned char *ptr1 = &(unsigned char &)(*Input);
	
	// copy over the two bytes
	*ptr1 = *ptr;
	ptr++;
	ptr1++;
	*ptr1 = *ptr;
}

/************************************************************************
* void GSendCurrNode(unsigned short CurrNodeIndex)
*
* PURPOSE
* Sends over the current node index, for display purposes
*************************************************************************/
unsigned short GSendCurrNode(void)
{
	if (CurrDecisionPtr == NULL)
		if (CurrPtr != NULL)
			return CurrPtr->Index;
		else
			return 0;
	else
		return CurrDecisionPtr->Index;
}

/************************************************************************
* void GSetupLoad(void)
*
* PURPOSE
* sets up a graph load
*************************************************************************/
void GSetupLoad(void)
{
	GInitialize();
}

/************************************************************************
* void GLoadGraph(char *ptr)
*
* PURPOSE
* Loads a node into the graph
*************************************************************************/
void GLoadGraph(char *ptr)
{
	// output information	
	unsigned short Index;

	// enter the information
	GEnumAppendRU(ptr,&Index);
	ptr+=2;
	GEnumAppendR(ptr, &(short &)Heap[Index].hasCloseLoopMarker);
	Heap[Index].hasCloseLoopMarker = 0; // should be set to zero, as not to interfere
	// with any new mappings
	ptr+=2;
	GEnumAppendR(ptr, &Heap[Index].LandMarkE);
	ptr+=2;
	GEnumAppendR(ptr, &Heap[Index].LandMarkN);
	ptr+=2;
	GEnumAppendR(ptr, &Heap[Index].LandMarkS);
	ptr+=2;
	GEnumAppendR(ptr, &Heap[Index].LandMarkW);
	ptr+=2;
	GEnumAppendRU(ptr, &Heap[Index].LinkE);
	ptr+=2;
	GEnumAppendRU(ptr, &Heap[Index].LinkN);
	ptr+=2;
	GEnumAppendRU(ptr, &Heap[Index].LinkS);
	ptr+=2;
	GEnumAppendRU(ptr, &Heap[Index].LinkW);
	ptr+=2;
	GEnumAppendR(ptr, &Heap[Index].Location.x);
	ptr+=2;
	GEnumAppendR(ptr, &Heap[Index].Location.y);
	ptr+=2;
	GEnumAppendR(ptr, &(short &)Heap[Index].NodeType);
	ptr+=2;
	GEnumAppendR(ptr, &(short &)Heap[Index].Special);	

}

/************************************************************************
* void GLoadStatus(char *ptr) 
*
* PURPOSE
* loads the four status shorts
*************************************************************************/
void GLoadStatus(char *ptr) 
{
	unsigned short Temp;

	GEnumAppendRU(ptr,&Temp);
	FreeHeapCount = Temp;
	ptr+=2;

	GEnumAppendRU(ptr,&Temp);
	FreeHeapIndex = Temp;
	ptr+=2;

	GEnumAppendRU(ptr,&Temp);
	if (Temp = 0)
		CurrPtr = NULL;
	else
		CurrPtr = &Heap[Temp];
	ptr+=2;

	GEnumAppendRU(ptr,&Temp);
	if (Temp = 0)
		CurrDecisionPtr = NULL;
	else 
		CurrDecisionPtr = &Heap[Temp];

}

/************************************************************************
* void GLoadStatus(char *ptr) 
*
* PURPOSE
* gets the four current state shorts
*************************************************************************/
void GEnumStatus(char *ptr)
{
	GEnumAppend(ptr, FreeHeapCount);
	ptr+=2;

	GEnumAppend(ptr, FreeHeapIndex);
	ptr+=2;

	if (CurrPtr == NULL)
		GEnumAppend(ptr, 0);
	else
		GEnumAppend(ptr, CurrPtr->Index);
	ptr+=2;

	if (CurrDecisionPtr == NULL)
		GEnumAppend(ptr, 0);
	else
		GEnumAppend(ptr, CurrDecisionPtr->Index);

}

/************************************************************************
* void GLoadStatus4(char *ptr) 
*
* PURPOSE
* gets the four current state shorts
*************************************************************************/
void GEnumStatus4(short Status[4])
{
	Status[0] = FreeHeapCount;
	Status[1] = FreeHeapIndex;

	
	if (CurrPtr == NULL)
		Status[2] = 0;
	else
		Status[2] = CurrPtr->Index;

	if (CurrDecisionPtr == NULL)
		Status[3] = 0;
	else
		Status[3] = CurrDecisionPtr->Index;
	
}

/************************************************************************
* void GCloseLoop(char TDirection, short Location)
*
* PURPOSE
* Closes the current loop, actually just gets position and calls
* another function
*************************************************************************/
void GCloseLoop(char TDirection, short Location)
{
	GPOINT Position;
	NODE *oldNode;

	if (CurrDecisionPtr == NULL)
		oldNode = CurrPtr;
	else
		oldNode = CurrDecisionPtr;

	// should never happen
	if (oldNode == NULL)
		return;

	// add its location
	Position = oldNode->Location;
	if ((TDirection & North) != 0)
	{
		Position.y -= Location;
	}
	else if ((TDirection & East) != 0)
	{
		Position.x += Location;
	}
	else if ((TDirection & South) != 0)
	{
		Position.y += Location;
	}
	else if ((TDirection & West) != 0)
	{
		Position.x -= Location;
	}

	// call the function that does all the real work
	CurrDecisionPtr = CLCloseLoop(TDirection, Position);
	// clear the loop stack for next time
	CLClearStack();

	if (GIsDoneBigLoop(CurrDecisionPtr) == FALSE)
	{
		// added to the stack because we have to keep on going
		CLAddtoStack(CurrDecisionPtr->Index);
	}

}

/************************************************************************
* void GGetCurrNode(void)
*
* PURPOSE
* Returns the current node, as a pointer
*************************************************************************/
NODE *GGetCurrNode(void)
{
	if (CurrDecisionPtr != NULL)
		return CurrDecisionPtr;
	else
		return CurrPtr;
}

/************************************************************************
* void GGetFirstNode(void)
*
* PURPOSE
* Returns the first node in the heap
*************************************************************************/
NODE *GGetFirstNode(void)
{
	if (Heap[1].NodeType != Empty)
	{
		return &Heap[1];
	}
	else
		return NULL;

}

/************************************************************************
* void GGetCurrNode(void)
*
* PURPOSE
* Returns the first node in the heap
*************************************************************************/
BOOL GSetCurrNode(unsigned short Node) // sets the current node
{
	if (Node >= MAXNODE)
		return FALSE;
	else if (Heap[Node].NodeType == Empty)
		return FALSE;
	else if (Heap[Node].NodeType == Landmark)
	{
		CurrPtr = &Heap[Node];
		CurrDecisionPtr = NULL;
		return TRUE;
	}
	else if (Heap[Node].NodeType == Decision)
	{
		CurrDecisionPtr = &Heap[Node];
		CurrPtr = NULL;
		return TRUE;
	}
	else if (Heap[Node].NodeType == Unknown)
	{
		// I guess it has to be done, but it is debatable.
		CurrPtr = &Heap[Node];
		CurrDecisionPtr = NULL;
		return TRUE;
	}


	return FALSE;

}

/************************************************************************
* void GisNodeValid(unsigned short Node) 
*
* PURPOSE
* Returns true if the current node is valid
*************************************************************************/
BOOL GisNodeValid(unsigned short Node)
{
	if (Node >= MAXNODE)
		return FALSE;
	else if (Heap[Node].NodeType == Empty)
		return FALSE;

	return TRUE;

}

/************************************************************************
* NODE *GGetNode(unsigned short Node); 
*
* PURPOSE
* Returns the node ptr if the node is valid
*************************************************************************/
NODE *GGetNode(unsigned short Node)
{
	if (GisNodeValid(Node) == TRUE)
		return &Heap[Node];

	return NULL; // otherwise
}

/************************************************************************
* NODE* GCloneDecisionNode(NODE *TobeCloned, char OffsetDirection, short Offset, BOOL PremptStack, BOOL isFourPointsJ)
*
* PURPOSE
* Clones a node, and return the pointer
* INPUT
* TobeCloned, the reference node
* OffsetDirection, the direction in which the node is offseted, and connected,
* BOOL PremptStack whether it should be added to the stack normally or prempted an earlier entry
* with TobeCloned
* Offset, the offset
* OUTPUT
* BOOL isFourPointsJ sends back if it is a four point decision cluster
*************************************************************************/
NODE* GCloneDecisionNode(NODE *TobeCloned, char OffsetDirection, short Offset, BOOL PremptStack, BOOL *isFourPointsJ)
{
	unsigned short NewIndex;
	NodePtr newNode;
	NodePtr tNodePtr;
	char tDirection,fDirection1,fDirection2;
	char ClkDirection,CtrDirection,CDirection;
	BOOL AddOffset,Done;

	if (FreeHeapCount == 0)
		return TobeCloned;

	// 1) get a new node index;
	NewIndex = GGetNewNode();
	newNode = &Heap[NewIndex];

	newNode->NodeType = Decision;
	newNode->hasCloseLoopMarker = CloseLoopMarker; // null the marker

	newNode->Location = TobeCloned->Location;

	// we have to figure out the true offset direction, based on travel direction and 
	// avialable links.
	// by definintion the node to be cloned must have connected nodes, so one of the remained
	// two are the ones.  To decided between the two use this rule
	// If it is opposite of the direction of travel use it, If is the same as the direction
	// of travel use the other one.


	fDirection1 = fDirection2 = -1;
	if (TobeCloned->LinkE == NULL)
	{		
		fDirection1 = East;
	}
	else if (Heap[TobeCloned->LinkE].NodeType == Unknown)
	{
		fDirection1 = East;
	}
	if (TobeCloned->LinkW == NULL)
	{		
		if (fDirection1 == -1)
			fDirection1 = West;
		else
			fDirection2 = West;
	}
	else if (Heap[TobeCloned->LinkW].NodeType == Unknown)
	{
		if (fDirection1 == -1)
			fDirection1 = West;
		else
			fDirection2 = West;
	}
	if (TobeCloned->LinkS == NULL)
	{		
		if (fDirection1 == -1)
			fDirection1 = South;
		else
			fDirection2 = South;
	}
	else if (Heap[TobeCloned->LinkS].NodeType == Unknown)
	{
		if (fDirection1 == -1)
			fDirection1 = South;
		else
			fDirection2 = South;
	}
	if (TobeCloned->LinkN == NULL)
	{		
		if (fDirection1 == -1)
			fDirection1 = North;
		else
			fDirection2 = North;
	}
	else if (Heap[TobeCloned->LinkN].NodeType == Unknown)
	{
		if (fDirection1 == -1)
			fDirection1 = North;
		else
			fDirection2 = North;
	}

	// check if opposite.
	tDirection = UShiftDirectionClk(OffsetDirection);
	tDirection = UShiftDirectionClk(tDirection);

	AddOffset = TRUE;

	if (CurrPtr == NULL)
	{
		// do thing because this is the first node in the loop, so it
		// must be equal to offset direction
	}
	else if (CurrPtr->NodeType == Decision)
	{
			// do thing because this is the first node in the loop, so it
		// must be equal to offset direction
	}
	else if ((tDirection == fDirection1) || (tDirection == fDirection2))
	{
		// keep the offset direction as is
	}
	else
	{
		if (OffsetDirection == fDirection1)
		{
			// it should be the direction shifted 180	
			OffsetDirection = UShiftDirectionClk(fDirection2);
			OffsetDirection = UShiftDirectionClk(OffsetDirection);
			// the offset direction is screwed in certain cases by 180, and since I don't want to touch it
			// this is what I'll do.
		/*	if (PremptStack == FALSE)
			{
				OffsetDirection = UShiftDirectionClk(OffsetDirection);
				OffsetDirection = UShiftDirectionClk(OffsetDirection);
			}*/
		}
		else if (OffsetDirection == fDirection2)
		{
			// it should be the direction shifted 180	
			OffsetDirection = UShiftDirectionClk(fDirection1);
			OffsetDirection = UShiftDirectionClk(OffsetDirection);
			// the offset direction is screwed in certain cases by 180, and since I don't want to touch it
			// this is what I'll do.
		/*	if (PremptStack == FALSE)
			{
				OffsetDirection = UShiftDirectionClk(OffsetDirection);
				OffsetDirection = UShiftDirectionClk(OffsetDirection);
			}*/
		}	
		else
		{
			// special case, check out the LastCloseLoopOffset
			if ((OffsetDirection == East) || (OffsetDirection == West)) 
			{
				if (LastCloseLoopOffset.y > 0)
					OffsetDirection = South;
				else
					OffsetDirection = North;			
			}
			else
			{
				if (LastCloseLoopOffset.x > 0)				
					OffsetDirection = East;				
				else
					OffsetDirection = West;			
			}

		}
		AddOffset = FALSE;
	}






	// now figure out its direction, note because input direction is direction of travel
	// these if statements seem reversed
	if (OffsetDirection == South)	
	{		
		//newNode->Location.y -= Offset;
		newNode->Location.y = TobeCloned->Location.y - Offset;
		// there might be a previous linked unknown node, if so nuke it.
		if (TobeCloned->LinkN != NULL)
		{
			if (Heap[TobeCloned->LinkN].NodeType == Unknown)
				GDeleteNode(&Heap[TobeCloned->LinkN]);
		}
		TobeCloned->LinkN = NewIndex;
		newNode->LinkS = TobeCloned->Index;
	}
	else if (OffsetDirection == North)	
	{
		//newNode->Location.y += Offset;
		newNode->Location.y = TobeCloned->Location.y + Offset;
		if (TobeCloned->LinkS != NULL)
		{
			if (Heap[TobeCloned->LinkS].NodeType == Unknown)
				GDeleteNode(&Heap[TobeCloned->LinkS]);
		}
		TobeCloned->LinkS = NewIndex;
		newNode->LinkN = TobeCloned->Index;
	}
	else if (OffsetDirection == West)	
	{
	//	newNode->Location.x += Offset;
		newNode->Location.x = TobeCloned->Location.x + Offset;
		if (TobeCloned->LinkE != NULL)
		{
			if (Heap[TobeCloned->LinkE].NodeType == Unknown)
				GDeleteNode(&Heap[TobeCloned->LinkE]);
		}
		TobeCloned->LinkE = NewIndex;
		newNode->LinkW = TobeCloned->Index;
	}
	else if (OffsetDirection == East)	
	{
		//newNode->Location.x -= Offset;
		newNode->Location.x = TobeCloned->Location.x - Offset;
		if (TobeCloned->LinkW != NULL)
		{
			if (Heap[TobeCloned->LinkW].NodeType == Unknown)
				GDeleteNode(&Heap[TobeCloned->LinkW]);
		}
		TobeCloned->LinkW = NewIndex;
		newNode->LinkE = TobeCloned->Index;
	}

	// special case, the coordinates must align to both previous nodes
	if (AddOffset == FALSE)
	{
		if ((OffsetDirection == North) || (OffsetDirection == South))	
		{
			newNode->Location.y = CurrPtr->Location.y;
			newNode->Location.x = TobeCloned->Location.x;
		}
		else
		{	
			newNode->Location.x = CurrPtr->Location.x;
			newNode->Location.y = TobeCloned->Location.y;
		}
	}

	// in this case it is the rael first entry, it should prempt the the first entry,
	// and become the closedloopptr, because it is the first node in the new loop
	if (PremptStack == TRUE)
	{
		CLAddtoStackP(newNode->Index);
		CloseLoopPtr = newNode;
	}

	// check for the 4 point connection
	

    // note new node should always be going away from the old node
	ClkDirection = UShiftDirectionClk(OffsetDirection);
	CtrDirection = UShiftDirectionCtr(OffsetDirection);

	tNodePtr = UGetNode(TobeCloned, ClkDirection);

	Done = FALSE;
	if (tNodePtr != NULL)
	{
		if (tNodePtr->NodeType == Decision)
		{
			tDirection = UShiftDirectionClk(ClkDirection);
			tNodePtr = UGetNode(tNodePtr, tDirection);
			if (tNodePtr != NULL)
			{
				if (tNodePtr->NodeType == Decision)
				{					
					Done = TRUE;
					CDirection = ClkDirection;
				}
			}
		}
	}
	// try the other direction then
	if (Done == FALSE)
	{
		tNodePtr = UGetNode(TobeCloned, CtrDirection);
	
		if (tNodePtr != NULL)
		{
			if (tNodePtr->NodeType == Decision)
			{
				tDirection = UShiftDirectionCtr(CtrDirection);
				tNodePtr = UGetNode(tNodePtr, tDirection);
				if (tNodePtr != NULL)
				{
					if (tNodePtr->NodeType == Decision)
					{							
						Done = TRUE;
						CDirection = CtrDirection;
					}
				}
			}
		}
	}
	

	if (Done == TRUE)
	{
		// link them according to there CDirection
		if (CDirection == North)
		{		
			newNode->LinkN = tNodePtr->Index;
			tNodePtr->LinkS = newNode->Index;
			newNode->Location.x = tNodePtr->Location.x;
		}
		else if (CDirection == South)
		{
			newNode->LinkS = tNodePtr->Index;
			tNodePtr->LinkN = newNode->Index;
			newNode->Location.x = tNodePtr->Location.x;
		}
		else if (CDirection == West)
		{			
			newNode->LinkW = tNodePtr->Index;
			tNodePtr->LinkE = newNode->Index;
			newNode->Location.y = tNodePtr->Location.y;
		}
		else if (CDirection == East)
		{	
			newNode->LinkE = tNodePtr->Index;
			tNodePtr->LinkW = newNode->Index;
			newNode->Location.y = tNodePtr->Location.y;
		}
	}

	// send back if it is a four point decision node
	*isFourPointsJ = Done;


	return newNode;

}

/************************************************************************
* void GSetCloseLoopMark(unsigned short In)
*
* PURPOSE
* To set the increment of the current loop
*************************************************************************/
void GSetCloseLoopMark(unsigned short In)
{
	CloseLoopMarker = In;
}


/************************************************************************
* GPOINT GGetLastCLOffset(void)
*
* PURPOSE
* Returns the last offset. Usefull when closing the loop
*************************************************************************/
short GGetLastCLOffset(void)
{
	short tempo;

	tempo = abs(LastCloseLoopOffset.x) + abs(LastCloseLoopOffset.y);
	return tempo;
}

/************************************************************************
* OOL GIsDoneBigLoop(void);
*
* PURPOSE
* Checks if the robot returned to where is started the current loop, only applicable
* on not the first loop
*************************************************************************/
BOOL GIsDoneBigLoop(NODE *NodePtr)
{
	// in this case nothing to go on, so say it is.
	if (CloseLoopPtr == NULL)
		return TRUE;
	else if (NodePtr == NULL)
		return FALSE; // some error checking
	else if (NodePtr == CloseLoopPtr)
		return TRUE;
	else
		return FALSE;
}

	
/************************************************************************
* Function FileOpenGraphAscii()
*
* PURPOSE
* Opens, the graph, or more precilely loads in the table from the header file.
*************************************************************************/
void GLoadGraphAscii()
{
	short LineCount,ix,Index;

	LineCount = 0;

	GInitialize(); // clear the graph

	// run the loop for as many nodes there are in the array
	for (ix = 0; ix < GraphArraySize; ix++)
	{
		if (LineCount == 0)
		{
			Index = GGraph[ix];
			// each node already has the correct index
		}
		else if (LineCount == 1)
		{
			Heap[Index].hasCloseLoopMarker = 0;
		}
		else if (LineCount == 2)
		{
			Heap[Index].LandMarkE = GGraph[ix];
		}
		else if (LineCount == 3)
		{
			Heap[Index].LandMarkN = GGraph[ix];
		}
		else if (LineCount == 4)
		{
			Heap[Index].LandMarkS = GGraph[ix];
		}
		else if (LineCount == 5)
		{
			Heap[Index].LandMarkW = GGraph[ix];
		}
		else if (LineCount == 6)
		{
			Heap[Index].LinkE = GGraph[ix];
		}
		else if (LineCount == 7)
		{
			Heap[Index].LinkN = GGraph[ix];
		}
		else if (LineCount == 8)
		{
			Heap[Index].LinkS = GGraph[ix];
		}
		else if (LineCount == 9)
		{
			Heap[Index].LinkW = GGraph[ix];
		}
		else if (LineCount == 10)
		{
			Heap[Index].Location.x = GGraph[ix];
		}
		else if (LineCount == 11)
		{
			Heap[Index].Location.y = GGraph[ix];
		}
		else if (LineCount == 12)
		{
			Heap[Index].NodeType = (NODETYPE)GGraph[ix];
		}
		else if (LineCount == 13)
		{
			Heap[Index].Special = (SPECIAL)GGraph[ix];
		}
		LineCount++;
		if (LineCount == 14)
		{		
			LineCount = 0;
		}
	}
	
	// now load the status
	FreeHeapCount = GStatus[0];
	FreeHeapIndex = GStatus[1];

	if (GStatus[2] == 0)
		CurrPtr = NULL;
	else
		CurrPtr = &Heap[GStatus[2]];

	if (GStatus[3] == 0)
		CurrDecisionPtr = NULL;
	else
		CurrDecisionPtr = &Heap[GStatus[3]];


}
	
	