// AIExplorer.cpp: implementation of the AIExplorer class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Simulator.h"
#include "AIExplorer.h"
#include "AIGraphH.h"
#include "AIRoutePlanner.h"
#include "AICloseLoop.h"
#include "AIUtils.h"
#include "MessageMap.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

AIExplorer::AIExplorer()
{
    DebugAdjust = 0;	

}

AIExplorer::~AIExplorer()
{

}


/************************************************************************
* Function BOOL XPRunTime()
*
* This function is used to run the XP Main loop only a certain amount of times 
* per second.  Based on Kernel Ticks
*************************************************************************/
BOOL AIExplorer::XPRunTime()
{
    short tempticks = KRNLGetTicks();

    if (TicksDelta <= 0)
    {
        XPReloadTicks();
        return TRUE;
    }

    if (tempticks < TicksStart)
    {
        TicksDelta -= 1000+tempticks - TicksStart;
    }
    else
    {
        TicksDelta -= tempticks-TicksStart;
    }

    TicksStart = tempticks;

    if (TicksDelta <= 0)
    {
        XPReloadTicks();
        return TRUE;
    }

    return FALSE;

}

/************************************************************************
* Function void XPInitialize()
*
* Initializes the explorer
*************************************************************************/
void AIExplorer::AIInitialize()
{
    TicksReloadValue = 10;//25; // i.e 1000/25 -> 40 times to run a second
    IR.FWStraightenDelta = 0;
    XPReloadTicks();
    XPFlushIR();

    XPPrevState = Initial;
    XPState = XPDecision;
	XPIState = XPIInitial;

 
    FWStruct.TurningStage = TRStage1;  
	FWStruct.TotalTurnTicks = 0;
	FWStruct.DeadStraight = FALSE;
	FWStruct.SideIR = &IR.RightIRV;
	FWStruct.DeadResetSE2 = FALSE;
	FWStruct.FowardCollisionAlert = FALSE;

    Options.FollowWallLeft = FALSE;
    Options.FollowWallRight = TRUE;

	Options.MinSideDistance = 30;
	Options.MaxSideDistance = 80;
    Options.FrontThreshHold = 35; // cms
	Options.FrontSideThreshHold = 15;
    Options.FrontCollisionThreadHold = 18;

    Options.MaximumTurnTicks = 70; // around 80 centimeters on a turn before giving up   
	Options.MaxUnknownTicks = 150;
	Options.MaximimumDeadStraightTicks = 100;


	//////////////////////////////////////////////////////////////
	// IR module values											//
	//////////////////////////////////////////////////////////////
	IR.IRLowPassThreshold = 5;  // the threshold of when to consider that a change in reading
	// is actually an object of different depth.

	

	MStruct.LMState = LMStage1;
	MStruct.LMStage1Distance = NODETECTIOND;
	MStruct.LMStage3Distance = NODETECTIOND;
	MStruct.Starting = FALSE; // not used anymore
	MStruct.AddDecision = FALSE;
	MStruct.Direction = North;
	MStruct.IgnoreOnce = FALSE;
	MStruct.isWallDecisionNode = FALSE;

	MStruct.h_FirstDecisionNode = TRUE;
	MStruct.h_GoingtoNavPoint = FALSE;
	MStruct.h_HasFirstLoop = FALSE;
	MStruct.h_InstructionValid = FALSE;
	MStruct.CloseLoopIndex = 0;
	GSetCloseLoopMark(MStruct.CloseLoopIndex);
	
	Options.MainActivity = Mapping;
	NavStruct.StartNodePtr = GGetFirstNode();
	NavStruct.GoalNodePtr = GGetFirstNode();
	NavStruct.CurrDirection = MStruct.Direction;
	NavStruct.NavStage = NavInitial;
	NavStruct.NavError = FALSE;

	FirstTime = TRUE;

	GInitialize();


}

/************************************************************************
* Function void XPReloadTicks()
*
* Reloads the tick delay
*************************************************************************/
void AIExplorer::XPReloadTicks()
{
    TicksDelta = TicksReloadValue;
    TicksStart = KRNLGetTicks();

}

/************************************************************************
* Function void XPRun()
*
* The main running loop of the explorer
*************************************************************************/
void AIExplorer::AIRun()
{
	AISendUpdate();
	AISendStates();
    // checks if it is time to run
    if (XPRunTime() == TRUE)
    {
		// update the sensors
        XPUpdateBumper();
        XPUpdateIR();

		// take a look at past information for map building
		if (Options.MainActivity == Mapping)
		{			
			XPUpdateMap();
		}

		if (Options.MainActivity == Navigating)
		{
			// the navigator
			XPRunNavigation();		
			return;
		}

		// O.K now do an action
        if (XPCheckforCollision() == TRUE)
        {
            if (Options.MainActivity == Mapping)
			{
				XPMappingDecision();
			}
        }
        else if (MVIsBlocking() == TRUE)
        {
        }
		else if (SNRIsBlocking() == TRUE)
		{
		}
        else if ((XPState == XPDecision) || (XPState == Collision))
        {
			if (Options.MainActivity == Mapping)
			{
				XPMappingDecision();
			}			
        }
        else
        {
        
            // find wall
            if (XPState == Initial)
            {
               /* if (XPFWCheckForTurn() == FALSE)
                {
                    if (CheckForWall() == FALSE)
                    {
                        MVGoFoward(6);
                    }
                }*/
				XPState = XPDecision;
            }        
			else if (XPState == TurnInside)
			{
				XPTurnInside();
			}
			else if (XPState == TurnOutside)
			{
				XPTurnOutside();
			}
            else if (XPState == FollowWall)
            {               
				XPFW();			
            }
        }
    }

}

/************************************************************************
* Function void XPUpdateBumper()
* USAGE
* Explorer Low Level
* FUNCTIONALITY
* Updates the status of the bumper for use in the high level 
*************************************************************************/
void AIExplorer::XPUpdateBumper()
{
    char BumperByte = BMPGetByte();

    if (((BumperByte & FRONTLEFT) != 0) 
        || ((BumperByte & FRONTRIGHT) != 0))
    {
        if ((BumperByte & FRONTRIGHT) == 0)
            BumperState = FrontLeft;
        else if ((BumperByte & FRONTLEFT) == 0)
            BumperState = FrontRight;
        else
            BumperState = Front;
    }
    else if (((BumperByte & REARLEFT) != 0) 
        || ((BumperByte & REARRIGHT) != 0))
    {
        BumperState = Rear;
    }
    else if (((BumperByte & LEFTFRONT) != 0) 
        || ((BumperByte & LEFTBACK) != 0))
    {
        BumperState = LeftSide;
    }
    else if (((BumperByte & RIGHTFRONT) != 0) 
        || ((BumperByte & RIGHTBACK) != 0))
    {
        BumperState = RightSide;
    }

}

/************************************************************************
* Function void XPUpdateIR()
* USAGE
* Explorer Low Level
* FUNCTIONALITY
* Updates the status of the IRs for use in the wall following level
*************************************************************************/
void AIExplorer::XPUpdateIR()
{
    short ix;//,temp,temp1;

    // first update all of the IR arrays
    for (ix = IRFILTERING-1; ix > 0; ix--)
    {
          IR.RightIR[ix] = IR.RightIR[ix-1];
          IR.FrontIR[ix] = IR.FrontIR[ix-1];
          IR.LeftIR[ix] = IR.LeftIR[ix-1];
          IR.RFrontIR[ix] = IR.RFrontIR[ix-1];
          IR.LFrontIR[ix] = IR.LFrontIR[ix-1];
		  IR.LRearIR[ix] = IR.LRearIR[ix-1];
		  IR.RRearIR[ix] = IR.RRearIR[ix-1];
    }

	// copy the current readings into the first position.
    IR.RightIR[0] = IRGetRightD();
    IR.FrontIR[0] = IRGetFrontD();
    IR.LeftIR[0] = IRGetLeftD();
    IR.RFrontIR[0] = IRGetRFrontD();
    IR.LFrontIR[0] = IRGetLFrontD();
	IR.LRearIR[0] = IRGetLRearD();
	IR.RRearIR[0] = IRGetRRearD();

	// example averaging filters
/*	temp = 0;
	temp1 = 0;
	for (ix = 0; ix < 10; ix++)
	{
		if ( IR.RightIR[ix] != NODETECTIOND)
		{
			temp += IR.RightIR[ix];
			temp1++;
		}
	}

	if (temp1 > 0)
		IR.RightIRV = temp/temp1;
	else
		IR.RightIRV = NODETECTIOND;*/

	

	// do any filtering of the array
	// none at present
	// do some averaging filtering
	
	// update the current IR values
	IR.FrontIRV = IR.FrontIR[0];
	IR.RightIRV = IR.RightIR[0];
	IR.LeftIRV = IR.LeftIR[0];
	IR.RFrontIRV = IR.RFrontIR[0];
	IR.LFrontIRV = IR.LFrontIR[0];
	IR.LRearIRV = IR.LRearIR[0];
	IR.RRearIRV = IR.RRearIR[0];

	// add the right side IRs to the log if required
	if (IR.RightIRV != IR.IRLogValueR[IR.IRLogIndexR])
	{
		XPUpdateIRLogR();		

	}

	// add the left side IRs to the log if required
	if (IR.LeftIRV != IR.IRLogValueL[IR.IRLogIndexL])
	{
		XPUpdateIRLogL();	
	}
}

/************************************************************************
* Function XPUpdateIRLog()
*
* PURPOSE
* To update the IR logs (rate of change)
* USAGE
* When updating the IRs if the value has changed, or after a flush, to put two values
* into the log.
*************************************************************************/
void AIExplorer::XPUpdateIRLogR()
{
	// adjust the frequency parameter, that adjusts for occilations
	if (IR.IRLogPositiveSlopeR == TRUE)
	{
		if (IR.RightIRV < IR.IRLogValueR[IR.IRLogIndexR])
		{
			IR.IRLogPositiveSlopeR = FALSE;
			IR.IRLogDeltaR++;
		}
	}
	else // IR.IRLogPositiveSlopeR == FALSE
	{
		if (IR.RightIRV > IR.IRLogValueR[IR.IRLogIndexR])
		{
			IR.IRLogPositiveSlopeR = TRUE;
			IR.IRLogDeltaR++;
		}
	}
	// Calculate the lock
	if ((IR.IRLogDeltaR > 3) && (IR.IRLogLockR == 0))
		IR.IRLogLockR = IR.RightIRV;

	DebugAdjust = IR.IRLogDeltaR;

	// change in value
	IR.IRLogIndexR++;
	if (IR.IRLogIndexR >= NIRLOG)
		IR.IRLogIndexR = 0;

	IR.IRLogValueR[IR.IRLogIndexR] = IR.RightIRV;
	IR.IRLogTimeR[IR.IRLogIndexR] = KRNLGetTicksL();
	IR.IRLogisNewR[IR.IRLogIndexR] = TRUE;

}

/************************************************************************
* Function XPUpdateIRLog()
*
* PURPOSE
* To update the IR logs (rate of change)
* USAGE
* When updating the IRs if the value has changed, or after a flush, to put two values
* into the log.
*************************************************************************/
void AIExplorer::XPUpdateIRLogL()
{
	// adjust the frequency parameter, that adjusts for occilations
	if (IR.IRLogPositiveSlopeL == TRUE)
	{
		if (IR.LeftIRV < IR.IRLogValueL[IR.IRLogIndexL])
		{
			IR.IRLogPositiveSlopeL = FALSE;
			IR.IRLogDeltaL++;
		}
	}
	else // IR.IRLogPositiveSlopeR == FALSE
	{
		if (IR.RightIRV > IR.IRLogValueL[IR.IRLogIndexL])
		{
			IR.IRLogPositiveSlopeL = TRUE;
			IR.IRLogDeltaL++;
		}
	}
	// Calculate the lock
	if ((IR.IRLogDeltaL > 3) && (IR.IRLogLockL == 0))
		IR.IRLogLockL = IR.LeftIRV;

	DebugAdjust = IR.IRLogDeltaL;

	// change in value
	IR.IRLogIndexL++;
	if (IR.IRLogIndexL >= NIRLOG)
		IR.IRLogIndexL = 0;

	IR.IRLogValueL[IR.IRLogIndexL] = IR.LeftIRV;
	IR.IRLogTimeL[IR.IRLogIndexL] = KRNLGetTicksL();
	IR.IRLogisNewL[IR.IRLogIndexL] = TRUE;

}


