//#pragma SRC
//#define DEBUGGING

#include <at89x55.h>
#define NULL 0
#define INT_UART	4			/* 0x23 */


#define SENT_LEN 55
#define DIST_THRESHOLD	5	// < 30' feet
#define DIST_SLOW				10	// < 60' feet


// C routines
void start_motors();
void stop_motors();
void motors_half_speed();
void turn(unsigned char from, unsigned char to);
unsigned char	desired_bearing();
unsigned int dist_to_waypoint();


unsigned int get_int_val(unsigned char start, unsigned char len);

char char_buf[SENT_LEN+1]="GPRMC,tttttt,A,dd12337,N,ddd12851,W,s010,150h,mmddyy,";
unsigned char char_buf_ptr;


extern void asm_set_servo(unsigned char c_a, unsigned char servo_data);
extern void asm_get_waypoint(void);
extern void asm_send_waypoint(void);
extern void asm_init(void);
extern void	asm_lcd_print(void);

extern unsigned int data d_lon;
extern unsigned int data d_lat;
extern char data message[16];

unsigned int data current_lat;
unsigned int data current_lng;
unsigned char data current_spd;
unsigned char data current_head;

unsigned int work_lat;
unsigned int work_lng;


// This is _the_ pilot. It tells all of its minions what to do
//*  MAIN PILOT CODE *******************************************************************
void main() {
	unsigned char current_b, b_to_target;


  asm_init();

#ifndef DEBUGGING
	stop_motors();
#endif

	// c_init()
	RCAP2H = 0xFF;		// 4807 Baud
	RCAP2L = 0x30;

	T2MOD = 0x02;			// Enable timer 2 output
	T2CON	= 0x34;			// Set up timer 2 and start baud generator

	IE = 0x90;				// Disable tiemr 2 ints, enable serial ints
	SCON = 0x70;				// Set up ser. register & enable
		// end c_init()


	do {
#ifndef DEBUGGING
		asm_get_waypoint();  // <- Pilot blocks here while navigator/bomber does task
		start_motors();
#endif	
		message[0] = ' ';
		message[1] = ' ';
		message[2] = ' ';
		message[3] = ' ';
		message[4] = ' ';
		message[5] = ' ';
		message[11] = (d_lat & 0x00FF);
		message[12] = (d_lat & 0xFF00) >> 8;
		message[13] = (d_lon & 0x00FF);
		message[14] = (d_lon & 0xFF00) >> 8;
		message[15] = 0;

		do {
			IE &= 0x7F;				// Disable interrupts
			work_lng = current_lng;		// Make a local copy of all the shared data
			work_lat = current_lat;
			current_b = current_head;

			message[6] = (work_lat & 0x00FF);
			message[7] = (work_lat & 0xFF00) >> 8;
			message[8] = (work_lng & 0x00FF);
			message[9] = (work_lng & 0xFF00) >> 8;
			message[10] = '>';

			IE |= 0x80;				// Enable interrupts again

		asm_lcd_print();

#ifndef DEBUGGING
		if (dist_to_waypoint() < DIST_SLOW) {
			message[0] = 'O';
			message[1] = 'K';
			message[2] = 0;
			motors_half_speed();
		}
#endif
			// Figure out if we're headed in the right direction
			b_to_target = desired_bearing();

			//Decide which direction to turn and turn rudder
#ifndef DEBUGGING
			turn(current_b, b_to_target);
#endif

		} while (dist_to_waypoint() > DIST_THRESHOLD);
#ifndef DEBUGGING
		stop_motors();
#endif

		d_lat = current_lat;
		d_lon = current_lng;
#ifndef DEBUGGING
		asm_send_waypoint(); //	<- Returns as soon as message is finished.
#endif
	} while (1);

}
// **********************************************************************************/

/**************************  TEST COMM CODE  *****************************************
void main() {
	//unsigned char b_to_target, current_b;

  asm_init();

	// c_init()
	RCAP2H = 0xFF;		// 4807 Baud
	RCAP2L = 0x30;

	T2MOD = 0x02;			// Enable timer 2 output
	T2CON	= 0x34;			// Set up timer 2 and start baud generator

	IE = 0x90;				// Disable tiemr 2 ints, enable serial ints
	SCON = 0x70;				// Set up ser. register & enable
		// end c_init()

	do {
		asm_get_waypoint();  // <- Pilot blocks here while navigator/bomber does task

		message[1] = (d_lat & 0xFF00) >> 8;
		message[0] = (d_lat & 0x00FF);
		message[3] = (d_lon & 0xFF00) >> 8;
		message[2] = (d_lon & 0x00FF);
		message[4] = 0;
		asm_lcd_print();

		d_lat = 12345;
		d_lon = 10101;
		asm_send_waypoint(); //	<- Returns as soon as message is finished.

	} while (1);
}

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

/*******************  GPS TEST CODE *********************************************
void main() {
	unsigned char current_b;

  asm_init();

	// c_init()
	RCAP2H = 0xFF;		// 4807 Baud
	RCAP2L = 0x30;

	T2MOD = 0x02;			// Enable timer 2 output
	T2CON	= 0x34;			// Set up timer 2 and start baud generator

	IE = 0x90;				// Disable tiemr 2 ints, enable serial ints
	SCON = 0x70;				// Set up ser. register & enable
		// end c_init()

	do {
		IE &= 0x7F;				// Disable interrupts
		work_lng = current_lng;		// Make a local copy of all the shared data
		work_lat = current_lat;
		current_b = current_head;
		IE |= 0x80;				// Enable interrupts again

		message[0] = (work_lat & 0x00FF);
		message[1] = (work_lat & 0xFF00) >> 8;
		message[2] = 32;
		message[3] = (work_lng & 0x00FF);
		message[4] = (work_lng & 0xFF00) >> 8;
		message[5] = 32;
		message[6] = current_b;
		message[7] = 0;
		asm_lcd_print();
	} while (1);
}
//*************************************************************************************/



