/****************************************************************************
 Module
 SMRequestBall.c

 Description
 Request Ball State Machine.  While at a dispenser, this state machine checks
 that the dispenser has balls available, and if so, requests one.
 ****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
#include "GlobalHeader.h"
#include "SMGameplay.h"
#include "SMRequestBall.h"
#include "SMMaster.h"
#include "SMOffensiveCoordinator.h"

/*----------------------------- Module Defines ----------------------------*/
//state timeouts
#define EMPTY_QUERY_INTERVAL 500
#define QUERYING_TIMEOUT 3000
#define WAITING_TIMEOUT 3000
#define MIGHTBEEMPTY_TIMEOUT 4000
#define MACHINE_TIMEOUT 30000

/*---------------------------- Module Functions ---------------------------*/
static Event_t DuringQuerying( Event_t Event);
static Event_t DuringWaiting( Event_t Event);
static Event_t DuringMightBeEmpty( Event_t Event);
static Event_t DuringEmpty( Event_t Event);

/*---------------------------- Module Variables ---------------------------*/
static RequestState_t CurrentState;

//timeout variables
static unsigned int StateStartTime;
static unsigned int StateTimeoutTime;
static unsigned int MachineStartTime;

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
 QueryRequestTimeout

 Parameters
 none

 Returns
 1 if state has timed out, 0 otherwise

 Description
 Checks if timeout has occured for current state.
 ****************************************************************************/
unsigned char QueryRequestTimeout (void)
{
	if ((CurrentState != Request_Empty) && (TMRS12_GetTime() - StateStartTime > StateTimeoutTime))
	{
		return 1;
	}
	return 0;
}

/****************************************************************************
 Function
 RunRequestBallSM

 Parameters
 Event_t

 Returns
 Event_t

 Description
 Runs Request Ball state machine.  Checks if dispenser has balls, and if so
 transitions to requesting a ball.  If no balls are left, transitions to a
 might-be-empty state.  if this state times out, the machien transitions to
 an empty state, from which it is permissible to leave the dispenser.
 ****************************************************************************/