/************************************************************************
* Function void XPFlushIR()
* USAGE
* Explorer Low Level
* FUNCTIONALITY
* Initilizes some of the IR stuff
*************************************************************************/
void AIExplorer::XPFlushIR()
{
    short ix;

	// kill the filtering
    for (ix = 0; ix < IRFILTERING; ix++)
    {
         IR.FrontIR[ix] = NODETECTIOND;
         IR.RightIR[ix] = NODETECTIOND;
         IR.LeftIR[ix] = NODETECTIOND;
         IR.RFrontIR[ix] = NODETECTIOND;
         IR.LFrontIR[ix] = NODETECTIOND;
    }

	// kill the log
	for (ix = 0; ix < NIRLOG; ix++)
    {
		IR.IRLogValueR[ix] = NODETECTIOND;
		IR.IRLogTimeR[ix] = 0;
		IR.IRLogisNewR[ix] = FALSE;

		IR.IRLogValueL[ix] = NODETECTIOND;
		IR.IRLogTimeL[ix] = 0;
		IR.IRLogisNewL[ix] = FALSE;
	}
	IR.IRLogIndexL = 0;
	IR.IRLogIndexR = 0;
	IR.IRLogLockR = 0;
	IR.IRLogLockL = 0;
	IR.IRLogDeltaR = 0;
	IR.IRLogDeltaL = 0;
	IR.IRLogPositiveSlopeR = TRUE;
	IR.IRLogPositiveSlopeL = TRUE;

	MStruct.LMState = LMStage1;

	// get initial values
	XPUpdateIR(); 
	// force and extra update, so a large RoC isn't reported because only one value
	XPUpdateIRLogR();
	XPUpdateIRLogL();

	IR.IRLogDeltaR = 0;
	IR.IRLogDeltaL = 0;


}
/************************************************************************
* Function void XPAddTicks(short ticks)
*
* PURPOSE
* Adds addition time to delay before next time quanta, in milleseconds
*************************************************************************/
void AIExplorer::XPAddTicks(short ticks)
{
    TicksDelta += ticks;
}

/************************************************************************
* Function BOOL XPCheckForTurn()
*
* PURPOSE
* Checks for the turning state.  If so set the robot to the apprioprate turning action
* NOTES
* Turning state when
* 1) Front IR is less then the threshold
* 2) Left/Right IR depending on which follow wall mode is less then threshold.
This function is obsolete
*************************************************************************/
BOOL AIExplorer::XPFWCheckForTurn()
{
    if ((IR.FrontIRV < Options.FrontThreshHold)
		|| (IR.RFrontIRV < Options.FrontSideThreshHold)
		|| (IR.LFrontIRV < Options.FrontSideThreshHold))
    {
		XPPrevState = XPState;
		XPState = XPDecision;       
        return TRUE;
    }  
    
    return FALSE;   

}



/************************************************************************
* Function void CheckForWall()
*
* PURPOSE
* Checks for a wall, for the initial state
*************************************************************************/
BOOL AIExplorer::CheckForWall()
{
    if (IR.LeftIRV != NODETECTIOND)
    {
		XPPrevState = XPState;
        XPState = FollowWall;
        Options.FollowWallLeft = TRUE;
        Options.FollowWallRight = FALSE;
        return TRUE;
    }
    else if (IR.RightIRV != NODETECTIOND)
    {
		XPPrevState = XPState;
        XPState = FollowWall;
        Options.FollowWallLeft = FALSE;
        Options.FollowWallRight = TRUE;
        return TRUE;
    }
    return FALSE;


}

/************************************************************************
* Function void AIGetStates(CString &State1, CString &State2, CString &State3)
*
* PURPOSE
* Debuging function only used for the simulater
*************************************************************************/
void AIExplorer::AISendStates()
{
	unsigned short State1,State2,State3,State4;
	char Message[30];
    if (XPState == Initial)
     {
         State1 = 1;
     }
     else if (XPState == FollowWall)
     {
         State1 = 2;
     }
     else if (XPState == TurnInside)
     {
         State1 = 3;
     }
     else if (XPState == TurnOutside)
     {
         State1 = 4;
     }
     else if (XPState == XPDecision)
     {
         State1 = 5;
     }
	 else if (XPState == NavUnknownTo)
     {
         State1 = 6;
     }
	 else if (XPState == GotoDecision)
     {
         State1 = 7;
     }
	 else if (XPState == NavUnknownFrom)
     {
         State1 = 8;
     }

	if ((MStruct.Direction & North) != 0)	
		State2 = 1;
	else if ((MStruct.Direction & East) != 0)	
		State2 = 2;
	else if ((MStruct.Direction & South) != 0)	
		State2 = 3;
	else if ((MStruct.Direction & West) != 0)	
		State2 = 4;   

     if (Options.FollowWallLeft == TRUE)
     {
         State3 = 1;
     }
     else if (Options.FollowWallRight == TRUE)
     {
         State3 = 2;
     }
     else
     {
         State3 = 3;
     }

	 if (Options.MainActivity == Navigating)
     {
         State4 = 1;
     }
     else if (Options.MainActivity == Mapping)
     {
         State4 = 2;
     }
	 sprintf(Message,"<5%d|%d|%d|%d>",State1,State2,State3,State4);
	 AISend(Message);

}

/************************************************************************
* Function BOOL XPMappingDecision()
*
* PURPOSE
* When a state (follow wall, turn, etc) signals that its state has changed,
* this function is used to make a decision as what to do next
*************************************************************************/
void AIExplorer::XPMappingDecision()
{
	short SNRDistance[3]; // to report the sonar activity
  
    if (XPState == Collision)
    {
        // oh oh a collision may have occured
        if (BMPGetByte() != 0x00)
        {
            XPDBumperCollision();    
            XPState = Initial; // set back to previous state
        }
        else if ((IR.FrontIRV < Options.FrontCollisionThreadHold)
			|| (IR.RFrontIRV < Options.FrontCollisionThreadHold)
			|| (IR.LFrontIRV < Options.FrontCollisionThreadHold))			
        {
            MVGoFowardT(-5,20); // move back around 25 cm
            XPState = XPPrevState; // set back to previous state
        }
    }
    //////////////////////////////////////////////////////////////////////////////
    // O.K time to decide in which direction to head                            //
    //////////////////////////////////////////////////////////////////////////////
    else if (XPState == XPDecision)
    {
    
		// looks for a foward collision
		if (FWStruct.FowardCollisionAlert == TRUE)
		{
			FWStruct.FowardCollisionAlert = FALSE;
			

			XPPrevState = XPState;
			XPState = TurnInside;
			Beep(1,1);
		
			if (Options.FollowWallRight == TRUE)
			{
				FWStruct.SideIR = &IR.RightIRV;
				// force the foward one into being a hallway.
				SNRDistance[0] = SNRWall;
				SNRDistance[1] = SNRWall;
				SNRDistance[2] = SNRHallway;
			
			}
			else
			{
				FWStruct.SideIR = &IR.LeftIRV;
				// force the foward one into being a hallway.
				SNRDistance[2] = SNRWall;
				SNRDistance[1] = SNRWall;
				SNRDistance[0] = SNRHallway;			
			}


			// now need to add the decision node
			MStruct.isWallDecisionNode = TRUE;
			XPMappingAddDecisionNode(SNRDistance,0,0, TRUE);

			if (Options.FollowWallRight == TRUE)
			{
				XPLogTurnCtr();
			}
			else
			{
				XPLogTurnClk();
			}

			// this causes a landmark not to be logged in the inside turn
			FirstTime = FALSE;		

		}
		// if not need, to make a decision as to were to turn
		else if (XPIState == XPIInitial)
		{
			// move foward a bit to get more data
			SEResetTick2(); // probably reset elsewhere too
			// don't move foward if the absolute first decision
			if (MStruct.h_FirstDecisionNode == TRUE)
			{
			}
			else
			{
		  		MVGoFowardT(5,25);
			}
			XPIState = MFGetMoreData;
			if (Options.FollowWallRight == TRUE)
			{
				FWStruct.SideIR = &IR.RightIRV;
			}
			else
				FWStruct.SideIR = &IR.LeftIRV;
		}
		else if (XPIState == MFGetMoreData)
		{		
			MVAllStop();		
			// get sonar data, note this is a blocking call
			SNRSet3PointScan();
			XPIState = MakeDecision;
		}
		else if (XPIState == MakeDecision)
		{
			XPIState = XPIInitial;			

			// see which areas are open
			SNRGet3PointDistance(SNRDistance);		

			if (((SNRDistance[0] == SNRUnknown) || (SNRDistance[0] == SNRWall))
				&& (SNRDistance[1] == SNRUnknown)
				&& ((SNRDistance[2] == SNRUnknown) || (SNRDistance[0] == SNRWall)))
			{
				// assume hallway straight ahead
				// go straight, until contact
				FWStruct.DeadStraight = TRUE;
				XPPrevState = XPState;
				XPFlushIR();				
				XPState = FollowWall;

				// force the foward one into being a hallway.
				SNRDistance[1] = SNRHallway;

				// now need to add the decision node
				XPMappingAddDecisionNode(SNRDistance,0,0, TRUE);

			}
			else if ((SNRDistance[0] == SNRHallway) && (Options.FollowWallRight == TRUE))
			{
				// always turn right if there is a hallway there
				// Turn inside
				XPPrevState = XPState; 
				XPState = TurnInside;
				// set the correct side IR up
				if (Options.FollowWallRight == TRUE)
				{
					FWStruct.SideIR = &IR.RightIRV;
				}
				else
					FWStruct.SideIR = &IR.LeftIRV;

				// now need to add the decision node
				if (XPMappingAddDecisionNode(SNRDistance,0,0,FALSE) == FALSE)
				{
					XPState = XPDecision; // irrelevent because it does go anywhere else
				}
				else
				{					
					if (Options.FollowWallRight == TRUE)
					{
						// log the turn
						XPLogTurnClk();
					}
					else // followallleft == TRUE
					{
						XPLogTurnCtr();
					}
				}
				XPIState = XPIInitial;
			}
			else if ((SNRDistance[2] == SNRHallway) && (Options.FollowWallLeft == TRUE))
			{
				// always turn right if there is a hallway there
				// Turn inside
				XPPrevState = XPState; 
				XPState = TurnInside;
				// set the correct side IR up
				if (Options.FollowWallRight == TRUE)
				{
					FWStruct.SideIR = &IR.RightIRV;
				}
				else
					FWStruct.SideIR = &IR.LeftIRV;

				// now need to add the decision node
				if (XPMappingAddDecisionNode(SNRDistance,0,0,FALSE) == FALSE)
				{
					XPState = XPDecision; // irrelevent because it does go anywhere else
				}
				else
				{					
					if (Options.FollowWallRight == TRUE)
					{
						// log the turn
						XPLogTurnClk();
					}
					else // followallleft == TRUE
					{
						XPLogTurnCtr();
					}
				}
				XPIState = XPIInitial;
			}
			else
			{
				// go straight, until contact
				FWStruct.DeadStraight = TRUE;
				XPPrevState = XPState;
				XPFlushIR();				
				XPState = FollowWall;

				// now need to add the decision node
				XPMappingAddDecisionNode(SNRDistance,0,0,TRUE);
			}

		}	
    }
}

