/****************************************************************************
 Module
   OffsenseSM.c

 Revision
   2.0.1

 Description
   This implements OffenseSM, called by LolaGameMaster

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
03.05        Sai Koppaka      first pass                  
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"

#include <string.h>
#include <stdio.h>
#include <stdbool.h>
#include <stdint.h>

#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"  
#include "driverlib/gpio.h"

#include "inc/hw_pwm.h"
#include "inc/hw_nvic.h"
#include "inc/hw_timer.h"

#include "inc/tm4c123gh6pm.h" // Define PART_TM4C123GH6PM in project
#include "inc/hw_ssi.h" 

#include "MotorCommands.h"
#include "LolaGameMaster.h" 
#include "OffenseSM.h"
#include "IRBeaconService.h"
#include "BeamBreak.h" 

/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines

#define ENTRY_STATE BackUpFromReload
#define ONE_SEC 1000
#define SHOT_TIME (ONE_SEC*3)  //Time it takes for the ball to reach goal 
#define REVERSE_TIME 1500 //2s
#define SHOOTING_TIME 1000 //1s

/*---------------------------- 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_t DuringBackUpFromReload( ES_Event_t Event);
static ES_Event_t DuringAlignWithBeacon ( ES_Event_t Event);
static ES_Event_t DuringWaitingToShoot  ( ES_Event_t Event);
static ES_Event_t DuringShooting        ( ES_Event_t Event);

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static OffenseSM_State_t CurrentState;
//static uint16_t FlywheelStartTime; //does this need to be static

//static uint8_t TurnToBeaconTime 500;

static const uint16_t FlywheelDesiredOnTime = 8000;
static uint16_t SweepAlignTime = 840;
static bool ReadyToShoot = false;

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
    RunOffenseSM

 Parameters
   ES_Event: the event to process

 Returns
   ES_Event: an event to return

 Description
   add your description here
 Notes
   uses nested switch/case to implement the machine.
 Author
   s koppaka 
****************************************************************************/
ES_Event_t RunOffenseSM( ES_Event_t CurrentEvent )
{
   bool MakeTransition = false;/* are we making a state transition? */
   OffenseSM_State_t NextState = CurrentState;
   ES_Event_t EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
   ES_Event_t ReturnEvent = CurrentEvent; // assume we are not consuming event

   switch ( CurrentState )
   {
       case BackUpFromReload :       // If current state is BackUpFromReload
        
         ReturnEvent = CurrentEvent = DuringBackUpFromReload(CurrentEvent);
         //process any events
         if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
         {
            switch (CurrentEvent.EventType)
            {
               case ES_TIMEOUT : //If event is ES_TIMEOUT
                    if(CurrentEvent.EventParam == OFFENSE_TIMER)
                    {
                    //Set the NextState to AlignWithBeacon
                    NextState = AlignWithBeacon;//Decide what the next state will be
                    MakeTransition = true; //mark that we are taking a transition
                    ReturnEvent.EventType = ES_NO_EVENT;
                    }
                    if(CurrentEvent.EventParam == FLYWHEEL_TIMER)
                    {
											//Set ReadyToShoot to true
                      ReadyToShoot = true;
                    ReturnEvent.EventType = ES_NO_EVENT;
                    }
                 break; //break out of event
            }
         }
       break; //break out of case
        
        case AlignWithBeacon :       // If current state is AlignWithBeacon 
         ReturnEvent = CurrentEvent = DuringAlignWithBeacon(CurrentEvent);
         //process any events
         if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
         {
            switch (CurrentEvent.EventType)
            {
               case ALIGNED_BEACON : //If event is ALIGNED_BEACON
                  // Execute action function for state one : event one
									//Stop the motors
                  Stop();      
									//Set NextState to WaitingToShoot
                  NextState = WaitingToShoot;//Decide what the next state will be
                  // for internal transitions, skip changing MakeTransition
                  MakeTransition = true; //mark that we are taking a transition
                  ReturnEvent.EventType = ES_NO_EVENT;
               
							 //If ThisEvent is an ES_TIMEOUT
               case ES_TIMEOUT : 
							 //If this is the SWEEP_OFFENSE_TIMER
               if(CurrentEvent.EventParam == SWEEP_OFFENSE_TIMER)
               {
                 //Stop the motors
                  Stop();
                  //Set the NextState fto WaitingToShoot
                  NextState = WaitingToShoot;//Decide what the next state will be
                  // for internal transitions, skip changing MakeTransition
                  MakeTransition = true; //mark that we are taking a transition
                  ReturnEvent.EventType = ES_NO_EVENT;
               }                 
               if(CurrentEvent.EventParam == FLYWHEEL_TIMER)
                    {
											//Set ReadyToShoot to true
                      ReadyToShoot = true;
                    ReturnEvent.EventType = ES_NO_EVENT;
                    }
                  break;   //break out of event 
            }
          }
         break; //break out of case
          
          case WaitingToShoot :       // If current state is WaitingToShoot
         ReturnEvent = CurrentEvent = DuringWaitingToShoot(CurrentEvent);
         //process any events
         if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
         { 
            switch (CurrentEvent.EventType)
            {
               case ES_TIMEOUT : //If event is ES_TIMEOUT
                 if(CurrentEvent.EventParam == FLYWHEEL_TIMER || ReadyToShoot)
                 {
									//Set NextState to Shooting
                  NextState = Shooting;//Decide what the next state will be
                  // for internal transitions, skip changing MakeTransition
                  MakeTransition = true; //mark that we are taking a transition
                  ReturnEvent.EventType = ES_NO_EVENT;
                 }
                  break;   //break out of event
            }
          }
        break;    //break out of case 

        case Shooting :       // If current state is Shooting
         ReturnEvent = CurrentEvent = DuringShooting(CurrentEvent);
         //process any events
         if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
         {
            switch (CurrentEvent.EventType)
            {
               case ES_TIMEOUT: //If event is ES_TIMEOUT
                  // Execute action function for state one : event one
                  if (CurrentEvent.EventParam == OFFENSE_TIMER)
                  {
									//Remap the event as a NO_BALL event
                  ReturnEvent.EventType = NO_BALL;
                  } 
                break;                  
            }
          }
         break;
        }
    
    //   If we are making a state transition
    if (MakeTransition == true)
    {
       //   Execute exit function for current state
       CurrentEvent.EventType = ES_EXIT;
       RunOffenseSM(CurrentEvent);

       CurrentState = NextState; //Modify state variable

       //   Execute entry function for new state
       // this defaults to ES_ENTRY
       RunOffenseSM(EntryEventKind);
     }
     return(ReturnEvent);
  
   }
