/*******************************************************************
**
**                 *****  CPE 470/670 *****
**
**  FILE    :  TankRamrod.ic
**
**  AUTHORS :  John Yurtinus
**             Mike Dye
**
**  DATE    :  05-December-2005
**
********************************************************************/

/******************************************************************/
/* DEFINES */

/* Motor defines */
#define LEFT_MOTOR              0
#define RIGHT_MOTOR             1
#define CAGE_MOTOR              3
#define ROTATE_90               0.6
#define MOVE_6                  0.5
#define DROP_BACK               0.28
#define DROP_FORWARD            0.15

/* Sensor defines */
#define LEFT_BUMP_SENSOR        10
#define RIGHT_BUMP_SENSOR       11
#define LEFT_REFLECTO_SENSOR    5
#define RIGHT_REFLECTO_SENSOR   6

/* Directional defines */
#define LEFT                    0
#define RIGHT                   1

/* Gate defines */
#define GATE_DOWN               0
#define GATE_UP                 1

/* Light sensor defines */
#define LIGHT_DARKNESS          10
#define LIGHT_FLOOR             6

/* END DEFINES */
/******************************************************************/

/******************************************************************/
/* STRUCTURES */

/* Almighty Tank Ramrod's internal state */
struct SInternalState
{
    int bInitialRun;            /* Status of initial run */
    int bGateStatus;            /* Status of gate (0 -- down, 1 -- up ) */
    
    int bLeftBumpSensor;        /* Left bump sensor flag */
    int bRightBumpSensor;       /* Right bump sensor flag */
    
    int bLeftReflectoSensor;    /* Left reflective sensor flag */
    int bRightReflectoSensor;   /* Right reflective sensor flag */
    
};

/* END STRUCTURES */
/******************************************************************/

/******************************************************************/
/* GLOBAL VARIABLES */

/* Thread variables */
int idSensors  = 0;         /* Sensor thread process ID */
int idMovement = 0;         /* Main movement thread process ID */
int idCounter = 0;

/* Almighty Tank Ramrod variables */
struct SInternalState g_internalState_s;

/* END GLOBAL VARIABLES
/******************************************************************/

/*******************************************************************
**
**  PRE      :  NULL
**
**  POST     :  Exits program
**
**  PASSED   :  NULL
**
**  RETURNED :  Exit code (0)
**
********************************************************************/
int main ( void )
{
    while ( 1 )
    {
        while ( !start_button() )
            ;

        InitializeTheAwesomeness();

        /* Start sensor thread */
        idSensors = start_process( Clairvoyance() );

        /* Execute initial run */
        FaceBwalls();
        idCounter = start_process( RunCounter( 2.5 ) );

        /* Start general movement thread */
        idMovement = start_process( RunRamrodRun() );

        while ( !stop_button() )
            ;

        kill_process( idSensors );
        kill_process( idMovement );
        alloff();
    }  /* END WHILE ( 1 ) */

    return 0;
}  /* END FUNCTION main */

/*******************************************************************
**
**  PRE      :  NULL
**
**  POST     :  Initialize the variables in preparation to pwn
**
**  PASSED   :  NULL
**
**  RETURNED :  NULL
**
********************************************************************/
void InitializeTheAwesomeness ( void )
{
    g_internalState_s.bInitialRun = 1;
    g_internalState_s.bGateStatus = GATE_UP;

    motor ( CAGE_MOTOR, -20 );
    sleep (0.2);
    alloff();

    g_internalState_s.bLeftBumpSensor  = 0;
    g_internalState_s.bRightBumpSensor = 0;

    g_internalState_s.bLeftReflectoSensor  = 0;
    g_internalState_s.bRightReflectoSensor = 0;
}  /* END FUNCTION InitializeTheAwesomeness */

