/****************************************************************************
 Module
	 ReloadSM.c

 Revision
	 2.0.1

 Description
	 This is for a lower level of the LolaGameMaster Service, responsible for reloading the ball.

 Notes
 ****************************************************************************/
/*----------------------------- 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 "ReloadSM.h"
#include "IRBeaconService.h"
#include "IRReloadService.h"
#include "CommunicationsMaster.h"
#include "MotorCommands.h"
#include "LimitDebounce.h"
#include "BeamBreak.h" 
#include "LolaGameMaster.h"
#include "TeamColorAndGameLEDs.h"

/*----------------------------- Module Defines ----------------------------*/
// define constants for the states for this machine
// and any other local defines

#define ENTRY_STATE AligningWithBeacon
#define BALL_RELOAD_TIME 4000 //4s

#define MOVE_TO_RETRY_TIME 2000 //2s

#define RETRY_TURN_TIME 600
#define RETRY_FORWARD_TIME 2000
#define RETRY_BACKWARD_TIME (RETRY_FORWARD_TIME*2)

/*---------------------------- 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 DuringAligningWithBeacon( ES_Event_t Event);
static ES_Event_t DuringDriveToTape( ES_Event_t Event);
static ES_Event_t DuringAlignWithBlackLine( ES_Event_t Event);
static ES_Event_t DuringLineFollow( ES_Event_t Event);
static ES_Event_t DuringMovingToRetry( ES_Event_t Event);
static ES_Event_t DuringSensingIRReload( ES_Event_t Event);
static ES_Event_t DuringWaitingForBall( ES_Event_t Event);

static bool RetryReverseFromWall = false;
static bool RetryRotate = false;
static bool RetryForward = false;
static bool RetryReverse = false;

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

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
		RunReloadSM

 Parameters
	 ES_Event: the event to process

 Returns
	 ES_Event: an event to return

 Description
	 add your description here
 Notes
	 uses nested switch/case to implement the machine.
 Author
	 R. Adenekan
****************************************************************************/
ES_Event_t RunReloadSM( ES_Event_t CurrentEvent )
{
	 bool MakeTransition = false;/* we are not making a state transition */
	ReloadSM_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 AligningWithBeacon
     case AligningWithBeacon: 
		 ReturnEvent = CurrentEvent = DuringAligningWithBeacon(CurrentEvent);
		 //process any events
				 if ( CurrentEvent.EventType != ES_NO_EVENT ) 
				 {
						switch (CurrentEvent.EventType)
						{
							//If the EventType is DETECTED_BEACON
							case DETECTED_BEACON	: 
							//NextState equals DriveToTape
							NextState = DriveToTape; //Decide what the next state will be
							MakeTransition = true; //mark that we are taking a transition
							ReturnEvent.EventType = ES_NO_EVENT;
							break;
								}//EndSwitch
				 } //End Case
				 break;
         
		 //if the CurrentState is DriveToTape		 
		 case DriveToTape: 
		 ReturnEvent = CurrentEvent = DuringDriveToTape(CurrentEvent);
		 //process any events
				 if ( CurrentEvent.EventType != ES_NO_EVENT ) 
				 {
						switch (CurrentEvent.EventType)
						{
							//If the EventType is TAPE_DETECTED
							case TAPE_DETECTED	: 
								if (CurrentEvent.EventParam ==3) //Only back tape detected
								{
									//Set the NextState equal to AlignWithBlackLine
									NextState = AlignWithBlackLine; //Decide what the next state will be
									MakeTransition = true; //mark that we are taking a transition
									ReturnEvent.EventType = ES_NO_EVENT;
							break;
							}//EndIf
								}//EndSwitch
				 } //End Case
				 break;

			//if the CurrentState is AlignWithBlackLine				
			case AlignWithBlackLine: 
				ReturnEvent = CurrentEvent = DuringAlignWithBlackLine(CurrentEvent);
				if ( CurrentEvent.EventType != ES_NO_EVENT ) 
				 {
						switch (CurrentEvent.EventType)
						{
							case TAPE_DETECTED	: 
								if ((CurrentEvent.EventParam ==0) || (CurrentEvent.EventParam==4) || (CurrentEvent.EventParam==7)) //any front tape detected or front and back tape detected
								{
									//Stop the motors
                  Stop();
									//Set NextState equal to LineFollow
									NextState = LineFollow; //Decide what the next state will be
									MakeTransition = true; //mark that we are taking a transition
									ReturnEvent.EventType = ES_NO_EVENT;
							break;
							}//EndIf
								}//EndSwitch
				 } //End Case
				 break;				

			//if the CurrentState is LineFollow
			case LineFollow: 
				ReturnEvent = CurrentEvent = DuringLineFollow(CurrentEvent);
				//process any events
				 if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
				 {
						switch (CurrentEvent.EventType)
						{
							//If the EventType is ES_LimitSwitchHit
							case ES_LimitSwitchHit : //Hit Limit Switch
							//stop motors
							Stop();
							//Set NextState equal to SensingIRReload
							NextState = SensingIRReload; //Decide what the next state will be
							MakeTransition = true; //mark that we are taking a transition
							ReturnEvent.EventType = ES_NO_EVENT;
							break;
						}
				 }
				 break;

		 //If the CurrentState is SensingIRReload
		 case SensingIRReload: 
			 ReturnEvent = CurrentEvent = DuringSensingIRReload(CurrentEvent);
			 //process any events
				 if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
				 {
						switch (CurrentEvent.EventType)
						{
							//If the EventType is BAD_RELOAD_ALIGNMENT
							case BAD_RELOAD_ALIGNMENT : //not reading steady signal from IR emitter
							//Set the NextState equal to MovingToRetry
							NextState = MovingToRetry ; //Decide what the next state will be
							ReturnEvent.EventType = ES_NO_EVENT;
							break;
							
							//If the CurrentState is WAITING_FOR_BALL
							case WAITING_FOR_BALL : //read signal from IR emitter and pulsed twice the frequency
							//Set NextState equal to WaitingForBall
							NextState = WaitingForBall ; //Decide what the next state will be
							MakeTransition = true; //mark that we are taking a transition
							ReturnEvent.EventType = ES_NO_EVENT;
							break;													 
						}
				 }
				 break;		 

		 //If the CurrentState is MovingToRetry
		 case MovingToRetry: 
		 ReturnEvent = CurrentEvent = DuringMovingToRetry(CurrentEvent);
		 //process any events
				 if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
				 {
						switch (CurrentEvent.EventType)
						{
							//If the EventType is ES_TIMEOUT
							case ES_TIMEOUT : 
								//If the EventParam is MOVE_TO_RETRY_TIMER
								if (CurrentEvent.EventParam==MOVE_TO_RETRY_TIMER) //set to 1s
								{ 
									//If RetryReverseFromWall is true
                  if(RetryReverseFromWall)
                  {
                    Stop(); //stop motor
                    //Set RetryReverseFromWall to false
                    RetryReverseFromWall = false;
										//Set RetryRotate to true
                    RetryRotate = true;
                    //Rotate CCW
                    RotateCCW(); // 90 degrees
										//Restart the MOVE_TO_RETRY_TIMER
                    ES_Timer_InitTimer(MOVE_TO_RETRY_TIMER, RETRY_TURN_TIME);
                  }
									//Else if RetryRotate is true
                  else if(RetryRotate)
                  {
                    Stop(); //stop motor
										//move forward at half speed
                    ForwardHalf();
                    //Set RetryRotate to false
                    RetryRotate = false;
										//Set RetryForward to true
                    RetryForward = true;
                    //Restart MOVE_TO_RETRY_TIMER
                    ES_Timer_InitTimer(MOVE_TO_RETRY_TIMER, RETRY_FORWARD_TIME);
                  }
									//Else if RetryForward is true
                  else if(RetryForward)
                  {
                    Stop(); //stop motor
										//Reverse at half speed
                    ReverseHalf();
                    //Set RetryForward to false
                    RetryForward = false;
										//Set Retry to true
                    RetryReverse = true;
                    //Restart MOVE_TO_RETRY_TIMER
                    ES_Timer_InitTimer(MOVE_TO_RETRY_TIMER, RETRY_BACKWARD_TIME);
                  }
									//Else if RetryReverse is true
                  else if(RetryReverse)
                  {
										//Stop the motors
                    Stop();
										//Set RetryReverse to true
                    RetryReverse = false;
                  }
                  
									//Set NextState to MovingToRetry 
									NextState = MovingToRetry; 
									ReturnEvent.EventType = ES_NO_EVENT;
								}
                break;
              //If the CurrentState is TAPE_DETECTED  
              case TAPE_DETECTED :
                  if (CurrentEvent.EventParam == 3) //see center tape
                  {
										//Stop the motors
                    Stop();
										//Set the NextState equal to AlignWithBlackLine
                    NextState = AlignWithBlackLine; //Decide what the next state will be
                    MakeTransition = true; //mark that we are taking a transition
                  ReturnEvent.EventType = ES_NO_EVENT;
                  }
              break;
						}
				 break;
          }
				 
		 //If the CurrentState is WaitingForBall
		 case WaitingForBall: 
		 ReturnEvent = CurrentEvent = DuringWaitingForBall(CurrentEvent);
		 //process any events
				 if ( CurrentEvent.EventType != ES_NO_EVENT ) //If an event is active
				 {
						switch (CurrentEvent.EventType)
						{
							//If the EventType is ES_TIMEOUT
							case ES_TIMEOUT : //timeout reload 
								//If the EventParam is the BALL_RELOAD_TIMER)
								if (CurrentEvent.EventParam==BALL_RELOAD_TIMER) //set to 2s
								{
											if (HaveBall()) //sucessfully reloaded 
											{
												//On offense
												if (GetTeamColor()==GetCurrentPosession()) 
												{
													//Return a HAVE_BALL_O event
													ReturnEvent.EventType = HAVE_BALL_O;																			
												}
												//On defense
												else
												{
													//Return a HAVE_BALL_D event
													ReturnEvent.EventType = HAVE_BALL_D;					 
												}
											}
											 //did not successfully reload or we're in TieBreak
											else if (!HaveBall() || (GetGameStatus() == 3)) //
											{
												//Set the NextState to MovingToRetry
												NextState = MovingToRetry ; //Decide what the next state will be
												MakeTransition = true; //mark that we are taking a transition
												ReturnEvent.EventType = ES_NO_EVENT; //is this correct??
											}
										}
								break;
									}
								}
				 break;
				}
		
		//	 If we are making a state transition
		if (MakeTransition == true)
		{
			 //	 Execute exit function for current state
			 CurrentEvent.EventType = ES_EXIT;
			 //Run ReloadSM
			 RunReloadSM(CurrentEvent);

			 CurrentState = NextState; //Modify state variable

			 //	 Execute entry function for new state
			 // this defaults to ES_ENTRY
			 //Run ReloadSM
			 RunReloadSM(EntryEventKind);
		 }
		 return(ReturnEvent);
}				 

