/****************************************************************************
 Module
   NormalGameplayHSM.c

 Revision
   2.0.1

 Description
   This is the NormalGameplay state machine file.
   Handles normal gameplay movement

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 Feb 2014		Royal Order of King Arthur's Homies for the Abolition of the Global Variable		Winning 218B

****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
// Basic includes for a program using the Events and Services Framework
#include "ES_Configure.h"
#include "ES_Framework.h"
#include "MasterHSM.h" 		//For defines
#include <stdio.h>         //Everybody wants to talk to computers
#include "ForwardControlHSM.h"   //Gotta include those sub-state machines
#include "ReverseControlHSM.h"      //If you want to see weird behavior, try commenting this line out.  Crazy shit happens.
#include "ReloadControlHSM.h"
#include "Timers.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 "NormalGameplayHSM.h"
#include "EventCheckers.h" //For QueryOverarcingStage functiion
/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines

/*---------------------------- 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
*/
//During functiosn for all sub-states
static ES_Event DuringRecess( ES_Event Event);
static ES_Event DuringDrivingReverse(ES_Event Event);
static ES_Event DuringDrivingForward(ES_Event Event);
static ES_Event DuringQueryingJSR(ES_Event Event);

/*---------------------------- Module Variables ---------------------------*/
// everybody needs a state variable, you may need others as well
static NormalGameplayState_t CurrentState;
static unsigned int        RecessCount =1;      //Number of the recess you are on, indexing from one so it makes sense to talk about
static unsigned char CurrentOverarcingStage=WAITING_FOR_START;    //Module level variable keeping track of what the game thinks is going on

static unsigned int RequestCount = 0;
static unsigned int MaxRequestCount = 5;
/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
    RunNormalGameplaySM

 Parameters
   ES_Event: the event to process

 Returns
   ES_Event: an event to return

 Description
   Handles NormalGameplay tracking algorithm

 Notes
   uses nested switch/case to implement the machine.
 Author
The Royal Order, Feb 2014

