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

Ultrasonic distance module

TIM1, Port T5, timer 5 (output compare, PWM signal out)
TIM0, Port T6, timer 6 (rising + falling edges)



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

/*---------------------------- Include Files ----------------------------*/
#include <stdio.h>
#include <hidef.h>      /* common defines and macros */
#include <mc9s12e128.h> /* derivative information */
#include "ME218_E128.h"	/* bit definitions */
#include "S12eVec.h"		/* interrupts */
#include <timers12.h>
#include "game.h"

#include "ultrasonic.h"

/*--------------------------- Module Defines ----------------------------*/
#define _4US_ *3
#define _1MS_ *750

#define OC_HIGH (2 _4US_)
#define OC_LOW (25 _1MS_)
#define OC_WAIT (100 _4US_)
#define PWM_SIGNAL (20 _4US_)

/*---------------------------- Module Types -----------------------------*/
/*-------------------------- Module Variables ---------------------------*/
static unsigned int lastRisingEdge;
static unsigned int pulseWidth;
unsigned char bit6state;

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

void InitializeUltrasonic(void) {

	// Initialize initial periods
	lastRisingEdge = pulseWidth = 0;

	DDRT |= (BIT5HI | BIT4HI);
	DDRT &= BIT6LO;
	PTT &= BIT5LO;
	PTT |= BIT4HI;

	TIM1_TSCR1 = _S12_TEN; // Turn the Timer system on
	TIM1_TSCR2 = (_S12_PR0 | _S12_PR2); // Prescale: 32, so 24 MHz / 8 = 750 kHz

	// Left beacon
	// Input capture
	TIM1_TIOS &= ~(_S12_IOS6); // Input Capture
	TIM1_TCTL3 |= (_S12_EDG6A | _S12_EDG6B); // Capture rising/falling edges
	TIM1_TFLG1 = _S12_C6F; // Clear flag
	TIM1_TIE |= _S12_C6I; // Enable input capture

	// Output compare
	TIM1_TIOS = _S12_IOS5; // Output compare
	TIM1_TCTL1 = TIM1_TCTL1 & ~(_S12_OL5 | _S12_OM5); // Set No Pin Connected to OC Interrupt
	TIM1_TFLG1 = _S12_C5F; // Clear Flag
	TIM1_TIE |= _S12_C5I; // Enable interrupt
	TIM1_TC5 = TIM1_TCNT + OC_HIGH; // next rising edge

	EnableInterrupts; //Globally Enable Interrupts
}

unsigned int ReturnUltrasonicDistance(void) {

	unsigned int period, distance;
	period = ((3*pulseWidth)/4)/2; // microseconds
	distance = period / 17;
	return distance;
}

// OutputPulse: OutputPulse, takes nothing, returns nothing
void interrupt _Vec_tim1ch5 OutputPulse(void){

	static unsigned char controlFlag = 0;

	if	((PTT & BIT5HI) == BIT5HI && controlFlag == 0) { // currently high
		PTT &= BIT5LO; // falling edge
		TIM1_TC5 = TIM1_TCNT + OC_WAIT; // wait to relinquish control of line
		controlFlag = 1; // holding control
	}
	else if(controlFlag == 1) { // currently low, relinquish control
		TIM1_TC5 = TIM1_TCNT + OC_LOW; // next rising edge
		DDRT &= BIT5LO; // Switch to input mode, wait for response
		controlFlag = 0; // release control
	}
	else { // currently low
		DDRT |= BIT5HI; // Get control back
		PTT |= BIT5HI; // rising edge
		TIM1_TC5 = TIM1_TCNT + OC_HIGH; // wait for falling edge
	}
	// Clear flag
	TIM1_TFLG1 = _S12_C5F;
}

// InputPulse: Input Capture, takes nothing, returns nothing
void interrupt _Vec_tim1ch6 InputPulse(void){

	unsigned int width;

	if((PTT & BIT6HI) == BIT6HI) { // currently high
		lastRisingEdge = TIM1_TCNT;
		PTT |= BIT4HI;
	}
	else	{ // currently low
		width = TIM1_TCNT - lastRisingEdge;
		if (width > PWM_SIGNAL) // ignore our PWM pulses
			pulseWidth = TIM1_TCNT - lastRisingEdge;
		PTT &= BIT4LO;
	}
	// Clear flag
	TIM1_TFLG1 = _S12_C6F;
}