/****************************************************************************

Module
ShootingSM.c

Description
This module contains the shooting state machine.

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* include header files for the framework and this service
*/
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "ES_DeferRecall.h"

#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "inc/hw_ssi.h"
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h" // Define PART_TM4C123GH6PM in project
#include "driverlib/gpio.h"
#include "termio.h"
#include "hw_nvic.h"
#include "hw_pwm.h"
#include "hw_timer.h"
#include "hw_ssi.h"

#include "MotorDrive.h"
#include "ShootingSM.h"
#include "MasterSM.h"

/*----------------------------- Module Defines ----------------------------*/

//define servo reset timing
#define SERVO_MOVE_TIMING 250 //general wait for servo to move
#define BALL_LOAD_TIMING 500 //wait for ball to roll into cup
#define LOAD_RULER_TIMING 1000 //wait for ruler to load
#define LAUNCH_RULER_TIMING 250 //wait for ruler to launch
#define SCORE_TIMING 2000 //wait to check if we scored, otherwise shoot again

/*---------------------------- Module Functions ---------------------------*/

static ES_Event_t DuringResettingServos(ES_Event_t Event);
static ES_Event_t DuringLoadingBall(ES_Event_t Event);
static ES_Event_t DuringLoadingRuler(ES_Event_t Event);
static ES_Event_t DuringShootingBall(ES_Event_t Event);

/*---------------------------- Module Variables ---------------------------*/

static ShootingState_t CurrentState;
static uint8_t CurrentLaunchPos;
static uint8_t CurrentReloadPos;
static uint8_t CurrentFulcrumPos;
static bool BallLoaded = false;

/*------------------------------ Module Code ------------------------------*/

/****************************************************************************
Function
RunShootingSM

Parameters
ES_Event_t ThisEvent, event to process

Returns
 ES_Event_t ThisEvent, event to return
Description
Posts an event to the queue of the SM
Notes

Author
S. Park
****************************************************************************/
ES_Event_t RunShootingSM(ES_Event_t CurrentEvent)
{
bool MakeTransition = false;/* are we making a state transition? */
ShootingState_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)
{
//if state is ResettingServos
case ResettingServos:
{
printf("RESETTING SERVOS\n\r");
ReturnEvent = CurrentEvent = DuringResettingServos(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
//if event is servo move timeout
case ES_TIMEOUT:
{
if (CurrentEvent.EventParam == SERVO_MOVE_TIMER)
{
//if no ball loaded
if (!BallLoaded)
{
//load a ball
MoveReloadServo(RELOAD_SERVO_LOAD);
BallLoaded = true;

//start servo move timer for ball load timing

ES_Timer_InitTimer(SERVO_MOVE_TIMER, BALL_LOAD_TIMING);

MakeTransition = true;
NextState = LoadingBall;
}
//else if ball is already loaded
else
{
//load ruler
MoveLaunchServo(LAUNCH_SERVO_LOADED);

//start servo move timer

ES_Timer_InitTimer(SERVO_MOVE_TIMER, LOAD_RULER_TIMING);

MakeTransition = true;
NextState = LoadingRuler;
}
}
}
break;
}
}
}
break;
//if state is loading ball
case LoadingBall:
{
printf("LOADING BALL\n\r");
ReturnEvent = CurrentEvent = DuringLoadingBall(CurrentEvent);
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
//if event is servo move timeout
case ES_TIMEOUT:
{
if (CurrentEvent.EventParam == SERVO_MOVE_TIMER)
{
//load ruler
MoveLaunchServo(LAUNCH_SERVO_LOADED);

//reset reload servo

MoveReloadServo(RELOAD_SERVO_HOLD);

//start servo move timer

ES_Timer_InitTimer(SERVO_MOVE_TIMER, LOAD_RULER_TIMING);

MakeTransition = true;
NextState = LoadingRuler;
}
}
break;
}
}
}
break;
case LoadingRuler:
{
printf("LOADING RULER\n\r");
ReturnEvent = CurrentEvent = DuringLoadingRuler(CurrentEvent);
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
//if event is servo move timeout
case ES_TIMEOUT:
{
if (CurrentEvent.EventParam == SERVO_MOVE_TIMER)
{
//launch ball
MoveLaunchServo(LAUNCH_SERVO_LAUNCHED);
DecrementNumBalls();
BallLoaded = false;

//start servo move timer

ES_Timer_InitTimer(SERVO_MOVE_TIMER, LAUNCH_RULER_TIMING);

MakeTransition = true;
NextState = ShootingBall;
}
}
break;
}
}
}
break;
case ShootingBall:
 {
printf("SHOOTING BALL\n\r");
ReturnEvent = CurrentEvent = DuringShootingBall(CurrentEvent);
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
//if event is servo move timeout
case ES_TIMEOUT:
{
if (CurrentEvent.EventParam == SERVO_MOVE_TIMER)
{
//reset launch servo
MoveLaunchServo(LAUNCH_SERVO_IDLE);

//if out of balls

if (QueryNumBalls() == 0)
{
ReturnEvent.EventType = OUT_OF_BALLS;
}
//else remap event TODO what should this be? go on defense? try to shoot
again after a timeout?
else
{
MakeTransition = true;
NextState = ResettingServos;
//start servo move timer to check if we scored
ES_Timer_InitTimer(SERVO_MOVE_TIMER, SCORE_TIMING);
//consume event
ReturnEvent.EventType = ES_NO_EVENT;
}
}
}
break;
}
}
}
break;
}

