/**************************************************************************

Beacon detection module

Left beacon:
TIM0, Port T0, timer 4 (rising + falling edges)
TIM0, Port T2, timer 6 (output compare)

Right beacon:
TIM0, Port T1, timer 5 (rising and falling edges)
TIM0, Port T3, timer 7 (output compare)

**************************************************************************/

/*---------------------------- Include Files ----------------------------*/

#include <stdio.h>
#include <hidef.h>
#include <mc9s12e128.h>
#include "ME218_E128.h"
#include "S12eVec.h"
#include <timers12.h>
#include "game.h"

#include "beacon.h"

/*--------------------------- Module Defines ----------------------------*/
#define _10US_ *15
#define _MS_ *1500

#define OC_PERIOD (3 _MS_)
#define PERIOD_880US (88 _10US_)
#define PERIOD_1120US (112 _10US_)
#define HITIME_880US (88 _10US_)
#define HITIME_720US (72 _10US_)
#define HITIME_400US (40 _10US_)
#define HITIME_240US (24 _10US_)
#define HITIME_560US (56 _10US_)
#define PER_NOISE (10 _10US_)
#define ERROR (4 _10US_)

/*---------------------------- Module Types -----------------------------*/

/*-------------------------- Module Variables ---------------------------*/
static unsigned int leftPeriod, leftHighTime;
static unsigned int rightPeriod, rightHighTime;
static unsigned int lastLeftHI, lastLeftValidHI, lastRightHI, lastRightValidHI;
static unsigned int lastLeftLO, lastRightLO;
static unsigned int lTimeHigh, rTimeHigh; // unrounded high times
static unsigned int lPeriod, rPeriod;

/*-------------------------- Module Functions ---------------------------*/
void HandleLeftEdge(void);
void HandleRightEdge(void);

/*----------------------------- Module Code -----------------------------*/

void InitializeBeacons(void) {

	TIM0_TSCR1 = _S12_TEN; // Turn the Timer system on
	TIM0_TSCR2 = _S12_PR2; // Prescale: 16, so 24 MHz / 16 = 1.5 MHz

	// Left beacon
	// Input capture
	TIM0_TIOS &= ~(_S12_IOS4); // Input Capture on Ch4 [T0] (rising + falling edges)
	TIM0_TCTL3 |= (_S12_EDG4A | _S12_EDG4B); // Capture rising/falling edges
	TIM0_TFLG1 = _S12_C4F; // Clear flag
	TIM0_TIE |= _S12_C4I; // Enable interrupt
	// Output compare
	TIM0_TIOS = _S12_IOS6; // Output compare on Ch6 [T2]
	TIM0_TCTL1 = TIM0_TCTL1 & ~(_S12_OL6 | _S12_OM6); // Set No Pin Connected to OC Interrupt
	TIM0_TFLG1 = _S12_C6F; // Clear Output Compare 4 Flag

	// Right beacon
	// Input capture
	TIM0_TIOS &= ~(_S12_IOS5); // Input Capture on Ch5 [T1] (rising + falling edges)
	TIM0_TCTL3 |= (_S12_EDG5A | _S12_EDG5B); // Capture rising/falling edges
	TIM0_TFLG1 = _S12_C5F; // Clear flag
	TIM0_TIE |= _S12_C5I; // Enable interrupt
	// Output compare
	TIM0_TIOS = _S12_IOS7; // Output compare on Ch7 [T3]
	TIM0_TCTL1 = TIM0_TCTL1 & ~(_S12_OL7 | _S12_OM7); // Set No Pin Connected to OC Interrupt
	TIM0_TFLG1 = _S12_C7F; // Clear Output Compare 4 Flag

	// Initialize initial periods
	leftPeriod = leftHighTime = 0;
	rightPeriod = rightHighTime = 0;
	lastLeftHI = lastLeftValidHI = 0;
	lastRightHI = lastRightValidHI = 0;
	lastLeftLO = lastRightLO = 0;
	lTimeHigh = rTimeHigh = 0;

	EnableInterrupts; //Globally Enable Interrupts
}


