// AIExplorer.h: interface for the AIExplorer class.
//
//////////////////////////////////////////////////////////////////////

#if !defined(AFX_AIEXPLORER_H__C4A63DFF_BB55_40E9_A979_495A61C80F9A__INCLUDED_)
#define AFX_AIEXPLORER_H__C4A63DFF_BB55_40E9_A979_495A61C80F9A__INCLUDED_

#include "GenericModel.h"	// Added by ClassView
#include "AIGraph.h"
#include "AIGraphH.h"
#if _MSC_VER > 1000
#pragma once
#endif // _MSC_VER > 1000


// Tranlation of the Robot API from the simulator to the embedded system
#define IRGetRightD TheModel->TheModel.Sensors.IRSensors.GetRightIR
#define IRGetFrontD TheModel->TheModel.Sensors.IRSensors.GetFrontIR
#define IRGetRFrontD TheModel->TheModel.Sensors.IRSensors.GetRFowardIR
#define IRGetLeftD TheModel->TheModel.Sensors.IRSensors.GetLeftIR
#define IRGetLFrontD TheModel->TheModel.Sensors.IRSensors.GetLFowardIR
#define IRGetRRearD TheModel->TheModel.Sensors.IRSensors.GetRRearIR
#define IRGetLRearD TheModel->TheModel.Sensors.IRSensors.GetLRearIR

#define MVGoFoward TheModel->MVGoFoward
#define MVAllStop TheModel->MVAllStop
#define MVIsBlocking TheModel->MVIsBlocking
#define MVTurn TheModel->MVTurn
#define MVGoFowardWA TheModel->MVGoFowardWA
#define MVGoFowardT TheModel->MVGoFowardT
#define MVReset TheModel->MVReset

#define BMPGetByte TheModel->TheModel.Sensors.BumpSensors.GetBumpSensorByte

// used for turning
#define SEGetTick1 TheModel->TheModel.Sensors.ShaftEncoders.SEGetTick1
#define SEResetTick1 TheModel->TheModel.Sensors.ShaftEncoders.SEResetTick1

// used for logging distance
#define SEGetTick2 TheModel->TheModel.Sensors.ShaftEncoders.SEGetTick2
#define SEResetTick2 TheModel->TheModel.Sensors.ShaftEncoders.SEResetTick2

#define KRNLGetTicks TheModel->KRNLGetTicks // returns a number from 0 to 999
#define KRNLGetTicksL TheModel->KRNLGetTicksL // returns a unsigned long value

#define SNRIsBlocking TheModel->TheModel.Sensors.SonarSensor.SNRIsBlockingA
#define SNRReset TheModel->TheModel.Sensors.SonarSensor.SNRResetA
#define SNRSetFowardScan TheModel->TheModel.Sensors.SonarSensor.SNRSetFowardScanA
#define SNRGetFowardDistance TheModel->TheModel.Sensors.SonarSensor.SNRGetFowardDistanceA
#define SNRSet3PointScan TheModel->TheModel.Sensors.SonarSensor.SNRSet3PointScanA
#define SNRGet3PointDistance TheModel->TheModel.Sensors.SonarSensor.SNRGet3PointDistanceA

#define IRFILTERING 30 // Used to store past raw IR values, for filtering

#define NIRLOG 100 // How many values are stored in the IR log

#define Ratio45 0.707f

// macro for the sin or cos of 45 degrees integer approximation
#define RATIO45(in) (in*7)/10

// macro for is the two integers the same sign?  Might be faster then multiplication
#define SAMESIGN(in1,in2) ((in1 > 0) && (in2 > 0)) || ((in1 < 0) && (in2 < 0)) || in1 == 0 || in2 == 0

#define COUNTERCLOCKWISE 1
#define CLOCKWISE -1
#define TICK180 24 // the amount of ticks for a 180 degree turn

#ifdef SIMULATOR
#define AISend(in) ::SendMessage(ParentPtr->m_hWnd,WM_UPDATE_MESSAGE,(WPARAM) &in,0)
#endif

class AIExplorer  
{
public:

	void AISendCommand(XPMessageStruct Message);	

		// functionality for sending sending data.
	void AISendStreamAck(void);
	STREAMMODE StreamMode;
	void AIAck(void);
	CWnd *ParentPtr;
	void AISendUpdate(void);
	void AIParseMessage(char* ptr, int DummyLength);
	void AISendStates(void);


	// stores all of the IR information
    struct IRSTRUCT
    {
        short adjustment;

        short FWStraightenDelta; 		