// If we are making a state transition

if (MakeTransition == true)
{
printf("Making a transition\n\r");
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunShootingSM(CurrentEvent);

CurrentState = NextState; //Modify state variable

// Execute entry function for new state

// this defaults to ES_ENTRY
RunShootingSM(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
StartShootingSM
 Parameters
ES_Event_t CurrentEvent

Returns
nothing

Description
Does any required initialization for this state machine
Notes

Author
S. Park
****************************************************************************/
void StartShootingSM(ES_Event_t CurrentEvent)
{
printf("StartingShootingSM\n\r");
//Initialize to ResettingServos
CurrentState = ResettingServos;

//Reset servo positions

MoveLaunchServo(LAUNCH_SERVO_IDLE);
MoveReloadServo(RELOAD_SERVO_HOLD);
MoveFulcrumServo(QueryOffenseStrat());

//Start servo timer

ES_Timer_InitTimer(SERVO_MOVE_TIMER, SERVO_MOVE_TIMING);

//Run the SM
RunShootingSM(CurrentEvent);

return;
}

/***************************************************************************
private functions
***************************************************************************/
static ES_Event_t DuringResettingServos(ES_Event_t Event)
{
//During function for stopped
ES_Event_t ReturnEvent = Event; // assme no re-mapping or comsumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
// No entry action
}
else if (Event.EventType == ES_EXIT)
{
// No exit action
}
else
{
// No standard during actions
}
// return Event without remapping
return ReturnEvent;
}

static ES_Event_t DuringLoadingBall(ES_Event_t Event)

{
//During function for stopped
ES_Event_t ReturnEvent = Event; // assme no re-mapping or comsumption
 // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
// No entry action
}
else if (Event.EventType == ES_EXIT)
{
// No exit action
}
else
{
// No standard during actions
}
// return Event without remapping
return ReturnEvent;
}

static ES_Event_t DuringLoadingRuler(ES_Event_t Event)

{
//During function for stopped
ES_Event_t ReturnEvent = Event; // assme no re-mapping or comsumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
// No entry action
}
else if (Event.EventType == ES_EXIT)
{
// No exit action
}
else
{
// No standard during actions
}
// return Event without remapping
return ReturnEvent;
}

static ES_Event_t DuringShootingBall(ES_Event_t Event)

{
//During function for stopped
ES_Event_t ReturnEvent = Event; // assme no re-mapping or comsumption

// process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events

if ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
// No entry action
}
else if (Event.EventType == ES_EXIT)
{
// No exit action
}
else
{
// No standard during actions
}
// return Event without remapping
return ReturnEvent;
}