/************************************************************************
* Function BOOL XPCheckforCollision()
*
* PURPOSE
* Although Ideally a collision should never occur, this function checks if
* a collision is imminant, or has occured
*************************************************************************/
BOOL AIExplorer::XPCheckforCollision()
{
    
    if (BMPGetByte() != 0x00)
    {
        // don't want to keep on moving
        MVAllStop();
        MVReset();
        XPPrevState = XPState;
        XPState = Collision;
        return TRUE;
    }
    else if ((IR.FrontIRV < Options.FrontCollisionThreadHold)
		|| (IR.RFrontIRV < Options.FrontCollisionThreadHold)
		|| (IR.LFrontIRV < Options.FrontCollisionThreadHold))
    {
        // don't want to keep on moving
        MVAllStop();
        MVReset();
        XPPrevState = XPState;
        XPState = Collision;
        return TRUE;
    }

    return FALSE;


}

/************************************************************************
* Function void XPDBumperCollision()
*
* PURPOSE
* When a bumper collision occurs.
* USAGE
* By the XPDecision function.
*************************************************************************/
void AIExplorer::XPDBumperCollision()
{
     // it is a bumper collision
    char BumperByte = BMPGetByte();

    if (((BumperByte & FRONTLEFT) != 0) 
        || ((BumperByte & FRONTRIGHT) != 0))
    {
        if ((BumperByte & FRONTRIGHT) == 0)
            MVGoFowardWA(-5,2);
        else if ((BumperByte & FRONTLEFT) == 0)
            MVGoFowardWA(-5,-2);
        else
            MVGoFoward(-5);

        
        XPAddTicks(1000);
    }

    else if (((BumperByte & REARLEFT) != 0) 
        || ((BumperByte & REARRIGHT) != 0))
    {
        MVGoFoward(5);
        XPAddTicks(1000);
    }
    else if (((BumperByte & LEFTFRONT) != 0) 
        || ((BumperByte & LEFTBACK) != 0))
    {
        MVGoFowardWA(5,6);       
        XPAddTicks(1000);
    }
    else if (((BumperByte & RIGHTFRONT) != 0) 
        || ((BumperByte & RIGHTBACK) != 0))
    {
        MVGoFowardWA(5,-6);        
        XPAddTicks(1000);
    }

}

/************************************************************************
* Function BOOL XPFWCheckForDecision()
*
* PURPOSE
* Checks if a decision state is neccessary.  If so it forces decision mode
* USAGE
* By the XPDecision function.
*************************************************************************/
BOOL AIExplorer::XPFWCheckForDecision()
{
	short SNRDistance;
	SNRGetFowardDistance(&SNRDistance);

    if ((IR.FrontIRV < Options.FrontThreshHold)
		|| (IR.RFrontIRV < Options.FrontSideThreshHold)
		|| (IR.LFrontIRV < Options.FrontSideThreshHold))	
		
    {
		// SNRGetFowardDistance
		FWStruct.FowardCollisionAlert = TRUE;
        MVAllStop();
        XPPrevState = XPState;
        XPState = XPDecision;
        return TRUE;
    }
	else if (FWStruct.DeadStraight == TRUE)
	{
		// do nothing if going dead straight.
	}
    else if (Options.FollowWallRight == TRUE)
    {
        if (IR.RightIRV == NODETECTIOND)
        {
            MVAllStop();
            XPPrevState = XPState;
            XPState = XPDecision;
			// so a landmark will get added
			MStruct.LMState = LMStage3;
			MStruct.LMStage3Distance = IR.RightIRV;
		//	ASSERT (MStruct.LMStage1Distance != NODETECTIOND);
			MStruct.LMStage1Distance = 60; // make it anything could otherwise the robot will 
			// turn into the wall.

            return TRUE;
        }
    }
    else if (Options.FollowWallLeft == TRUE)
    {
        if (IR.LeftIRV == NODETECTIOND)
        {
            MVAllStop();
            XPPrevState = XPState;
            XPState = XPDecision;

			// so a landmark will get added
			MStruct.LMState = LMStage3;
			MStruct.LMStage3Distance = IR.LeftIRV;
			MStruct.LMStage1Distance = 60; // make it anything could otherwise the robot will 

            return TRUE;
        }
    }

    return FALSE;




}

/************************************************************************
* Function void XPFW()
*
* PURPOSE
* Main follow wall function
* USAGE
* In AIRun()
*************************************************************************/
void AIExplorer::XPFW()
{
	char Adjust = 0;
	short DeltaDistance,DeltaIndex,tempadjust;
	long DeltaTime;	
	unsigned char SideIR;
	BOOL IsNew = FALSE;
	// Oh oh, perhaps a collision, note this is legacy
	if (XPFWCheckForDecision() == TRUE)
    {
        return;
    }

	// warning, no timeout on dead straight
	if (FWStruct.DeadStraight == TRUE)
	{
		if (FWStruct.DeadResetSE2 == TRUE)
		{
			FWStruct.DeadResetSE2 = FALSE;
			SEResetTick2();
		}

		if (SEGetTick2() > Options.MaximimumDeadStraightTicks)
		{
			MVAllStop();
			XPState = XPDecision;
			FWStruct.DeadStraight = FALSE;
		}
		else if (*FWStruct.SideIR == NODETECTIOND)
		{
			MVGoFoward(5);
			return;
		}		
		else
		{
			// contact found, resume normal follow wall procedure.
			FWStruct.DeadStraight = FALSE;
			MStruct.IgnoreOnce = TRUE;
			// add a landmark node
			MStruct.LMStage1Distance = NODETECTIOND;
			MStruct.LMStage3Distance = *FWStruct.SideIR;
			MStruct.LMState = LMStage3;	
			// the update is here to eliminate any lag to report it, and the since the normal
			// fw continues, it could interfere
			if (Options.MainActivity == Mapping)
				XPUpdateMap();
			else if (Options.MainActivity == Navigating)
				XPNavigationLandmark();

		}

	}

	if (Options.FollowWallLeft == TRUE)
	{
		if (IR.IRLogisNewL[IR.IRLogIndexL] == TRUE)
		{			
			DeltaIndex = ULogIndex(IR.IRLogIndexL,-1);
			DeltaDistance = IR.IRLogValueL[IR.IRLogIndexL] - IR.IRLogValueL[DeltaIndex];
			DeltaTime = IR.IRLogTimeL[IR.IRLogIndexL] - IR.IRLogTimeL[DeltaIndex];

			// should never happen
			if (DeltaDistance == 0)
				DeltaDistance = 1;
			else
				tempadjust = (DeltaTime/DeltaDistance);
			if (tempadjust == 0)
				tempadjust = 1;				

			Adjust = (1000/tempadjust);

			// make sure that we don't creep into a wall
			if (IR.LeftIRV < Options.MinSideDistance)
			{
				Adjust -= 5;
			}
			else if (IR.LeftIRV > Options.MaxSideDistance)
			{
				Adjust += 5;
			}

		
			// a bit extra code, to lock onto a distance to prevent creeping towards the wall
			if (IR.IRLogLockL != 0)
			{
				Adjust = -(IR.IRLogLockL - IR.LeftIRV)/4; // 2 is good

				if (IR.LRearIRV != NODETECTIOND)
				{
					// also use the right rear to adjust
					short ratio45 = RATIO45(IR.LRearIRV);
					Adjust -= (-IR.LeftIRV+ratio45)/3; // 3 is good
				}
				// make sure that we don't creep into a wall
				if (IR.LeftIRV < Options.MinSideDistance)
				{
					Adjust -= 3;
				}
				else if (IR.LeftIRV > Options.MaxSideDistance)
				{
					Adjust += 5;
				}

			}


			SideIR = IR.LeftIRV;
			IsNew = TRUE;	


		}
	}
	else if (Options.FollowWallRight == TRUE)
	{
		if (IR.IRLogisNewR[IR.IRLogIndexR] == TRUE)
		{
			DeltaIndex = ULogIndex(IR.IRLogIndexR,-1);
			DeltaDistance = IR.IRLogValueR[IR.IRLogIndexR] - IR.IRLogValueR[DeltaIndex];
			DeltaTime = IR.IRLogTimeR[IR.IRLogIndexR] - IR.IRLogTimeR[DeltaIndex];

			// should never happen
			if (DeltaDistance == 0)
				DeltaDistance = 1;
			else
				tempadjust = (DeltaTime/DeltaDistance);
			if (tempadjust == 0)
				tempadjust = 1;

			Adjust = (-1000/tempadjust);

			// make sure that we don't creep into a wall
			if (IR.RightIRV < Options.MinSideDistance)
			{
				Adjust += 5;
			}
			else if (IR.RightIRV > Options.MaxSideDistance)
			{
				Adjust -= 5;
			}

			// a bit extra code, to lock onto a distance to prevent creeping towards the wall
			if (IR.IRLogLockR != 0)
			{
				Adjust = (IR.IRLogLockR - IR.RightIRV)/4; // 2 is good
				// also use the right rear to adjust
				if (IR.RRearIRV != NODETECTIOND)
				{
					short ratio45 = RATIO45(IR.RRearIRV);
					Adjust += (-IR.RightIRV+ratio45)/3; // 3 is good				
				}		
				
				// make sure that we don't creep into a wall
				if (IR.RightIRV < Options.MinSideDistance)
				{
					Adjust += 3;
				}
				else if (IR.RightIRV > Options.MaxSideDistance)
				{
					Adjust -= 5;
				}

			}

			SideIR = IR.RightIRV;	
			IsNew = TRUE;
		
		}
	}
	if (IsNew == TRUE)
	{
		// do mapping adjustments
		// if rate of changes greater then a threshold
		if (abs(Adjust) > 30)
		{			
			Adjust = 0;
			MStruct.LMState = LMStage2; // temp hack			
		}
		// sudden drop could only be accounted by a large change.
		else if (abs(DeltaDistance) > 1)
		{			
			Adjust = 0;
			MStruct.LMState = LMStage2; // temp hack
		}
		// must be back to normal, log the landmark though
		else if (MStruct.LMState == LMStage2)
		{			
			if (MStruct.IgnoreOnce == FALSE)
			{
				// temp hack, // might have to add more robust detection
				// scheme in real life
				if (abs(MStruct.LMStage1Distance - SideIR) < 5)
				{
					// Don't bother reporting this.
					MStruct.LMState = LMStage1;	

				}
				else if (MStruct.LMStage1Distance== NODETECTIOND)
				{
					// this is taken care of by another module, so don't report it.
					MStruct.LMState = LMStage1;					
				}
				else
				{

					MStruct.LMState = LMStage3;
					MStruct.LMStage3Distance = SideIR;
					// temp hack, should always go into decsion mode sometime
					if (SideIR == NODETECTIOND)
					{
						XPPrevState = XPState;
						XPState = XPDecision;
					}
				}
			}
			else
			{
				MStruct.IgnoreOnce = FALSE;
				MStruct.LMState = LMStage1;
			}
		}

		// temp hack
		if (MStruct.LMState == LMStage1)
		{
			MStruct.LMStage1Distance = SideIR;	
			MStruct.IgnoreOnce = FALSE;
		}
		
		if (MStruct.LMState != LMStage1)
		{
			Adjust = 0;
			IR.IRLogLockR = 0;
			IR.IRLogDeltaR = 0;
			IR.IRLogLockL = 0;
			IR.IRLogDeltaL = 0;
		}


		MVGoFowardWA(5,(char)Adjust);
	}
}

/************************************************************************
* Function XPLogTurnCtr()
*
* PURPOSE
* After a turn, the turn has to be logged, e.i change the current direction
*************************************************************************/
void AIExplorer::XPLogTurnCtr()
{
	NavStruct.CurrDirection = MStruct.Direction = UShiftDirectionCtr(MStruct.Direction);
	FWStruct.TurnClockWise = FALSE;
	FWStruct.TurningStage = TRStage1; // just in case
	FirstTime = TRUE; // just in case
}

