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

Module
NavToShootReloadSM.c

Description
This module contains the navigating state machine for the reload shot.

****************************************************************************/
/*----------------------------- 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 "AligningToBeacon.h"
#include "NavToShootReloadSM.h"

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

#define DRIVE_BACK_DIS (4) //2
#define DRIVE_BACK_RPM (30)
/*---------------------------- Module Functions ---------------------------*/
static ES_Event_t DuringDrivingBackward(ES_Event_t Event);
static ES_Event_t DuringAligning(ES_Event_t Event);

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

static NavToShootReloadState_t CurrentState;

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

/****************************************************************************
Function
RunNavToShootReloadSM

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 RunNavToShootReloadSM(ES_Event_t CurrentEvent)
{
bool MakeTransition = false;/* are we making a state
transition? */
NavToShootReloadState_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 DrivingBackward
case DrivingBackward:
{
printf("DRIVING BACKWARD\n\r");
ReturnEvent = CurrentEvent = DuringDrivingBackward(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
//if event is reached encoder limit
case ENCODER_LIMIT_REACHED:
{
//align to their goal beacon
NextState = Aligning;
MakeTransition = true;
EntryEventKind.EventParam = THEIR_GOAL;
}
break;
}
}
}
break;
//if state is aligning to beacon
case Aligning:
{
printf("ALIGNING TO BEACON\n\r");
CurrentEvent = DuringAligning(CurrentEvent);
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
//if event is aligned to beacon
case ALIGNED_TO_BEACON:
{
//remap return event
ReturnEvent.EventType = READY_TO_SHOOT;
}
break;
}
}
}
break;
}

// If we are making a state transition

if (MakeTransition == true)
{
//Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunNavToShootReloadSM(CurrentEvent);

CurrentState = NextState; //Modify state variable

// Execute entry function for new state

// this defaults to ES_ENTRY
 RunNavToShootReloadSM(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
StartNavToShootSM

Parameters
ES_Event_t CurrentEvent

Returns
nothing

Description
Does any required initialization for this state machine
Notes

Author
S. Park
****************************************************************************/
void StartNavToShootReloadSM(ES_Event_t CurrentEvent)
{
printf("StartNavToShootSM\n\r");
//Initialize to DrivingBackward
CurrentState = DrivingBackward;

//Drive backward with encoders for 6 inches

DriveBackwardStraight(DRIVE_BACK_RPM, true, DRIVE_BACK_DIS);

//Run the SM
RunNavToShootReloadSM(CurrentEvent);

return;
}

/***************************************************************************
private functions
***************************************************************************/
static ES_Event_t DuringDrivingBackward(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 ((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 DuringAligning(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 ((Event.EventType == ES_ENTRY) ||
(Event.EventType == ES_ENTRY_HISTORY))
{
SetAligningDirection(ALIGN_CCW);
StartAligningToBeacon(Event);
}
else if (Event.EventType == ES_EXIT)
{
RunAligningToBeacon(Event);
}
else
{
ReturnEvent = RunAligningToBeacon(Event);
}
// return Event without remapping
return ReturnEvent;
}