/****************************************************************************
 Function
     StartOffenseSM

 Parameters
     None

 Returns
     None

 Description
     Does any required initialization for this state machine
 Notes

 Author
     s koppaka 
****************************************************************************/
void StartOffenseSM ( ES_Event_t 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 = ENTRY_STATE;
   }
   // call the entry function for RunOffenseSM
   RunOffenseSM(CurrentEvent);
}

/****************************************************************************
 Function
     QueryOffenseSM

 Parameters
     None

 Returns
     TemplateState_t The current state of the Template state machine

 Description
     returns the current state of the Template state machine
 Notes

 Author
     S Koppaka 
****************************************************************************/
OffenseSM_State_t QueryOffenseSM ( void )
{
   return(CurrentState);
}

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

static ES_Event_t DuringBackUpFromReload( ES_Event_t Event)
{
    ES_Event_t ReturnEvent = Event; // assume no re-mapping or consumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
		//If this is an ES_ENTRY event
    if ( (Event.EventType == ES_ENTRY) ||
         (Event.EventType == ES_ENTRY_HISTORY) )
    {
        //If I have a ball, then I will turn on the flywheel and start get my time value
        if (HaveBall())
        {
					//Turn the flywheel on
          FlywheelOn(); 
          //Set ReadyToShoot to false
          ReadyToShoot = false;
					//Start the FLYWHEEL_TIMER
          ES_Timer_InitTimer(FLYWHEEL_TIMER, FlywheelDesiredOnTime);
          //Reverse at half speed 
          ReverseHalf(); 
          // Starting timer to reverse
          ES_Timer_InitTimer(OFFENSE_TIMER, REVERSE_TIME);
        }
        else //else I post to LolaGameMaster that I do not have a ball 
        {
					//Create an Event2Post variable
          ES_Event_t Event2Post;
					//Set the Event2Post to NO_BALL
          Event2Post.EventType = NO_BALL;
          //PostLolaGameMasterSM(Event2Post); 
          PostLolaGameMasterSM(Event2Post);
          //Reset the EventType to ES_NO_EVENT
          Event2Post.EventType = ES_NO_EVENT;
					//Set the ReturnEvent to Event2Post
          ReturnEvent = Event2Post;
        }
    }
		//If this is an ES_EXIT event
    else if ( Event.EventType == ES_EXIT )
    {
        //Stop the motors
        Stop(); 
    }else
    {
    }
    return(ReturnEvent);
}

static ES_Event_t DuringAlignWithBeacon( ES_Event_t Event)
{
    ES_Event_t ReturnEvent = Event; // assume no re-mapping or consumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
		//If this is an ES_ENTRY event
    if ( (Event.EventType == ES_ENTRY) ||
         (Event.EventType == ES_ENTRY_HISTORY) )
    {
			 //turning the servo off 
			 ServoOff();

			 //Rotate clockwise 
			 RotateCCW();
			 //Start the SWEEP_OFFENSE_TIMER
       ES_Timer_InitTimer(SWEEP_OFFENSE_TIMER, SweepAlignTime);
    }
    else if ( Event.EventType == ES_EXIT )
    {
    }else
    {
    }
    return(ReturnEvent);
}

static ES_Event_t DuringWaitingToShoot ( ES_Event_t Event)
{
    ES_Event_t ReturnEvent = Event; // assume no re-mapping or consumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
         (Event.EventType == ES_ENTRY_HISTORY) )
    {
			//Stop the motors
      Stop();
    }
    else if ( Event.EventType == ES_EXIT )
    {
    }else
    {
    }
    return(ReturnEvent);
}

static ES_Event_t DuringShooting ( ES_Event_t Event)
{
    ES_Event_t ReturnEvent = Event; // assume no re-mapping or consumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
		//If this is an ES_ENTRY event
    if ( (Event.EventType == ES_ENTRY) ||
         (Event.EventType == ES_ENTRY_HISTORY) )
    {
      //Move the servo to the drop ball position
       ServoDropBall();
			 
         ES_Timer_InitTimer(OFFENSE_TIMER, SHOOTING_TIME);
        
        // after that start any lower level machines that run in this state
        //StartLowerLevelSM( Event );
        // repeat the StartxxxSM() functions for concurrent state machines
        // on the lower level
    }
    else if ( Event.EventType == ES_EXIT )
    {
      // Reset the servo
      ServoRest();
      
      // Turn off the flywheel
      FlywheelOff();
      
      //Turn off the servo 
       ServoOff();
        // on exit, give the lower levels a chance to clean up first
        //RunLowerLevelSM(Event);
        // repeat for any concurrently running state machines
        // now do any local exit functionality
      
    }else
    // do the 'during' function for this state
    {
        // run any lower level state machine
      
        // ReturnEvent = RunLowerLevelSM(Event);
       
        // repeat for any concurrent lower level machines
      
        // 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);
}