/****************************************************************************
 Module
   TemplateService.c

 Revision
   1.0.1

 Description
   This is a template file for implementing a simple service under the 
   Gen2 Events and Services Framework.

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 01/16/12 09:58 jec      began conversion from TemplateFSM.c
****************************************************************************/
/*----------------------------- Include Files -----------------------------*/
/* 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 "ES_Configure.h"
#include "ES_Framework.h"
#include "LanceMotorControl.h"
#include "ServoLibE128Mod.h"
#include "TopHSM.h"

/*----------------------------- Module Defines ----------------------------*/
#define INACTIVE_DUTY 1000
#define SWEEPMIN_DUTY 1000
#define CHOP1_DUTY  2200
#define CHOP2_DUTY 2200
#define SWEEPMAX_DUTY 1500

#define CHOPMOTORCHANNEL 1
#define SWEEPMOTORCHANNEL 0

#define TIMER_LENGTH 200



/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this service.They should be functions
   relevant to the behavior of this service
*/

/*---------------------------- Module Variables ---------------------------*/
// with the introduction of Gen2, we need a module level Priority variable
static uint8_t MyPriority;
static unsigned char stepNumber;


/*------------------------------ Module Code ------------------------------*/
bool InitLanceService ( uint8_t Priority )
{
  ES_Event ThisEvent;
  MyPriority = Priority;
  stepNumber = 0;
  ThisEvent.EventType = ES_INIT;
  if (ES_PostToService( MyPriority, ThisEvent) == true)
  {
      return true;
  }else
  {
      return false;
  }
}

bool PostLanceService( ES_Event ThisEvent )
{
  return ES_PostToService( MyPriority, ThisEvent);
}

ES_Event RunLanceService( ES_Event ThisEvent )
{
  static unsigned char timerNumber;
  ES_Event ReturnEvent;
  if (ThisEvent.EventType == ES_DEPLOY_LANCE) {
	ES_Timer_InitTimer(LANCE_TIMER, TIMER_LENGTH);
  	printf("Event Recieved");
  } else if( (ThisEvent.EventType == ES_TIMEOUT) && (timerNumber == 1) ){
		printf("Lance Control");
		switch (stepNumber) {
			case 0:
				if (Servo12_SetPulseWidth(CHOPMOTORCHANNEL, CHOP1_DUTY) != SERVO12_Err) {
					printf("CHOP 1");
				}
				break;
			case 1:
				if (Servo12_SetPulseWidth(SWEEPMOTORCHANNEL, SWEEPMAX_DUTY) != SERVO12_Err) {
					printf("Sweep Out");
				}
				break;
			case 2:
				if (Servo12_SetPulseWidth(SWEEPMOTORCHANNEL, SWEEPMIN_DUTY) != SERVO12_Err) {
					printf("Sweep Back");
				}
				break;
			case 3:
				if (Servo12_SetPulseWidth(CHOPMOTORCHANNEL, CHOP2_DUTY) != SERVO12_Err) {
					printf("CHOP 2");
				}
				break;
			case 4:
				if (Servo12_SetPulseWidth(SWEEPMOTORCHANNEL, SWEEPMAX_DUTY) != SERVO12_Err) {
					printf("Sweep Out");
				}
				break;
			case 5:
				if (Servo12_SetPulseWidth(SWEEPMOTORCHANNEL, SWEEPMIN_DUTY) != SERVO12_Err) {
					printf("Sweep Back");
				}
				break;
			case 6:
				if (Servo12_SetPulseWidth(CHOPMOTORCHANNEL, INACTIVE_DUTY) != SERVO12_Err) {
					printf("END LANCE");
				}
				break;
		}
		if (stepNumber <6) 
		{
			stepNumber ++;
			ES_Timer_InitTimer(LANCE_TIMER, TIMER_LENGTH);
		} else {
			stepNumber = 0;
			ReturnEvent.EventType = ES_MOVE;
			PostMasterSM(ReturnEvent);
		}
		
  }
 if( (ThisEvent.EventType == ES_TIMEOUT)){
	  if (timerNumber == 1) {
			timerNumber = 0;
	  } else {

		  timerNumber ++;
    		ES_Timer_InitTimer(LANCE_TIMER, TIMER_LENGTH);
 	  }
 }
  ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
  return ReturnEvent;
}

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

/*------------------------------- Footnotes -------------------------------*/
/*------------------------------ End of file ------------------------------*/