beacon_t CheckBeaconDetected(unsigned char sensorSide) {

	beacon_t detectedBeacon = NO_BEACON;

	switch(sensorSide) {
		case LEFT:
			if (leftPeriod == PERIOD_1120US && leftHighTime == HITIME_880US)
				detectedBeacon = DISPENSER_0;
			else if (leftPeriod == PERIOD_1120US && leftHighTime == HITIME_720US)
				detectedBeacon = DISPENSER_1;
			else if (leftPeriod == PERIOD_880US && leftHighTime == HITIME_240US)
				detectedBeacon = DISPENSER_2;
			else if (leftPeriod == PERIOD_880US && leftHighTime == HITIME_400US)
				detectedBeacon = DISPENSER_3;
			else if (leftPeriod == PERIOD_1120US && leftHighTime == HITIME_560US)
				detectedBeacon = HOOP_RED;
			else if (leftPeriod == PERIOD_880US && leftHighTime == HITIME_560US)
				detectedBeacon = HOOP_GREEN;
			break;
		case RIGHT:
			if (rightPeriod == PERIOD_1120US && rightHighTime == HITIME_880US)
				detectedBeacon = DISPENSER_0;
			else if (rightPeriod == PERIOD_1120US && rightHighTime == HITIME_720US)
				detectedBeacon = DISPENSER_1;
			else if (rightPeriod == PERIOD_880US && rightHighTime == HITIME_240US)
				detectedBeacon = DISPENSER_2;
			else if (rightPeriod == PERIOD_880US && rightHighTime == HITIME_400US)
				detectedBeacon = DISPENSER_3;
			else if (rightPeriod == PERIOD_1120US && rightHighTime == HITIME_560US)
				detectedBeacon = HOOP_RED;
			else if (rightPeriod == PERIOD_880US && rightHighTime == HITIME_560US)
				detectedBeacon = HOOP_GREEN;
			break;
	}
	return detectedBeacon;
}

// ReturnHighTime: take LEFT or RIGHT beacon, return current high time
unsigned int ReturnHighTime(unsigned char beacon) {

	switch(beacon) {
		case LEFT: return 2*lTimeHigh/3; break;
		case RIGHT: return 2*rTimeHigh/3; break;
	}
}

unsigned int ReturnPeriod(unsigned char beacon) {

  switch(beacon) {
		case LEFT: return 2*lPeriod/3; break;
		case RIGHT: return 2*rPeriod/3; break;
	}
}

void HandleLeftEdge(void) {

	static unsigned char validHI, edgeState;
	unsigned int currTime, period;

	currTime = TIM0_TC4;
	edgeState = PTT & BIT0HI;

	if (edgeState == BIT0HI) {	// Rising edge

		period = currTime - lastLeftValidHI;
		lPeriod = period;

		// Noise
		if ((currTime - lastLeftLO) < PER_NOISE)
			validHI = 0;
		else if (period > (PERIOD_880US - ERROR) && period < (PERIOD_880US + ERROR)) {
			validHI = 1;
			leftPeriod = PERIOD_880US;
		}
		else if (period > (PERIOD_1120US - ERROR) && period < (PERIOD_1120US + ERROR)) {
			validHI = 1;
			leftPeriod = PERIOD_1120US;
		}
		// Unexpected period (or first time through)
		else {
			validHI = 0;
			leftPeriod = 0;
			lastLeftValidHI = currTime;
		}
		lastLeftHI = currTime; // Update rising edge time
	}

	// Falling edge
	else {

		validHI = 0; // current edge not a valid rising edge
		lTimeHigh = currTime - lastLeftValidHI;

		// Noise
		if (currTime - lastLeftHI < PER_NOISE); // ignore
		else  if (lTimeHigh > (HITIME_240US - ERROR) && lTimeHigh < (HITIME_240US + ERROR))
			leftHighTime = HITIME_240US;
		else  if (lTimeHigh > (HITIME_400US - ERROR) && lTimeHigh < (HITIME_400US + ERROR))
			leftHighTime = HITIME_400US;
		else  if (lTimeHigh > (HITIME_560US - ERROR) && lTimeHigh < (HITIME_560US + ERROR))
			leftHighTime = HITIME_560US;
		else  if (lTimeHigh > (HITIME_720US - ERROR) && lTimeHigh < (HITIME_720US + ERROR))
			leftHighTime = HITIME_720US;
		else  if (lTimeHigh > (HITIME_880US - ERROR) && lTimeHigh < (HITIME_880US + ERROR))
			leftHighTime = HITIME_880US;
		else // unexpected period
			leftHighTime = 0;

		lastLeftLO = currTime; // Update falling edge time
	}

	// We had a valid rising edge
	if (validHI == 1)
		lastLeftValidHI = currTime; // update last valid rising edge time
}

