/****************************************************************************
 Module
   HSMTemplate.c

 Revision
   2.0.1

 Description
   This is a template file for implementing state machines.

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
s koppaka
****************************************************************************/
/*----------------------------- 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 "CommunicationsMaster.h"
//#include "DefenseSM.h"      // PUT THIS BACKKKK
#include "IRBeaconService.h"
#include "BeamBreak.h" 
#include "OffenseSM.h"
#include "ReloadSM.h"
#include "PlayingSM.h"
#include "TeamColorAndGameLEDs.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
*/

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

#define ENTRY_STATE Reload

/*---------------------------- 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 DuringReload ( ES_Event_t Event);
static ES_Event_t DuringOffense ( ES_Event_t Event);
static ES_Event_t DuringDefense ( ES_Event_t Event);

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static PlayingSM_State_t CurrentState;
//static PlayingSM_State_t LastState; 
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
    RunPlayingSM

 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
   J. Edward Carryer, 2/11/05, 10:45AM
****************************************************************************/
ES_Event_t RunPlayingSM( ES_Event_t CurrentEvent )
{
   bool MakeTransition = false;/* are we making a state transition? */
   PlayingSM_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 Reload :       // If current state is Reload
         ReturnEvent = CurrentEvent = DuringReload(CurrentEvent);
         //process any events
         if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
         {
            switch (CurrentEvent.EventType)
            {
               case HAVE_BALL_O : //If event is HAVE_BALL_O
									//Set NextState to Offense
                  NextState = Offense;//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;
                // repeat cases as required for relevant events

               case HAVE_BALL_D : //If event is HAVE_BALL_D
									//Set NextState to Defense
									NextState = Defense;//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;

       case Offense :       // If current state is Offense
         ReturnEvent = CurrentEvent = DuringOffense(CurrentEvent);
         //process any events
         if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
         {
            switch (CurrentEvent.EventType)
            {
                case TIE_BREAK : //If event is TIE_BREAK
                  //Set NextState equal to Reload
                  NextState = Reload;//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;
               
               case NO_BALL : //If event is NO_BALL
                  //Set NextState to Reload
                  NextState = Reload;//Decide what the next state will be
                  MakeTransition = true; //mark that we are taking a transition
                  ReturnEvent.EventType = ES_NO_EVENT;
                  break;
                // repeat cases as required for relevant events

               case CHANGE_POSESSION : //If event is CHANGE_POSESSION
                  //NextState = Defense
                  NextState = Defense;//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;         

       case Defense :       // If current state is Defense
         ReturnEvent = CurrentEvent = DuringDefense(CurrentEvent);
         //process any events
         if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
         {
            switch (CurrentEvent.EventType)
            {
               case NO_BALL : //If event is NO_BALL
                 //NextState equals Reload
                  NextState = Reload;//Decide what the next state will be
                  MakeTransition = true; //mark that we are taking a transition
                  ReturnEvent.EventType = ES_NO_EVENT;
                  break;
                
                case TIE_BREAK : //If event is TIE_BREAK
                  //Set NextState to Reload
                  NextState = Reload;//Decide what the next state will be
                  MakeTransition = true; //mark that we are taking a transition
                  ReturnEvent.EventType = ES_NO_EVENT;
                  break;                  

               case CHANGE_POSESSION : //If event is CHANGE_POSESSION
                 //Set NextState to Offense
                  NextState = Offense;//Decide what the next state will be
                  MakeTransition = true; //mark that we are taking a transition
                  ReturnEvent.EventType = ES_NO_EVENT;
                  break;               
            }
         }
       break;                  
         
    }
   
    //   If we are making a state transition
    if (MakeTransition == true)
    {
       //   Execute exit function for current state
       CurrentEvent.EventType = ES_EXIT;
			 //Run PlayingSM
       RunPlayingSM(CurrentEvent);

       CurrentState = NextState; //Modify state variable

       //   Execute entry function for new state
       // this defaults to ES_ENTRY
			 //Run PlayingSM
       RunPlayingSM(EntryEventKind);
     }
     return(ReturnEvent);
}
/****************************************************************************
 Function
     StartPlayingSM

 Parameters
     None

 Returns
     None

 Description
     Does any required initialization for this state machine
 Notes

 Author
    s koppaka 
****************************************************************************/
void StartPlayingSM ( ES_Event_t CurrentEvent )
{
	//If we are not entering with history
   if ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
   {
				//Set the CurrentState to ENTRY_STATE
        CurrentState = ENTRY_STATE;
   }
    // call the entry function (if any) for the ENTRY_STATE
   RunPlayingSM (CurrentEvent);
}

/****************************************************************************
 Function
     QueryPlayingSM

 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 k 
****************************************************************************/
PlayingSM_State_t QueryPlayingSM ( void )
{
   return(CurrentState);
}

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

static ES_Event_t DuringReload( 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) )
    {
				//Start ReloadSM
        StartReloadSM(Event);
    }
    else if ( Event.EventType == ES_EXIT )
    {
        //Run ReloadSM
        RunReloadSM(Event);
    }else
    {
        //Set ReturnEvent equal to RunReloadSM
         ReturnEvent = RunReloadSM(Event);
    }
    // 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);
}   
    
static ES_Event_t DuringOffense( 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) )
    {
      //Start OffenseSM
        StartOffenseSM(Event);
    }
    else if ( Event.EventType == ES_EXIT )
    {
        //RunOffenseSM
        RunOffenseSM(Event);          
    }else
    // do the 'during' function for this state
    {
        //Set ReturnEvent equal to RunOffenseSM
          ReturnEvent = RunOffenseSM(Event);
    }
    // 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);
    
 }
static ES_Event_t DuringDefense( 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 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);
           
}