#include "stdafx.h" // for simulator only
#include "AIGraphH.h"
#include "AICloseLoop.h"
#include "AIGraph.h"
#include "AIUtils.h"

// the heap that is maniplulated from AIGraph.c
extern NODE Heap[MAXNODE];

unsigned short CLStack[CLMAXSTACK];
unsigned short CLStackIndex;
unsigned short CLTotalX; // total X displacement
unsigned short CLTotalY; // total Y displacement

unsigned short CLCLList[CLMAXLIST];
unsigned short CLCLListIndex; // stores the last entry of the list
NODE *CloseLoopNodePtr;

BOOL AlgamateNodes = TRUE;
short FuzzyBoundery = 3; // allow loop closing on 3 out of 4 possibilities

/************************************************************************
* BOOL CLAddtoStack(unsigned short Node)
*
* PURPOSE
* To add a new node to the stack.  Should be called with every new node creation
*************************************************************************/
BOOL CLAddtoStack(unsigned short Node)
{
	// overflow protection
	if (CLStackIndex == CLMAXSTACK)
		return FALSE;

	CLStack[CLStackIndex] = Node;

	if (CLStackIndex > 0)
	{
		CLTotalX += abs(Heap[CLStack[CLStackIndex]].Location.x - Heap[CLStack[CLStackIndex-1]].Location.x);
		CLTotalY += abs(Heap[CLStack[CLStackIndex]].Location.y - Heap[CLStack[CLStackIndex-1]].Location.y);
	}

	CLStackIndex++;

	return TRUE;
}

/************************************************************************
* BOOL CLAddtoStackP(unsigned short Node)
*
* PURPOSE
* To add a new node to the stack, but add it one before the earlier one
*************************************************************************/
BOOL CLAddtoStackP(unsigned short Node)
{
	unsigned short tempo;
	// overflow protection
	if (CLStackIndex == CLMAXSTACK)
		return FALSE;

	if (CLStackIndex == 0)
		CLAddtoStack(Node); // add it normally nothing to prempt
	else
	{
		// switch entries with the last one.
		tempo = CLStack[CLStackIndex-1];
		CLStack[CLStackIndex-1] = Node;
		CLStack[CLStackIndex] = tempo;

		if (CLStackIndex > 0)
		{
			CLTotalX += abs(Heap[CLStack[CLStackIndex]].Location.x - Heap[CLStack[CLStackIndex-1]].Location.x);
			CLTotalY += abs(Heap[CLStack[CLStackIndex]].Location.y - Heap[CLStack[CLStackIndex-1]].Location.y);
		}

		CLStackIndex++;
	}

	return TRUE;
}

/************************************************************************
* void CLClearStack(void)
*
* PURPOSE
* To initialize the stack.
*************************************************************************/
void CLClearStack(void)
{
	unsigned short ix;
	for (ix = 0; ix < CLMAXSTACK; ix++)
	{
		CLStack[ix] = 0;
	}
	CLTotalX = 0;
	CLTotalY = 0;
	CLStackIndex = 0;

}