****************************************************************************/
ES_Event RunNormalGameplaySM( ES_Event CurrentEvent )
{

    //************* Initial stuff required to make HSM work *********
    //Hahahah MakeTransition SHOULD NEVER BE NOT STATIC.
    boolean MakeTransition = FALSE;           /* are we making a state transition? */
    NormalGameplayState_t NextState = CurrentState;
    ES_Event EntryEventKind = { ES_ENTRY, 0 };      // default to normal entry to new state
    ES_Event ReturnEvent = CurrentEvent;            // assume we are not consuming event

    //**************Big switch on states*************
    switch ( CurrentState )
        {

            //*********Querying JSR State *****************
        case STATE_QUERYING_JSR :       // If current state is state one
            // Execute During function for state one. ES_ENTRY & ES_EXIT are
            // processed here allow the lowere level state machines to re-map
            // or consume the event
            CurrentEvent = DuringQueryingJSR(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
                {

                    switch (CurrentEvent.EventType)
                        {

                            //We can enter into ANY state from here because of abnormal gameplay, so this is a
                            //long ass switch statement

                        case ES_ENTRY:		                                          ///Entry function
                            MakeTransition=FALSE;   //No transition unless
                            printf("\r\n \t\t\tEntered Querying JSR State! \r\n");

                            //Every time we enter, we want to query module level variable in event checkers and use this to select
                            //our state.  We do this here.
                            //Upon entry we want to query the current stage of the game according to the JSR and enter into that stage
                            CurrentOverarcingStage=QueryOverarcingStage();
                            printf("Overarcing stage was %d",CurrentOverarcingStage);

                            switch (CurrentOverarcingStage)                 //Pitch ourselves an event depending on what the JSR said
                                {
                                case WAITING_FOR_START:
                                    MakeTransition=FALSE;
                                    NextState=STATE_QUERYING_JSR;
                                    break;

                                case PASDARMES_1:
                                    MakeTransition=TRUE;
                                    NextState=STATE_DRIVING_FORWARD;
                                    break;

                                case RECESS_1:
                                    MakeTransition=TRUE;
                                    NextState=STATE_RECESS;
                                    break;

                                case PASDARMES_2:
                                    MakeTransition=TRUE;
                                    NextState=STATE_DRIVING_REVERSE;
                                    break;

                                case RECESS_2:
                                    MakeTransition=TRUE;
                                    NextState=STATE_RECESS;
                                    break;

                                case PASDARMES_3:
                                    MakeTransition=TRUE;
                                    NextState=STATE_DRIVING_FORWARD;

                                    break;

                                case RECESS_3:
                                    MakeTransition=TRUE;
                                    NextState=STATE_RECESS;
                                    break;

                                case SUDDEN_DEATH:
                                    MakeTransition=TRUE;
                                    NextState=STATE_DRIVING_REVERSE;
                                    break;

                                }

                            break;      //Break on entry

                        case ES_CHANGE_STAGE_WAITING: //If we are told to stay waiting
                            MakeTransition=FALSE;
                            NextState=CurrentState;
                            break;

                        case ES_CHANGE_STAGE_RND1_OR_3:  //If we are told to enter round 1 or 3
                            MakeTransition=TRUE;
                            NextState=STATE_DRIVING_FORWARD;
                            //Init stuff is handled elsewhere
                            break;

                        case ES_CHANGE_STAGE_RND2: //if we are told to
                            MakeTransition=TRUE;
                            NextState=STATE_DRIVING_REVERSE;
                            break;

                        case ES_CHANGE_STAGE_RECESS:
                            MakeTransition=TRUE;
                            NextState=STATE_RECESS;
                            break;

                            //

                            //Add your transitions here
                        }
                }
            break;

//*********Driving Forward State *****************

        case STATE_DRIVING_FORWARD:       // If current state is state one
            // Execute During function for state one. ES_ENTRY & ES_EXIT are
            // processed here allow the lowere level state machines to re-map
            // or consume the event
            CurrentEvent = DuringDrivingForward(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
                {
                    switch (CurrentEvent.EventType)
                        {

                        case ES_ENTRY:		///Entry function
                            MakeTransition=FALSE;			                        //We are not transitioning

                            printf("\r\n \t\t\t Entered DrivingForward State! \r\n");			                        //We are not transitioning
                            break;

                        case ES_CHANGE_STAGE_RECESS:
                            MakeTransition=TRUE;
                            NextState=STATE_RECESS;
                            break;

                        case ES_GAME_OVER:
                            //It ain't over 'till the power switch flips
                            //Really, though, this handles the case of a playoff.  Just keep going until it's really over

                            MakeTransition=TRUE;
                            NextState=STATE_RECESS; //Ain't over 'till it's over
                            break;

                        }
                }
            break;
            // repeat state pattern as required for other states

            //*********Recess State *****************
        case STATE_RECESS :       // If current state is state one
            // Execute During function for state one. ES_ENTRY & ES_EXIT are
            // processed here allow the lowere level state machines to re-map
            // or consume the event
            CurrentEvent = DuringRecess(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
                {
                    switch (CurrentEvent.EventType)
                        {

                        case ES_ENTRY:		///Entry function
                            MakeTransition=FALSE;			//We are not transitioning
                            //Do entry things
                            printf("\r\n \t\t\t Entered Recess State! \r\n");			                        //We are not transitioning
                            break;

                        case ES_CHANGE_STAGE_RND2:          //If you are going into stage 2
                            MakeTransition=TRUE;
                            NextState=STATE_DRIVING_REVERSE;

                            //BallMotorControl(Run)
                            break;

                        case ES_CHANGE_STAGE_RND1_OR_3:     //if you are going into stage 2
                            MakeTransition=TRUE;
                            NextState=STATE_DRIVING_FORWARD; //Ain't over 'till it's over
                            break;

                        }
                }
            break;

            //*********Driving Reverse STate *****************
        case STATE_DRIVING_REVERSE:       // If current state is state one
            // Execute During function for state one. ES_ENTRY & ES_EXIT are
            // processed here allow the lowere level state machines to re-map
            // or consume the event
            CurrentEvent = DuringDrivingReverse(CurrentEvent);
            //process any events
            if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
                {
                    switch (CurrentEvent.EventType)
                        {

                        case ES_ENTRY:		///Entry function
                            MakeTransition=FALSE;			//We are not transitioning
                            //Do entry things

                            printf("\r\n \t\t\t Entered Driving Reverse State! \r\n");   //We are not transitioning
                            break;

                        case ES_CHANGE_STAGE_RECESS:     //if you are going into recess
                            MakeTransition=TRUE;
                            NextState=STATE_RECESS;
                            break;

                        case ES_CHANGE_STAGE_RND2:
                            printf("Received Change Stage Rnd 2 event\r\n");
                            break;

                        case ES_TIMEOUT:     //if you are going into recess

                            switch (CurrentEvent.EventType) //Depends on which timer
                                {
                                case SHOOT_TIME_TIMER: //If it was the timer that controls shutting off the ball shooter
                                    MakeTransition=FALSE;
                                    NextState=CurrentState;
                                    //BallMotorControl(off)
                                    break;
                                }

                            break;

                        }
                }

            break;
            // repeat state pattern as required for other states

        }//End switch on state

    //****************Standard stuff to make HSM work ****************
    if (MakeTransition==True)
        {
            //Exit, move and then enter

            //Exit
            CurrentEvent.EventType=ES_EXIT;
            RunNormalGameplaySM(CurrentEvent);

            //Change
            CurrentState=NextState;

            //Enter
            CurrentEvent.EventType=ES_ENTRY;
            RunNormalGameplaySM(CurrentEvent);

        }

    //CurrentState=NextState;							//Update states
    //printf("Current state is %d \r\n",CurrentState);
    return(ReturnEvent);
}

/****************************************************************************
 Function
     StartNormalGameplaySM

 Parameters
     None

 Returns
     None

 Description
     Does any required initialization for this state machine

 Notes

 Author
The Royal Order, Feb 2014

****************************************************************************/
void StartNormalGameplaySM ( ES_Event CurrentEvent )
{
    // local variable to get debugger to display the value of CurrentEvent
    ES_Event LocalEvent = 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 = STATE_QUERYING_JSR;			//Entry state is STATE_Querying_JSR
        }

    // call the entry function (if any) for the ENTRY_STATE
    RunNormalGameplaySM(CurrentEvent);
}

/****************************************************************************
 Function
     QueryNormalGameplaySM

 Parameters
     None

 Returns
     NormalGameplayState_t The current state of the NormalGameplay state machine

 Description
     returns the current state of the NormalGameplay state machine
 Notes

 Author
     J. Edward Carryer, 2/11/05, 10:38AM
****************************************************************************/
NormalGameplayState_t QueryNormalGameplaySM ( void )
{
    return(CurrentState);
}

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

/****************************************************************************
 Function
     DuringQueryingJSR

 Parameters
     ES_Event

 Returns
     Event, possibly modified

 Description
     The during function for sweeping right.  Not very exciting because there are no sub-states here.

      Notes

 Author
  The Royal Order, February 2014
****************************************************************************/
static ES_Event DuringQueryingJSR( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption
    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
        {

            //***Entry actions for state***
            //No entry actions
            //****Start Lower Level Machines***
            //No lower level machines
        }
    else if ( Event.EventType == ES_EXIT )
        {
            // on exit, give the lower levels a chance to clean up first
            printf("Event type was ES_EXIT");
            //***Run lower level machines to clean up first***
            //No lower level machines

            //***Do local exit functiosn ***
            //No local exit functions

        }
    else
        // do the 'during' function for this state
        {
            // ***Run lower level state machine***
            //No lower level SM's

            // **Do any activity that is repeated as long as we are in this state

        }
    // 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);
}

/****************************************************************************
 Function
     DuringDrivingForward

 Parameters
     ES_Event

 Returns
     Event, possibly modified

 Description
     The during function for driving forward.  Very exciting because this is where we call the internal driving foward state machine.

      Notes

 Author
  The Royal Order, February 2014
****************************************************************************/
static ES_Event DuringDrivingForward( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
        {

            //***Entry actions for state***
            //No entry actions

            //****Start Lower Level Machines***
            StartForwardControlSM(Event);   //Start the sub state machine

        }
    else if ( Event.EventType == ES_EXIT )
        {
            // on exit, give the lower levels a chance to clean up first
            printf("Exit of Sweeping Left \r\n");
            //***Run lower level machines to clean up first***
            RunForwardControlSM(Event);

            //***Do local exit functiosn ***
            //This is all cleanup in case we go into abnormal state by accident

            //Stop the LanceTimer
            //Call LanceServo(Retract)
            //Toggle the LanceVariable to Retracted
            ControlDriveMotors(M_STOP);
        }
    else
        // do the 'during' function for this state
        {
            // ***Run lower level state machine***
            RunForwardControlSM(Event);
            // **Do any activity that is repeated as long as we are in this state
            //No during actions
        }
    // 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);
}

/****************************************************************************
 Function
     DuringDrivingReverse

 Parameters
     ES_Event

 Returns
     Event, possibly modified

 Description
    During function for driving in reverse
      Notes

 Author
  The Royal Order, February 2014
****************************************************************************/
static ES_Event DuringDrivingReverse( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
        {

            //***Entry actions for state***
            ControlMotorsIndivid(BALL_MTR_DUTY,BALL_MOTOR);     //Start spinning up the shooting motor

            //****Start Lower Level Machines***
            StartReverseControlSM(Event);
        }
    else if ( Event.EventType == ES_EXIT )
        {
            // on exit, give the lower levels a chance to clean up first

            //***Run lower level machines to clean up first***
            //No lower level machines

            //***Do local exit functiosn ***
            //No local exit functions

        }
    else
        // do the 'during' function for this state
        {
            // ***Run lower level state machine***
            RunReverseControlSM(Event);

            // **Do any activity that is repeated as long as we are in this state

        }
    // 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);
}

/****************************************************************************
Function
DuringRecess

Parameters
ES_Event

Returns
Event, possibly modified

Description
During function for recess
 Notes

Author
The Royal Order, February 2014
****************************************************************************/
static ES_Event DuringRecess( ES_Event Event)
{
    ES_Event ReturnEvent = Event; // assmes no re-mapping or comsumption

    // process ES_ENTRY, ES_ENTRY_HISTORY & ES_EXIT events
    if ( (Event.EventType == ES_ENTRY) ||
            (Event.EventType == ES_ENTRY_HISTORY) )
        {

            //***Entry actions for state***
            ControlMotorsIndivid(BALL_MTR_DUTY,BALL_MOTOR);     //Start spinning up the shooting motor

            //****Start Lower Level Machines***
            //StartReloadControlSM(Event);

            //Init Output compare Depot
            InitOutputCompareDepot();

        }
    else if ( Event.EventType == ES_EXIT )
        {
            // on exit, give the lower levels a chance to clean up first

            //***Run lower level machines to clean up first***
            //RunReloadControlSM(Event);

            //***Do local exit functiosn ***
            RecessCount++;     //Increment recess count

        }
    else
        // do the 'during' function for this state
        {
            // ***Run lower level state machine***
            //RunReloadControlSM(Event);
            switch (Event.EventType)
                {

                case ES_TIMEOUT:
                    switch (Event.EventParam)
                        {

                        case RELOAD_TIMER:
                            if (RequestCount < MaxRequestCount)
                                {
                                    //inc request count
                                    RequestCount++;
                                    //Re-enable interrupt for query
                                    InitOutputCompareDepot();

                                }

                            else if (RequestCount == MaxRequestCount)
                                {
                                    //Do nothing
                                    ;
                                }

                            break;
                        }
                    // **Do any activity that is repeated as long as we are in this state
                }
        }
    // 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);
}

/****************************************************************************
Function
  QueryRecessCount

Returns
  The current recess count

Description
So event checkers can know

Author
The Royal Order, February 2014
****************************************************************************/

unsigned int QueryRecessCount(void)
{
    return RecessCount;
}