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

#include "stdafx.h"
#include "Simulator.h"
#include "Subsumption.h"

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

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

Subsumption::Subsumption()
{
    AIInitialize();
   

}

Subsumption::~Subsumption()
{

}

/************************************************************************
* Function void SUBRun()
*
* PURPOSE
* This is the function called in the main line to run the AI
*************************************************************************/
void Subsumption::AIRun()
{
    if (SUBCheckforCollision())
    {
        SUBCollision();
    }
    else if (MVIsBlocking())
    {
        // do nothing
    }
    else if (SUBCheckDelay())
    {
        // in a delay do nothing
    }
    else
    {
        if (States == Initial)
        {
            SUBInitial();
        }
        else if (States == Turning)
        {
            SUBTurning();
        }
        else if (States == FollowWall)
        {
            SUBFollowWall();
        }
        else if (States == Collision)
        {
            SUBCollision();
        }
        else if (States == FWExceedError)
        {
            SUBFWExceedError();
        }
        else if (States == FWStraightenUp)
        {
            SUBFWStraightenUp();
        }
        else if (States == FWWithinError)
        {
            SUBFWWithinError();
        }
        else if (States == TurnArc)
        {
            SUBTurnArc();
        }
    }

}

/************************************************************************
* Function void SUBTurning()
*
* PURPOSE
* Implementation of the Turning State Algorithm
* NOTE
* MVTurn(TurnDirection); is a blocking call
*************************************************************************/
void Subsumption::SUBTurning()
{
    unsigned char Distance = IRGetFrontD();   


    if (BeginTurn)
    {
        // choose the distance
        if (Distance < Options.FrontThreshHold)
        {
            TurnDirection = 2;
        }
        else
            TurnDirection = -2;
        BeginTurn = false;
        RightIRDistance = IRGetRightD();
        TotalTurnTicks = 0;
    }


    Distance = IRGetRightD();    
    if (Distance != NODETECTIOND)
    {
        if (Distance > RightIRDistance)
        {
            States = FWWithinError;
            ErrorMemory = 0;
            BeginTurn = true; // for next time
        }
        else
        {   
            RightIRDistance = Distance;
            TotalTurnTicks += TurnDirection;
            MVTurn(TurnDirection);
        }
    }
    else
    {
        TotalTurnTicks += TurnDirection;
        MVTurn(TurnDirection);
    }

    if (abs(TotalTurnTicks) > TICKS360)
    {
        MVAllStop();
        MVReset(); // clears any potentialblocking
        BeginTurn = true;
        States = Initial;
    }

}

/************************************************************************
* Function void SUBFollowWall()
*
* PURPOSE
* Implementation of Follow Wall State Algorithm concept 1.  Doesn't work that well
* ALGORITHM
* If there is a detection in the foward sensor closer then a threshold turn
* If there is no detection in the right sensor turn
* Else try to keep the distance to the wall around the Right threshold, adjust the 
* Speed accordingly
*************************************************************************/
void Subsumption::SUBFollowWall()
{
    unsigned char Distance;

    Distance = IRGetFrontD();

    if (Distance < Options.FrontThreshHold)
    {
        MVAllStop();
        if (PollCount != 0)
        {
            PollCount--;
            if (PollCount <= 0)
            {
                States = Turning;  
                PollCount = 0;                
            }
        }
        else
        {
            PollCount = Options.PollingCount;
            SUBSetDelay(Options.PollingDelay); // 50 milleseconds for another poll

        }
        return;
    }


    Distance = IRGetRightD();

    if (Distance == NODETECTIOND)
    {
        MVAllStop();
        if (PollCount != 0)
        {
             PollCount--;
            if (PollCount <= 0)
            {
                States = Turning;  
                PollCount = 0;                
            }
        }
        else
        {
            PollCount = Options.PollingCount;
            SUBSetDelay(Options.PollingDelay); // 50 milleseconds for another poll

        }
        return;
    }

    // If we are here all is somewhat well;
    // +ve (counterclockwise) if shorter, -ve (clockwise) if longer.

    char Extra;
    // a bit of hysterisis
    if (PrevClockWise)
        Extra = 5;
    else 
        Extra = -5;

    char AngleAdjust = (Options.RightIdeal + Extra - Distance)/6;

    if (PrevDistance != NODETECTIOND)
    {
        if (PrevDistance < Distance)
            if (!PrevClockWise)
                if (AngleAdjust < 0)
                AngleAdjust = - AngleAdjust;
        else if (PrevDistance > Distance)
            if (PrevClockWise)
                if (AngleAdjust > 0)
                AngleAdjust = - AngleAdjust;
    }

    if (PrevDistance != Distance)
    {
        PrevDistance = Distance;
        if (AngleAdjust > 0)
            PrevClockWise = true;
        else
            PrevClockWise = false;
    }


    
    MVGoFowardWA(5,AngleAdjust); 
   

}

