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

#if !defined(AFX_SUBSUMPTION_H__5AFFAC17_C651_4F0D_8589_A45449F93ACE__INCLUDED_)
#define AFX_SUBSUMPTION_H__5AFFAC17_C651_4F0D_8589_A45449F93ACE__INCLUDED_

#include "GenericModel.h"	// Added by ClassView
#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 MVGoFoward TheModel->MVGoFoward
#define MVAllStop TheModel->MVAllStop
#define MVIsBlocking TheModel->MVIsBlocking
#define MVTurn TheModel->MVTurn
#define MVGoFowardWA TheModel->MVGoFowardWA
#define MVReset TheModel->MVReset

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

#define KRNLGetTicks TheModel->KRNLGetTicks

#define TICKS360 48+24 // 360 + 180 degrees

enum STATES
{
    Turning,
    FollowWall, // concept 1
    FWWithinError, // concept 2
    FWExceedError, // concept 2
    FWStraightenUp,// concept 2
    Initial,
    Collision,
    TurnArc
};

class Subsumption  
{
public:	
	void SUBTurnArc();


    struct SUBSUMPTIONOPTIONS
    {
        short FrontThreshHold;
        short ErrorMargin;        
        short RightIdeal;
        short PollingCount;
        short PollingDelay;
        

        short FWExceedSpeed;
        short FWExceedAngleAdjust;
        short FWExceedDelay;

        short FWStraightenSpeed;
        short FWStraightenAngleAdjust;
        short FWStraightenDelay;

        short FWWithinSpeed;
        short FWWithinAngleAdjust;
    };

    void LoadSettings(SUBSUMPTIONOPTIONS Options);

    SUBSUMPTIONOPTIONS Options;
    
    // to store previous distance of an IR detection.
    unsigned char PrevDistance;


    // for the concepts one (control system)
	short AccumulatedError;
	short TotalTurnTicks;
	int ErrorMemory;
    BOOL PrevClockWise;

    BOOL TurnArcBOOL; // to alternate between turning and going foward

    // stores the direction of turn
	short TurnDirection;

    // basic BOOLs
    BOOL BeginStraightening; // two phase procedure, toggles between phases
	BOOL BeginTurn;  // decides the direction of turn



    // used for checking polling for errors
    short PollCount;
	
	SGenericModel *TheModel;
	unsigned char RightIRDistance;
	STATES States;

    // check if should turn, at the begining of several routines
    BOOL SUBCheckIfShouldTurn();

    // to check if the process is delayed
    BOOL SUBCheckDelay();
  
    // check for collision check at the begining of each loop
    BOOL SUBCheckforCollision();

    // set the delay
    void SUBSetDelay(unsigned short Milliseconds);

    // variables used for the delay routine
    short DelayTicks;
    short StartTicks;
	

    // three sub states for follow wall states, concept 2
	void SUBFWStraightenUp();
	void SUBFWExceedError();
	void SUBFWWithinError();

	void AIInitialize();

    // the four main states
	void SUBCollision();
	void SUBInitial();
	void SUBFollowWall(); // concept 1
	void SUBTurning();

    // called by the simulator/ embedded system main loop
	void AIRun();

	Subsumption();
	virtual ~Subsumption();

};

#endif // !defined(AFX_SUBSUMPTION_H__5AFFAC17_C651_4F0D_8589_A45449F93ACE__INCLUDED_)
