// Module defines
#define PWM_6_7 3
#define REQ_FREQ 50  // units in Hz

#define FINAL_SERVO_POS 1950
#define POS_INCREMENT 3
#define REV_POS_INCREMENT 10
#define INITIAL_POS 2450
#define SERVO_GAME_LOSE_POS 1930
#define SERVO_WAIT_FOR_START_POS 2250

#define ONE_SEC 976
#define SERVO_DELAY (ONE_SEC/4)


// Module variables
static ServoState_t CurrentState;
static uint8_t MyPriority;

// Function prototypes
bool InitServo ( uint8_t Priority );
bool PostServo ( ES_Event ThisEvent );
ES_Event RunServo ( ES_Event ThisEvent );

// Initialize the port for the servo as a PWM output
bool InitServo ( uint8_t Priority )
{
//enable GPIO Port B/ Bit 1
	HWREG (SYSCTL_RCGCGPIO) |= (SYSCTL_RCGCGPIO_R1);
//wait until the peripheral reports that its clock is ready
	while ((HWREG(SYSCTL_PRGPIO) & SYSCTL_PRGPIO_R1) != SYSCTL_PRGPIO_R1);
	
//Initialize the MyPriority variable with the passed in parameter.
	MyPriority = Priority; 
//Initialize the PWM system, set frequency to 500Hz 1% resolution, 0% DC
	PWM_TIVA_Init();
//Set the frequency for a PWM channel group
	PWM_TIVA_SetFreq( REQ_FREQ, PWM_6_7);
//Upon initialization, set the CurrentState to ServoWaitingForStart
	CurrentState = ServoWaitingForStart;
	ES_Event ThisEvent;
  ThisEvent.EventType = ES_INIT;
	PostServo (ThisEvent);
	
	return true;
} //End of InitFlame

//Set up the post function for Flame service that will be called by other functions
bool PostServo ( ES_Event ThisEvent )
{
  return ES_PostToService( MyPriority, ThisEvent);
}

// RunServo state machine 	
ES_Event RunServo ( ES_Event ThisEvent )
{
  //initialize local variable ReturnValue to ES_NO_EVENT
	ES_Event ReturnValue;
  ReturnValue.EventType = ES_NO_EVENT; // assume no errors
	
	//Declare local variable NextState
	ServoState_t NextState;	
  //Set NextState to CurrentState
	NextState = CurrentState;
	
	//Declare local variables for servo position
	static uint16_t NewServoPos = INITIAL_POS;
	static uint16_t NewServoReversePos;
	
//Based on the state of the CurrentState variable choose one 
	//of the following blocks of code:
	switch (CurrentState){
		// Servo state is ServoWaitingForStart
		case ServoWaitingForStart : 
// 		If ThisEvent is ES_INIT
		  if (ThisEvent.EventType == ES_INIT){ 
				//Place servo in initial position
			PWM_TIVA_SetPulseWidth( SERVO_WAIT_FOR_START_POS, 1);
			ES_Timer_InitTimer(SERVO_TIMER, SERVO_DELAY);
			NewServoReversePos = SERVO_WAIT_FOR_START_POS;
			NextState = ServoReturnStart;
// 		Else if ThisEvent is ES_START_GAME
			} else if (ThisEvent.EventType == ES_GAME_START){ 
				//Advance servo
			PWM_TIVA_SetPulseWidth( INITIAL_POS, 1);
			NewServoPos = INITIAL_POS;
				//Start servo delay timer
		  ES_Timer_InitTimer(SERVO_TIMER, SERVO_DELAY);
			NextState = ServoFWD;
			} break;  //End ServoWaitingForStart block
		// Servo state is ServoFWD	
		case ServoFWD :
		  //If this event is ES_TIMEOUT
			//Increment the servo slowly in non-blocking way
			if ( ThisEvent.EventType == ES_TIMEOUT ){
			PWM_TIVA_SetPulseWidth( NewServoPos, 1 );
				if (NewServoPos >= FINAL_SERVO_POS){
					ES_Timer_InitTimer(SERVO_TIMER, SERVO_DELAY);	
				}
//  	Else if ThisEvent is ES_GAME_TIMER_OUT
			}else if (ThisEvent.EventType == ES_GAME_TIMER_OUT){ 
//		Send servo to game lose position
			PWM_TIVA_SetPulseWidth( SERVO_GAME_LOSE_POS, 1 );
//    Set current position to previous position
			NewServoReversePos = SERVO_GAME_LOSE_POS;
			//Remain in current state and wait for GAME_RESET event
//		Else if this event is ES_ALL_FLAME_OFF
			}	else if (ThisEvent.EventType == ES_ALL_FLAME_OFF){ 
//		Maintain servo in current position
			PWM_TIVA_SetPulseWidth( NewServoPos, 1 );
//    Set current position to previous reverse position
			NewServoReversePos = NewServoPos;
			// Start servo delay timer
			ES_Timer_InitTimer(SERVO_TIMER, SERVO_DELAY);
//		Set NextState to ServoReturn
			NextState = ServoReturn;
// Else if this event is ES_GAME_RESET
			}	else if (ThisEvent.EventType == ES_GAME_RESET){
			// Start servo delay timer
			ES_Timer_InitTimer(SERVO_TIMER, SERVO_DELAY);
//		Set NextState to ServoReturn
			NextState = ServoReturn;
			}
			break;  //End ServoFWD block
		// Servo state is ServoReturn
		case ServoReturn :
//		If the event is ES_TIMEOUT
//		Return servo to initial position slowly in non-blocking manner
			if ( ThisEvent.EventType == ES_TIMEOUT ){
			PWM_TIVA_SetPulseWidth( NewServoReversePos, 1 ); 
				if (NewServoReversePos <= SERVO_WAIT_FOR_START_POS){
					ES_Timer_InitTimer(SERVO_TIMER, SERVO_DELAY);	
				} else {
// 				Make sure servo doesn't go past its limit
					PWM_TIVA_SetPulseWidth( SERVO_WAIT_FOR_START_POS, 1 );
//    		Set current state to waiting for start
					NextState = ServoWaitingForStart;
			}
		}
		// Servo state is ServoReturnStart
		case ServoReturnStart :
		// If the event is ES_TIMEOUT
			if ( ThisEvent.EventType == ES_TIMEOUT ){
				// Return servo to start position
				PWM_TIVA_SetPulseWidth( NewServoReversePos, 1 );
					if (NewServoReversePos < INITIAL_POS){
					ES_Timer_InitTimer(SERVO_TIMER, SERVO_DELAY);	
				} else if (NewServoReversePos >= INITIAL_POS){
//    		Set current state to ServoWaitingForStart
					NextState = ServoWaitingForStart;
		}
	}
			break;  //End ServoReturn block
	}  // End switch statement
	
	// Increment the servo forward position
	// Increment the servo reverse position
	// Set CurrentState to NextState
	NewServoPos -= POS_INCREMENT;
	NewServoReversePos += REV_POS_INCREMENT;
	CurrentState = NextState;
	
// return ReturnValue
	return ReturnValue;
} //End ServoSM


