/****************************************************************************
 Module
   Beacon_Module.c

 Revision
   1.0.0

 Description
   This module will work with Hall Effect Sensors

 Notes

 History
 When           Who     What/Why
 -------------- ---     --------
 01/16/12 09:58 jec     began conversion from TemplateFSM.c
 01/25/17	08:10	mch			create Motor Module
****************************************************************************/
/*----------------------------- 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
*/

// the common headers for C99 types 
#include <stdint.h>
#include <stdbool.h>
#include <inttypes.h>
#include <math.h>

// the headers to access the GPIO subsystem
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "inc/hw_gpio.h"
#include "inc/hw_sysctl.h"
#include "inc/hw_pwm.h"
#include "inc/hw_timer.h"
#include "inc/hw_nvic.h"

// the headers to access the TivaWare Library
#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"
#include "driverlib/gpio.h"
#include "driverlib/timer.h"
#include "driverlib/interrupt.h"
#include "driverlib/pwm.h"
#include "termio.h"

#include "BITDEFS.H"

#include "ES_Port.h"
#include "ES_Configure.h"

//My Includes
#include "Hardware.h"
#include "Constants.h"
#include "MasterSM.h"
#include "ShootingSM.h"

#include "Beacon_Module.h"

/*----------------------------- Module Defines ----------------------------*/
static uint16_t numPulses = 0;
static uint16_t numSamples = 0;

static float BeaconPeriod = 0.0;
static float LastCapture = 0.0;
static float BeaconPeriodSum = 0.0;

static const float BeaconGoodPeriod = 690;

void BeaconTimerISR(void);
void StartBeaconTimer(uint32_t time);
bool CorrectBeaconFound (float Period);
void BeaconInputCaptureResponse( void);

#define PERIOD_PRINTS
/*---------------------------- Module Variables ---------------------------*/

/*----------------------------- Public Functions --------------------------*/
/************INTERRUPT*************/

void StartBeaconInputCapture(void) {
	//enable clock
	HWREG(WTIMER5_BASE+TIMER_O_CTL) |= (TIMER_CTL_TAEN | TIMER_CTL_TASTALL);
	printf("Start Input Capture for Beacon \n\r");
}

