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

Module
FaceoffNav.c

****************************************************************************/
/*----------------------------- 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 "ReloadIRCaptureModule.h"
#include "FaceoffNav.h"
#include "MotorDrive.h"
#include "LineFollowingSM.h"
#include "NewtonStep.h"

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

// define constants for the states for this machine
// and any other local defines

#define ENTRY_STATE DRIVING_FAST

#define FAST_RPM (70)
#define SLOW_RPM (30)
#define ROTATE_RPM (23)
#define TAPE_TIME_MS (200) // TODO need to tune this!

/*---------------------------- 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 DuringDrivingFast(ES_Event_t Event);
static ES_Event_t DuringDrivingSlow(ES_Event_t Event);
static ES_Event_t DuringRotateCW(ES_Event_t Event);
static ES_Event_t DuringLineFollowing(ES_Event_t Event);
static ES_Event_t DuringMeasuring(ES_Event_t Event);
static ES_Event_t DuringNewtonStepping(ES_Event_t Event);

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

// everybody needs a state variable, you may need others as well
static FaceoffNavState_t CurrentState;

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

/****************************************************************************
Function
RunTemplateSM

Parameters
ES_Event_t: the event to process

Returns
ES_Event_t: 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 RunFaceoffNav(ES_Event_t CurrentEvent)
{
bool MakeTransition = false;/* are we making a state transition? */
FaceoffNavState_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)
{
//while we're driving fast off wall
case DRIVING_FAST:
{
ReturnEvent = CurrentEvent = DuringDrivingFast(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
if (CurrentEvent.EventType == LINE_ON)
{
NextState = DRIVING_SLOW;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
}
else if ((CurrentEvent.EventType == LEFT_BUMPER_HIT) ||
(CurrentEvent.EventType == RIGHT_BUMPER_HIT))
{
ReturnEvent.EventType = UNEXPECTED_BUMPER;
}
}
}
break;

case DRIVING_SLOW:
{
ReturnEvent = CurrentEvent = DuringDrivingSlow(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case ENTERED_TAPE:
{
NextState = ROTATE_CW;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
}
break;
//adding doomsday timer
case ES_TIMEOUT:
{
if (CurrentEvent.EventParam == MOVEMENT_TIMER)
{
NextState = ROTATE_CW;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
}
}
break;
}
}
}
break;

case ROTATE_CW:
 {
ReturnEvent = CurrentEvent = DuringRotateCW(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case LINE_ON:
{
NextState = LINE_FOLLOW;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
}
break;
}
}
}
break;

case LINE_FOLLOW:
{
ReturnEvent = CurrentEvent = DuringLineFollowing(CurrentEvent);
//process any events
//printf("we got a %d event\r\n", ReturnEvent.EventType);
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
if ((CurrentEvent.EventType == RIGHT_BUMPER_HIT) ||
(CurrentEvent.EventType == LEFT_BUMPER_HIT))
{
//printf("GOT TO FINAL POSIT.\r\n");
NextState = MEASURING;
MakeTransition = true;
ReturnEvent.EventType = ES_NO_EVENT;
}
}
}
break;

case MEASURING:
{
ReturnEvent = CurrentEvent = DuringMeasuring(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
if (CurrentEvent.EventType == FOUND_RELOAD_IR)
{
DisableReloadIR();
ReturnEvent.EventType = READY_FOR_RELOAD;
}
}
}
break;

case NEWTON_STEPPING: // If current state is DrivingBackwardTime

{ // Execute During function for DrivingBackwardTime state.
ReturnEvent = CurrentEvent = DuringNewtonStepping(CurrentEvent);
//process any events
if (CurrentEvent.EventType != ES_NO_EVENT) //If an event is active
{
switch (CurrentEvent.EventType)
{
case NEWTON_DONE: //If event is NEWTON_DONE
{ EntryEventKind.EventType = ES_ENTRY;
// Return READY_FOR_RELOAD and leave SM
 ReturnEvent.EventType = READY_FOR_RELOAD;
ReturnEvent.EventParam = CurrentEvent.EventParam;
}
break;
}
}
}
break;
}
// If we are making a state transition
if (MakeTransition == true)
{
// Execute exit function for current state
CurrentEvent.EventType = ES_EXIT;
RunFaceoffNav(CurrentEvent);

CurrentState = NextState; //Modify state variable

// Execute entry function for new state

// this defaults to ES_ENTRY
RunFaceoffNav(EntryEventKind);
}
return ReturnEvent;
}

/****************************************************************************
Function
StartTemplateSM

Parameters
None

Returns
None

Description
Does any required initialization for this state machine
Notes

Author
J. Edward Carryer, 2/18/99, 10:38AM
****************************************************************************/
void StartFaceoffNav(ES_Event_t CurrentEvent)
{
// to implement entry to a history state or directly to a substate
// you can modify the initialization of the CurrentState variable
// otherwise just start in the entry state every time the state machine
// is started
if (ES_ENTRY_HISTORY != CurrentEvent.EventType)
{
CurrentState = ENTRY_STATE;
}
// call the entry function (if any) for the ENTRY_STATE
RunFaceoffNav(CurrentEvent);
}

/****************************************************************************
Function
QueryTemplateSM

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
J. Edward Carryer, 2/11/05, 10:38AM
****************************************************************************/
FaceoffNavState_t QueryFaceoffNav(void)
{
return CurrentState;
}

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

static ES_Event_t DuringDrivingFast(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))
{
//printf("Entered faceoff nav\n\r");
EnableLineOn();
DriveForwardStraight(FAST_RPM, false, 0);
}
else if (Event.EventType == ES_EXIT)
{}
else
{}
return ReturnEvent;
}

static ES_Event_t DuringDrivingSlow(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))
{
ES_Timer_InitTimer(MOVEMENT_TIMER, TAPE_TIME_MS);
DriveForwardStraight(SLOW_RPM, false, 0);
}
else if (Event.EventType == ES_EXIT)
{
ES_Timer_StopTimer(MOVEMENT_TIMER);
StopDrive();
}
else
{}
return ReturnEvent;
}

static ES_Event_t DuringRotateCW(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))
 {
EnableLineOn();
DriveCWStraight(ROTATE_RPM, false, 0);
}
else if (Event.EventType == ES_EXIT)
{
StopDrive();
}
else
{}
return ReturnEvent;
}

static ES_Event_t DuringLineFollowing(ES_Event_t Event)

{
//During function for LineFollowing
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))
{
StartLineFollowingSM(Event);
}
else if (Event.EventType == ES_EXIT)
{
RunLineFollowingSM(Event);
StopDrive();
}
else
{}
// return Event without remapping
return ReturnEvent;
}

static ES_Event_t DuringMeasuring(ES_Event_t Event)

{
//During function for LineFollowing
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))
{
EnableReloadIR();
}
else if (Event.EventType == ES_EXIT)
{
DisableReloadIR();
}
else
{
// ReturnEvent = RunLineFollowingSM(Event);
}
// return Event without remapping
return ReturnEvent;
}

static ES_Event_t DuringNewtonStepping(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))
{
StartNewtonStep(Event);
}
else if (Event.EventType == ES_EXIT)
{
// Stop drive on exit
RunNewtonStep(Event);
}
else
{
ReturnEvent = RunNewtonStep(Event);
}
return ReturnEvent;
}