Event_t RunRequestBallSM( Event_t CurrentEvent )
{
	unsigned char MakeTransition = FALSE;/* are we making a state transition? */
	PlayState_t NextState = CurrentState;

	switch ( CurrentState )
	{
		case Request_Querying :
			CurrentEvent = DuringQuerying(CurrentEvent);
			//process any events
			if ( CurrentEvent != EV_NO_EVENT )        //If an event is active
			{
				unsigned char LastCommand;
				unsigned char Response;

				switch (CurrentEvent)
				{
					case EV_OC_NewResponse :
						LastCommand = QueryOCLastCommand();
						Response = QueryOCResponse();
						if (LastCommand == (OC_QueryCommandBase | QueryCurrentDispenserNumber())) //if a response to this query
						{
							//printf("Last Command Recognized as 0x%x,  Response 0x%x\r\n",LastCommand,Response);
							if ((Response & ~OC_ResponseDispenserMask & ~OC_BallMask) == OC_QueryResponseBase) //if response is # of balls
							{
								unsigned char Dispenser = ((Response & OC_ResponseDispenserMask) >> 2);
								unsigned char BallsAvailable = (Response & OC_BallMask);

								if (BallsAvailable > 0) //if there are balls, request one
								{
									NextState = Request_Waiting;
									MakeTransition = TRUE;
								}
								else //if there aren't, the dispenser might be empty.  wait to make sure.
								{
									NextState = Request_MightBeEmpty;
									MakeTransition = TRUE;
								}
							}
							else
							{
								OC_AddCommand(LastCommand,2); //if the response is something else, try again.
							}

						}
						break;
					case EV_OC_CounterExpired : //if the query times out
						LastCommand = QueryOCLastCommand();
						Response = QueryOCResponse();
						if (LastCommand == (OC_QueryCommandBase | QueryCurrentDispenserNumber()))
						{
							//printf("Last Command Recognized as 0x%x,  Response 0x%x\r\n",LastCommand,Response);
							OC_AddCommand(LastCommand,2);	//try query again
						}
						break;
					case EV_Request_Timeout:
						NextState = Request_Waiting; //if querying times out, request a ball anyway.
						//NextState = Request_Querying;
						MakeTransition = TRUE;
						break;
				}
			}
			break;

		case Request_Waiting :
			CurrentEvent = DuringWaiting(CurrentEvent);
			//process any events
			if ( CurrentEvent != EV_NO_EVENT )        //If an event is active
			{
				switch (CurrentEvent)
				{
					case EV_OC_NewResponse :
						if (QueryOCLastCommand() == (OC_RequestCommandBase | QueryCurrentDispenserNumber()))
						{  //if we get a response to our request, go back to querying
							NextState = Request_Querying;
							MakeTransition = TRUE;
						}
						break;
					case EV_OC_CounterExpired : //If our request times out, go back to querying
						if (QueryOCLastCommand() == (OC_RequestCommandBase | QueryCurrentDispenserNumber()))
						{
							NextState = Request_Querying;
							MakeTransition = TRUE;
						}
						break;
					case EV_Request_Timeout: // if the stae times out, go back to querying
						NextState = Request_Querying;
						MakeTransition = TRUE;
						break;
				}
			}
			break;

		case Request_MightBeEmpty :
			CurrentEvent = DuringMightBeEmpty(CurrentEvent);
			//process any events
			if ( CurrentEvent != EV_NO_EVENT )        //If an event is active
			{
				switch (CurrentEvent)
				{
					case EV_OC_NewResponse : //if there is a new response to our query
						if (QueryOCLastCommand() == (OC_QueryCommandBase | QueryCurrentDispenserNumber()))
						{
							unsigned char Response = QueryOCResponse();

							if ((Response & ~OC_ResponseDispenserMask & ~OC_BallMask) == OC_QueryResponseBase)
							{

								unsigned char Dispenser = ((Response & OC_ResponseDispenserMask) >> 2);
								unsigned char BallsAvailable = (Response & OC_BallMask);

								if (BallsAvailable > 0) //there are now balls available
								{
									NextState = Request_Waiting; //request one
									MakeTransition = TRUE;
								}
							}
						}
						break;
					case EV_Request_Timeout: //if there haven't been balls for a while, it's empty
            			NextState = Request_Empty;
            			MakeTransition = TRUE;
						break;
				}
			}
			break;

		case Request_Empty :
			CurrentEvent = DuringEmpty(CurrentEvent);
			//process any events
			if ( CurrentEvent != EV_NO_EVENT )        //If an event is active
			{
				switch (CurrentEvent)
				{
					case EV_OC_NewResponse : //if there is a new response to our query
						if (QueryOCLastCommand() == (OC_QueryCommandBase | QueryCurrentDispenserNumber()))
						{
							unsigned char Response = QueryOCResponse();

							if ((Response & ~OC_ResponseDispenserMask & ~OC_BallMask) == OC_QueryResponseBase)
							{

								unsigned char Dispenser = ((Response & OC_ResponseDispenserMask) >> 2);

								unsigned char BallsAvailable = (Response & OC_BallMask);

								if ((Dispenser == QueryCurrentDispenserNumber()) && (BallsAvailable > 0))
								{ //if there are actually balls available
									NextState = Request_Waiting; //request one
									MakeTransition = TRUE;
								}
							}
						}
						break;
				}
			}
			break;
    }
    //   If we are making a state transition
    if (MakeTransition == TRUE)
    {
		//   Execute exit function for current state
		(void)RunRequestBallSM(EV_EXIT);
		printf("entering state %d\r\n\r\n",NextState);
		CurrentState = NextState; //Modify state variable
		//   Execute entry function for new state
		(void)RunRequestBallSM(EV_ENTRY);
	}
	return(CurrentEvent);
}

/****************************************************************************
 Function
 StartRequestBallSM

 Parameters
 Event_t

 Returns
 none

 Description
 Start request ball state machine.
 ****************************************************************************/
