/****************************************************************************
 Module
   TurretHSM.c

 Revision
   2.0.1

 Description
   This is the Turret state machine file.

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 Feb 2014		Sir Ben Sumers, Royal Order of King Arthur's Homies for the Abolition of the Global Variable		Winning 218B

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "MasterHSM.h" 		//For defines
#include <stdio.h>         //Everybody wants to talk to computers
#include "float.h"
#include "EventCheckers.h"
#include "ADS12.h"
#include "Drive.h"
#include "PWM.h"

/* include header files for this state machine as well as any machines at the
   next lower level in the hierarchy that are sub-machines to this machine
*/
#include "TurretHSM.h"
#include "Servo.h"   //Defines for servo
/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines

#define TURR_CTRL_RATE 30       //ms, control rate for turret

/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this machine, things like during
   functions, entry & exit functions.They should be functions relevant to the
   behavior of this state machine
*/
static ES_Event DuringSweepingRight( ES_Event Event);
static ES_Event DuringSweepingLeft(ES_Event Event);
static ES_Event DuringLockedOn(ES_Event Event);

int QueryLeftEye(void);
int QueryRightEye(void);

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static TurretState_t CurrentState;
static float TurretAng = 0;          //Desired angle of the servo.  Set to middle to start.  Has to be module so you can query it
static float Kp = 0.7;         //Desired Control Tuning Parameters
static float Kd = 0;
static float Ki = 0;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
    RunTurretSM

 Parameters
   ES_Event: the event to process

 Returns
   ES_Event: an event to return

 Description
   Handles turret tracking algorithm

 Notes
   uses nested switch/case to implement the machine.
 Author
The Royal Order, Feb 2014