/****************************************************************************
 Function
		 StartReloadSM

 Parameters
		 None

 Returns
		 None

 Description
		 Does any required initialization for this state machine
 Notes

 Author
		 R. Adenekan
****************************************************************************/
void StartReloadSM ( ES_Event_t CurrentEvent )
{
	//If we are not entering with history
	 if ( ES_ENTRY_HISTORY != CurrentEvent.EventType )
	 {
     //Set the CurrentState to ENTRY_STATE
		 CurrentState = ENTRY_STATE;
	 }
	 //Run ReloadSM
	 RunReloadSM(CurrentEvent);
	// printf("in start reload sm \r\n");
}

/****************************************************************************
 Function
		 QueryReloadSM 

 Parameters
		 None

 Returns
		 ReloadSMState_t The current state of the ReloadSM state machine

 Description
		 returns the current state of the ReloadSM state machine
 Notes

 Author
		 R. Adenekan
****************************************************************************/
ReloadSM_State_t QueryReloadSM ( void )
{
	 return(CurrentState);
}

/***************************************************************************
 private functions
 ***************************************************************************/
static ES_Event_t DuringAligningWithBeacon( 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 this is an ES_ENTRY or ES_ENTRY_HISTORY event
		if ( (Event.EventType == ES_ENTRY) ||
				 (Event.EventType == ES_ENTRY_HISTORY) )
		{
				// implement any entry actions required for this state machine
				//Post an ALIGN_WITH_BEACON event to IRBeaconService with the EventParam 2
        ES_Event_t Event2Post;
        Event2Post.EventType = ALIGN_WITH_BEACON;
				Event2Post.EventParam = 2; //Lola Goal (definition note: if Lola is Red team, her goal is red goal)
        PostIRBeaconService(Event2Post);      
		}
		else if ( Event.EventType == ES_EXIT )
		{
		}
		else
		{
		}
		return(ReturnEvent);
}