/************************************************************************
* void CLCloseLoop(void);
*
* PURPOSE
* The almighty close loop function call.
* INPUT
* char TDirection, the last direction traveled, to resolve the first adjustment
* ALGORITHM
* 1)	Figure out the total error, by taking the difference of the first node
*       in the stack, to the current position given by a function argument 
* 2)	Now go from the start of the stack onwards to find the next decision node,
*       figure out if the connection is vertical or horizontal, and then positive
*       or negative compared to the error.
* 3)	Take the distance and add to it or subtract from it, its share of the error
*       (in practice it will be more then its share, so the last node is mostly accurate).
*       Log the amount of error that has been eliminated
* 3b)   Perform node algomation if desired, do this when it is the previous node
* 4)	Go between these two decision nodes, and change the position of all of the landmark
*       nodes in between by proportionately their amount.
* 5)	Find the next decision node, if there is one goto 3.  Else goto 6
* 6)	At this point there is one decision node left to link to the first one.
*       In this case, there should be very little or no error, especially in the direction 
*       with no movement allowed.  Reference the first member of the stack as the second
*    	decision node.  Also link the last node on the stack to the first node, after everything else is done.
* RETURNS
* The current decision node
*************************************************************************/
NODE* CLCloseLoop(char TDirection, GPOINT CurrPosition)
{
	short XError,YError,XDelta,YDelta;
	NODE *PrevNodePtr, *CurrNodePtr;
	unsigned short tPrevDecisionNode,tCurrDecisionNode; // temporary indexs
	GPOINT PrevLocation;
	BOOL Horizontal;
	short Factor = 5; //i.e propagate the error three as fast as need be so rounding doesn't
	short temp,DeltaError,ix; // general temp
	short Delta1,temp1; // for the propagation down a link
	BOOL Done;
	// occur

	// shouldn't happen
	if (CLStackIndex == 0)
		return NULL;

	CLStackIndex--; // index the first member
	/////////////////////////////////////////////////////////////////////
	// 1) find the actual error in the loop
	/////////////////////////////////////////////////////////////////////
	// half error because any adjustment propogates X2 because of the looping
	XError = -(CurrPosition.x - CloseLoopNodePtr->Location.x);
	YError = -(CurrPosition.y - CloseLoopNodePtr->Location.y);

	tCurrDecisionNode = 0;	
	PrevLocation = Heap[CLStack[0]].Location;

	Done = FALSE;
	while (Done == FALSE)
	{
		tPrevDecisionNode = tCurrDecisionNode;
		tCurrDecisionNode++;

		/////////////////////////////////////////////////////////////////////
		// 2) Find the next decision node
		/////////////////////////////////////////////////////////////////////
		while ((Heap[CLStack[tCurrDecisionNode]].NodeType != Decision) && (tCurrDecisionNode != CLStackIndex))
		{
			// shouldn't happen
			tCurrDecisionNode++;
		}

		PrevNodePtr = &Heap[CLStack[tPrevDecisionNode]];
		CurrNodePtr = &Heap[CLStack[tCurrDecisionNode]];
		
		/////////////////////////////////////////////////////////////////////
		// 3b) Algamate nodes if required
		/////////////////////////////////////////////////////////////////////
		if (AlgamateNodes == TRUE)
		{
			CLAlgamateNodes(PrevNodePtr); 
		}

		if (tCurrDecisionNode == CLStackIndex)
		{
			/////////////////////////////////////////////////////////////////////
			// 5) Last Node
			/////////////////////////////////////////////////////////////////////
			Done = TRUE;
			// by now the error should be propigated correctly to all the decision nodes
			// the only thing left to do, is adjust all of the landmark nodes, then link the 
			// last node with the first
			if ((TDirection == North) || (TDirection == South))
				Horizontal = FALSE;
			else
				Horizontal = TRUE;

			if (Horizontal== TRUE)
			{
				// go through the stack.
				XDelta = Heap[CLStack[0]].Location.x - PrevLocation.x; 

				// some large problem occured
				if (XDelta == 0)
				{
					// perhaps then there is only one decision node in the list, mabey should pick the close loop pointer
					// as the second one?
					XDelta = CloseLoopNodePtr->Location.x - Heap[CLStack[0]].Location.x;
					if (XDelta == 0)
						return CloseLoopNodePtr;
				}

				for (ix = tPrevDecisionNode+1; ix != tCurrDecisionNode+1; ix++)
				{

					Delta1 = Heap[CLStack[ix]].Location.x - PrevLocation.x;
					temp1 = (XError*Delta1)/XDelta;
					Delta1 += temp1;
					Heap[CLStack[ix]].Location.x = Delta1 + PrevNodePtr->Location.x;
					// change the Y to reflect perhaps a past change
					Heap[CLStack[ix]].Location.y = PrevNodePtr->Location.y;
				}
			}
			else //if (Horizontal== FALSE)
			{
				// go through the stack.
				YDelta = Heap[CLStack[0]].Location.y - PrevLocation.y;
				// some large problem occured
				if (YDelta == 0)
				{
					// perhaps then there is only one decision node in the list, mabey should pick the close loop pointer
					// as the second one?
					YDelta = CloseLoopNodePtr->Location.y - Heap[CLStack[0]].Location.y;
					if (YDelta == 0)
						return CloseLoopNodePtr;
				}
					
				for (ix = tPrevDecisionNode+1; ix != tCurrDecisionNode+1; ix++)
				{
					// go through the stack.
					Delta1 = Heap[CLStack[ix]].Location.y - PrevLocation.y;
					temp1 = (YError*Delta1)/YDelta;
					Delta1 += temp1;
					Heap[CLStack[ix]].Location.y = Delta1 + PrevNodePtr->Location.y;
					// change the X to reflect perhaps a past change
					Heap[CLStack[ix]].Location.x = PrevNodePtr->Location.x;
				}
			}
			// now link the node correctly
			GLinkNodes(&Heap[CLStack[tCurrDecisionNode]],CloseLoopNodePtr,TDirection);
			// done

		}
		else
		{
			/////////////////////////////////////////////////////////////////////
			// 3) Find outs it proper length (with the error propagation
			/////////////////////////////////////////////////////////////////////
			// by this convention if the length is + then the direction is either south
			// or east.  - means north or west
			// should always be positive because the error vector decides the sign
			YDelta = abs(CurrNodePtr->Location.y - PrevLocation.y);
			XDelta = abs(CurrNodePtr->Location.x - PrevLocation.x);

			// determine the direction
			if (XDelta == 0)
				Horizontal = FALSE;
			else
				Horizontal = TRUE;

			if (Horizontal == TRUE)
			{
				if (XError == 0)
				{
					// just propogate the change from any ealier change
					temp = PrevNodePtr->Location.x - PrevLocation.x;	
					DeltaError = 0;
				}
				else
				{
					// calculate how much error is this particular link going to obsorb
					temp = (short)(((long)XError*(long)Factor*(long)XDelta)/CLTotalX);
					// in case temp < 0 but still some error left, because we don't allow floats
					if (temp == 0)
					{
						if (XError > 0)
						{										
							temp = 1;
					
						}
						else if (XError < 0)
						{					
							temp = -1;
					
						}
					}

					// don't want to be larger then the error			
					if (XError > 0)
					{
						if (temp > XError)					
							temp = XError;
					}
					else
					{
						if (temp < XError)
							temp = XError;
					}
	

					// now account for the reduction in error
				
					XError -= temp;	
					DeltaError = temp;
					// propograte the change from a previous node
					temp += PrevNodePtr->Location.x - PrevLocation.x;
				}
				
			

				PrevLocation = CurrNodePtr->Location;
				// now change is actual location
				CurrNodePtr->Location.x += temp;
				// change the Y to reflect perhaps a past change
				CurrNodePtr->Location.y = PrevNodePtr->Location.y;

				if (XDelta == 0)
					return 0;

				/////////////////////////////////////////////////////////////////////
				// 4) now propogate the error throughout the link
				/////////////////////////////////////////////////////////////////////				
				for (ix = tPrevDecisionNode+1; ix != tCurrDecisionNode; ix++)
				{
					// go through the stack.
					// figure out how much of the delta error should be tacked on
					// to the landmark node, the proprogating error always get tacked on.
					Delta1 = abs(PrevLocation.x - Heap[CLStack[ix]].Location.x);				
					temp1 = DeltaError - (DeltaError*Delta1)/XDelta;
					// add the delta error, plus the propogating error together
					Heap[CLStack[ix]].Location.x += temp1+(temp-DeltaError);
					// change the Y to reflect perhaps a past change
					Heap[CLStack[ix]].Location.y = PrevNodePtr->Location.y;
				}

				YDelta = CurrNodePtr->Location.y - PrevLocation.y;

				// now check for any Unknown nodes attached to the decision node
				if (CurrNodePtr->LinkE != 0)
					if (Heap[CurrNodePtr->LinkE].NodeType == Unknown)
					{
						Heap[CurrNodePtr->LinkE].Location.x += temp;
						Heap[CurrNodePtr->LinkE].Location.y = PrevNodePtr->Location.y;
					}

				if (CurrNodePtr->LinkW != 0)
					if (Heap[CurrNodePtr->LinkW].NodeType == Unknown)
					{
						Heap[CurrNodePtr->LinkW].Location.x += temp;
						Heap[CurrNodePtr->LinkW].Location.y = PrevNodePtr->Location.y;
					}
				if (CurrNodePtr->LinkS != 0)
					if (Heap[CurrNodePtr->LinkS].NodeType == Unknown)
					{
						Heap[CurrNodePtr->LinkS].Location.x = CurrNodePtr->Location.x;
						Heap[CurrNodePtr->LinkS].Location.y += YDelta;
					}
				if (CurrNodePtr->LinkN != 0)
					if (Heap[CurrNodePtr->LinkN].NodeType == Unknown)
					{
						Heap[CurrNodePtr->LinkN].Location.x = CurrNodePtr->Location.x;
						Heap[CurrNodePtr->LinkN].Location.y += YDelta;
					}
				
			}
			else // (Horizontal == FALSE)
			{
				if (YError == 0)
				{
					// just propogate the change from any ealier change
					temp = PrevNodePtr->Location.y - PrevLocation.y;
					DeltaError = 0; // no new error being propogated
				}
				else
				{		
					// calculate how much error is this particular link going to obsorb
					temp = (short)(((long)YError*(long)Factor*(long)YDelta)/CLTotalY);
					// always for some error, because we are not using floats
					if (temp == 0)
					{
						if (YError > 0)
						{												
							temp = 1;
						
						}
						else
						{
							temp = -1;						
						}
					}

					// don't want to be larger then the error
					if (YError > 0)
					{
						if (temp > YError)					
							temp = YError;
					}
					else
					{
						if (temp < YError)
							temp = YError;								
					}


					// now account for the reduction in error
					YError -= temp;	
					// propograte the change from a previous node
					DeltaError = temp;
					temp += PrevNodePtr->Location.y - PrevLocation.y;
				}

				PrevLocation = CurrNodePtr->Location;
				// now change is actual location
				CurrNodePtr->Location.y += temp;
				// change the X to reflect perhaps a past change
				CurrNodePtr->Location.x = PrevNodePtr->Location.x;

				// some weird error better to leave now though
				if (YDelta == 0)
					return NULL; 

				/////////////////////////////////////////////////////////////////////
				// 4) now propogate the error throughout the link
				/////////////////////////////////////////////////////////////////////	
				for (ix = tPrevDecisionNode+1; ix != tCurrDecisionNode; ix++)
				{	
					// go through the stack.
					Delta1 = abs(PrevLocation.y - Heap[CLStack[ix]].Location.y);				
					temp1 = DeltaError - (DeltaError*Delta1)/YDelta;
					// add the delta error, plus the propogating error together
					Heap[CLStack[ix]].Location.y += temp1+(temp-DeltaError);
					// change the Y to reflect perhaps a past change
					Heap[CLStack[ix]].Location.x = PrevNodePtr->Location.x;
				}

				// now check for any Unknown nodes attached to the decision node
				XDelta = CurrNodePtr->Location.x - PrevLocation.x;
				if (CurrNodePtr->LinkE != 0)
					if (Heap[CurrNodePtr->LinkE].NodeType == Unknown)
					{
						Heap[CurrNodePtr->LinkE].Location.y = CurrNodePtr->Location.y;
						Heap[CurrNodePtr->LinkE].Location.x += XDelta;
					}
				if (CurrNodePtr->LinkW != 0)
					if (Heap[CurrNodePtr->LinkW].NodeType == Unknown)
					{
						Heap[CurrNodePtr->LinkW].Location.y = CurrNodePtr->Location.y;
						Heap[CurrNodePtr->LinkE].Location.x += XDelta;
					}
				if (CurrNodePtr->LinkS != 0)
					if (Heap[CurrNodePtr->LinkS].NodeType == Unknown)
					{
						Heap[CurrNodePtr->LinkS].Location.y += temp;
						Heap[CurrNodePtr->LinkS].Location.x = CurrNodePtr->Location.x;
					}
				if (CurrNodePtr->LinkN != 0)
					if (Heap[CurrNodePtr->LinkN].NodeType == Unknown)
					{
						Heap[CurrNodePtr->LinkN].Location.y += temp;
						Heap[CurrNodePtr->LinkN].Location.x = CurrNodePtr->Location.x;
					}

			}
				
		}
	}		
	return CloseLoopNodePtr;
}