/************************************************************************
* Function void SUBInitial()
*
* PURPOSE
* Implementation of Initial State Algorithm
*************************************************************************/
void Subsumption::SUBInitial()
{
     unsigned char Distance = IRGetFrontD();
     if (Distance != NODETECTIOND)
     {
         MVAllStop();
         MVGoFoward(4);
         if (Distance < Options.FrontThreshHold)
         {
             MVAllStop();
             States = Turning;
         }
     }
     else if (IRGetRightD() != NODETECTIOND)
     {
         MVAllStop();
         States = FWWithinError;
     }
     else
     {
         // race foward at maximum speed
         MVGoFoward(9);
         // go foward
     }

}

/************************************************************************
* Function void SUBCollision()
*
* PURPOSE
* A collision has occured.  Find out from what direction and take
* Appropriate action
*************************************************************************/
void Subsumption::SUBCollision()
{
    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);


        States = Initial;
        SUBSetDelay(1000);
    }

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

/************************************************************************
* Function void SUBInitialize()
*
* PURPOSE
* To be called on initialization
*************************************************************************/
void Subsumption::AIInitialize()
{
    States = Turning;
    RightIRDistance = 255;
    PrevDistance = 255;
    BeginTurn = true;
    BeginStraightening = true;
    ErrorMemory = 0;
    AccumulatedError = 0;
    PrevClockWise = false;
    TurnArcBOOL = false;
    DelayTicks = 0;

    Options.FrontThreshHold = 50; // 50 cm
    Options.PollingDelay = 50; // 50 ms
    Options.RightIdeal = 40; // 40 cms
    Options.PollingCount = 1; // 1 poll
    Options.ErrorMargin = 5; // 5 cm

    Options.FWExceedSpeed = 4; // speed factor
    Options.FWExceedAngleAdjust = 1; // a divided by factor
    Options.FWExceedDelay = 200; // 200 ms    

    Options.FWStraightenSpeed = 6; // speed factor
    Options.FWStraightenAngleAdjust = 3; // a multiple by factor
    Options.FWStraightenDelay = 200; // 200 ms

    Options.FWWithinSpeed = 8; // multiplying factor
    Options.FWWithinAngleAdjust = 2; // divided by factor
}


/************************************************************************
* Function void SUBCheckDelay()
*
* PURPOSE
* Runs the non blocking delay
* NOTE
* I can't guarentee anything about overflow or that each time this function
* is called one tick will pass.  That is why I need the extra variables
* RETURNS
* true if delay over, false otherwise
*************************************************************************/
BOOL Subsumption::SUBCheckDelay()
{
    short tempticks = KRNLGetTicks();

    if (DelayTicks == 0)
        return false;

    if (tempticks < StartTicks)
    {
        DelayTicks -= 1000+tempticks - StartTicks;
    }
    else
    {
        DelayTicks -= tempticks-StartTicks;
    }

    StartTicks = tempticks;

    if (DelayTicks <= 0)
    {
        DelayTicks = 0;
        return false;
    }

    return true;

}

/************************************************************************
* Function void SUBCheckDelay()
*
* PURPOSE
* Runs the non blocking delay
*************************************************************************/
void Subsumption::SUBSetDelay(unsigned short Milliseconds)
{
   StartTicks = KRNLGetTicks();
   DelayTicks = Milliseconds;
}


/************************************************************************
* Function void SUBCheckIfShouldTurn()
*
* PURPOSE
* For the follow wall.  Checks if turning should happen
*************************************************************************/
BOOL Subsumption::SUBCheckIfShouldTurn()
{
    unsigned char Distance;

    Distance = IRGetFrontD();

    if (Distance < Options.FrontThreshHold)
    {
        //MVAllStop();
        if (PollCount != 0)
        {
            PollCount--;
            if (PollCount <= 0)
            {
                States = Turning;  
                PollCount = 0;                
            }
        }
        else
        {
            PollCount = Options.PollingCount;
            SUBSetDelay(Options.PollingDelay); 

        }
        return true;
    }


    Distance = IRGetRightD();

    if (Distance == NODETECTIOND)
    {       
     //   MVAllStop();
        if (PollCount != 0)
        {
             PollCount--;
            if (PollCount <= 0)
            {
                States = TurnArc;  
                PollCount = 0;                
            }
        }
        else
        {
            PollCount = Options.PollingCount;
            SUBSetDelay(Options.PollingDelay); 

        }
        return true;
    }

    return false;

}