static ES_Event_t DuringDriveToTape( 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 this is an ES_ENTRY or ES_ENTRY_HISTORY event
		if ( (Event.EventType == ES_ENTRY) ||
				 (Event.EventType == ES_ENTRY_HISTORY) )
		{
      //move forward at half speed
			ForwardHalf();
		}
		//Else if this is an ES_EXIT event
		else if ( Event.EventType == ES_EXIT )
		{
			//stop
			Stop();
		}
		else
		{
		}
		return(ReturnEvent);
}

static ES_Event_t DuringAlignWithBlackLine( 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 this is an ES_ENTRY or ES_ENTRY_HISTORY event
		if ( (Event.EventType == ES_ENTRY) ||
				 (Event.EventType == ES_ENTRY_HISTORY) )
		{
			//rotate
			RotateCW();
		}
		//Else if this is an ES_EXIT event
		else if ( Event.EventType == ES_EXIT )
		{
			//stop
			Stop();
		}
		else
		{
		}
		return(ReturnEvent);
}

static ES_Event_t DuringLineFollow( 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) )
		{
		}
		else if ( Event.EventType == ES_EXIT )
		}
		// do the 'during' function for this state
		{
			if (Event.EventType==TAPE_DETECTED) 
			{
				if ((Event.EventParam==0) || (Event.EventParam==4)) //Only front Sensor Detected Tape or only front and back detected tape
				{
					//move foward	
					ForwardHalf();
				}
				else if (Event.EventParam==5) //only Left Sensor Detected Tape
				{
				//move forward to left	
				VeerLeftHalf();
				}
				else if (Event.EventParam==6) // only Right Sensor Detected Tape
				{
				//move forward to right
				VeerRightHalf();
				}
			}
		}
		return(ReturnEvent);
}