/************************************************************************
* void CLCreateCloseLoopList()
*
* PURPOSE
* Creates a list which is used in runtime to see if the current decision node, actually
* closes a loop
*************************************************************************/
void CLCreateCloseLoopList()
{
	unsigned short ix;

	CLCLListIndex = 0;

	for (ix = 1; ix < MAXNODE; ix++)
	{
		if (Heap[ix].NodeType == Decision)
		{
			if ((Heap[ix].LandMarkE == SNRHallway) 
				|| (Heap[ix].LandMarkS == SNRHallway)
				|| (Heap[ix].LandMarkW == SNRHallway)
				|| (Heap[ix].LandMarkN == SNRHallway))
			{
				// if it has one empty hallway pointer, add it to the list.
				CLCLList[CLCLListIndex] = ix;
				CLCLListIndex++;
			}
		}
	}
}

/************************************************************************
* void CLIsLoopClosed(char TDirection, short Possibilities[4], short Location)
*
* PURPOSE
* To check if the current decision node, actually closes the loop
* ALGORITHM
* Looks at the close loop list
* 1) First check if the possible directions are the same
* 2) If they are check the distance, and record
* 3) If by the end of the search if there is one that distance is within a minimum
*    Then the loop should be closes.  Store a ptr to this node.
*************************************************************************/
BOOL CLIsLoopClosed(char TDirection, short Possibilities[4], GPOINT Location)
{
	CloseLoopNodePtr = NULL;

	unsigned short Distance = 9999;
	unsigned short tempDistance,ix,tNodeIndex,tNodeIndex1,Fuzzy,SavedFuzzy;


	for (ix = 0; ix < CLCLListIndex; ix++)
	{		
		Fuzzy = 0;
		tNodeIndex1 = CLCLList[ix];
	/*	if (Possibilities[0] == Heap[tNodeIndex1].LandMarkN)
			if (Possibilities[1] == Heap[tNodeIndex1].LandMarkE)
				if (Possibilities[2] == Heap[tNodeIndex1].LandMarkS)
					if (Possibilities[3] == Heap[tNodeIndex1].LandMarkW)
					{
						tempDistance = abs(Location.x - Heap[tNodeIndex1].Location.x) 
							+ abs(Location.y - Heap[tNodeIndex1].Location.y);
						if (tempDistance < Distance)
						{
							Distance = tempDistance;
							tNodeIndex = ix;
						}
					}*/
		if (Possibilities[0] == Heap[tNodeIndex1].LandMarkN)
			Fuzzy++;
		if (Possibilities[1] == Heap[tNodeIndex1].LandMarkE)
			Fuzzy++;
		if (Possibilities[2] == Heap[tNodeIndex1].LandMarkS)
			Fuzzy++;
		if (Possibilities[3] == Heap[tNodeIndex1].LandMarkW)
			Fuzzy++;
		if (Fuzzy >= FuzzyBoundery) // 3 out of 4 is fine
		{
			tempDistance = abs(Location.x - Heap[tNodeIndex1].Location.x) 
						+ abs(Location.y - Heap[tNodeIndex1].Location.y);
			if (tempDistance < Distance)
			{
				Distance = tempDistance;
				tNodeIndex = ix;
				SavedFuzzy = Fuzzy;
			}
		}

	}

	if (Distance < 100) // 1 meters grace should be enough
	{
		CloseLoopNodePtr = &Heap[CLCLList[tNodeIndex]];
		return TRUE;
	}
	else
		return FALSE;
}

