/****************************************************************************
 Module
   LolaGameMaster.c

 Revision
   2.0.1

 Description
   This is a the top level Hierarchical state machine for Lola Rabbot that
   deals with gameplay

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 02/20/17 14:30 jec      updated to remove sample of consuming an event. We 
                         always want to return ES_NO_EVENT at the top level 
                         unless there is a non-recoverable error at the 
                         framework level
 02/03/16 15:27 jec      updated comments to reflect small changes made in '14 & '15
                         converted unsigned char to bool where appropriate
                         spelling changes on true (was True) to match standard
                         removed local var used for debugger visibility in 'C32
                         removed Microwave specific code and replaced with generic
 02/08/12 01:39 jec      converted from MW_MasterMachine.c
 02/06/12 22:02 jec      converted to Gen 2 Events and Services Framework
 02/13/10 11:54 jec      converted During functions to return Event_t
                         so that they match the template
 02/21/07 17:04 jec      converted to pass Event_t to Start...()
 02/20/07 21:37 jec      converted to use enumerated type for events
 02/21/05 15:03 jec      Began Coding
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* 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 "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"

// This SM
#include "LolaGameMaster.h"

// Lower SM
#include "PlayingSM.h"
#include "ReloadSM.h"
#include "OffenseSM.h"
//#include "DefenseSM.h" 

// Helper Modules
#include "MotorCommands.h"
#include "TeamColorAndGameLEDs.h"
#include "BeamBreak.h"
#include "IRBeaconService.h"

/*----------------------------- Module Defines ----------------------------*/
#define ONE_SEC         976
#define FIVE_SEC        ONE_SEC*5
#define TURN_TIME       ONE_SEC*49/32
#define SERVO_TIME      20        //20 ms

/*---------------------------- Module Functions ---------------------------*/
static ES_Event_t DuringWaitingForStart( ES_Event_t Event);
static ES_Event_t DuringPlaying( ES_Event_t Event);

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, though if the top level state machine
// is just a single state container for orthogonal regions, you could get
// away without it
static LolaGameMasterState_t CurrentState;
// with the introduction of Gen2, we need a module level Priority var as well
static uint8_t MyPriority;

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
     InitLolaGameMasterSM

 Parameters
     uint8_t : the priorty of this service

 Returns
     boolean, False if error in initialization, True otherwise

 Description
     Saves away the priority,  and starts
     the top level state machine
 Notes

 Author
     R. Adenekan
****************************************************************************/
bool InitLolaGameMasterSM ( uint8_t Priority )
{
	//Create a ThisEvent variable
  ES_Event_t ThisEvent;

  MyPriority = Priority;  // save our priority
  //Start with motors stopped
  Stop();

	//Set EventTpe to ES_ENTRY
  ThisEvent.EventType = ES_ENTRY;
  // Start the Master State machine
  StartLolaGameMasterSM( ThisEvent );

  return true;
}

/****************************************************************************
 Function
     PostLolaGameMasterSM

 Parameters
     ES_Event ThisEvent , the event to post to the queue

 Returns
     boolean False if the post operation failed, True otherwise

 Description
     Posts an event to this state machine's queue
 Notes

 Author
     R. Adenekan
****************************************************************************/
bool PostLolaGameMasterSM( ES_Event_t ThisEvent )
{
  return ES_PostToService( MyPriority, ThisEvent);
}