static ES_Event_t DuringMovingToRetry( 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 this is an ES_ENTRY or ES_ENTRY_HISTORY event
		if ( (Event.EventType == ES_ENTRY) ||
				 (Event.EventType == ES_ENTRY_HISTORY) )
		{
					//reverse motor
					ReverseHalf();
					//start timer for moving to retry reload
					ES_Timer_InitTimer(MOVE_TO_RETRY_TIMER, MOVE_TO_RETRY_TIME);
					//Set RetryReverseFromWall to true
          RetryReverseFromWall = true;
		}
		else if ( Event.EventType == ES_EXIT )
		{
		}
		else
		{
		}
		return(ReturnEvent);
}

static ES_Event_t DuringSensingIRReload( 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) )
		{
				// post AT_RELOAD_STATION to IRReloadService
				ES_Event_t Event2Post;
				Event2Post.EventType=AT_RELOAD_STATION;
				PostIRReloadService(Event2Post);		 
		}
		else if ( Event.EventType == ES_EXIT )
		{
		}
		else
		{
		}
		return(ReturnEvent);
}

static ES_Event_t DuringWaitingForBall( 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) )
		{
			//start timer for ball reload
			ES_Timer_InitTimer(BALL_RELOAD_TIMER, BALL_RELOAD_TIME);
		}
		else if ( Event.EventType == ES_EXIT )
      //Turning off the IR LED upon exit of waiting for Ball 
      IRReloadOff(); 
		}
		else
		{
		}
		return(ReturnEvent);
}