/************************************************************************
* Function XPLogTurnClk()
*
* PURPOSE
* After a turn, the turn has to be logged, e.i change the current direction
*************************************************************************/
void AIExplorer::XPLogTurnClk()
{
	NavStruct.CurrDirection = MStruct.Direction = UShiftDirectionClk(MStruct.Direction);
	FWStruct.TurnClockWise = TRUE;
	FWStruct.TurningStage = TRStage1; // just in case
	FirstTime = TRUE; // just in case
}




/************************************************************************
* Function XPUpdateMap()
*
* PURPOSE
* This function is the one that handles all the mapmaking.  It takes a look
* at output from the other modules, and if the output specifies a map feature
* this function adds that feature.
* USAGE
* In the main AIRun Loop.  It is run every cycle
* NOTE
* This function only does stuff if running in mapping mode, otherwise it 
* just exits
* Also it now just handles landmarks, the decision nodes are taken care of elsewere
*************************************************************************/
void AIExplorer::XPUpdateMap()
{
	char LandmarkDirection;
	LANDMARK Type;
	short RoC;

	if (Options.MainActivity != Mapping)
		return;

	if ((XPState == FollowWall) && (MStruct.Starting == TRUE))
	{
		// log the starting point
		GAddSpecialNode(StartingPoint,MStruct.Direction);
		MStruct.Starting = false;
		SEResetTick2(); // reset the distance counter
	}
/*	else if (MStruct.AddDecision == TRUE)
	{
		// log the decision, for now don't include the possibilities
		int tempo = SEGetTick2();
		GAddDecisionNode(MStruct.Direction,0,SEGetTick2());
		SEResetTick2();		
		MStruct.AddDecision = FALSE;
	}*/
	
	else if (MStruct.LMState == LMStage3)
	{
		// First figure out which direction the landmark came from

		if (Options.FollowWallRight == TRUE)
		{
			// happened on the right side
			LandmarkDirection = UShiftDirectionClk(MStruct.Direction);

		}
		else if (Options.FollowWallLeft == TRUE)
		{
			// happended on the left side
			LandmarkDirection = UShiftDirectionCtr(MStruct.Direction);
		}

		// figure out what kind of landmark
		RoC = 0;
		if (MStruct.LMStage1Distance == NODETECTIOND)
			Type = GainDetection;
		else if (MStruct.LMStage3Distance == NODETECTIOND)
			Type = LostDetection;
		else
		{
			Type = RateofChange;
			RoC = MStruct.LMStage3Distance - MStruct.LMStage1Distance;
		}

		GAddLandmarkNode(MStruct.Direction,LandmarkDirection,Type,RoC,SEGetTick2());
		MStruct.LMState = LMStage1;
		SEResetTick2();
	}

}


/************************************************************************
* Function XPTurnInside()
*
* PURPOSE
* Perform the inside turn, as is specified in this turns flow chart
* It is a right turn if following right, a left turn if following left.
* USAGE
* Part of the turn module
*************************************************************************/
void AIExplorer::XPTurnInside()
{
	if (FWStruct.TurningStage == TRStage1)
	{
		// first turn 90 degrees
		if (FWStruct.TurnClockWise)
			MVTurn(-12);
		else
			MVTurn(12);
		FWStruct.TurningStage = TRStage2;
	}
	else if (FWStruct.TurningStage == TRStage2)
	{
		MVAllStop(); // stop the momentum
		MVGoFowardT(4,4); // go foward a few ticks.
		FWStruct.TurningStage = TRStage3;
		SEResetTick1(); // reset past ticks
		SEResetTick2(); // reset the tick counter for the graph
	}
	else if (FWStruct.TurningStage == TRStage3)
	{
		MVGoFoward(4);
		if (*FWStruct.SideIR != NODETECTIOND)
		{
			if (FirstTime == TRUE)
			{
				FirstTime = FALSE;
				SEResetTick1();	
				
				// add a landmark node
				MStruct.LMStage1Distance = NODETECTIOND;
				MStruct.LMStage3Distance = *FWStruct.SideIR;
				MStruct.LMState = LMStage3;	
				if (Options.MainActivity == Mapping)
					XPUpdateMap();
				else if (Options.MainActivity == Navigating)
				{
					XPNavigationLandmark();
					// check special case if path way is done, so variable could be reset
					if ((NavStruct.GoalNodePtr != NULL) && (NavStruct.StartNodePtr != NULL))
						if (NavStruct.GoalNodePtr->Index == NavStruct.StartNodePtr->Index)
						{
							// the end of the path has been reached, go foward a bit anyway
							MVGoFowardT(4,6);
							FWStruct.TurningStage = TRStage1;
							FirstTime = TRUE;
						}
				}
			}
			FWStruct.PrevSideIRDistance = *FWStruct.SideIR;

			MVGoFoward(4);

			// oh oh, must have been a really thin wall, have to log it
			if (*FWStruct.SideIR == NODETECTIOND)
			{
				MStruct.LMStage1Distance = 40; // any number should do.
				MStruct.LMStage3Distance = NODETECTIOND;
				MStruct.LMState = LMStage3;			
				if (Options.MainActivity == Mapping)
					XPUpdateMap();
				else if (Options.MainActivity == Navigating)
					XPNavigationLandmark();
				FirstTime = TRUE;
				FWStruct.TurningStage = TRStage1; // reset the stage
				XPPrevState = XPState;
				XPState = XPDecision; // start following the wall
			}
			// go up to 4 ticks foward before going back to decision mode
			if (SEGetTick1() > 4)
			{
				FWStruct.TurningStage = TRStage1; // reset the stage
				XPPrevState = XPState;
				XPState = FollowWall; // put it into follow wall mode mode
				FirstTime = TRUE;
			}
			
		}
		else if (SEGetTick1() > Options.MaximumTurnTicks)			
		{
			// error, reset back to decision state
			FWStruct.TurningStage = TRStage1; // reset the stage
			XPPrevState = XPState;
			XPState = XPDecision; // put it into decision mode
		}
	}
}

/************************************************************************
* Function XPTurnOutside()
*
* PURPOSE
* Perform the outside turn, as is specified in this turns flow chart
* It is a left turn if following right, a right turn if following left.
* It is the turn performed if the foward sensor is sensing a collision
* USAGE
* Part of the turn module
*************************************************************************/
 void AIExplorer::XPTurnOutside()
{
	if (FWStruct.TurningStage == TRStage1)
	{
		if (FWStruct.TurnClockWise == TRUE)		
			MVTurn(-8); // approx 60 degrees
		else
			MVTurn(8); // approx 60 degrees
		FWStruct.TurningStage = TRStage2;
		FWStruct.PrevSideIRDistance = 200; // any large number
	}
	else if (FWStruct.TurningStage == TRStage2)
	{	
		
		// since it only gets here after every 7.5 degrees, there
		// shouldn't be a problem using the =
        if (*FWStruct.SideIR >= FWStruct.PrevSideIRDistance)
        {
			MVAllStop();
			FWStruct.TurningStage = TRStage1;
			XPPrevState = XPState;
			XPState = XPDecision;
		}

		if (*FWStruct.SideIR < FWStruct.PrevSideIRDistance)
			 FWStruct.PrevSideIRDistance = *FWStruct.SideIR;

		if (FWStruct.TurnClockWise == TRUE)		
			MVTurn(-1); // approx 7.5 degrees
		else
			MVTurn(1); // approx 7.5 degrees
	}

}

/************************************************************************
* Function AISendCurrPosition(short *Distance, char TDirection)
*
* PURPOSE
* To send information about the current position for display
* USAGE
* Debuggin mainly
* OBSOLETE
*************************************************************************/
void AIExplorer::AISendCurrPosition(short *Distance, char *TDirection)
{
	*Distance = SEGetTick2();
	*TDirection = MStruct.Direction;	
}


/************************************************************************
* Function XPMappingAddDecisionNode(short Possibilities[])
*
* PURPOSE
* Simple helper function to tranlate a sonar list of possibilites into 
* its proper direction and then add the decision node
* INPUT
* CheatSE if it is the first node, and not 0, then the shaftencoder could,
* input could be cheated
* CheatDirection, if CheatSE is not equal to 0, then also cheat the direction of the robot
* BOOL GoingStraight, indicates if the loop should be closed or not (on a turn or on the end)
* RETURNS
* True if node added noramlly, false if last node, and should stop moving
*************************************************************************/
BOOL AIExplorer::XPMappingAddDecisionNode(short SNRDistance[], short CheatSE, char CheatDirection, BOOL GoingStraight)
{
	short Possibilities[4];
	NODE *tNodePtr;
	BOOL tReturn = TRUE;
	BOOL Dummy;

	if (MStruct.Direction == North)
	{
		Possibilities[0] = SNRHallway; // north
		Possibilities[2] = SNRHallway; // south
		Possibilities[1] = SNRDistance[0]; // east
		Possibilities[3] = SNRDistance[2]; // west
	}
	else if (MStruct.Direction == East)
	{
		Possibilities[1] = SNRHallway; // east
		Possibilities[0] = SNRDistance[2]; // north
		Possibilities[2] = SNRDistance[0]; // south
		Possibilities[3] = SNRHallway; // west;
	}
	else if (MStruct.Direction == South)
	{
		Possibilities[2] = SNRHallway; // south
		Possibilities[0] = SNRHallway; // north
		Possibilities[1] = SNRDistance[2]; // east
		Possibilities[3] = SNRDistance[0]; // west
	}
	else if (MStruct.Direction == West)
	{
		Possibilities[3] = SNRHallway; // west
		Possibilities[1] = SNRHallway; // east
		Possibilities[0] = SNRDistance[0]; // north
		Possibilities[2] = SNRDistance[2]; // south
	}

	if (MStruct.h_FirstDecisionNode == TRUE)
	{
		MStruct.h_FirstDecisionNode = FALSE;
		if (CheatSE != 0)
		{
			GAddDecisionNode(CheatDirection,Possibilities,CheatSE,FALSE);			
		}
		else
		{
			GAddDecisionNode(MStruct.Direction,Possibilities,SEGetTick2(),FALSE);
		}
		// create the close loop list
		CLCreateCloseLoopList();
		SEResetTick2();	
	}
	else
	{
		// add the decision node
		if (GIsLoopClosed(MStruct.Direction,Possibilities,SEGetTick2()) == TRUE)
		{
			tNodePtr = CLGetCloseLoopNode();
		//	if ((GIsDoneBigLoop(tNodePtr) == TRUE) || (GoingStraight == FALSE))
		//	{
				if (tNodePtr->hasCloseLoopMarker != MStruct.CloseLoopIndex)
				{
					// will have to clone a new node for it.				
					tNodePtr = GCloneDecisionNode(tNodePtr,MStruct.Direction,GGetLastCLOffset(),FALSE,&Dummy);
					GAddPossibilities(tNodePtr,Possibilities); // add any hazard nodes, if applicable
					tNodePtr->hasCloseLoopMarker = MStruct.CloseLoopIndex;
					CLSetCloseLoopNode(tNodePtr);
				} 

				if (GIsDoneBigLoop(tNodePtr) == TRUE)
				{
					// make sure to get set it as the close loop pointer	
					CLSetCloseLoopNode(tNodePtr);
					Options.MainActivity = Navigating;
					tReturn = FALSE;
				}

				GCloseLoop(MStruct.Direction,SEGetTick2());
				Beep(3,1); // three short beeps in embedded mode, switch to navigator mode
				// temp hack, go into nav mode. and stay there until getting an order

			
				// else keep on mapping
			
			//	Options.MainActivity = Navigating;
				NavStruct.StartNodePtr = GGetCurrNode();
				NavStruct.GoalNodePtr = GGetCurrNode();
				NavStruct.CurrDirection = MStruct.Direction;
				MVReset(); // reset MV in case in the middle of a turn
		/*	}
			else
			{
				GAddDecisionNode(MStruct.Direction,Possibilities,SEGetTick2());
			}*/

		}
		else
		{
			GAddDecisionNode(MStruct.Direction,Possibilities,SEGetTick2(),MStruct.isWallDecisionNode);
			MStruct.isWallDecisionNode = FALSE;
		}

	
		SEResetTick2();		
	}

	return tReturn;

}

