/****************************************************************************
 Module
   IRmanager.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 "S12eVec.h"
#include "IRmanager.h"
#include "TopHSM.h"

/*----------------------------- Module Defines ----------------------------*/
#define KNIGHTMIN 80
#define KNIGHTMAX 150
#define GOALMIN 160
#define GOALMAX 380
#define SAMPLE_INTERVAL 100

/*---------------------------- Module Functions ---------------------------*/
/* prototypes for private functions for this service.They should be functions
   relevant to the behavior of this service
*/
//void interrupt _Vec_tim1ch5 RiseIncrementer5(void);
void interrupt _Vec_tim1ch6 RiseIncrementer6(void);
void interrupt _Vec_tim1ch7 RiseIncrementer7(void);

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

static unsigned int forwardRises;
//static unsigned int sideRises;
static unsigned int rearRises;


/*------------------------------ Module Code ------------------------------*/
/****************************************************************************
 Function
     InitIRmanager

 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 InitIRmanager ( uint8_t Priority )
{
  ES_Event ThisEvent;

  MyPriority = Priority;

  forwardRises = 0;
 // sideRises = 0;
  rearRises = 0;
  ES_Timer_InitTimer(IR_TIMER, SAMPLE_INTERVAL);
  TIM1_TSCR1 = _S12_TEN; 
  //Forward IR DETECTOR 
  /*TIM1_TIOS &= ~(_S12_IOS5); 
  TIM1_TIE |= _S12_C5I;
  TIM1_TFLG1 = _S12_C5F;
  TIM1_TCTL3 |= _S12_EDG5A;		*/
  //Side IR DETECTOR 
  TIM1_TIOS &= ~(_S12_IOS6); 
  TIM1_TIE |= _S12_C6I;
  TIM1_TFLG1 = _S12_C6F;
  TIM1_TCTL3 |= _S12_EDG6A;
  //Rear IR DETECTOR 
  TIM1_TIOS &= ~(_S12_IOS7); 
  TIM1_TIE |= _S12_C7I;
  TIM1_TFLG1 = _S12_C7F;
  TIM1_TCTL3 |= _S12_EDG7A;
  EnableInterrupts;
  // post the initial transition event
  ThisEvent.EventType = ES_INIT;
  if (ES_PostToService( MyPriority, ThisEvent) == true)
  {
      return true;
  }else
  {
      return false;
  }
}

/****************************************************************************
 Function
     PostIRmanager

 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 PostIRmanager( 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 RunIRmanager( ES_Event ThisEvent )
{
  ES_Event ReturnEvent;
  ES_Event IRevent;
  ReturnEvent.EventType = ES_NO_EVENT; // assume no errors
  if (ThisEvent.EventType == ES_TIMEOUT)
  {
 	//printf("Rises %d:", sideRises);
  	if ((forwardRises >= KNIGHTMIN ) && (forwardRises<=KNIGHTMAX)) {
		IRevent.EventType = ES_FORWARD_KNIGHT;
  		printf("Forward Knight Found");
		PostMasterSM(IRevent);
	}
	if ((rearRises >= KNIGHTMIN ) && (rearRises<=KNIGHTMAX)) {
		IRevent.EventType = ES_REAR_KNIGHT;
	 	printf("Rear Knight Found");
		PostMasterSM(IRevent);
	}
	/*if ((sideRises >= GOALMIN) && (sideRises<=GOALMAX)) {
		IRevent.EventType = ES_GOAL_DETECTED;
	    //printf("Goal Found");
		PostMasterSM(IRevent);
	} */
  	forwardRises = 0;
	//sideRises = 0;
	rearRises = 0;
  	ES_Timer_InitTimer(IR_TIMER, SAMPLE_INTERVAL);
  }
   
  return ReturnEvent;
}

/***************************************************************************
 private functions
 ***************************************************************************/
 
 /*void interrupt _Vec_tim1ch5 RiseIncrementer5(void)
 {
 	sideRises++;
 	TIM1_TFLG1 = _S12_C5F;	
 }*/

  void interrupt _Vec_tim1ch6 RiseIncrementer6(void)
 {
 	forwardRises++;
 	TIM1_TFLG1 = _S12_C6F;	
 }
  void interrupt _Vec_tim1ch7 RiseIncrementer7(void)
 {
 	rearRises++;
 	TIM1_TFLG1 = _S12_C7F;	
 }
 
 
 
/*------------------------------- Footnotes -------------------------------*/
/*------------------------------ End of file ------------------------------*/