/************************************************************************
* Function void SUBFWWithinError()
*
* PURPOSE
* Implementation of SUBFWWithinError Algorithm
*************************************************************************/
void Subsumption::SUBFWWithinError()
{
    // check if should turn
    if (SUBCheckIfShouldTurn())
        return;

    unsigned char Distance;

    Distance = IRGetRightD();

    if ((Distance < Options.RightIdeal - Options.ErrorMargin)
        || (Distance > Options.RightIdeal + Options.ErrorMargin))
    {
        States = FWExceedError;
        return;
    }


    // small adjustment
    char AngleAdjust = (Options.RightIdeal - Distance)/Options.FWWithinAngleAdjust;

    MVGoFowardWA((char)Options.FWWithinSpeed,(char)AngleAdjust); 



}

/************************************************************************
* Function void SUBFWExceedError()
*
* PURPOSE
* Implementation of SUBFWExceedError() Algorithm
*************************************************************************/
void Subsumption::SUBFWExceedError()
{

     unsigned char Distance = IRGetRightD();

    // check if should turn
    if (SUBCheckIfShouldTurn())
        return;

    if ((Distance > (Options.RightIdeal - Options.ErrorMargin))
        && (Distance < (Options.RightIdeal + Options.ErrorMargin)))
    {
        States = FWStraightenUp;
        return;
    }

     // large adjustment
    char AngleAdjust = (Options.RightIdeal - Distance)/Options.FWExceedAngleAdjust;

    MVGoFowardWA((char)Options.FWExceedSpeed,(char)AngleAdjust); 
    SUBSetDelay(Options.FWExceedDelay); // set a delay

    // next time striaghten up the mess
    States = FWStraightenUp;

}

/************************************************************************
* Function void SUBFWStraightenUp()
*
* PURPOSE
* Implementation of SUBFWStraightenUp() Algorithm
* ALGORITHM
* 1) Check which direction to turn to straighten up by moving foward and seeing
*    How the right IR reading changes direction
* 2) Based on the direction do and angular turn (while going straight) to get
*    The Right IR constant while going straight
*************************************************************************/
void Subsumption::SUBFWStraightenUp()
{

   unsigned char Distance;
   // check if should turn
   if (SUBCheckIfShouldTurn())
        return;

   // First check which direction to 
   if (BeginStraightening)
   {
        RightIRDistance = IRGetRightD();
        BeginStraightening = false;
        MVGoFoward((char)Options.FWStraightenSpeed);
        SUBSetDelay(Options.FWStraightenDelay);
        return;
   }
   else
   {
       Distance = IRGetRightD();

       char AngleAdjust = (RightIRDistance - Distance)*Options.FWStraightenAngleAdjust;

       if (abs(AngleAdjust) < 1)
       {
           States = FWWithinError;
           BeginStraightening = true; 
           return;
       }
       else
       {
            MVGoFowardWA((char)Options.FWStraightenSpeed,(char)AngleAdjust);
            RightIRDistance = Distance;
            SUBSetDelay(Options.FWStraightenDelay);
            BeginStraightening = true;
       }
   }

}

/************************************************************************
* Function void SUBCheckforCollision()
*
* PURPOSE
* Checks if a collision occurs, if so go into collision routine
*************************************************************************/
BOOL Subsumption::SUBCheckforCollision()
{
    if (BMPGetByte() != 0x00)
    {
        // don't want to keep on moving
        MVAllStop();
        MVReset(); // kill any blocking going on
        DelayTicks = 0; // kill any delay going on
        States = Collision;
        return true;
    }

    return false;

}

void Subsumption::LoadSettings(SUBSUMPTIONOPTIONS Options)
{
    this->Options = Options;

}

/************************************************************************
* Function void SUBTurnArc()
*
* If the turn is a right one, it should turn in an arc, so it won't miss the wall
*************************************************************************/
void Subsumption::SUBTurnArc()
{
    unsigned char Distance = IRGetFrontD();    
    if (Distance < Options.FrontThreshHold)
    {
        States = Turning;
        return;
    }

    Distance = IRGetRightD();  
    if (Distance < Options.RightIdeal)
    {
        States = FWStraightenUp;
        return;
    }

    // alternate between going foward and turning

    if (TurnArcBOOL)
    {
        MVGoFoward(4);
        SUBSetDelay(50);
        TurnArcBOOL = false;
    }
    else
    {
        MVTurn(-4);
        TurnArcBOOL = true;
    }

}
