/****************************************************************************
 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 "BallShootService.h"
#include "ServoLibE128Mod.h"
#include <s12evec.h> 
#include <stdio.h>

#include "TopHSM.h"

/*----------------------------- Module Defines ----------------------------*/
#define INACTIVE_PULSE 1000
#define ACTIVE_PULSE 1500
#define NCYCLES 15
#define TIMERLENGTH 250
#define SHOOTINGCHANNEL 2		 

#define PWMPERIOD 100;


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

//void interrupt _Vec_tim1ch4 MotorShootControlLoop (void);

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

/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
     InitTemplateService

 Parameters
     uint8_t : the priorty of this service

 Returns
     bool, false if error in initialization, true otherwise

 Description
     Saves away the priority, and does any 
     other required initialization for this service
 Notes

 Author
     J. Edward Carryer, 01/16/12, 10:00
****************************************************************************/
bool InitBallShootService ( uint8_t Priority )
{
  ES_Event ThisEvent;

  MyPriority = Priority;
  
  PWME |= _S12_PWME3; 
  PWMPRCLK |= 0; // 
  PWMCLK |=   _S12_PCLK3; //we want to use clock after scaling
  PWMSCLB = 24; //divide by an additional 48
  MODRR  |=  _S12_MODRR3; 
  PWMPOL |= _S12_PPOL3;
  PWMPER3 = PWMPERIOD; //b/c 24,000,000/48/100 = 5khz
  PWMDTY3 = 0;	 
  
  TIM1_TSCR1 = _S12_TEN; //enable timers
  TIM1_TIOS &= ~_S12_IOS4;//tape sensor input capture
  TIM1_TFLG1 = _S12_C4F;  //clear interrupt flaga
  TIM1_TCTL3 |= _S12_EDG4A; //capture rising edges
  EnableInterrupts;		  
 
  ThisEvent.EventType = ES_INIT;
  if (ES_PostToService( MyPriority, ThisEvent) == true)
  {
      return true;
  }else
  {
      return false;
  }
}

/****************************************************************************
 Function
     PostTemplateService

 Parameters
     EF_Event ThisEvent ,the event to post to the queue

 Returns
     bool false if the Enqueue operation failed, true otherwise

 Description
     Posts an event to this state machine's queue
 Notes

 Author
     J. Edward Carryer, 10/23/11, 19:25
****************************************************************************/
bool PostBallShootService( ES_Event ThisEvent )
{
  return ES_PostToService( MyPriority, ThisEvent);
}

/****************************************************************************
 Function
    RunTemplateService

 Parameters
   ES_Event : the event to process

 Returns
   ES_Event, ES_NO_EVENT if no error ES_ERROR otherwise

 Description
   add your description here
 Notes
   
 Author
   J. Edward Carryer, 01/15/12, 15:23
****************************************************************************/
ES_Event RunBallShootService( ES_Event ThisEvent )
{
  static unsigned char nTimeouts = 0;
  ES_Event moveEvent;
  ES_Event ReturnEvent;
  ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
  if (ThisEvent.EventType == ES_START_SHOOTER) 
  {
  	  PWMDTY3 = 45;
  }
 
 if (ThisEvent.EventType == ES_SHOOT) {
	PWMDTY3 = 45;
	//TIM1_TIE |= _S12_C4I;		//Enable Interrupts on timer
	if (Servo12_SetPulseWidth(SHOOTINGCHANNEL, ACTIVE_PULSE) != SERVO12_Err) {
		printf("Move Out");
	}
	ES_Timer_InitTimer(BALL_SHOOT_TIMER, TIMERLENGTH);
 } else if (ThisEvent.EventType == ES_TIMEOUT) {
	if (nTimeouts == NCYCLES) {
		if (Servo12_SetPulseWidth(SHOOTINGCHANNEL, INACTIVE_PULSE) != SERVO12_Err) {
			printf("Move In");
		}
		nTimeouts = 0;
		PWMDTY3= 0;
		moveEvent.EventType = ES_MOVE;
		PostMasterSM(moveEvent);
		//TIM1_TIE &= ~_S12_C4I; //Disable Interrupts on timer
	} else {
		nTimeouts++;
		ES_Timer_InitTimer(BALL_SHOOT_TIMER, TIMERLENGTH);
	}		 
 }
 
  
  return ReturnEvent;
}

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

/*void interrupt _Vec_tim1ch4 MotorShootControlLoop (void) 
{
	   static unsigned int lastEdge;
	   static unsigned int period;
	   static unsigned int count = 0;
	   static float sumError = 0.0;
	   static float periodError;
	   static int dutyCycle;
	   period = TIM0_TC4 - lastEdge;
	   lastEdge = TIM0_TC4;
	   TIM1_TFLG1 = _S12_C4F;
	   count++;
	   if (count == 50) {
			count = 0;
			printf("Period: %f\n", period);
			periodError = period - ShooterPeriod;
			sumError += periodError;
			dutyCycle = (int) (-Kp*periodError - Ki*sumError);
			 if (dutyCycle> 100)
                {
                    dutyCycle = 100;
                    sumError -= periodError;
                }
            else if (dutyCycle < 0)
                {
                    dutyCycle = 0;
                    sumError += periodError;
                }
            PWMDTY3 = (char) dutyCycle;

	   }
}	*/