/************************************************************************
* Function AIRunNavigation()
*
* PURPOSE
* Main call to upper-level navigation module.  Handles all of the navigation decisions
* USAGE
* In the main AIRun function
*************************************************************************/
void AIExplorer::XPRunNavigation()
{
	unsigned short tNode;
	NODE *tNodePtr;
	char Direction;
	if (BMPGetByte() != 0x00)
    {
        // don't want to keep on moving
        MVAllStop();
        MVReset();
		Beep(3,3);        
    }
	else if (MVIsBlocking() == TRUE)
    {
    }
	else if (SNRIsBlocking() == TRUE)
	{
	}
	else if ((NavStruct.GoalNodePtr == NULL) || (NavStruct.StartNodePtr == NULL))
	{
		MVAllStop();
	}
	else if (NavStruct.GoalNodePtr->Index == NavStruct.StartNodePtr->Index)
	{
		MVAllStop();
		// we have travelled to the correct nav point.  Go to function to 
		// continue
		if (MStruct.h_GoingtoNavPoint == TRUE)
			XPMappingSetupLoop();
	}    
	else if (NavStruct.NavStage == NavInitial)
	{
		// O.K figure out inital direction, special case, because who knows,
		// where we are near the current node.
		RPSetupEnum(); // setup peak mode		

		// get the first node out of the stack, should be equal to the current node
		if (RPEnum(&tNode) == FALSE)
		{
			// error!
			NavStruct.GoalNodePtr = NavStruct.StartNodePtr;
			return;
		}

		// get the second node out of the stack, if null then there is no path,
		// so exit
		if (RPEnum(&tNode) == FALSE)
		{
			// error!
			NavStruct.GoalNodePtr = NavStruct.StartNodePtr;
			return;
		}

		// special case if starting from an unknown node (room)
		if (NavStruct.StartNodePtr->NodeType == Unknown)
		{
			// no need to travel to the first node so pop it off the stack.
			RPPopStack(&tNode);
			// now get the destination node
			RPPopStack(&tNode);
			// assign the destination node
			NavStruct.DestNodePtr = GGetNode(tNode);

			NavStruct.NavStage = NavNormal;
			NavStruct.EnteringUnknown = TRUE;
			XPState = NavUnknownFrom;
			return; // boot out early
		}

		// figure out the relative direction of the first node
		if (NavStruct.StartNodePtr->LinkE == tNode)
			NavStruct.DesiredDirection = East;
		else if (NavStruct.StartNodePtr->LinkW == tNode)
			NavStruct.DesiredDirection = West;
		else if (NavStruct.StartNodePtr->LinkS == tNode)
			NavStruct.DesiredDirection = South;
		else if (NavStruct.StartNodePtr->LinkN == tNode)
			NavStruct.DesiredDirection = North;

		if ((NavStruct.DesiredDirection == NavStruct.CurrDirection))
		{
			// all right!  we are set
			// no need to travel to the first node so pop it off the stack.
			RPPopStack(&tNode);
			// now get the destination node
			RPPopStack(&tNode);
			// assign the destination node
			NavStruct.DestNodePtr = GGetNode(tNode);			 
		}
		else
		{
			// still special case because both nodes are the current one.
			NavStruct.DestNodePtr = NavStruct.StartNodePtr;
		}

		// O.K now we are in the general case where we are moving from one point
		// to the next.
		NavStruct.NavStage = NavSwitchDirection; // not in initial state
		XPState = XPDecision; // but in a decision state
		XPFlushIR(); // turns out this is something we can't do enough of either!!

		
	}
	else if (NavStruct.NavStage == NavSwitchDirection)
	{	
		// have to decision where to go
		if (NavStruct.DestNodePtr->Index == NavStruct.StartNodePtr->Index)
		{
			// hmmm were just in the inital state, will have to get back on track by performing
			// a blind turn
			// get the old ticks before the turn				
			NavStruct.OldTicks = SEGetTick2();

			
			if (NavStruct.OldTicks < 5) 
			{
				// won't have to do any backtracking if < 10 just turn into proper direction
				if (((NavStruct.DesiredDirection == North) && (NavStruct.CurrDirection == South))
					|| ((NavStruct.DesiredDirection == East) && (NavStruct.CurrDirection == West))
					|| ((NavStruct.DesiredDirection == South) && (NavStruct.CurrDirection == North))
					|| ((NavStruct.DesiredDirection == West) && (NavStruct.CurrDirection == East)))
				{
					// still has to do a 180
					// now do a 180
					MVTurn(TICK180);	
					// log the turn
					XPLogTurnClk();
					XPLogTurnClk();

					
					RPSetupEnum(); // setup peak mode		

					// get the second item out of the stack
					RPEnum(&tNode);
					RPEnum(&tNode);
					tNodePtr = GGetNode(tNode);						

					// if the item is a decision..
					if (tNodePtr->NodeType == Decision)
					{
						// special case of this, boot out
						NavStruct.NavStage = NavNormal;
						XPNavPickDirection(); 
						XPState = GotoDecision;
						FirstTime = TRUE;
						return;
					}
					
				}
				else
				{
					// figure out which instruction to give
					// turn +/- 90 degrees to get on track
					if (NavStruct.StartNodePtr->NodeType == Decision)
					{
						// in this case we do nothing because the decision nodes
						// inside or outside turn will take care of it, just pop the first node
						// off the stack
						RPPopStack(&tNode);
						XPState = XPDecision;
						NavStruct.NavStage = NavNormal;
						XPNavPickDirection();
						return; // skip out early for this case

					}
					else
					{
						Direction = UShiftDirectionClk(NavStruct.CurrDirection);
						if (Direction = NavStruct.DesiredDirection)
						{
							MVTurn(-12);
							XPLogTurnClk();
						}
						else
						{
							MVTurn(12);
							XPLogTurnCtr();
						}
					}
				}

			}			
			else
			{
				// now do a 180, now we are on track
				MVTurn(TICK180);
				// log the turn
				XPLogTurnClk();
				XPLogTurnClk();
			}
			XPState = FollowWall;
			NavStruct.NavStage = NavNormal;
			// might not have got contact at the start so put on dead straight mode
			FWStruct.DeadStraight = TRUE;
		}
		else
		{
			if (NavStruct.StartNodePtr->NodeType == Decision)
			{
				// if we are still here, that means that we are going straight, which means
				// it is a dead straight
				FWStruct.DeadStraight = TRUE;
			}
			NavStruct.NavStage = NavNormal;
			XPState = FollowWall;
		}
		XPNavPickDirection(); // and pick the right direction to travel in

	}
	else // NavStruct.NavStage == NavNormal
	{	
		if (MStruct.LMState == LMStage3)
		{
			XPNavigationLandmark(); // log the landmark
			MStruct.LMState = LMStage1; // make sure, I think this was a source of
			// a lot of problems
		}
		else
		{
			if (*FWStruct.SideIR == NODETECTIOND && (XPState == XPDecision))
			{
				int adf = 34;
			}
		}
		if ((XPState == XPDecision))
		{
			// a node of some kind has been reached, make a decision			
			XPNavigationDecision();
		}
		else if (XPState == NavUnknownTo)
		{
			XPNavUnknownTo();
		}
		else if (XPState == NavUnknownFrom)
		{
			XPNavUnknownFrom();
		}
		else if (XPState == GotoDecision)
		{
			XPPrevState = GotoDecision;
			XPGotoDecision();		
		}
		else if (XPState == TurnInside)
		{
			XPTurnInside();
		}
		else if (XPState == TurnOutside)
		{
			XPTurnOutside();
		}
		else if (XPState == FollowWall)
		{               
			XPFW();			
		}
		else if (XPState == BlindFoward)
		{		
			SEResetTick2();
			MVGoFowardT(4,NavStruct.BlindFowardTicks);
			XPState = XPDecision;			
		}
		
	}
	

}

/************************************************************************
* Function XPNavigationDecision()
*
* PURPOSE
* Main decision function for navigation module
* USAGE
* When a decision node has been reached, a decision has to be made as where to go
*************************************************************************/
void AIExplorer::XPNavigationDecision()
{
	char tDirection,tDirection2;
	unsigned short NextNode,DDistance;
	NODE *tNodePtr;
	BOOL Done;


	// don't do anything if this happens, could be some lag until the main 
	// loop blocks out this function call
	if (NavStruct.GoalNodePtr->Index == NavStruct.StartNodePtr->Index)
		return;

	if (NavStruct.DestNodePtr == NULL)
		return;

	// special case
	// looks for a foward collision
	if (FWStruct.FowardCollisionAlert == TRUE)
	{
		FWStruct.FowardCollisionAlert = FALSE;
		// if there isn't a forward collision within the link, then signal and error (i.e we
		// are going to collide with something
		if (NavStruct.DestNodePtr->Special != AtWallDecision)
		{
			RPSetupEnum();

			Done = FALSE;
			while (Done == FALSE)
			{
				if (RPEnum(&NextNode) == FALSE)
				{
					Done = TRUE;
					NavStruct.NavError = TRUE;
					NavStruct.GoalNodePtr = NavStruct.StartNodePtr;
				}
				else
				{
					tNodePtr = GGetNode(NextNode);
					if (tNodePtr->Special == AtWallDecision)
					{
						Done = TRUE;				
						NavStruct.DestNodePtr = tNodePtr;
						// great this is what is it supposed to be
					}
					else if (tNodePtr->NodeType == Decision)
					{
						// we are sunk there is a collision allert
						Done = TRUE;
						NavStruct.NavError = TRUE;
						NavStruct.GoalNodePtr = NavStruct.StartNodePtr;
					}
					
				}
			}
		}
	}
	else if (NavStruct.DestNodePtr->NodeType != Decision)
	{
		// if the destanation is note equal to a decision node, then search for a decision
		// node in the stack, if there isn't one then we must have missed a landmark node,
		// and we should quite and flag an error
		RPSetupEnum();

		Done = FALSE;
		while (Done == FALSE)
		{
			if (RPEnum(&NextNode) == FALSE)
			{
				Done = TRUE;
				NavStruct.NavError = TRUE;
				NavStruct.GoalNodePtr = NavStruct.StartNodePtr;
			}
			else
			{
				tNodePtr = GGetNode(NextNode);
				if (tNodePtr->NodeType == Decision)
				{
					// we are in business
					Done = FALSE;
					while (Done == FALSE)
					{
						RPPopStack(&NextNode);
						if (NextNode == tNodePtr->Index)
							Done = TRUE;
					}
					NavStruct.DestNodePtr = tNodePtr;
				}
			}
		}
	}
	
	// find out what direction to go in.
	NavStruct.StartNodePtr = NavStruct.DestNodePtr;
	//set the current node in the graph
	GSetCurrNode(NavStruct.StartNodePtr->Index);

	Done = FALSE;
	while (Done == FALSE)
	{
		if (RPPopStack(&NextNode) == FALSE)
		{		
			// just in case, should be the same though
			NavStruct.GoalNodePtr = NavStruct.StartNodePtr;
			SEResetTick2();
			return;
		}
		else
		{
			NavStruct.DestNodePtr = GGetNode(NextNode);
			Done = TRUE;
		}
		if (NavStruct.DestNodePtr->Index == NavStruct.StartNodePtr->Index)
			Done = FALSE;
	// it while either find a decision node, or quite out
	}

	// special case if it is an unknown node.  It has its own function call to take care of it
	if (NavStruct.DestNodePtr->NodeType == Unknown)
	{
		XPState = NavUnknownTo;
		NavStruct.EnteringUnknown = TRUE;
		return;
	}

	// find out which direction is the link
	if (NavStruct.StartNodePtr->LinkE == NavStruct.DestNodePtr->Index)		
		tDirection = East;		
	else if (NavStruct.StartNodePtr->LinkW == NavStruct.DestNodePtr->Index)
		tDirection = West;
	else if (NavStruct.StartNodePtr->LinkS == NavStruct.DestNodePtr->Index)
		tDirection = South;
	else if (NavStruct.StartNodePtr->LinkN == NavStruct.DestNodePtr->Index)
		tDirection = North;

	tDirection2 = UShiftDirectionCtr(NavStruct.CurrDirection);
	XPNavPickDirection(); // always make sure we are looking at the right side, in terms of which to follow

	if (NavStruct.DestNodePtr->NodeType == Decision)
	{
		// calculate the distance between the nodes
		DDistance = abs(NavStruct.StartNodePtr->Location.x - NavStruct.DestNodePtr->Location.x)
					+abs(NavStruct.StartNodePtr->Location.y - NavStruct.DestNodePtr->Location.y);
 
		// add some undershoot because overshoot is a big problem
		if (DDistance > 3)
			DDistance -= 3;
		// special case
		if (tDirection == NavStruct.CurrDirection)
		{
			MVGoFowardT(4,DDistance);
			XPState = XPDecision;
		}
		else if (tDirection == tDirection2)
		{
			// First turn then go foward
			MVTurn(12);
			XPLogTurnCtr();
			if (NavStruct.DestNodePtr->Special == AtWallDecision)
			{
				XPState = FollowWall;
			}
			else
			{
				NavStruct.BlindFowardTicks = DDistance;
				XPState = BlindFoward;			
			}		
		}
		else
		{
			// first go foward, then go into a turning state
			MVTurn(-12);
			XPLogTurnClk();
			if (NavStruct.DestNodePtr->Special == AtWallDecision)
			{
				XPState = FollowWall;
			}
			else
			{
				NavStruct.BlindFowardTicks = DDistance;
				XPState = BlindFoward;			
			}	
		}
		// Note after turning done, this function will be called again
		
	}
	else if (tDirection == NavStruct.CurrDirection)
	{
		// go straight
		FWStruct.DeadStraight = TRUE;
		XPPrevState = XPState;
		XPFlushIR();				
		XPState = FollowWall;
	}
	else if (tDirection == tDirection2)
	{
		// go ctr clockwise	
		if (Options.FollowWallRight == TRUE)
			XPLogTurnCtr();
		else if (Options.FollowWallLeft == TRUE)
			XPLogTurnCtr();
		//ASSERT(XPPrevState == GotoDecision);
		XPState = TurnInside;
		// special case for wall decision node, so a landmark node isn't "discovered on the inside turn"
		if (NavStruct.StartNodePtr->Special == AtWallDecision)
			FirstTime = FALSE;
	}
	else 
	{
		if (Options.FollowWallRight == TRUE)
			XPLogTurnClk();
		else if (Options.FollowWallLeft == TRUE)
			XPLogTurnClk();
//		ASSERT(XPPrevState == GotoDecision);
		XPState = TurnInside;
		// special case for wall decision node, so a landmark node isn't "discovered on the inside turn"
		if (NavStruct.StartNodePtr->Special == AtWallDecision)
			FirstTime = FALSE;
	}

	XPNavPickDirection(); // even though this waste time, it need to be done
	// to make sure we are following in the right direction


}