		// the filtering storage
        unsigned char FrontIR[IRFILTERING];
        unsigned char RightIR[IRFILTERING];
        unsigned char LeftIR[IRFILTERING];
        unsigned char RFrontIR[IRFILTERING];
        unsigned char LFrontIR[IRFILTERING];
		unsigned char RRearIR[IRFILTERING];
        unsigned char LRearIR[IRFILTERING];


		// the current reported values
		unsigned char FrontIRV;
		unsigned char RightIRV;
		unsigned char LeftIRV;
		unsigned char RFrontIRV;
		unsigned char RRearIRV;
		unsigned char LFrontIRV;
		unsigned char LRearIRV;
		
		// the right side IR log
		unsigned char IRLogValueR[NIRLOG];
		unsigned long IRLogTimeR[NIRLOG];
		BOOL IRLogisNewR[NIRLOG];
		short IRLogIndexR; // because the log is a circular array
		short IRLogDeltaR; // logs a change in slope.
		BOOL IRLogPositiveSlopeR; // tracks the current slope
		unsigned char IRLogLockR; // a lock Distance

		// the left side IR log
		unsigned char IRLogValueL[NIRLOG];
		unsigned long IRLogTimeL[NIRLOG];
		BOOL IRLogisNewL[NIRLOG];
		short IRLogIndexL; // because the log is a circular array
		short IRLogDeltaL; // logs a change in slope.
		BOOL IRLogPositiveSlopeL;  // tracks the current slope
		unsigned char IRLogLockL; // a lock Distance
	
		short IRLowPassThreshold;  // the threshold of when to consider that a change in reading

		// is actually an object of different depth.
    };
 
    IRSTRUCT IR;

    enum BUMPERSTATE
    {
        None,
        FrontRight,
        FrontLeft,
        Front,
        LeftSide,
        RightSide,
        Rear
    };

    BUMPERSTATE BumperState;

	enum MAINACTIVITY {Mapping,Navigating};

	// options such as varius thresholds
    struct XPOPTIONS
    {
        short FrontThreshHold; // when to start turning when the front detects this distance
        short FrontCollisionThreadHold; // for the front sensor
		short FrontSideThreshHold; // for the front side sensors (15 degrees offset
		short MinSideDistance; // the minimum distance to the side wall.
		short MaxSideDistance; // the maximum distance to be away from a wall

        short MaximumTurnTicks; // the maximum distance without a contact to given up     
		short MaxUnknownTicks; // the maximum distance to go inside an unknown area
		short MaximimumDeadStraightTicks; // the maximum distance to go dead straight without giving up

        BOOL FollowWallLeft;
        BOOL FollowWallRight;
		MAINACTIVITY MainActivity;		
    }; 

    XPOPTIONS Options;

	// main explorer states
    enum XPSTATE
    {
        Initial,
        XPDecision,
		GotoDecision, // basically go foward for 25 ticks
		NavUnknownTo,
		NavUnknownFrom,
        Collision,
        FollowWall,
        TurnInside,
        TurnOutside, 
		BlindFoward,
    };

	 enum XPINTERNALSTATE
    {
	   XPIInitial,
       MFGetMoreData, // move foward and get more data	  
	   MakeDecision
    };

    XPSTATE XPState;
    XPSTATE XPPrevState;  
	
	XPINTERNALSTATE XPIState;

	// turning sub states
    enum TURNINGSTATE
    {
        TRStage1,
        TRStage2,
        TRStage3
    }; 

    // contains for FW and turning varaibles
    struct FWSTRUCT
    {
		BOOL DeadStraight; // go straight until wall contact is found
		BOOL DeadResetSE2;
        TURNINGSTATE TurningStage; // also used for turning
        short PrevSideIRDistance; // a general storage for previous IR distance, // used for the turning functions
		short TotalTurnTicks; // used in turning to timeout eventially		
		unsigned char *SideIR; // could be either left and right
		BOOL TurnClockWise; // which way to turn
		BOOL FowardCollisionAlert; // if a foward collision is going to happen

    } FWStruct;

	enum LANDMARKSTAGES
    {
        LMStage1, // in smooth area, no landmarks
        LMStage2, // in a landmark transient area
        LMStage3, // back in a smooth area, should log landmark
    }; 

	//////////////////////////////////////////////////////////////
	// Mapping functionality
	//////////////////////////////////////////////////////////////

	// contains variables used for maintaining (creating) the graph (map)
	struct MSTRUCT
	{
		// lowel level variables
		BOOL IgnoreOnce;	
		LANDMARKSTAGES LMState;
		unsigned char LMStage1Distance; // the "before" distance
		unsigned char LMStage3Distance; // the "after" distance	
		BOOL Starting; // the mapping has just begun
		BOOL AddDecision; // tells it to add a decision node, temp hack
		BOOL isWallDecisionNode;
		char Direction; // the direction of travel
	