/*******************************************************************
**
**  PRE      :  Initial run started
**
**  POST     :  Have the almighty Tank Ramrod face the bwalls
**
**  PASSED   :  NULL
**
**  RETURNED :  NULL
**
********************************************************************/
int FaceBwalls ( void )
{
    int i,                         /* Looping variable */
        iLeftHit =  0,             /* Saves left bump sensor state */
        iRightHit = 0,             /* Saves right bump sensor state */
        iPrevLeftHit = 0,          /* Saves previous left bump sensor state */
        iPrevRightHit = 0;         /* Saves previous right bump sensor state */


    /* Move forwards for about 6 inches */
    fd( LEFT_MOTOR );
    fd( RIGHT_MOTOR );
    sleep( MOVE_6 );
    alloff();

    iPrevLeftHit = g_internalState_s.bLeftBumpSensor;
    iPrevRightHit = g_internalState_s.bRightBumpSensor;

    /* Backup the 6 inches */
    bk( LEFT_MOTOR );
    bk( RIGHT_MOTOR );
    sleep( MOVE_6 * 1.0 );
    alloff();

    /* Rotate left 90 degrees */
    fd( RIGHT_MOTOR );
    bk( LEFT_MOTOR );
    sleep( ROTATE_90 );
    alloff();

    /* Move forwards for about 6 inches */
    fd( LEFT_MOTOR );
    fd( RIGHT_MOTOR );
    sleep( MOVE_6 );
    alloff();    

    iLeftHit  = g_internalState_s.bLeftBumpSensor;
    iRightHit = g_internalState_s.bRightBumpSensor;

    /* Now that we know our orientation, rotate to face them bwalls */
    if ( (iLeftHit || iRightHit) && (iPrevLeftHit || iPrevRightHit) )
    {
        /* Backup the 3 inches */
        bk( LEFT_MOTOR );
        bk( RIGHT_MOTOR );
        sleep( MOVE_6 * .5 );
        alloff();

        /* Rotate left 90 degrees */
        fd( RIGHT_MOTOR );
        bk( LEFT_MOTOR );
        sleep( ROTATE_90 );
        alloff();        
    }  /* END IF ( (iLeftHit || iRightHit) && (iPrevLeftHit && iPrevRightHit) ) */

    else if ( (iLeftHit || iRightHit) && (!iPrevLeftHit || !iPrevRightHit) )
    {
        /* Backup the 6 inches */
        bk( LEFT_MOTOR );
        bk( RIGHT_MOTOR );
        sleep( MOVE_6 * 1.0 );
        alloff();
          
        /* Rotate left 180 degrees */
        bk( RIGHT_MOTOR );
        fd( LEFT_MOTOR );
        sleep( ROTATE_90 * 1.8 );
        alloff();
    }  /* END ELSE IF (iLeftHit || iRightHit) && (iPrevLeftHit && iPrevRightHit) ) */
    else if ( (!iLeftHit || !iRightHit) && (!iPrevLeftHit || !iPrevRightHit) )
    {
        /* Backup the 6 inches */
        bk( LEFT_MOTOR );
        bk( RIGHT_MOTOR );
        sleep( MOVE_6 * 1.0 );
        alloff();
        
        /* Rotate right 90 degrees */
        bk( RIGHT_MOTOR );
        fd( LEFT_MOTOR );
        sleep( ROTATE_90 );
        alloff();
    }  /* END ELSE IF ( (!iLeftHit || !iRightHit) && (!iPrevLeftHit || !iPrevRightHit) ) */
    else
    {
        /* Backup the 6 inches */
        bk( LEFT_MOTOR );
        bk( RIGHT_MOTOR );
        sleep( MOVE_6 * 1.0 );
        alloff();
    }
    for ( i = 0; i < 3; i++ )
    {
        bk( LEFT_MOTOR );
        bk( RIGHT_MOTOR );
        sleep( 1.0 );
        off( RIGHT_MOTOR );
        sleep( 0.5 );
        off( LEFT_MOTOR );
        bk( RIGHT_MOTOR );
        sleep( 0.5 );
    }  /* END FOR ( i = 0; i < 2; i++ ) */
    
}  /* END FUNCTION FaceBwalls */

/*******************************************************************
**
**  PRE      :  Sensor thread started
**
**  POST     :  Main function responsible for checking and updating
**                  the proverbial eyes, ears, hands, nose, and
**                  taste for the almighty Tank Ramrod
**
**  PASSED   :  NULL
**
**  RETURNED :  NULL
**
********************************************************************/
void Clairvoyance ( void )
{
    while ( 1 )
    {
        g_internalState_s.bLeftBumpSensor  = digital( LEFT_BUMP_SENSOR );
        g_internalState_s.bRightBumpSensor = digital( RIGHT_BUMP_SENSOR );

        if ( analog( LEFT_REFLECTO_SENSOR ) > LIGHT_DARKNESS )
            g_internalState_s.bLeftReflectoSensor  = 1;
        else
            g_internalState_s.bLeftReflectoSensor  = 0;

        if ( analog( RIGHT_REFLECTO_SENSOR ) > LIGHT_DARKNESS )
            g_internalState_s.bRightReflectoSensor  = 1;
        else
            g_internalState_s.bRightReflectoSensor  = 0;
    }  /* END WHILE ( 1 ) */
}  /* END FUNCTION Clairvoyance */