/************************************************************************
* Function AISetCurrNode(unsigned short Node)
*
* PURPOSE
* Someone outside changes where we are on the graph
*************************************************************************/
void AIExplorer::AISetCurrNode(unsigned short Node)
{
	if (GSetCurrNode(Node) == TRUE)
	{
		NavStruct.StartNodePtr = GGetNode(Node);
		NavStruct.GoalNodePtr = GGetNode(Node);
	}
}

/************************************************************************
* Function AISetDestination(unsigned short Node)
*
* PURPOSE
* From outside someone set the destination node, lets travel to it
*************************************************************************/
void AIExplorer::AISetDestination(unsigned short Node)
{
	if (GisNodeValid(Node) && (NavStruct.StartNodePtr != NULL))
	{		
		NavStruct.GoalNodePtr = GGetNode(Node);
		NavStruct.NavStage = NavInitial;
		RPGenerateShortestPath(NavStruct.StartNodePtr->Index,NavStruct.GoalNodePtr->Index);
		XPFlushIR();
	}
}


/************************************************************************
* Function BOOL UIsNodeMatch(NODE *In, short InRoC, char Direction)
*
* PURPOSE
* Helper function for navigation, return true if the node are the same, 
* false if not
*************************************************************************/
BOOL AIExplorer::UIsNodeMatch(NODE *In, short InRoC, char Direction)
{
	char nLandmarkDirection;
	short NodeRoC;
	// now look for the landmark in the node list
	if (In->LandMarkE != 0)
	{
		nLandmarkDirection = East;
		NodeRoC = NavStruct.DestNodePtr->LandMarkE;
	}
	else if (In->LandMarkW != 0)
	{
		nLandmarkDirection = West;
		NodeRoC = In->LandMarkW;
	}
	else if (In->LandMarkS != 0)
	{
		nLandmarkDirection = South;
		NodeRoC = In->LandMarkS;
	}
	else if (In->LandMarkN != 0)
	{
		nLandmarkDirection = North;
		NodeRoC = In->LandMarkN;
	}

	// first make sure the landmark direction is accurate
	// consider lost and gain detections to be the same, because it is relative to
	// the approach dirction
	if (nLandmarkDirection == Direction)
	{
		if ((NodeRoC == InRoC)
			|| (abs(NodeRoC - InRoC) < 10)
			|| (NodeRoC == -InRoC))
		{
			return TRUE;
		}
	}
	return FALSE;
}

/************************************************************************
* Function XPNavigationLandmark()
*
* PURPOSE
* This function is called when a landmark has been found, and when the 
* Navigator is running
*************************************************************************/
void AIExplorer::XPNavigationLandmark()
{
	char LandmarkDirection;
	unsigned short NextNode;
	short RoC,Ticks;
	NODE *tNodePtr;
	LANDMARK Type;
	BOOL Done;
	

	// don't do anything if this happens, could be some lag until the main 
	// loop blocks out this function call
	if (NavStruct.GoalNodePtr->Index == NavStruct.StartNodePtr->Index)
		return;


	// temp hack, want to make sure it actually is a landmark
	ASSERT((MStruct.LMStage1Distance != NODETECTIOND) || (MStruct.LMStage3Distance != NODETECTIOND));
	if 	((MStruct.LMStage1Distance != NODETECTIOND) || (MStruct.LMStage3Distance != NODETECTIOND))
	{
		// it is a landmark node;
		if (Options.FollowWallRight == TRUE)
		{
			// happened on the right side
			LandmarkDirection = UShiftDirectionClk(MStruct.Direction);

		}
		else if (Options.FollowWallLeft == TRUE)
		{
			// happended on the left side
			LandmarkDirection = UShiftDirectionCtr(MStruct.Direction);
		}

		// figure out the RoC;
		RoC = 0;
		if (MStruct.LMStage1Distance == NODETECTIOND)
		{
			if (Options.FollowWallRight == TRUE)
				RoC = RoCGainDetection;
			else //(Options.FollowWallLeft == TRUE
				RoC = RoCLostDetection; // it is relative on which direction you approach
						// the landmark
		}
		else if (MStruct.LMStage3Distance == NODETECTIOND)
		{
			if (Options.FollowWallRight == TRUE)
				RoC = RoCLostDetection;
			else //(Options.FollowWallLeft == TRUE
				RoC = RoCGainDetection;
		}
		else
		{
			Type = RateofChange;
			RoC = MStruct.LMStage3Distance - MStruct.LMStage1Distance;
			if ((NavStruct.CurrDirection == West) || (NavStruct.CurrDirection == South))
				RoC = -RoC;
		}
		Ticks = SEGetTick2();
	
		if (UIsNodeMatch(NavStruct.DestNodePtr,RoC,LandmarkDirection) == TRUE)
		{
			NavStruct.StartNodePtr = NavStruct.DestNodePtr;
			//set the current node in the graph
			GSetCurrNode(NavStruct.StartNodePtr->Index);
			if (RPPopStack(&NextNode) == FALSE)
			{				
				// just in case, should be the same though
				NavStruct.GoalNodePtr = NavStruct.StartNodePtr;
				MVGoFowardT(5,4); // go foward a bit
				SEResetTick2();				

				return;
			}
			else
				NavStruct.DestNodePtr = GGetNode(NextNode);
			
			// Landmark nodes always move straight so
			if (NavStruct.DestNodePtr->NodeType == Landmark)
				XPState = FollowWall;
			// otherwise it is XPState = decision and a decision is made
			SEResetTick2();
		}
		else
		{
			// O.K start to look elsewhere in the stack, to see if there is the correct
			// node before the decision node
			RPSetupEnum(); // initalize the peak
			Done = FALSE;
			while (Done == FALSE)
			{

				if (RPEnum(&NextNode) == FALSE)
				{
					Done = TRUE;
				}
				else
				{
					tNodePtr = GGetNode(NextNode);
					if (tNodePtr->NodeType == Decision)
					{
						Done = TRUE;						
					}
					else if (UIsNodeMatch(tNodePtr,RoC,LandmarkDirection))
					{
						NavStruct.StartNodePtr = tNodePtr;
						// O.K this is the right node, it is now the starting node.
						RPPopStack(&NextNode); // pop until the right node is presented
						while (NextNode =! tNodePtr->Index)
						{
							RPPopStack(&NextNode);
						}
						// have to get the next one;
						if (RPPopStack(&NextNode) == FALSE)
						{				
							// just in case, should be the same though
							NavStruct.GoalNodePtr = NavStruct.StartNodePtr;
							GSetCurrNode(NavStruct.StartNodePtr->Index);
							SEResetTick2();
							
							return;
						}
						else
						{
							NavStruct.DestNodePtr = GGetNode(NextNode);
							GSetCurrNode(NavStruct.StartNodePtr->Index);
						}

						Done = TRUE;

						// Landmark nodes always move straight so
						if (NavStruct.DestNodePtr->NodeType == Landmark)
							XPState = FollowWall;
						// otherwise it is XPState = decision and a decision is made
						
					}
				}
			}
		}

		MStruct.LMState = LMStage1;		

		if (NavStruct.DestNodePtr->Special == AtWallDecision)
		{
				XPState = FollowWall;
		}
		else if (NavStruct.DestNodePtr->NodeType == Decision)
		{
			XPState = GotoDecision;
			FirstTime = TRUE;
		}
		else if (NavStruct.DestNodePtr->NodeType == Landmark)
		{
			XPState = FollowWall;
		}
		// can't reset it enough :)
		SEResetTick2();
	}

}