//----------------------------------------------------------------------------
unsigned int get_int_val(unsigned char start, unsigned char len) {
	unsigned int rv;
	unsigned int twice_old_val;
	unsigned char c;

	rv = 0;
	for (c=0;c<len;c++) {
		twice_old_val = (rv << 1);
		rv = (rv << 3) + twice_old_val;
		rv = rv + (char_buf[start++] - 48);
	}

	return rv;
}

unsigned char get_char_val(unsigned char start) {
	unsigned char rv = 0;
	unsigned char c;
	for (c=0;c<3;c++)
		rv = (10 * rv) + (char_buf[start++] - 48);
	return rv;
}

//----------------------------------------------------------------------------
void serial_isr() interrupt INT_UART {
	char the_char;

	RI = 0;

	the_char = SBUF;
	switch (the_char) {
		case '.':											// Ignore these characters

			break;

		case '$':
			char_buf[char_buf_ptr] = 0;	// Null terminate the last string
			char_buf_ptr = 0;						// Return the pointer to the start

			if (char_buf[4] != 'C') return;	// Ignore it if it's not the RMC string

//			for (the_char=0;the_char < 5; the_char++)
//				message[the_char] = char_buf[the_char+17];
	
			// char_buf = "GPRMC,tttttt,A,ddmmmmm,N,dddmmmmm,W,ssss,hhhh,mmddyy,
				
			current_lat =  get_int_val(17, 5);

			current_lng = get_int_val(28, 5);

			current_spd = get_char_val(37);

			current_head = (unsigned char)(get_int_val(41,3) >> 1);

			break;

		default:
			if (char_buf_ptr < SENT_LEN) {		// Put characters in the queue without overflowing
			  char_buf[char_buf_ptr++] = the_char;
			}
	}
}


unsigned char	desired_bearing() {
	unsigned int distx, disty;
	unsigned char rotation;
	unsigned char temp;

	if (work_lat < d_lat) {		// Upper half
		if (work_lng < d_lon) {	// Upper Left quadrant
			distx = d_lon - work_lng;
			disty = d_lat - work_lat;
			rotation = 135;
		} else {											// Upper Right quadrant
			distx = work_lng - d_lon;
			disty = d_lat - work_lat;
			rotation = 0;
		}
	} else {												// Lower quadrant
		if (work_lng < d_lon) {	// Lower Left quadrant
			distx = d_lon - work_lng;
			disty = work_lat - d_lat;
			rotation = 90;
		} else {											// Lower Right
			distx = work_lng - d_lon;
			disty = work_lat - d_lat;
			rotation = 45;
		}
	}
	temp = (unsigned char)(((unsigned long)disty << 7) / (unsigned long)distx);
	if (temp < 19) {
		return rotation;
	} else if (temp < 67) {
		return rotation + 22;
	} else return rotation + 45;
}

unsigned int dist_to_waypoint() {
	unsigned int la, lb;
	// We're using unsigned numbers, and don't have room
  //   for subtraction to a negative number
/*		message[1] = (d_lat & 0xFF00) >> 8;
		message[0] = (d_lat & 0x00FF);
		message[3] = (d_lon & 0xFF00) >> 8;
		message[2] = (d_lon & 0x00FF);

		message[4] = '-';
		message[5] = (work_lat & 0x00FF);
		message[6] = (work_lat & 0xFF00) >> 8;
		message[7] = (work_lng & 0x00FF);
		message[8] = (work_lng & 0xFF00) >> 8;

		message[9] = 0;
*/
	if (work_lat < d_lat) {
		la = d_lat - work_lat;
	} else {
		la = work_lat - d_lat;
	}

	if (work_lng < d_lon) {
		lb = d_lon - work_lng;
	} else {
		lb = work_lng - d_lon;
	}

	return (la < lb) ? lb : la;
}

void start_motors() { 
	asm_set_servo(0, 255);
}

void stop_motors() {
	asm_set_servo(0,128);
}

void motors_half_speed() {
	asm_set_servo(0,196);
}

void do_turn(bit dir, unsigned char amount) {
	unsigned int temp;

	temp = (unsigned int)amount;
	temp = temp * 85 / 90;		// Limits servo to 30 deg each way.
	amount = (unsigned char)temp;
	
	if (dir == 0)	{	// Turn right by moving servo left
		asm_set_servo(1, 128 + amount);
	} else {				// Turn left by moving servo right
		asm_set_servo(1, 128 - amount);
  }
}

void turn(unsigned char from, unsigned char to) {
	unsigned char amount = (unsigned char)((((int)to - (int)from) + 180) % 180);
	if (amount < 90) {
		do_turn(0, amount);
	} else {
		do_turn(1, 180 - amount);
	}
}