/************************************************************************
* void CLSetCloseLoopNode(NODE* NodePtr)
*
* PURPOSE
* when the second loop, etc is closed, the connection node,
* is cloned, so this sets the cloned node to being the close loop node.
* NOTE
* No error checking, be carefull!
*************************************************************************/
void CLSetCloseLoopNode(NODE* NodePtr)
{
	CloseLoopNodePtr = NodePtr;
}

/************************************************************************
* NODE* CLGetCloseLoopNode(void)
*
* PURPOSE
* to get the decision node, to be cloned, see above
*************************************************************************/
NODE* CLGetCloseLoopNode(void)
{
	return 	CloseLoopNodePtr;
}

/************************************************************************
* void CLAlgamateNodes(NODE *NodePtr)
*
* PURPOSE
* when a decision node has a hallway or unknown node connection,
* it might connect to another decision node.  This function does a search during
* the close loop algorithm.  If so it makes the connection, and moves the AlgamatedNode
* NOTE 
* This is done during the close loop algorithm
*************************************************************************/
void CLAlgamateNodes(NODE *NodePtr)
{
	unsigned short ix,iy;
	char PDirection[4];
	char tDirection;
	unsigned short SmallTolerace = 20;
	unsigned short LargeTolerace = 200;
	short tDistance;
	NODE *tMatchPtr;
	BOOL Match,SafetoProceed;

	if (NodePtr == NULL)
		return;

	if (NodePtr->NodeType != Decision)
		return;

	if ((NodePtr->LandMarkN == SNRHallway) || (NodePtr->LandMarkN == SNRUnknown) )
		PDirection[0] = North;
	else
		PDirection[0] = -1;
	if ((NodePtr->LandMarkE == SNRHallway) || (NodePtr->LandMarkE == SNRUnknown) )
		PDirection[1] = East;
	else
		PDirection[1] = -1;
	if ((NodePtr->LandMarkS == SNRHallway) || (NodePtr->LandMarkS == SNRUnknown) )
		PDirection[2] = South;
	else
		PDirection[2] = -1;
	if ((NodePtr->LandMarkW == SNRHallway) || (NodePtr->LandMarkW == SNRUnknown) )
		PDirection[3] = West;
	else
		PDirection[3] = -1;

	for (iy = 0; iy < 4; iy++)
	{
		if (PDirection[iy] == -1)
		{
			// do nothing
		}
		else
		{
			// do a 180
			tDirection = UShiftDirectionClk(PDirection[iy]);
			tDirection = UShiftDirectionClk(tDirection);
			// now look for a match within a reasonalbe area
			for (ix = 1; ix < MAXNODE; ix++)
			{
				if (ix != NodePtr->Index)
				{
					tMatchPtr = &Heap[ix];
					if (tMatchPtr->NodeType == Decision)
					{
						Match = FALSE;
						if (tDirection == North)						
						{
							if ((tMatchPtr->LandMarkN == SNRHallway) || (tMatchPtr->LandMarkN == SNRUnknown) )							
								Match = TRUE;							
						}
						else if (tDirection == East)
						{
							if ((tMatchPtr->LandMarkE == SNRHallway) || (tMatchPtr->LandMarkE == SNRUnknown) )							
								Match = TRUE;						
						}
						else if (tDirection == South)
						{
							if ((tMatchPtr->LandMarkS == SNRHallway) || (tMatchPtr->LandMarkS == SNRUnknown) )
								Match = TRUE;

						}
						else if (tDirection == West)
						{
							if ((tMatchPtr->LandMarkN == SNRHallway) || (tMatchPtr->LandMarkN == SNRUnknown) )
								Match = TRUE;
						}
						if (Match == TRUE)
						{
							// now check out the distance
							if (tDirection == North)						
							{
								// note that the sign is important
								tDistance = tMatchPtr->Location.y - NodePtr->Location.y;
								if ((tDistance < LargeTolerace) && (tDistance >= 0))
								{
									// if tDistance == 0, they are on the same path so ignore them
									tDistance = abs(tMatchPtr->Location.x - NodePtr->Location.x);
									if ((tDistance < SmallTolerace) && (tDistance >= 0)) 
									{
										// we have a match!!! don't bother looking for another
										// one because the requirements are really stringent there
										// should only be one match
										SafetoProceed = TRUE;
										if (tMatchPtr->LinkN != NULL)
										{
											if (Heap[tMatchPtr->LinkN].NodeType != Unknown)
											{
												SafetoProceed = FALSE;
											}
											else
											{
												// This function will delete and unlink the node.
												GDeleteNode(&Heap[tMatchPtr->LinkN]);
											}
										}
										if (NodePtr->LinkS != NULL)
										{
											if (Heap[NodePtr->LinkS].NodeType != Unknown)
											{
												SafetoProceed = FALSE;
											}
											else
											{
												// This function will delete and unlink the node.
												GDeleteNode(&Heap[NodePtr->LinkS]);
											}
										}

										if (SafetoProceed == TRUE)
										{
											NodePtr->Location.x = tMatchPtr->Location.x;
											NodePtr->LinkS = tMatchPtr->Index;
											tMatchPtr->LinkN = NodePtr->Index;
											// update the attached unknown node if it exists
											if (NodePtr->LinkN != NULL)
											{
												if (Heap[NodePtr->LinkN].NodeType == Unknown)
												{
													Heap[NodePtr->LinkN].Location.x = NodePtr->Location.x;
												}
											}											
											return;
										}
									}
								}
							}							
							else if (tDirection == South)						
							{
								// note that the sign is important
								tDistance = NodePtr->Location.y - tMatchPtr->Location.y ;
								if ((tDistance < LargeTolerace) && (tDistance >= 0))
								{
									tDistance = abs(tMatchPtr->Location.x - NodePtr->Location.x);
									// if tDistance == 0, they are on the same path so ignore them
									if ((tDistance < SmallTolerace) && (tDistance >= 0)) 
									{
										// we have a match!!! don't bother looking for another
										// one because the requirements are really stringent there
										// should only be one match
										SafetoProceed = TRUE;
										if (tMatchPtr->LinkS != NULL)
										{
											if (Heap[tMatchPtr->LinkS].NodeType != Unknown)
											{
												SafetoProceed = FALSE;
											}
											else
											{
												// This function will delete and unlink the node.
												GDeleteNode(&Heap[tMatchPtr->LinkS]);
											}
										}
										if (NodePtr->LinkN != NULL)
										{
											if (Heap[NodePtr->LinkN].NodeType != Unknown)
											{
												SafetoProceed = FALSE;
											}
											else
											{
												// This function will delete and unlink the node.
												GDeleteNode(&Heap[NodePtr->LinkN]);
											}
										}

										if (SafetoProceed == TRUE)
										{									

											NodePtr->Location.x = tMatchPtr->Location.x;
											NodePtr->LinkN = tMatchPtr->Index;
											tMatchPtr->LinkS = NodePtr->Index;
											// update the attached unknown node if it exists
											if (NodePtr->LinkS != NULL)
											{
												if (Heap[NodePtr->LinkS].NodeType == Unknown)
												{
													Heap[NodePtr->LinkS].Location.x = NodePtr->Location.x;
												}
											}
											return;
										}
									}
								}
							}
							else if (tDirection == East)						
							{
								// note that the sign is important
								tDistance = NodePtr->Location.x - tMatchPtr->Location.x;
								if ((tDistance < LargeTolerace) && (tDistance >= 0))
								{
									tDistance = abs(tMatchPtr->Location.y - NodePtr->Location.y);
									// if tDistance == 0, they are on the same path so ignore them
									if ((tDistance < SmallTolerace) && (tDistance >= 0)) 
									{
										// we have a match!!! don't bother looking for another
										// one because the requirements are really stringent there
										// should only be one match
										SafetoProceed = TRUE;
										if (tMatchPtr->LinkE != NULL)
										{
											if (Heap[tMatchPtr->LinkE].NodeType != Unknown)
											{
												SafetoProceed = FALSE;
											}
											else
											{
												// This function will delete and unlink the node.
												GDeleteNode(&Heap[tMatchPtr->LinkE]);
											}
										}
										if (NodePtr->LinkW!= NULL)
										{
											if (Heap[NodePtr->LinkW].NodeType != Unknown)
											{
												SafetoProceed = FALSE;
											}
											else
											{
												// This function will delete and unlink the node.
												GDeleteNode(&Heap[NodePtr->LinkW]);
											}
										}


										if (SafetoProceed == TRUE)
										{
											NodePtr->Location.y = tMatchPtr->Location.y;
											NodePtr->LinkW = tMatchPtr->Index;
											tMatchPtr->LinkE = NodePtr->Index;
											// update the attached unknown node if it exists
											if (NodePtr->LinkE != NULL)
											{
												if (Heap[NodePtr->LinkE].NodeType == Unknown)
												{
													Heap[NodePtr->LinkE].Location.y = NodePtr->Location.y;
												}
											}
											return;
										}
									}
								}
							}
							else if (tDirection == West)						
							{
								// note that the sign is important
								tDistance = tMatchPtr->Location.x - NodePtr->Location.x;
								if ((tDistance < LargeTolerace) && (tDistance >= 0))
								{
									tDistance = abs(tMatchPtr->Location.y - NodePtr->Location.y);
									// if tDistance == 0, they are on the same path so ignore them
									if ((tDistance < SmallTolerace) && (tDistance >= 0)) 
									{
										// we have a match!!! don't bother looking for another
										// one because the requirements are really stringent there
										// should only be one match
										SafetoProceed = TRUE;
										if (tMatchPtr->LinkW != NULL)
										{
											if (Heap[tMatchPtr->LinkW].NodeType != Unknown)
											{
												SafetoProceed = FALSE;
											}
											else
											{
												// This function will delete and unlink the node.
												GDeleteNode(&Heap[tMatchPtr->LinkW]);
											}
										}
										if (NodePtr->LinkE != NULL)
										{
											if (Heap[NodePtr->LinkE].NodeType != Unknown)
											{
												SafetoProceed = FALSE;
											}
											else
											{
												// This function will delete and unlink the node.
												GDeleteNode(&Heap[NodePtr->LinkE]);
											}
										}
										if (SafetoProceed == TRUE)
										{
											NodePtr->Location.y = tMatchPtr->Location.y;
											NodePtr->LinkE = tMatchPtr->Index;
											tMatchPtr->LinkW = NodePtr->Index;
											// update the attached unknown node if it exists
											if (NodePtr->LinkW != NULL)
											{
												if (Heap[NodePtr->LinkW].NodeType == Unknown)
												{
													Heap[NodePtr->LinkW].Location.y = NodePtr->Location.y;
												}
											}
											return;
										}
									}
								}
							}
						}
					}
				}
			}
		}
	}
}