/************************************************************************
* Function XPNavPickDirection()
*
* PURPOSE
* When the navigator initially decides on a path it must pick the correct following
* Direction, either right or left. otherwise there will be errors.
* ALGORITHM
* Peak on the stack. Ignore the first node if it is a first decision node and looks at the first landmark node,
* and use that to decide the direction, If there isn't a landmark node, when the next decision node
* is reached, or any other possibility, stick with the current direction
*************************************************************************/
void AIExplorer::XPNavPickDirection()
{
	unsigned short Node;
	NODE *NodePtr;
	char direction,direction2,LDirection;
	BOOL FirstOne,Done;
		
	FirstOne = TRUE;
	Done = FALSE;

	RPSetupEnum(); // set up the peek

	// loops once to twice
	while (Done == FALSE)
	{
		// due to the order which this function is called, it is possible
		// there is nothing in the stack, because the NavStruct contains the relevant node, 
		// force and exit though after this run
		if (RPEnum(&Node) == FALSE)
		{
			Done = TRUE;
			// check the destination node if possible
			if (NavStruct.DestNodePtr != NULL)
				Node = NavStruct.DestNodePtr->Index;
			else
				return;
		}

		NodePtr = GGetNode(Node);	
		if (NodePtr->NodeType == Decision)
		{
			if (FirstOne == TRUE)
			{
				FirstOne = FALSE;
			}
			else
			{
				Done = TRUE;
			}
		}
		else if (NodePtr->NodeType == Landmark)
		{
			direction = UShiftDirectionClk(NavStruct.CurrDirection);
			direction2 = UShiftDirectionCtr(NavStruct.CurrDirection);
			if (NodePtr->LandMarkE != 0)
				LDirection = East;
			else if (NodePtr->LandMarkW != 0)
				LDirection = West;
			else if (NodePtr->LandMarkS != 0)
				LDirection = South;
			else if (NodePtr->LandMarkN != 0)
				LDirection = North;

			if (direction == LDirection)
			{
				//O.K should be following it right
				Options.FollowWallRight = TRUE;
				Options.FollowWallLeft = FALSE;
				FWStruct.SideIR = &IR.RightIRV;
				
			}
			else if (direction2 == LDirection)
			{
				// O.K should be following it left
				Options.FollowWallRight = FALSE;
				Options.FollowWallLeft = TRUE;
				FWStruct.SideIR = &IR.LeftIRV;
			}
			else
			{
				// leave things the same
			}
			Done = TRUE;
		}
	}
}

/************************************************************************
* Function XPGotoDecision()
*
* PURPOSE
* The state where the robot need to go forward to its decision point
* USAGE
* For navigation, in mapping this state is rolled in with the turn states
*************************************************************************/
void AIExplorer::XPGotoDecision()
{
	if (FirstTime == TRUE)
	{
		FirstTime = FALSE;
		SEResetTick2();
	}

	MVGoFoward(5); // go foward a bit to a decision point	

	if (SEGetTick2() > 26)
	{	
		MVAllStop();
		XPState = XPDecision;
		SEResetTick2();
		XPFlushIR(); // for good measure
	}

}

/************************************************************************
* Function XPNavUnknownTo()
*
* PURPOSE
* This function is used when the robot is entering an unknown area, and wants
* to go in around a meter or so
* USAGE
* Main navigation control
* ALGORITHM
* The basic premise is to go foward a meter or so, perpindicular to the decision
* node.  However an unknown area often contains obstances, and a narrow passage way
* so this has to be taken into account
* NOTE
* This function and concept could make use of sonar navigation but I might not
* have time to do it.
*************************************************************************/
void AIExplorer::XPNavUnknownTo()
{
	char Direction,DirectionClk,DirectionCtr,adjust;
	// sanity check
	if (NavStruct.DestNodePtr->NodeType != Unknown)
	{
		XPState = XPDecision;
		NavStruct.NavError = TRUE;
		return;
	}
	
	if (NavStruct.EnteringUnknown == TRUE)
	{
		// first make sure we are facing the right direction
		NavStruct.EnteringUnknown = FALSE;
	
		// first make sure the direction is right
		if (NavStruct.StartNodePtr->LinkE == NavStruct.DestNodePtr->Index)	
			Direction = East;
		else if (NavStruct.StartNodePtr->LinkW == NavStruct.DestNodePtr->Index)	
			Direction = West;
		else if (NavStruct.StartNodePtr->LinkS == NavStruct.DestNodePtr->Index)	
			Direction = South;
		else if (NavStruct.StartNodePtr->LinkN == NavStruct.DestNodePtr->Index)	
			Direction = North;
		else
		{
			XPState = XPDecision;
			NavStruct.NavError = TRUE;
			return;
		}

		DirectionClk = UShiftDirectionClk(NavStruct.CurrDirection);
		DirectionCtr = UShiftDirectionCtr(NavStruct.CurrDirection);

		if (Direction == NavStruct.CurrDirection)
		{
		}
		else if (Direction == DirectionCtr) 
		{
			XPLogTurnCtr();
			MVTurn(12);
		}
		else if (Direction == DirectionClk) 
		{
			XPLogTurnClk();
			MVTurn(-12);
		}
		else // 180 turn
		{
			XPLogTurnCtr();
			XPLogTurnCtr();
			MVTurn(24);
		}
		// reverse logic so we don't have to reset it later
		FirstTime = FALSE;
	}
	else
	{
		if (FirstTime == FALSE)
		{
			// reverse logic so we don't have to reset it later
			FirstTime = TRUE;
			SEResetTick2();
			MVAllStop();
		}
		
		if ((IR.FrontIRV < Options.FrontCollisionThreadHold)
			|| (IR.RFrontIRV < Options.FrontSideThreshHold)
			|| (IR.LFrontIRV < Options.FrontSideThreshHold))
		{
			MVAllStop();
			// we are done
			NavStruct.StartNodePtr = NavStruct.DestNodePtr;
			GSetCurrNode(NavStruct.DestNodePtr->Index);
			NavStruct.EnteringUnknown = TRUE;
			NavStruct.NavError = TRUE;
			SEResetTick2();
			return;
		}
		else
		{
			adjust = 0;
			if (IR.LeftIRV < IR.RightIRV)
			{
				if (IR.LeftIRV < 20)
				{
					adjust = -3;
				}
				if (IR.LeftIRV < 13)
				{
						MVAllStop();
						// we are done
						NavStruct.StartNodePtr = NavStruct.DestNodePtr;
						GSetCurrNode(NavStruct.DestNodePtr->Index);
						NavStruct.EnteringUnknown = TRUE;
						SEResetTick2();
						NavStruct.NavError = TRUE;
						return;
				}
			}
			else
			{
				if (IR.RightIRV < 20)
				{
					adjust = 3;
				}
				if (IR.RightIRV < 13)
				{
						MVAllStop();
						// we are done
						NavStruct.StartNodePtr = NavStruct.DestNodePtr;
						GSetCurrNode(NavStruct.DestNodePtr->Index);
						NavStruct.EnteringUnknown = TRUE;
						SEResetTick2();
						NavStruct.NavError = TRUE;
						return;
				}
			}
		}

		if (SEGetTick2() > Options.MaxUnknownTicks)
		{
				MVAllStop();
				// we are done
				NavStruct.StartNodePtr = NavStruct.DestNodePtr;
				GSetCurrNode(NavStruct.DestNodePtr->Index);				
				NavStruct.EnteringUnknown = TRUE;
				SEResetTick2();
				NavStruct.NavError = TRUE;
				return;
			
		}

		// go foward slowly
		MVGoFowardWA(3,adjust);		
	}

}

/************************************************************************
* Function XPNavUnknownFrom()
*
* PURPOSE
* This function is used when the robot is leaving a unknown area to go to
* its decision node
* USAGE
* Main navigation control
* ALGORITHM
* The basic premise is to escape the area in which the you came from.  First rotate 180 (if possible), and either
* Go until the robot has reached its maximum without any wall contact. or until is has reached an open area, after
* hitting a contact
* NOTE
* This function and concept could make use of sonar navigation but I might not
* have time to do it.
*************************************************************************/
void AIExplorer::XPNavUnknownFrom()
{
	char Direction,adjust;
	// sanity check
	if (NavStruct.StartNodePtr->NodeType != Unknown)
	{
		XPState = XPDecision;
		NavStruct.NavError = TRUE;
		return;
	}
	
	// not the right name, but I might as well reuse it
	if (NavStruct.EnteringUnknown == TRUE)
	{
		// first make sure we are facing the right direction
		NavStruct.EnteringUnknown = FALSE;
	
		// first make sure the direction is right
		if (NavStruct.StartNodePtr->LinkE == NavStruct.DestNodePtr->Index)	
			Direction = East;
		else if (NavStruct.StartNodePtr->LinkW == NavStruct.DestNodePtr->Index)	
			Direction = West;
		else if (NavStruct.StartNodePtr->LinkS == NavStruct.DestNodePtr->Index)	
			Direction = South;
		else if (NavStruct.StartNodePtr->LinkN == NavStruct.DestNodePtr->Index)	
			Direction = North;
		else
		{
			XPState = XPDecision;
			NavStruct.NavError = TRUE;
			return;
		}

		// we are either in the right direction, or 180,
		// note I should do some checking to make sure that we could turn 180
		if (Direction == NavStruct.CurrDirection)
		{
		}
		
		else // 180 turn
		{
			XPLogTurnCtr();
			XPLogTurnCtr();
			MVTurn(24);
		}
		// reverse logic so we don't have to reset it later
		FirstTime = FALSE;
		NavStruct.GotDoorwayUnknown = FALSE;

	}
	else
	{
		// amble foward until either
		// 1) Maximum ticks reached
		// 2) a doorway contact was found then lost
		// 3) collision alert, flag nav error and quit out
		if (FirstTime == FALSE)
		{
			FirstTime = TRUE;
			SEResetTick2();
		}

		adjust = 0;
		if (IR.LeftIRV < IR.RightIRV)
		{
			if (IR.LeftIRV < 25)
			{
				adjust = -5;
			}
			if (IR.LeftIRV < 13)
			{
					MVAllStop();					
					// can't go anyfurther
					NavStruct.GoalNodePtr = NavStruct.StartNodePtr;
					GSetCurrNode(NavStruct.StartNodePtr->Index);
					NavStruct.EnteringUnknown = TRUE;
					SEResetTick2();
					NavStruct.NavError = TRUE;
					return;
			}
		}
		else
		{
			if (IR.RightIRV < 25)
			{
				adjust = 5;
			}
			if (IR.RightIRV < 13)
			{
					MVAllStop();
					// we are done
					NavStruct.StartNodePtr = NavStruct.DestNodePtr;
					GSetCurrNode(NavStruct.DestNodePtr->Index);
					NavStruct.EnteringUnknown = TRUE;
					SEResetTick2();
					return;
			}
		}
		// try to avoid hitting a wall on an angle;
		if ((IR.LFrontIRV < 60) && ((IR.FrontIRV == NODETECTIOND) || (IR.RFrontIRV == NODETECTIOND)))
		{
			adjust += -5;
		}
		else if ((IR.RFrontIRV < 60) && ((IR.FrontIRV == NODETECTIOND) || (IR.LFrontIRV == NODETECTIOND)))
		{
			adjust += 5;
		}


		if (SEGetTick2() > Options.MaxUnknownTicks)
		{
			// normal exit, to decision node
				MVAllStop();
				// we are done
				XPState = XPDecision;						
				NavStruct.EnteringUnknown = TRUE;
				SEResetTick2();
				return;
			
		}

		if (NavStruct.GotDoorwayUnknown == TRUE)
		{
			if ((IR.RightIRV == NODETECTIOND) && (IR.LeftIRV == NODETECTIOND))
			{
				// we are done
				XPState = XPDecision;			
				NavStruct.EnteringUnknown = TRUE;
				SEResetTick2();
				MVGoFowardT(5,30); // move foward 30 ticks for good measure though
				return;
			}
		}

		if ((IR.RightIRV != NODETECTIOND) && (IR.LeftIRV != NODETECTIOND)) 
		{
			NavStruct.GotDoorwayUnknown = TRUE;
		}

		// go foward slowly
		MVGoFowardWA(3,adjust);		
	}

}