void StartRequestBallSM ( Event_t CurrentEvent )
{
	CurrentState = Request_Querying;
	MachineStartTime = TMRS12_GetTime();

	// call the entry function (if any) for the ENTRY_STATE
	(void)RunRequestBallSM(EV_ENTRY);
}

/****************************************************************************
 Function
 QueryRequestBallSM

 Parameters
 none

 Returns
 RequestState_t - the current state of the machine

 Description
 returns the state of the state machine
 ****************************************************************************/
RequestState_t QueryRequestBallSM ( void )
{
	return(CurrentState);
}

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

/****************************************************************************
 Function
 DuringQuerying

 Parameters
 Event_t

 Returns
 Event_t

 Description
 On entry, start querying dispenser
 ****************************************************************************/
static Event_t DuringQuerying( Event_t Event)
{
    // process EV_ENTRY & EV_EXIT events
    if ( Event == EV_ENTRY)
    {
        StateStartTime = TMRS12_GetTime();
        StateTimeoutTime = QUERYING_TIMEOUT;
		OC_QueryDispenser(QueryCurrentDispenserNumber(),2);

    }else if ( Event == EV_EXIT)
    {
    }else
		// do the 'during' function for this state
    {
    }
    return(Event);
}

/****************************************************************************
 Function
 DuringWaiting

 Parameters
 Event_t

 Returns
 Event_t

 Description
 On entry, start requesting ball.  Also turn both rope lights on, for debugging.
 On exit, reset lights to team color.
 ****************************************************************************/
static Event_t DuringWaiting( Event_t Event)
{
    // process EV_ENTRY & EV_EXIT events
    if ( Event == EV_ENTRY)
    {

        StateStartTime = TMRS12_GetTime();
        StateTimeoutTime = WAITING_TIMEOUT;
		OC_RequestBall(QueryCurrentDispenserNumber(),2);
		TurnOnLEDs(); //turn on both lights to indicate ball request

    }else if ( Event == EV_EXIT)
    {
        (void)ReadColor(); //resets team color
    }else
		// do the 'during' function for this state
    {
    }
    return(Event);
}

/****************************************************************************
 Function
 DuringMightBeEmpty

 Parameters
 Event_t

 Returns
 Event_t

 Description
 On entry, and every QUERY_INTERVAL, query dispenser
 ****************************************************************************/
static Event_t DuringMightBeEmpty( Event_t Event)
{
	static unsigned int LastTime;
    // process EV_ENTRY & EV_EXIT events
    if ( Event == EV_ENTRY)
    {
		OC_QueryDispenser(QueryCurrentDispenserNumber(),2);
		LastTime = TMRS12_GetTime();
		StateStartTime = LastTime;
		StateTimeoutTime = MIGHTBEEMPTY_TIMEOUT;
    }else if ( Event == EV_EXIT)
    {
    }else
		// do the 'during' function for this state
    {

    	unsigned int CurrentTime = TMRS12_GetTime();
    	if (CurrentTime - LastTime > EMPTY_QUERY_INTERVAL)
    	{
    		OC_QueryDispenser(QueryCurrentDispenserNumber(),2);
    		LastTime = CurrentTime;
    	}
    }
    return(Event);
}

/****************************************************************************
 Function
 DuringEmpty

 Parameters
 Event_t

 Returns
 Event_t

 Description
 On entry, and every QUERY_INTERVAL, query dispenser
 ****************************************************************************/
static Event_t DuringEmpty( Event_t Event)
{
	static unsigned int LastTime;
    // process EV_ENTRY & EV_EXIT events
    if ( Event == EV_ENTRY)
    {
		OC_QueryDispenser(QueryCurrentDispenserNumber(),2);
		LastTime = TMRS12_GetTime();
    }else if ( Event == EV_EXIT)
    {
    }else
		// do the 'during' function for this state
    {

    	unsigned int CurrentTime = TMRS12_GetTime();
    	if (CurrentTime - LastTime > EMPTY_QUERY_INTERVAL)
    	{
    		OC_QueryDispenser(QueryCurrentDispenserNumber(),2);
    		LastTime = CurrentTime;
    	}
    }
    return(Event);
}