/* sonar.c */
#include "door.h"
#include "sonar.h"


/* states and flags */
//static unsigned int subseconds = 0;
static unsigned char dist = 0;		// count the cycle before echo comes back
static unsigned int count = 0;		// count the cycle before echo comes back
static bit countDist = 0;
static bit run = 1;
static bit echo = 0;
static bit pa = 1;
static bit pb = 1;
static bit pc = 1;
static bit pd = 1;
static bit pe = 1;
static bit pnum = 1;
static char xdata *ledaddress;
static unsigned char value = 0x77;	//0
static unsigned int num = 0;		// count the cycle before echo comes back




/* fire sonar and do distance calculations;
   signals serialTask to do RF stuff when user
   is near by
*/
void Sonar_Task() {	
	/* interrupt settings */
	EA = 1;
	ET1 = 0;
	ES = 0;

	/* set serial */
	SM0 = 0;
	SM1 = 1;

	/* set timer */
	TMOD = 0x21;
	PCON = PCON | 0x80;
	TH1 = 249;
	TR1 = 1;

	TI = 1;	

	/* Set up and enable Timer0 for a 1 millisecond timer   */
	TL0 = RELOAD_LO;	/* Load the timer high and low bytes        */
	TH0 = RELOAD_HI;
	TMOD = 0x21;		/* Timer1 & Timer0 as 16-bit timers         */
	TR0 = 1;			/* Start the timer 0             */
	ET0 = 1;			/* Enable the timer interrupt 0 (Page 55) */
	EX0 = 0;			/* Enable the external interrupt 0 */
	EA = 1;				/* Enable the global interrupt  */
	IT0 = 1;			/* Enable the TCON for falling edge trigger */
	P1_7 = 0;			/* P1.7 is connected to sonar INIT, pin 67 on the XS40 */
	P1_6 = 0;			/* P1.6 is connected to sonar BINH, pin 66 on the XS40 */
	count = 0;
	ledaddress = 0x0f000;  // the FPGA is configured to memory map led to this address
	*ledaddress = value;   // write initial data to the led
	pnum = 1;
	PT0 = 1;
	while (run) {
	}
}/* end Sonar_Task */


/* called by Ext0 ISR in DETECTING_USER state */
void Sonar_Ext0_Handler() {
	*ledaddress = 0xFF;

	//	if(echo == 0)
	//	if(num > 9)
	if(1)
	//	if(num > 90)
	{
		echo = 1;
		//EX0 = 0;
		if(pnum)
		{
			print('I');
			print('I');
			printInt(num);
			print('I');
			print('I');
			pnum = 0;
		}
	}
}/* end	Sonar_Ext0_Handler() */


/* called by Timer0 ISR in DETECTING_USER state */
void Sonar_Timer0_Handler() {
		TL0 = TL0 + RELOAD_LO ;   // Load the timer high and low bytes        
		TH0 = RELOAD_HI;
		subseconds = subseconds + 1;  // Increment the 0.1 millisecond counter
		count = count + 1;
		if(P1_5==1)P1_5=0;
		else
			P1_5=1;
		if((count >=0) && (count < 100))		// RESET
		//if((count >=0) && (count < 500))
		{
			EX0 = 0;
			pnum=1;
			if(pa)
			{	print('A'); pa = 0; pd = 1; }
				
			*ledaddress = 0x12;		//1
			P1_7 = 0;	// init
			P1_6 = 0;	// binh
			countDist = 0;
			dist = 0;
			echo = 0;
			num = 0;
//			pnum = 1;
		}
		else if((count >= 100) && (count < 15000)) 	// logic high
		//else if((count >= 500) && (count < 15500)) // logic high
		{
			if(pb)
			{	print('B'); pa = 1; pb = 0;}
//			print('B');
			*ledaddress = 0x5D;		//2
			P1_7 = 1;	// init = 1	
			
			if(echo == 0) 
				num = num + 1;
			if(count<122){
				IE0=0;
				EX0=0;

				}
			else
				EX0=1;
				}
		else if((count >= 15000) && (count < 60000))		//logic low
		//else if((count >= 15500) && (count < 30500))
		{
			if(pc) { print('C'); pb = 1; pc = 0; }
				
			*ledaddress = 0x5B;		//3
			P1_7 = 0;
			P1_6 = 0;
		}	
		else	
		{
			if(pd)
			{	print('D'); pc = 1; pd = 0; }

			*ledaddress = 0x6B;		//5
			count = 0;
//			run = 0;
		}
} /* end Sonar_Timer0_Handler() */


void print(char c)
{
	SBUF = c;
	while(TI == 0);
	TI = 0;
}

void putChar(unsigned char c) {
	unsigned char a = c/100;
	unsigned char b = c%100;
	unsigned char d = b/10;
	unsigned char e = b%10;
	print(a+48);
	print(d+48);
	print(e+48);
}

void printInt(unsigned int i)
{
	unsigned char a = ((i / 256) / 256); 
	unsigned char c = i / 256;
	unsigned char d = i % 256;
	print(0x2A);	//*
	putChar(a);
	print(0x7C);	// |
	putChar(c);
	print(0x7C);	// |
	putChar(d);
	print(0x2A);	//*
}