		// higher level variables
		BOOL h_HasFirstLoop; // if this is the first loop
		BOOL h_GoingtoNavPoint; // if the robot is travelling to the next point to start mapping
		BOOL h_FirstDecisionNode; // if this is the first decision node placed down in this loop
		BOOL h_InstructionValid;
		XPMessageStruct h_Instruction; // the instruction of what to do once the nav point, has 
									// been reached
		short CloseLoopIndex; // an always incrementing index for every loop

	} MStruct;

	void XPMappingSetupLoop();
	BOOL UIsNodeMatch(NODE *In, short InRoC, char Direction);
	
	BOOL XPMappingAddDecisionNode(short SNRDistance[3], short CheatSE, char CheatDirection, BOOL GoingStraight);
	BOOL FirstTime;

	enum NAVSTAGE
    {
        NavInitial, // a new path was just entered
        NavSwitchDirection, // time to switch direction on the path
        NavNormal // back to normal navigation
    };

	// Make a decision when mapping
	void XPMappingDecision(void);

	// map updating module
	void XPUpdateMap(void);

	//////////////////////////////////////////////////////////////
	// Navigation functionality
	//////////////////////////////////////////////////////////////

	// main nav struct
	struct NAVSTRUCT
	{		
		NODE *StartNodePtr; // the node which the robot is based out of.
		NODE *DestNodePtr;  // the current destination node.
		NODE *GoalNodePtr; // the goal to move to.  If equal to the start node, then the
						   // robot is done moving an may rest.
		char CurrDirection; // the current direction
		char DesiredDirection; // for when initially going to a new direction
		NAVSTAGE NavStage; // if the nav point has just been inputted
		short OldTicks; // tracks ticks;
		BOOL NavError; // status flag
		BOOL GotDoorwayUnknown; // boolean for the from unknown routine
		BOOL EnteringUnknown;
		short BlindFowardTicks; // Used when the blind foward state is used
	} NavStruct;

	// navigation functions
	void XPNavUnknownFrom(void);
	void XPNavUnknownTo(void);
	void XPGotoDecision(void);
	void XPNavPickDirection(void);
	void XPNavigationLandmark(void);
	void XPNavigationDecision(void);
	void AISetDestination(unsigned short Node);
	void AISetCurrNode(unsigned short Node);
	void XPRunNavigation(void);
  
	inline short ULogIndex(short CurrIndex, short Inc)
    {
		short tIndex = CurrIndex + Inc;

		if (Inc > 0 && Inc < NIRLOG)
			return tIndex;
		else if (Inc < 0)
		{
			while (tIndex < 0)
			{
				tIndex += NIRLOG;
			}
			return tIndex;
		}
		else // Inc >= NIRLOG
		{
			while (tIndex >= NIRLOG)
			{
				tIndex -= NIRLOG;
			}
			return tIndex;
		}
	}



	// follow wall module
	void XPFW(void);

	// main running function
    void AIRun(void);


	// turning modules
	void XPTurnOutside(void);
	void XPTurnInside(void);
	// helpers
	void XPLogTurnCtr(void);
	void XPLogTurnClk(void);

	// for simulator
	void AISendCurrPosition(short *Distance, char *TDirection);


	// debug output
	short DebugAdjust;
;


	// checks if the robot need to turn (i.e collision)
	BOOL XPFWCheckForDecision(void);
	
	// collision functions
	void XPDBumperCollision(void);
	BOOL XPCheckforCollision(void);	

	// check for collision functions
	BOOL CheckForWall(void);
	BOOL XPFWCheckForTurn(void);

	// IR module functions
    void XPFlushIR(void);
	void XPUpdateIR(void);
	void XPUpdateIRLogL(void);
	void XPUpdateIRLogR(void);

    // updates the bumper sensor
	void XPUpdateBumper(void);
	

    // the model pointer
	SGenericModel *TheModel;
    
	// one time initilization function
	void AIInitialize(void);

	// run time timed frequency module
	BOOL XPRunTime(void); // check if it is time to run
    short TicksStart;  // the starting tick value for XP Run Time
    short TicksDelta;  // how many ticks have to go by in order for XPRunTime to return true
    short TicksReloadValue;    
    void XPReloadTicks(void); // reload run time counter
    void XPAddTicks(short ticks); // add extra ticks to the delay. Like a sleep function
	
	AIExplorer(void);
	virtual ~AIExplorer();

};

#endif // !defined(AFX_AIEXPLORER_H__C4A63DFF_BB55_40E9_A979_495A61C80F9A__INCLUDED_)
