/****************************************************************************
 Module
   DefenseSM.c

 Revision
   2.0.1

 Description
	This is for a lower level of the LolaGameMaster Service, responsible for implementing our defensive strategy.

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 03.05.18        skoppaka   first pass
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.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 <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 "DefenseSM.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 ReverseForDefense

#define ReverseTime 3500 // 3.5 ms

/*---------------------------- 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 DuringReverseForDefense (ES_Event_t Event);
static ES_Event_t DuringAlignWithLolaGoal( ES_Event_t Event);
static ES_Event_t DuringGuarding( ES_Event_t Event);

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static DefenseSM_State_t CurrentState;

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
    RunDefenseSM

 Parameters
   ES_Event: the event to process

 Returns
   ES_Event: an event to return

 Description
	 Run function that responds to various event

 Notes
   uses nested switch/case to implement the machine.
 Author
   Sai Koppaka
****************************************************************************/
ES_Event_t RunDefenseSM( ES_Event_t CurrentEvent )
{
   bool MakeTransition = false;/* are we making a state transition? */
   DefenseSM_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 )
   {
		 //If the CurrentState is ReverseForDefense
     case ReverseForDefense :
		 //Set the ReturnEvent and the Current Event equal to the ReturnEvent of DuringReverseForDefense
       ReturnEvent = CurrentEvent = DuringReverseForDefense(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 == DEFENSE_TIMER)
                 {                   
                  NextState = AlignWithLolaGoal;//Set the NextState to AlignWithLolaGoal
                  MakeTransition = true; //mark that we are taking a transition
                  ReturnEvent.EventType = ES_NO_EVENT;
                 }
                  break;
            }
         }
         break;
         
       case AlignWithLolaGoal :       // If current state is AlignWithLolaGoal
			 	//Set the ReturnEvent and the Current Event equal to the ReturnEvent of DuringAlignWithLolaGoal
         ReturnEvent = CurrentEvent = DuringAlignWithLolaGoal(CurrentEvent);
         //process any events
         if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
         {
            switch (CurrentEvent.EventType)
            {
               case DETECTED_BEACON : //If event is DETECTED_BEACON
                  NextState = Guarding;//Set the NextState to Guarding
                  MakeTransition = true; //mark that we are taking a transition
                  ReturnEvent.EventType = ES_NO_EVENT;
                  break;
            }
         }
       break;
         
         case Guarding :       // If current state is Guarding
		 //Set the ReturnEvent and the Current Event equal to the ReturnEvent of DuringGuarding
         ReturnEvent = CurrentEvent = DuringGuarding (CurrentEvent);
         //process any events
         if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
         {
            switch (CurrentEvent.EventType)
            {
               case TAPE_DETECTED : //If event is TAPE_DETECTED
                 if (CurrentEvent.EventParam == 3) //back tape sensor only
                 {
                  //stop driving
                  Stop(); 
                  MakeTransition = false; //mark that we are not 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;
       RunDefenseSM(CurrentEvent);

       CurrentState = NextState; //Modify state variable

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

 Parameters
     None

 Returns
     None

 Description
     Does any required initialization for this state machine
 Notes

 Author
     Sai Koppaka
****************************************************************************/
void StartDefenseSM ( ES_Event_t CurrentEvent )
{
   if ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
   {
        CurrentState = ENTRY_STATE;
   }
   // Run the Defense state machine in the ENTRY_STATE
   RunDefenseSM(CurrentEvent);
}

/****************************************************************************
 Function
     QueryDefenseSM

 Parameters
     None

 Returns
     DefenseSM_State_t The current state of the defenseSM state machine

 Description
     returns the current state of the Defense state machine
 Notes

 Author
     Sai Koppaka
****************************************************************************/
DefenseSM_State_t QueryDefenseSM ( void )
{
	//Return CurrentState
   return(CurrentState);
}

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

static ES_Event_t DuringReverseForDefense( 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) )
    {
      // Reverse at half speed 
      ReverseHalf();
      
      // Start a timer to stop the reverse
      ES_Timer_InitTimer(DEFENSE_TIMER, ReverseTime);
    }
    else if ( Event.EventType == ES_EXIT )
    {
			//Stop the motors
      Stop();
    }
		else
    {	
		}
    return(ReturnEvent);
}

static ES_Event_t DuringAlignWithLolaGoal( 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();
			//Post ALIGH_WITH_BEACON to IRBeaconService
      ES_Event_t Event2Post; 
      Event2Post.EventType = ALIGN_WITH_BEACON;
      Event2Post.EventParam = 2; //Align with Lola Goal 
      PostIRBeaconService(Event2Post); 
    }
    else if ( Event.EventType == ES_EXIT )
    {
    }
		else
    {
    }
    return(ReturnEvent);
}

static ES_Event_t DuringGuarding( 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) )
    {
        //Drive backward at half speed
        ReverseHalf();
    }
    else if ( Event.EventType == ES_EXIT )
    {
    }
		else
    // do the 'during' function for this state
    {
    }
    return(ReturnEvent);
}