void HandleRightEdge(void) {

	static unsigned char validHI, edgeState;
	unsigned int currTime, period;

	currTime = TIM0_TC5;
	edgeState = PTT & BIT1HI;

	// Rising edge
	if (edgeState == BIT1HI) {

		period = currTime - lastRightValidHI;
		rPeriod = period;

		// Noise
		if ((currTime - lastRightLO) < PER_NOISE)
			validHI = 0;
		else if (period > (PERIOD_880US - ERROR) && period < (PERIOD_880US + ERROR)) {
			validHI = 1;
			rightPeriod = PERIOD_880US;
		}
		else if (period > (PERIOD_1120US - ERROR) && period < (PERIOD_1120US + ERROR)) {
			validHI = 1;
			rightPeriod = PERIOD_1120US;
		}
		// Unexpected period (or first time through)
		else {
			validHI = 0;
			rightPeriod = 0;
			lastRightValidHI = currTime;
		}
		lastRightHI = currTime; // Update rising edge time
	}

	// Falling edge
	else {

		validHI = 0; // current edge not a valid rising edge
		rTimeHigh = currTime - lastRightValidHI;

		// Noise
		if (currTime - lastRightHI < PER_NOISE); // ignore
		else  if (rTimeHigh > (HITIME_240US - ERROR) && rTimeHigh < (HITIME_240US + ERROR))
			rightHighTime = HITIME_240US;
		else  if (rTimeHigh > (HITIME_400US - ERROR) && rTimeHigh < (HITIME_400US + ERROR))
			rightHighTime = HITIME_400US;
		else  if (rTimeHigh > (HITIME_560US - ERROR) && rTimeHigh < (HITIME_560US + ERROR))
			rightHighTime = HITIME_560US;
		else  if (rTimeHigh > (HITIME_720US - ERROR) && rTimeHigh < (HITIME_720US + ERROR))
			rightHighTime = HITIME_720US;
		else  if (rTimeHigh > (HITIME_880US - ERROR) && rTimeHigh < (HITIME_880US + ERROR))
			rightHighTime = HITIME_880US;
		else // unexpected period
			rightHighTime = 0;

		lastRightLO = currTime; // Update falling edge time
	}

	// We had a valid rising edge
	if (validHI == 1)
		lastRightValidHI = currTime; // update last valid rising edge time
}

// LeftEdgeDetected: Input Capture, takes nothing, returns nothing
void interrupt _Vec_tim0ch4 LeftEdgeDetected(void){

	// Schedule output compare to trigger when signal out of range
	TIM0_TC6 = TIM0_TCNT + OC_PERIOD;
	// Turn on OC interrupt
	TIM0_TIE |= _S12_C6I;

	// Figure out period/high time
	HandleLeftEdge();

	// Clear flag
	TIM0_TFLG1 = _S12_C4F;
}

// RightEdgeDetected: Input Capture, takes nothing, returns nothing
void interrupt _Vec_tim0ch5 RightEdgeDetected(void){

	// Schedule output compare to trigger when signal out of range
	TIM0_TC7 = TIM0_TCNT + OC_PERIOD;
	// Turn on OC interrupt
	TIM0_TIE |= _S12_C7I;

	// Figure out period/high time
	HandleRightEdge();

	// Clear flag
	TIM0_TFLG1 = _S12_C5F;
}

// Output Compare: Left beacon signal lost
void interrupt _Vec_tim0ch6 LeftBeaconLost(void){

	// Clear flag
	TIM0_TFLG1 = _S12_C6F;
	// Turn off OC interrupt
	TIM0_TIE &= ~_S12_C6I;

	// Get ready for next signal detection
	leftPeriod = leftHighTime = 0;
}

// Output Compare: Right beacon signal lost
void interrupt _Vec_tim0ch7 RightBeaconLost(void){

	// Clear flag
	TIM0_TFLG1 = _S12_C7F;
	// Turn off OC interrupt
	TIM0_TIE &= ~_S12_C7I;

	// Get ready for next signal detection
	rightPeriod = rightHighTime = 0;
}