/************************************************************************
* Function AISendCommand(XPMessageStruct Message)
*
* PURPOSE
* This function sends a directive such as map this area
* NOTE
* Since the mapper starts off, at this point there should already be one loop
*************************************************************************/
void AIExplorer::AISendCommand(XPMessageStruct Message)
{
	if (Message.MappingMode == TRUE)
	{
		MStruct.h_Instruction = Message;
		MStruct.h_InstructionValid = TRUE;
		// note NavStruct has the StartNodePtr from the close loop routine
		if (GisNodeValid(Message.DecisionNode) && (NavStruct.StartNodePtr != NULL))
		{			
			MStruct.CloseLoopIndex++; // increment, a new loop is being formed
			GSetCloseLoopMark(MStruct.CloseLoopIndex);
			MStruct.h_FirstDecisionNode = FALSE; // set to false, because a previous loop
			// has to exist
			MStruct.h_GoingtoNavPoint = TRUE;
			NavStruct.GoalNodePtr = GGetNode(Message.DecisionNode);
			NavStruct.NavStage = NavInitial;
			RPGenerateShortestPath(NavStruct.StartNodePtr->Index,NavStruct.GoalNodePtr->Index);
			XPFlushIR(); // reset it
			MVReset(); // reset it
			SNRReset(); // reset it
			if (NavStruct.StartNodePtr->Index == NavStruct.GoalNodePtr->Index)
			{
				RPReset(); // so it doesn't add just one link
				Options.MainActivity = Mapping;
				XPMappingSetupLoop();
			}
			else			
				Options.MainActivity = Navigating; // should be anyways
		}
	}
	else if (Message.NavigationMode == TRUE)
	{
		Options.MainActivity = Navigating;
	}

}

/************************************************************************
* Function XPMappingSetupLoop()
*
* PURPOSE
* After the first loop is mapped, every other loop is commanded.  This function
* is called after the robot has moved to the correct destination to start mapping again
*************************************************************************/
void AIExplorer::XPMappingSetupLoop()
{
	char DirClk,DirCtr;
	short tPossibilities[3];
//	NODE *tNodePtr;
	// setup the correct modes
	MStruct.h_GoingtoNavPoint = FALSE;
	Options.MainActivity = Mapping;
	MStruct.h_FirstDecisionNode = FALSE; 

	// clear the stack
	CLClearStack();

	// make sure the direction is correct.
	MStruct.Direction = NavStruct.CurrDirection;
	if (MStruct.Direction != MStruct.h_Instruction.Direction)
	{
		// ohoh will have to rotate the robot.
		DirClk = UShiftDirectionClk(MStruct.Direction);
		DirCtr = UShiftDirectionCtr(MStruct.Direction);
		if (DirClk == MStruct.h_Instruction.Direction)
		{
			// turn clockwise
			MVTurn(-12);
			XPLogTurnClk();
		}
		else if (DirCtr == MStruct.h_Instruction.Direction)
		{
			// turn counterclockwise
			MVTurn(12);
			XPLogTurnCtr();
		}
		else
		{
			// do a 180			
			MVTurn(24);
			XPLogTurnCtr();
			XPLogTurnCtr();
		}
	}
	// now log the new node
	GSetCurrNode(NavStruct.GoalNodePtr->Index); // set the current node in the graph

	// from the last decision node, figure out the correct input to
	// the add decision node function
	if (MStruct.Direction == North)
	{
		tPossibilities[0] = NavStruct.GoalNodePtr->LandMarkE; 
		tPossibilities[2] = NavStruct.GoalNodePtr->LandMarkN;
		tPossibilities[1] = NavStruct.GoalNodePtr->LandMarkW;
	}
	else if (MStruct.Direction == East)
	{
		tPossibilities[0] = NavStruct.GoalNodePtr->LandMarkS; 
		tPossibilities[2] = NavStruct.GoalNodePtr->LandMarkE;
		tPossibilities[1] = NavStruct.GoalNodePtr->LandMarkN;
	}
	else if (MStruct.Direction == South)
	{
		tPossibilities[0] = NavStruct.GoalNodePtr->LandMarkW; 
		tPossibilities[2] = NavStruct.GoalNodePtr->LandMarkS;
		tPossibilities[1] = NavStruct.GoalNodePtr->LandMarkE;
	}
	else if (MStruct.Direction == West)
	{
		tPossibilities[0] = NavStruct.GoalNodePtr->LandMarkN; 
		tPossibilities[2] = NavStruct.GoalNodePtr->LandMarkW;
		tPossibilities[1] = NavStruct.GoalNodePtr->LandMarkS;
	}
	// now add the decision node, cheat a bit by moving it foward 5 ticks, so
	// it looks better graphically
	/*XPMappingAddDecisionNode(tPossibilities,20,MStruct.h_Instruction.OffsetDirection);
	tNodePtr = GGetCurrNode();
	// give it the proper index;
	tNodePtr->hasCloseLoopMarker = MStruct.CloseLoopIndex;*/

	

	// since the direction was forces to be the one we want to go, go dead straight
	XPState = FollowWall;
	FWStruct.DeadStraight = TRUE;
	
	// map the right direction.
	if (MStruct.h_Instruction.RightSideMapping == TRUE)
	{
		Options.FollowWallRight = TRUE;
		Options.FollowWallLeft = FALSE;
		FWStruct.SideIR = &IR.RightIRV;
	}
	else
	{
		Options.FollowWallLeft = TRUE;
		Options.FollowWallRight = FALSE;
		FWStruct.SideIR = &IR.LeftIRV;
	}

	FWStruct.DeadResetSE2 = TRUE; // causes a reset because there may have been turns

}

/************************************************************************
* Function AIParseMessage(char *ptr, int DummyLength)
*
* PURPOSE
* Parses an incomming message
*************************************************************************/
void AIExplorer::AIParseMessage(char *ptr, int DummyLength)
{
	unsigned short DecisionNode,Direction,Side;
	char *ptr2;

	if (*ptr != '<')
		return;

	if (StreamMode == ReceivingStream)
	{		
		// stream mode command
		ptr++;
		ptr2 = ptr+1;
		// the double BB won't exist is a node data
		if ((*ptr == 'B') && (*ptr2 == 'B'))
		{

			ptr++;
			ptr++;
			if (*ptr == '1')
			{
				// the PC is trying to load a map, setup stream mode
				// GInitialize(); // when it actual robot
				StreamMode = ReceivingStream;
				AISendStreamAck();
			}
			else if (*ptr == '2')
			{
				StreamMode = Off;
				AISendStreamAck();
			}
		}
		else
		{
		// O.K load load data into graph
			GLoadGraph(ptr);
			AISendStreamAck();
		}
	}
	else
	{		
		ptr++;
		if (*ptr == '6')
		{
			
			// it is an instruction message
			ptr++;
			if (*ptr == '1') 
			{
				AIAck(); // Acknowedge the message
				// it is a mapping command
				ptr +=2;
				sscanf(ptr,"%d|%d|%d>",&DecisionNode,&Direction,&Side);

				if (Side == 1)
				{
					MStruct.h_Instruction.RightSideMapping = TRUE;
					MStruct.h_Instruction.LeftSideMapping = FALSE;
				}
				else //(Side == 2)
				{
					MStruct.h_Instruction.RightSideMapping = FALSE;
					MStruct.h_Instruction.LeftSideMapping = TRUE;
				}

				if (Direction == 1)
				{
					MStruct.h_Instruction.OffsetDirection = MStruct.h_Instruction.Direction
						= North;
				}
				else if (Direction == 2)
				{
					MStruct.h_Instruction.OffsetDirection = MStruct.h_Instruction.Direction
						= East;
				}
				else if (Direction == 3)
				{
					MStruct.h_Instruction.OffsetDirection = MStruct.h_Instruction.Direction
						= South;
				}
				else if (Direction == 4)
				{
					MStruct.h_Instruction.OffsetDirection = MStruct.h_Instruction.Direction
						= West;
				}
				MStruct.h_Instruction.MappingMode = TRUE;
				MStruct.h_Instruction.NavigationMode = FALSE;
				MStruct.h_Instruction.DecisionNode = DecisionNode;
				MStruct.h_InstructionValid = TRUE;
				// note NavStruct has the StartNodePtr from the close loop routine
				if (NavStruct.StartNodePtr == NULL)
				{
					GSetCurrNode(1);
					NavStruct.StartNodePtr = GGetCurrNode();
				}
				if (GisNodeValid(DecisionNode) && (NavStruct.StartNodePtr != NULL))
				{			
					MStruct.CloseLoopIndex++; // increment, a new loop is being formed
					GSetCloseLoopMark(MStruct.CloseLoopIndex);
					MStruct.h_FirstDecisionNode = FALSE; // set to false, because a previous loop
					// has to exist
					MStruct.h_GoingtoNavPoint = TRUE;
					NavStruct.GoalNodePtr = GGetNode(DecisionNode);
					NavStruct.NavStage = NavInitial;
					RPGenerateShortestPath(NavStruct.StartNodePtr->Index,NavStruct.GoalNodePtr->Index);
					XPFlushIR(); // reset it
					MVReset(); // reset it
					SNRReset(); // reset it
					if (NavStruct.StartNodePtr->Index == NavStruct.GoalNodePtr->Index)
					{
						RPReset(); // so it doesn't add just one link
						Options.MainActivity = Mapping;
						XPMappingSetupLoop();
					}
					else			
						Options.MainActivity = Navigating; // should be anyways
				}	
			}
			
			else if (*ptr == '2') 
			{
				AIAck(); // Acknowedge the message
				// it is a navigation message
				ptr += 2;
				sscanf(ptr,"%d>",&DecisionNode);	

				MStruct.h_Instruction.MappingMode = FALSE;
				MStruct.h_Instruction.NavigationMode = TRUE;
				Options.MainActivity = Navigating;

				if (NavStruct.StartNodePtr == NULL)
				{
					GSetCurrNode(1);
					NavStruct.StartNodePtr = GGetCurrNode();
				}
				
				AISetDestination(DecisionNode);	
			}		
		}
		else if (*ptr == '8')
		{
			AIAck(); // Acknowedge the message
			// time to set the direction
			ptr ++;
			sscanf(ptr,"%d|%d>",&DecisionNode,&Direction);

			if (Direction == 1)
			{
				MStruct.Direction = NavStruct.CurrDirection
					= North;
			}
			else if (Direction == 2)
			{
				MStruct.Direction = NavStruct.CurrDirection
					= East;
			}
			else if (Direction == 3)
			{
				MStruct.Direction = NavStruct.CurrDirection
					= South;
			}
			else if (Direction == 4)
			{
				MStruct.Direction = NavStruct.CurrDirection
					= West;
			}

			GSetCurrNode(DecisionNode);
			NavStruct.StartNodePtr = GGetCurrNode();
			AISendUpdate();
		}
		else if (*ptr == 'B')
		{
			// stream mode command
			ptr++;
			ptr++;
			if (*ptr == '1')
			{
				// the PC is trying to load a map, setup stream mode
				// GInitialize(); // when it actual robot
				StreamMode = ReceivingStream;
				AISendStreamAck();
			}
			else if (*ptr == '2')
			{
				StreamMode = Off;
				AISendStreamAck();
			}
		}
		else
		{
			return;
		}
	}


}

/************************************************************************
* Function AISendUpdate()
*
* PURPOSE
* Sends an update of position, currnode and direction if in navigating mode
*************************************************************************/
void AIExplorer::AISendUpdate()
{
	unsigned short Direction;
	NODE *nPtr;
	char Message[30];

	nPtr = GGetCurrNode();

	if (MStruct.Direction == North)
		Direction = 1;
	else if (MStruct.Direction == East)
		Direction = 2;
	else if (MStruct.Direction == South)
		Direction = 3;
	else if (MStruct.Direction == West)
		Direction = 4;

	if (nPtr != NULL)
	{

		sprintf(Message,"<A%d|%d|%d>",nPtr->Index,Direction,SEGetTick2());
	}

	AISend(Message);




}

/************************************************************************
* Function AIAck()
*
* PURPOSE
* Acknoweldges a command message
*************************************************************************/
void AIExplorer::AIAck()
{
	char message[10];
	sprintf(message,"<C>");
	AISend(message);

}

void AIExplorer::AISendStreamAck()
{
	char message[10];
	sprintf(message,"<BB5>");
	AISend(message);

}