/****************************************************************************
 Function
    RunLolaGameMasterSM

 Parameters
   ES_Event: the event to process

 Returns
   ES_Event: an event to return

 Description
   the run function for the top level state machine 
 Notes
   uses nested switch/case to implement the machine.
 Author
   R. Adenekan
****************************************************************************/
ES_Event_t RunLolaGameMasterSM( ES_Event_t CurrentEvent )
{
   bool MakeTransition = false;/* are we making a state transition? */
   LolaGameMasterState_t NextState = CurrentState;
   ES_Event_t EntryEventKind = { ES_ENTRY, 0 };// default to normal entry to new state
   ES_Event_t ReturnEvent = { ES_NO_EVENT, 0 }; // assume no error

    switch ( CurrentState )
   {

       case WaitingForStart :       // If current state is WaitingForStart
         CurrentEvent = DuringWaitingForStart(CurrentEvent);
         //process any events
         if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
         {
            switch (CurrentEvent.EventType)
            { 
						//If the EventTpe is FACE_OFF
              case FACE_OFF: 
								//Set the NextState to Playing
                NextState = Playing;
                MakeTransition = true; //mark that we are taking a transition
                break;
              
							//If the EventType is ES_TIMEOUT
              case ES_TIMEOUT: 
							//If this is the SERVO_TIMER
              if (CurrentEvent.EventParam == SERVO_TIMER)
              {
								//Turn the Servo Off
                ServoOff();
              }
            break;
            }
         }
         break;
         
				 //If the CurrentState is Playing
         case Playing :       
         CurrentEvent = DuringPlaying(CurrentEvent);
         //process any events
         if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
         {
            switch (CurrentEvent.EventType)
            {
              //If the EventType is GAME_OVER
              case GAME_OVER :
								//Set the NextState to WaitingForStart
                NextState = WaitingForStart;//Decide what the next state will be
                MakeTransition = true; //mark that we are taking a transition
                break;
            }
         }
         break;   
    }
    //   If we are making a state transition
    if (MakeTransition == true)
    {
       //   Execute exit function for current state
       CurrentEvent.EventType = ES_EXIT;
       RunLolaGameMasterSM(CurrentEvent);

       CurrentState = NextState; //Modify state variable

       // Execute entry function for new state
       // this defaults to ES_ENTRY
       RunLolaGameMasterSM(EntryEventKind);
     }
   // in the absence of an error the top level state machine should
   // always return ES_NO_EVENT, which we initialized at the top of func
   return(ReturnEvent);
}
/****************************************************************************
 Function
     StartLolaGameMasterSM

 Parameters
     ES_Event CurrentEvent

 Returns
     nothing

 Description
     Does any required initialization for this state machine
 Notes

 Author
     R. Adenekan
****************************************************************************/
void StartLolaGameMasterSM ( ES_Event_t CurrentEvent )
{
  //Set CurrentState to WaitingForStart
  CurrentState = WaitingForStart;
  // now we need to let the Run function init the lower level state machines
  // use LocalEvent to keep the compiler from complaining about unused var
  RunLolaGameMasterSM(CurrentEvent);
  
  return;
}

//to query state of LolaGameMaster
LolaGameMasterState_t QueryLolaGameMaster(void) 
{
  return(CurrentState);
}

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

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

    // 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) )
    {
      // Stop the robot from moving 
      Stop();
      
      // Turn off the GameLED
      GameLEDOff();

      // turn the servo to resting postion
      ServoRest(); 
      
			//Start the SERVO_TIMER
      ES_Timer_InitTimer(SERVO_TIMER, SERVO_TIME);
    }
		//If this is an ES_EXIT event
    else if ( Event.EventType == ES_EXIT )
    {
      // turn servo off
      ServoOff();
    }
    else
    // do the 'during' function for this state
    {
    }
    return(ReturnEvent);
}

static ES_Event_t DuringPlaying( ES_Event_t Event)
{
    ES_Event_t ReturnEvent = Event; // assme no re-mapping or comsumption

    // 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) )
    {
      // Turn on the GameLED
      GameLEDOn();
        
      //StartPlayingSM with the passed in event
      StartPlayingSM(Event);
    }
    else if ( Event.EventType == ES_EXIT )
    {
      // on exit, give the lower levels a chance to clean up first
      //RunLowerLevelSM(Event);
      RunPlayingSM(Event);

      //stop motors
      Stop();     
    }
    else
    // do the 'during' function for this state
    {
      //Set ReturnEvent equal to the return of RunPlayingSM
         ReturnEvent = RunPlayingSM(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);
}