****************************************************************************/
ES_Event RunTurretSM( ES_Event CurrentEvent )
{
    boolean MakeTransition = FALSE;           /* are we making a state transition? */
    TurretState_t NextState = CurrentState;
    ES_Event EntryEventKind = { ES_ENTRY, 0 };      // default to normal entry to new state
    ES_Event ReturnEvent = CurrentEvent;            // assume we are not consuming event

    //ControlMotorsIndivid(200,BALL_MOTOR);

    switch ( CurrentState )
        {

            //*********Sweeping Left State *****************
        case STATE_SWEEPING_LEFT :       // If current state is state one
            // Execute During function for state one. ES_ENTRY & ES_EXIT are
            // processed here allow the lowere level state machines to re-map
            // or consume the event
            CurrentEvent = DuringSweepingLeft(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
                {
                    switch (CurrentEvent.EventType)
                        {

                        case ES_ENTRY:		                                          ///Entry function
                            MakeTransition=FALSE;			                        //We are not transitioning
                            ES_Timer_InitTimer(SERVO_SWEEP_TIMER,TURRET_SERVO_RATE); 	   //Start ServoSweepTImer
                            NextState=STATE_SWEEPING_LEFT;                        //Stay in this state
                            //printf("Transition to ES_SWEEPING_LEFT \r\n");
                            //printf("Current TurretAng is %d \r\n", TurretAng);
                            break;                                                      //Break on ES_ENTRY

                        case ES_TIMEOUT :                                           //If we have timed out
                            //printf("CurrentEvent.EventParam is %d",CurrentEvent.EventParam);
                            switch (CurrentEvent.EventParam)                         //Depending on which timer (allows us to break out if we want)
                                {

                                case SERVO_SWEEP_TIMER:
                                    MakeTransition=FALSE;                               //We are not transitioning
                                    TurretAng-=SERVO_STEP;                              //Increment the TurretAng by adding SERVO_STEP
                                    ServoSetPulse(LANCE_PAN_SERVO_NUM,TurretAng);         //Set the servo output rate
                                    ES_Timer_InitTimer(SERVO_SWEEP_TIMER,TURRET_SERVO_RATE);   //Reset the ServoSweepTimer	to TURRET_SERVO_RATE
                                    //printf("Set turret angle to %d \r\n",TurretAng);

                                    break;

                                }
                            break;

                        case ES_LEFT_LIMIT_HIT:			                              //If you have swept all the way left
                            MakeTransition=TRUE;			                           //We are transitioning
                            NextState=STATE_SWEEPING_RIGHT;                       //Set your next state

                            break;                        //Break on ES_LEFT_LIMIT_HIT

                        case ES_LOCKED_ON:            //If you have locked onto the other robot
                            MakeTransition=TRUE;			//We are transitioning
                            NextState=STATE_LOCKED_ON;

                            break;                        //Break on ES_LOCKED_ON
                        }
                }
            break;

//*********Sweeping Right State *****************

        case STATE_SWEEPING_RIGHT:       // If current state is state one
            // Execute During function for state one. ES_ENTRY & ES_EXIT are
            // processed here allow the lowere level state machines to re-map
            // or consume the event
            CurrentEvent = DuringSweepingRight(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
                {
                    switch (CurrentEvent.EventType)
                        {

                        case ES_ENTRY:		///Entry function
                            MakeTransition=FALSE;			                        //We are not transitioning
                            ES_Timer_InitTimer(SERVO_SWEEP_TIMER,TURRET_SERVO_RATE);   //Reset the ServoSweepTimer	to TURRET_SERVO_RATE
                            //printf("Current TurretAng is %d \r\n", TurretAng);
                            //printf("CurrentState is ES_SWEEPING_RIGHT \r\n");
                            break;

                        case ES_TIMEOUT : //If event is a timeout
                            switch (CurrentEvent.EventParam)                         //Depending on which timer (allows us to break out if we want)
                                {
                                case SERVO_SWEEP_TIMER:
                                    MakeTransition=FALSE;                               //We are not transitioning
                                    TurretAng+=SERVO_STEP;                              //Increment the TurretAng by adding SERVO_STEP
                                    ServoSetPulse(LANCE_PAN_SERVO_NUM,TurretAng);                          //Set the servo output rate
                                    ES_Timer_InitTimer(SERVO_SWEEP_TIMER,TURRET_SERVO_RATE);   //Reset the ServoSweepTimer	to TURRET_SERVO_RATE
                                    NextState=STATE_SWEEPING_RIGHT;                      //Reset the state to the same thing just to be safe
                                    //printf("Current TurretAng is %d \r\n", TurretAng);

                                    break;

                                }

                            break;      //Break on ES_TIMEOUT case

                        case ES_RIGHT_LIMIT_HIT:			                                 //If you have swept all the way right
                            MakeTransition=TRUE;
                            NextState=STATE_SWEEPING_LEFT;                                    //NExt state is sweeping left

                            break;

                        case ES_LOCKED_ON:
                            MakeTransition=TRUE;			//We are transitioning
                            NextState=STATE_LOCKED_ON;

                            break;
                        }
                }
            break;
            // repeat state pattern as required for other states

            //*********Locked On State *****************
        case STATE_LOCKED_ON :       // If current state is state one
            // Execute During function for state one. ES_ENTRY & ES_EXIT are
            // processed here allow the lowere level state machines to re-map
            // or consume the event
            CurrentEvent = DuringLockedOn(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
                {
                    switch (CurrentEvent.EventType)
                        {

                        case ES_ENTRY:		///Entry function
                            MakeTransition=FALSE;			//We are not transitioning
                            NextState=STATE_LOCKED_ON;
                            ES_Timer_InitTimer(SERVO_CTRL_TIMER,TURR_CTRL_RATE);   //Reset the ServoSweepTimer	to TURRET_SERVO_RATE

                            break;

                        case ES_TIMEOUT :
                            MakeTransition=FALSE;				//We are not transitioning
                            switch (CurrentEvent.EventParam)                         //Depending on which timer (allows us to break out if we want)
                                {
                                case SERVO_CTRL_TIMER:                                 //if your control loop is ready to
                                {
                                    static float IRErrorSignal=0;                 //These error signals should be signed bc can be negative
                                    static float IRErrorSignalLast = 0;
                                    float ErrorComp = 0;                           //Amount of Error Compensation to add to the curr command
                                    static unsigned int i = 0;
                                    MakeTransition=FALSE;                               //We are not transitioning
                                    NextState=STATE_LOCKED_ON;
                                    //Run your control loop

                                    //Obtain error
                                    IRErrorSignal=(( QueryRightEye() - QueryLeftEye())*5)/1024;

                                    //Compute Error Compensation
                                    ErrorComp = Kp*IRErrorSignal + Kd*(IRErrorSignal - IRErrorSignalLast) + Ki*(IRErrorSignal + IRErrorSignalLast); //PD CONTROL

                                    //Add the Error Compensation to the current Command
                                    TurretAng+=ErrorComp;
                                    if (TurretAng > SERVO_FULL_RIGHT_DEGREES)
                                        {
                                            TurretAng = SERVO_FULL_RIGHT_DEGREES;
                                            IRErrorSignalLast = 0;
                                        }
                                    else if (TurretAng < SERVO_FULL_LEFT_DEGREES)
                                        {
                                            TurretAng = SERVO_FULL_LEFT_DEGREES;
                                            IRErrorSignalLast = 0;
                                        }
                                    else
                                        {

                                            IRErrorSignalLast = IRErrorSignal;
                                        }

                                    //Update the servo command
                                    ServoSetPulse(LANCE_PAN_SERVO_NUM,TurretAng);

                                    ES_Timer_InitTimer(SERVO_CTRL_TIMER,TURR_CTRL_RATE);   //Reset the ServoControlTimer to TURR_CTRL_RATE
                                    //printf("CurrentState is ES_LOCKED_ON \r\n");
                                    //printf("Current TurretAng is %d \r\n", TurretAng);
                                    //Update the IRErrorSignalLast
                                    /* i++;
                                    if(i%100000)
                                    {
                                       printf("ERROR COMMAND: %d\r\n",ErrorComp);
                                       printf("SERVO ANGLE: %d\r\n",TurretAng);
                                       i = 0;
                                    } */
                                }
                                break;

                                }
                            break;

                        case ES_LEFT_EYE_SLIPPED:                                      //if your left eye fell off
                            ES_Timer_InitTimer(SERVO_CTRL_TIMER,TURR_CTRL_RATE);        //Reset the ServoSweepTimer	to TURRET_SERVO_RATE
                            MakeTransition=TRUE;                                        //Set MakeTransition to TRUE bc we are transitioning
                            NextState=STATE_SWEEPING_RIGHT;                          //Sweep right
                            break;

                        case ES_RIGHT_EYE_SLIPPED:                                      //if your right eye fell off
                            ES_Timer_InitTimer(SERVO_CTRL_TIMER,TURR_CTRL_RATE);        //Reset the ServoSweepTimer	to TURRET_SERVO_RATE
                            MakeTransition=TRUE;                                        //Set MakeTransition to TRUE bc we are transitioning
                            NextState=STATE_SWEEPING_LEFT;                          //Sweep left
                            break;

                        case ES_BOTH_EYES_SLIPPED:                                      //if both eye fell off
                            ES_Timer_InitTimer(SERVO_CTRL_TIMER,TURR_CTRL_RATE);        //Reset the ServoSweepTimer	to TURRET_SERVO_RATE
                            MakeTransition=TRUE;                                        //Set MakeTransition to TRUE bc we are transitioning
                            //NextState=STATE_SWEEPING_LEFT;                          //Sweep left arbitrarily
                            //command to full right position
                            //ServoSetPulse(LANCE_PAN_SERVO_NUM,SERVO_FULL_RIGHT_DEGREES);

                            //check to see if your last servo angle was greater than 90, if so go full that way
                            if (TurretAng > SERVO_MIDDLE_DEGREES)
                                {
                                    ServoSetPulse(LANCE_PAN_SERVO_NUM,SERVO_FULL_RIGHT_DEGREES);
                                    NextState = STATE_SWEEPING_LEFT;
                                    TurretAng = SERVO_FULL_RIGHT_DEGREES;
                                }
                            else
                                {
                                    ServoSetPulse(LANCE_PAN_SERVO_NUM,SERVO_FULL_LEFT_DEGREES);
                                    NextState = STATE_SWEEPING_RIGHT;
                                    TurretAng = SERVO_FULL_LEFT_DEGREES;
                                }

                            //Reset turret ang
                            printf("BOTH EYES SLIPPED \r\n");
                            //TurretAng = SERVO_FULL_RIGHT_DEGREES;

                            break;

                        }
                }
            break;
            // repeat state pattern as required for other states

        } //End switch on state

    //If we are making a state transition

    if (MakeTransition==True)
        {
            //Exit, move and then enter

            //Exit
            CurrentEvent.EventType=ES_EXIT;
            RunTurretSM(CurrentEvent);

            //Change
            CurrentState=NextState;

            //Enter
            CurrentEvent.EventType=ES_ENTRY;
            RunTurretSM(CurrentEvent);

        }

    //CurrentState=NextState;							//Update states

    return(ReturnEvent);
}
/****************************************************************************
 Function
     StartTurretSM

 Parameters
     None

 Returns
     None

 Description
     Does any required initialization for this state machine

 Notes

 Author
The Royal Order, Feb 2014

****************************************************************************/
void StartTurretSM ( ES_Event CurrentEvent )
{
    // local variable to get debugger to display the value of CurrentEvent
    ES_Event LocalEvent = CurrentEvent;
    // to implement entry to a history state or directly to a substate
    // you can modify the initialization of the CurrentState variable
    // otherwise just start in the entry state every time the state machine
    // is started

    if ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
        {
            CurrentState = STATE_SWEEPING_LEFT;			//Entry state is STATE_SWEEPING_LEFT
        }

    // call the entry function (if any) for the ENTRY_STATE
    TurretAng = SERVO_FULL_LEFT_DEGREES;
    RunTurretSM(CurrentEvent);
}

/****************************************************************************
 Function
     QueryTurretSM

 Parameters
     None

 Returns
     TurretState_t The current state of the Turret state machine

 Description
     returns the current state of the Turret state machine
 Notes

 Author
     J. Edward Carryer, 2/11/05, 10:38AM
****************************************************************************/
TurretState_t QueryTurretSM ( void )
{
    return(CurrentState);
}

/***************************************************************************
 private functions
 ***************************************************************************/

/****************************************************************************
 Function
     DuringSweepRight

 Parameters
     ES_Event

 Returns
     Event, possibly modified

 Description
     The during function for sweeping right.  Not very exciting because there are no sub-states here.

      Notes

 Author
  The Royal Order, February 2014
****************************************************************************/
static ES_Event DuringSweepingRight( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
        {

            //***Entry actions for state***
            //No entry actions
            //****Start Lower Level Machines***
            //No lower level machines
        }
    else if ( Event.EventType == ES_EXIT )
        {
            // on exit, give the lower levels a chance to clean up first
            //***Run lower level machines to clean up first***
            //No lower level machines

            //***Do local exit functiosn ***
            //No local exit functions

        }
    else
        // do the 'during' function for this state
        {
            // ***Run lower level state machine***
            //No lower level SM's

            // **Do any activity that is repeated as long as we are in this state

        }
    // Return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.
    return(ReturnEvent);
}

/****************************************************************************
 Function
     DuringSweepingLeft

 Parameters
     ES_Event

 Returns
     Event, possibly modified

 Description
     The during function for sweeping left.  Not very exciting because there are no sub-states here.

      Notes

 Author
  The Royal Order, February 2014
****************************************************************************/
static ES_Event DuringSweepingLeft( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
        {

            //***Entry actions for state***
            //No entry actions

            //****Start Lower Level Machines***
            //No lower level machines

        }
    else if ( Event.EventType == ES_EXIT )
        {
            // on exit, give the lower levels a chance to clean up first
            //***Run lower level machines to clean up first***
            //No lower level machines

            //***Do local exit functiosn ***
            //No local exit functions

        }
    else
        // do the 'during' function for this state
        {
            // ***Run lower level state machine***
            //No lower level SM's

            // **Do any activity that is repeated as long as we are in this state

        }
    // Return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.

    return(ReturnEvent);
}

/****************************************************************************
 Function
     DuringLockedOn

 Parameters
     ES_Event

 Returns
     Event, possibly modified

 Description
     The during function for locked on.  May handle the control loop.

      Notes

 Author
  The Royal Order, February 2014
****************************************************************************/
static ES_Event DuringLockedOn( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
        {

            //***Entry actions for state***
            //No entry actions

            //****Start Lower Level Machines***
            //No lower level machines

        }
    else if ( Event.EventType == ES_EXIT )
        {
            // on exit, give the lower levels a chance to clean up first

            //***Run lower level machines to clean up first***
            //No lower level machines

            //***Do local exit functiosn ***
            //No local exit functions

        }
    else
        // do the 'during' function for this state
        {
            // ***Run lower level state machine***
            //No lower level SM's

            // **Do any activity that is repeated as long as we are in this state

        }
    // Return either Event, if you don't want to allow the lower level machine
    // to remap the current event, or ReturnEvent if you do want to allow it.

    return(ReturnEvent);
}

/****************************************************************************
 Function
     QueryTurretAngle

 Parameters
     None
 Returns
     Unsigned int containing turret angle in degrees (could be a char I guess, but it's really not worth saving the memory)
 Description                                      s
     Just a query function

      Notes

 Author
  The Royal Order, February 2014
****************************************************************************/
signed int QueryTurretAngle(void)
{
    return(TurretAng);
}

/****************************************************************************
 Function
     QueryLeftEye
 Description
Returns the voltage reported from the Left Eye
     Notes

 Author
The Royal Order, Feb 2014
Sir Darrel R. Deo
****************************************************************************/
int QueryLeftEye(void)
{
    //Return the voltage at AD input pin correlated to the left eye
    return(ADS12_ReadADPin(TURR_LEFT_IR_PIN));
}

/****************************************************************************
 Function
     QueryRightEye
 Description
Returns the voltage reported from the Right Eye
     Notes

 Author
The Royal Order, Feb 2014
Sir Darrel R. Deo
****************************************************************************/
int QueryRightEye(void)
{
    //Return the voltage at AD input pin correlated to the left eye
    return(ADS12_ReadADPin(TURR_RIGHT_IR_PIN));
}