/*******************************************************************
**
**  PRE      :  NULL
**
**  POST     :  Main function responsible for moving the almighty
**                  Tank Ramrod
**
**  PASSED   :  NULL
**
**  RETURNED :  NULL
**
********************************************************************/
void RunRamrodRun ( void )
{
    while ( 1 )
    {        
        if ( !g_internalState_s.bInitialRun && (g_internalState_s.bLeftReflectoSensor || g_internalState_s.bRightReflectoSensor) )
        {
            CupTheBwalls( GATE_DOWN );
            BlackHoleSun();
        }  /* END IF ( !g_internalState_s.bInitialRun && (g_internalState_s.bLeftReflectoSensor || g_internalState_s.bRightReflectoSensor) ) */
        else
        {
            fd( LEFT_MOTOR );
            fd( RIGHT_MOTOR );
        }  /* END ELSE ( g_internalState_s.bInitialRun || !(g_internalState_s.bLeftReflectoSensor && g_internalState_s.bRightReflectoSensor) ) */

        /* Check sensors and avoid if necessary */
        if ( g_internalState_s.bLeftBumpSensor )
            Avoid( LEFT );
        else if ( g_internalState_s.bRightBumpSensor )
            Avoid( RIGHT );
    }  /* END WHILE ( 1 ) */
}  /* END FUNCTION RunRamrodRun */

/*******************************************************************
**
**  PRE      :  NULL
**
**  POST     :  Avoid function.  Backs up and turns to avoid
**                  obstacles
**
**  PASSED   :  [in]     iSensorSide -- Side of sensor hit
**                                          0 - Left sensor hit
**                                          1 - Right sensor hit
**
**  RETURNED :  NULL
**
********************************************************************/
void Avoid ( int iSensorSide )
{
    CupTheBwalls( GATE_DOWN );

    /* For the initial run, we want to turn left approximately 135 degrees */
    if ( g_internalState_s.bInitialRun )
    {
        if ( !iSensorSide )
        {
            /*fd( RIGHT_MOTOR );
            sleep( ROTATE_90 * 1.1 );*/
        }  /* END IF ( !iSensorSide ) */
        else
        {
            fd( RIGHT_MOTOR );
            off( LEFT_MOTOR );
            sleep( 0.1 );
        }  /* END ELSE ( iSensorSide ) */
    }  /* END IF ( g_internalState_s.bInitialRun ) */
    else
    {
        /* Back up for 0.5 seconds */
        bk( LEFT_MOTOR );
        bk( RIGHT_MOTOR );
        sleep( 0.5 );

        if ( !iSensorSide )
            fd( LEFT_MOTOR );
        else
            fd( RIGHT_MOTOR );

        sleep( 0.8 );
        fd( LEFT_MOTOR );
        fd( RIGHT_MOTOR );
        sleep( 0.2 );
    }  /* END ELSE ( !g_internalState_s.bInitialRun ) */

    CupTheBwalls( GATE_UP );
}  /* END FUNCTION Avoid */

/*******************************************************************
**
**  PRE      :  NULL
**
**  POST     :  Toggle gate up or down depending on iGatePosition
**
**  PASSED   :  [in]     iGatePosition -- Position to move gate to
**                                            0 -- Move gate down
**                                            1 -- Move gate up
**
**
**  RETURNED :  NULL
**
********************************************************************/
void CupTheBwalls ( int iGatePosition )
{
    /* Nothing to do */
    if ( iGatePosition == g_internalState_s.bGateStatus )
        return;

    if ( iGatePosition )
    {
        /* Move it on up, to the East side */
        bk( CAGE_MOTOR );
        sleep( 0.1 );
        motor( CAGE_MOTOR, -40 );
        sleep( 0.1 );
        off( CAGE_MOTOR );

        g_internalState_s.bGateStatus = GATE_UP;
    }  /* END IF ( iGatePosition ) */
    else
    {
        /* Move back to the West side where you belong */
        fd( CAGE_MOTOR );
        sleep( 0.035 );
        off( CAGE_MOTOR );

        g_internalState_s.bGateStatus = GATE_DOWN;
    }  /* END ELSE ( !iGatePosition ) */
}  /* END FUNCTION CupTheBwalls */