void StopBeaconInputCapture(void) {
	HWREG(WTIMER5_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEN; 
	printf("Stop Input Capture for Beacon \n\r");
}

void ResetBeaconCounters(void) {
			numSamples = 0.0;
			BeaconPeriod = 0.0;
			BeaconPeriodSum = 0.0;
			LastCapture = 0.0;
}

/**********Private***********************/

void BeaconInputCaptureResponse( void) {
	float ThisCapture = 0.0;
	float ThisPeriod = 0.0;
	
	ShootingState_t CurrentState = QueryShootingSM();
	
	//clear source of interrupt
	HWREG(WTIMER5_BASE+TIMER_O_ICR) = TIMER_ICR_CAECINT;
	ThisCapture = HWREG(WTIMER5_BASE+TIMER_O_TAR);
	
	if (CurrentState == CHECKING_BEACON) {
			if (numSamples == 0) {
				LastCapture = ThisCapture;
			}
			else {
				ThisPeriod = ThisCapture - LastCapture;
				BeaconPeriodSum += ThisPeriod; //find period
			}
			
			if (numSamples == DESIRED_NUM_SAMPLES_BEACON) { //&& (deltaAverage <= DESIRED_DELTA)) {
				 BeaconPeriod = (BeaconPeriodSum/numSamples)/TicksPerUS;
				
				 #ifdef PERIOD_PRINTS
				 printf("BeaconSum: %f \n\r", BeaconPeriodSum); 
				 printf("numSamples: %i \n\r", numSamples); 
				 printf("BeaconPeriod: %f \n\r", BeaconPeriod);
				 #endif
					
				 ES_Event Event;
				
				 if ( CorrectBeaconFound(BeaconPeriod) == true ){
						Event.EventType = ES_ALIGNED;
						PostMasterSM(Event);
						ResetBeaconCounters();
				 } else {
						Event.EventType = ES_REALIGN;
						PostMasterSM(Event);
						ResetBeaconCounters();
				 }			 
			 }

				// Increase numSamples
			 numSamples++;
			 
	} else if ( CurrentState == ALIGNING ) {
		if (numPulses == BEACON_EDGES) {
			printf("Beacon edges \n\r");
			ES_Event NewEvent;
			NewEvent.EventType = ES_BEACON_FOUND;
			PostMasterSM(NewEvent);
			ResetBeaconCounters();
		}
		
		// Increase numPulses
		numPulses++;
	}
	
	//update LastCapture
	LastCapture = ThisCapture;	
}	 

//function to decide which code to send to comm
bool CorrectBeaconFound (float Period) {
	// Check if frequency of beacon is correct
	if ((Period > (BeaconGoodPeriod - BEACON_TOL)) && (Period < (BeaconGoodPeriod + BEACON_TOL))){
		return true;
	} else {
		return false;
	}		
}

void BeaconInputCaptureInit(void) {
	
//Start by enabling the clock to the timer (Wide Timer 1)
	HWREG(SYSCTL_RCGCWTIMER) |= SYSCTL_RCGCWTIMER_R5;	
		while( (HWREG( SYSCTL_PRWTIMER ) & SYSCTL_PRWTIMER_R5 ) != SYSCTL_PRWTIMER_R5 );

	//make sure that timer (Timer B) is disabled before configuring
	HWREG(WTIMER5_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEN; 
	
	//set it up in 32bit wide mode
	HWREG(WTIMER5_BASE+TIMER_O_CFG) = TIMER_CFG_16_BIT;
	
	//we want to use the full 32 bit count, so initialize the Interval Load
	//register to 0xffff.ffff (its default value)
	HWREG(WTIMER5_BASE+TIMER_O_TAILR) = CLOCK_FULL_LOAD;
	
	//set up timer B in capture mode for edge time and up-counting 
	HWREG(WTIMER5_BASE+TIMER_O_TAMR) = 
		(HWREG(WTIMER5_BASE+TIMER_O_TAMR) & ~TIMER_TAMR_TAAMS) | 
			(TIMER_TAMR_TACDIR | TIMER_TAMR_TACMR | TIMER_TAMR_TAMR_CAP);
			
	//set event to rising edge (on rising edge, we capture the time)
	HWREG(WTIMER5_BASE+TIMER_O_CTL) &= ~TIMER_CTL_TAEVENT_M;
	
	//set up the port to do the capture - set HALL EFECT as alternate function
	HWREG(BEACON_PORT+GPIO_O_AFSEL) |= BEACON_PIN;
	
	//map pin to alternative function to wide timer 1 module B. 7 i shte value to select. 
	//shift over 4 nibbles, so shift over 16 bits
	HWREG(BEACON_PORT+GPIO_O_PCTL) = (HWREG(BEACON_PORT+GPIO_O_PCTL) & 0xF0FFFFFF) + (7<<(6*4));
	
	//enable PD6 as digital input (up)
	HWREG(BEACON_PORT + GPIO_O_DEN) |= BEACON_PIN;
	HWREG(BEACON_PORT + GPIO_O_DIR) &= ~BEACON_PIN;
	
	//enable a local capture interrupt
	HWREG(WTIMER5_BASE+TIMER_O_IMR) |= TIMER_IMR_CAEIM;
	
	//enable Timer A in Wide Timer 5 interrupt in the NVIC
	//it is interrupt number 104 so appears in EN3 at bit 8
	HWREG(NVIC_EN3) |= BIT8HI;	
	
	// Change priority of edge capture to 0 (highest) 
	HWREG( NVIC_PRI26 ) = (HWREG( NVIC_PRI26 ) & ~NVIC_PRI26_INTA_M) +
		(PRIORITY_0 << NVIC_PRI26_INTA_S); 
	
	//make sure interrupts are enabled globally
	__enable_irq();
	
	//start timner by enabling it and enablign the timer to stall while stopped by the debugger
	//HWREG(WTIMER1_BASE+TIMER_O_CTL) |= (TIMER_CTL_TBEN | TIMER_CTL_TBSTALL);
}
/*----------------------------- Private Functions -------------------------*/