/*******************************************************************
**
**  PRE      :  NULL
**
**  POST     :  Release the bwalls being held by the almighty Tank
**                  Ramrod
**
**  PASSED   :  NULL
**
**  RETURNED :  NULL
**
********************************************************************/
void BlackHoleSun ( void )
{
    int bLeftLight,            /* Save left light sensor state */
        bRightLight;           /* Save right light sensor state */


    bLeftLight  = g_internalState_s.bLeftReflectoSensor;
    bRightLight = g_internalState_s.bRightReflectoSensor;

    /* Left sensor hit the darkness, turn towards it */
    if ( bLeftLight && !bRightLight )
    {
        bk( RIGHT_MOTOR );
        fd( LEFT_MOTOR );
        sleep( ROTATE_90 * 1.5 );
        fd( RIGHT_MOTOR );
        sleep( 0.4 );
    }  /* END IF ( bLeftLight && !bRightLight )) */

    /* Right sensor hit the darkness, turn towards it */
    else if ( bRightLight && !bLeftLight )
    {
          bk( LEFT_MOTOR );
          fd( RIGHT_MOTOR );
          sleep( ROTATE_90 * 1.5 );
          fd( LEFT_MOTOR );
          sleep( 0.4 );
    }  /* END ELSE IF ( bRightLight && !bLeftLight ) */

    bLeftLight  = g_internalState_s.bLeftReflectoSensor;
    bRightLight = g_internalState_s.bRightReflectoSensor;      

    /* Both sensors hit, release the bwalls */
    if ( bLeftLight && bRightLight )
    {
        alloff();
        sleep( 2.0 );

        CupTheBwalls( GATE_UP );
        CupTheBwalls( GATE_UP );
        bk( LEFT_MOTOR );
        bk( RIGHT_MOTOR );
        sleep( DROP_BACK );
        alloff();

        fd( LEFT_MOTOR );
        fd( RIGHT_MOTOR);
        sleep( DROP_FORWARD );
        alloff();
        CupTheBwalls( GATE_UP );
        CupTheBwalls( GATE_UP );
        sleep( DROP_FORWARD );
        bk( LEFT_MOTOR );
        bk( RIGHT_MOTOR );
        sleep( DROP_BACK );

        fd( LEFT_MOTOR );
        fd( RIGHT_MOTOR);
        sleep( DROP_FORWARD );
        alloff();
        CupTheBwalls( GATE_UP );
        sleep( DROP_FORWARD );
        bk( LEFT_MOTOR );
        bk( RIGHT_MOTOR );
        sleep( DROP_BACK );

        fd( LEFT_MOTOR );
        fd( RIGHT_MOTOR);
        sleep( DROP_FORWARD );
        CupTheBwalls( GATE_UP );
        bk( LEFT_MOTOR );
        bk( RIGHT_MOTOR );
        sleep( 2.0 );

        off( RIGHT_MOTOR );
        sleep( 0.7 );
        alloff();

        fd( LEFT_MOTOR );
        bk( RIGHT_MOTOR );
        sleep( 0.5 );
        alloff();
    }  /* END ELSE IF ( bLeftLight && bRightLight ) */
}  /* END FUNCTION BlackHoleSun */

/*******************************************************************
**
**  PRE      :  NULL
**
**  POST     :  Initial run counter.  Turns initial run flag off
**									at passed time
**
**  PASSED   :  NULL
**
**  RETURNED :  NULL
**
********************************************************************/
void RunCounter(float fTime)
{
    sleep( fTime );
    g_internalState_s.bInitialRun = 0;

    /* Initial run over, turn to face the black hole */
    fd( RIGHT_MOTOR );
    bk( LEFT_MOTOR );
    sleep( ROTATE_90 * 1.3 );
}  /* END FUNCTION RunCounter */