//
// ECE430 - Self-navigating Lego car
// Jeff Keacher & Joey Richey
// November, 2003
//
// Coded for PIC16F877A
//  

//******************
//	Includes
//******************
#include <pic.h>
#include "lcd.h"
#include "key.h"

//******************
//	Constants
//******************

//string literals used throughout program
#define LOCATION_PROMPT		"Destination:"
#define GETTING_SIGNAL		"Acquiring signal..."
#define DESTINATION_REACHED	"At Location!"

//defines if we are entering the latitude or longitude
#define LATITUDE	0
#define LONGITUDE	1

#define LAT_FOOT_PER_MINUTE		6	//number of feet in 1 minute of latitude
#define LON_FOOT_PER_MINUTE		9	//number of feet in 1 minute of longitude at 39 degrees

#define NUM_DIGITS	3	//of how many digits you can enter
#define DIRECTION	3	//the location in the array which holds the direction

//direction constatns
#define	NORTH	'N'
#define	SOUTH	'S'
#define	EAST	'E'
#define	WEST	'W'

#define BLANK	' '

#define NUM_LOOPS_PER_TURN	10
#define NO_DATA_WAITING 	0
#define SET 				1
#define CLEAR 				0
#define ENABLED 			1
#define DISABLED 			0
#define CLOCK_SPEED_MHZ 	20
#define SERIAL_RATE_BPS		9600
#define SENTENCE_START 		'@'
#define NULL_DATA 			'_'
#define FIRST_CHAR			1
#define ZERO				0
#define POSITION_LOST		0
#define POSITION_KNOWN		1
#define LAT_MIN_START		17
#define NS_SPEED_DIR		46
#define NS_SPEED_START		49
#define EW_SPEED_DIR		41
#define EW_SPEED_START		44
#define LON_MIN_START		26
#define END_OF_SENTENCE		57
#define ASCII_NUM_OFFSET	0x30
#define MS_PER_DEGREE		175

//******************
//	hardware/software interface
//******************

//ports that the motors are on
#define MOTOR_PORT_CTRL			TRISC
#define MOTOR_PORT_DIRECTION	0b11101011
#define RIGHT_MOTOR				RC2			//port for thr right motor
#define LEFT_MOTOR				RC4			//port for the left motor
#define MOTOR_PORT				PORTC
#define MOTORS_OFF				0b11101011
#define MOTORS_ON				0b00010100

//defintions of our keys
#define	BACKSPACE	KEY14
#define RETURN		KEY15
#define NE_KEY		KEY04
#define SW_KEY		KEY08

#define KEY_DIGIT_1	KEY01
#define KEY_DIGIT_2	KEY02 
#define KEY_DIGIT_3	KEY03
#define KEY_DIGIT_4	KEY05
#define KEY_DIGIT_5	KEY06
#define KEY_DIGIT_6	KEY07
#define KEY_DIGIT_7	KEY09
#define KEY_DIGIT_8	KEY10
#define KEY_DIGIT_9	KEY11
#define KEY_DIGIT_0	KEY13

//register to get data from the serial port
#define SERIAL_DATA_REG RCREG
#define SERIAL_FLAG 		RCIF


//******************
//	Function Prototypes
//******************
char digits_to_num(char *digits);
void num_to_digits(unsigned int num, char* digits);
void init_interrupt(void);
void keypress_callback(char key);
void update_LCD(void);
void strcat(char *dest, char *src);
void gps_add(unsigned int *ret_lat, unsigned int *ret_lon);
void receive_byte(char*);
void initialize_serial(void);
void serial_buffer(void);
void check_position(void);
char ASCIItoNum(char);
void turn_car(int);
int arctan (int,int);
int my_abs (int);
void done (void);
void stop_motors(void);
void start_motors(void);

//******************
//	Global Variables
//******************
//used for when user is entering input
char location[4];
char cur_location;	//the zero based location of the current digit you are editing
char input_type;
char loopindex;

//change in latitude and longitude that user entered
char delta_lat;
char delta_lon;

//the lat/lon coordinates of our destination location
bank1 unsigned int dest_lat;
bank1 unsigned int dest_lon;

//our current lat/lon coordintates
unsigned int cur_lat;
unsigned int cur_lon;

int _ns_speed; // Signed north/south velocity (negative number => south)
int _ew_speed; // Signed east/west velocity (negative number => west)
char _good_data; // A flag, zero when position stream is lost, and one when position is known
char _position_updated; // A flag, which when set indicates that the position has been updated

void main() {

	char digits[6];

	//turn off the motors, just incase they are on
	MOTOR_PORT_CTRL = MOTOR_PORT_DIRECTION;
	MOTOR_PORT &=MOTORS_OFF;
	
	LCD_init();
	init_interrupt();

	//get location (cannot put this in a function because it overflows the stack
	for (loopindex=0;loopindex<4;loopindex++) {
		location[loopindex] = BLANK;
	}

	write_char_to_LCD(0, START_LINE_1);	// start on line 1
	write_string_to_LCD(LOCATION_PROMPT);
	input_type = LATITUDE;

	while (RBIE == ON) {
		update_LCD();
	}
	//end get location

	//calculate our destination location
	gps_add(&dest_lat, &dest_lon);	
		
	write_char_to_LCD(0, CLEAR_DISPLAY);
	write_string_to_LCD(GETTING_SIGNAL);

	// Initialize the serial port
	initialize_serial();

	while (ON)
	{
		if (_position_updated) {
			//for latitude: write our current location, followed by our 
			//destination, follow by our speed
			write_char_to_LCD(0, START_LINE_1);
			num_to_digits(cur_lat, digits);
			write_string_to_LCD(digits);
			write_char_to_LCD(1, ' ');
			num_to_digits(cur_lon, digits);
			write_string_to_LCD(digits);
			write_char_to_LCD(1, ' ');
			num_to_digits((unsigned int)_ns_speed, digits);
			write_string_to_LCD(digits);	

			//for longitude on line 2: write our current location, followed by our 
			//destination, follow by our speed
			write_char_to_LCD(0, START_LINE_2);
			num_to_digits(dest_lat, digits);
			write_string_to_LCD(digits);
			write_char_to_LCD(1, ' ');
			num_to_digits(dest_lon, digits);
			write_string_to_LCD(digits);
			write_char_to_LCD(1, ' ');
			num_to_digits((unsigned int)_ew_speed, digits);
			write_string_to_LCD(digits);	
		}		
    	check_position();
	}
}

//
// Routine to handle all interrupts and call the appropriate one
//
#pragma interrupt_level 1
void interrupt inter () {
	// interupt handler
	if (RBIF && PEIE != ON) {
		handle_keyevent(keypress_callback);
	}
	// If the serial port has data, call the serial buffer function
	if (SERIAL_FLAG && PEIE)
	{
		serial_buffer();
		SERIAL_FLAG = NO_DATA_WAITING;
	}
}

//
//	Initilize the interrupts that we need for the program
//
void init_interrupt(void) {
	//	key strokes	
	PORTB = 0;		//ckear portb
	RBPU = OFF;		//enable PORTB pullup resistors
	
	TRISB = ENABLE_KEYPAD_INTERRUPT;	// Set RB3-0 as output
	PORTB = ENABLE_KEYPAD_INTERRUPT;

	RBIE = ON;		// turn on RB PORT Change interrupt
	RBIF = OFF;		// Clear interrupt flag
	GIE = ON;		// enable global interupt

}

//
//	This function is called after a key has been pressed (and succesfully debounced)
//	The parameter is the key that is pressed as defined in key.h--KEY01-KEY16
void keypress_callback(char key) {
	char value;
	
	if (key == NO_KEY_PRESSED) {
		return;
	} else if (key == BACKSPACE) {
		if (cur_location == 0) {
			location[cur_location] = BLANK;
		} else {
			location[cur_location - 1] = BLANK;
			cur_location--;
		}
	} else if (key == RETURN) {
		if (location[DIRECTION] != BLANK) {
			if (input_type == LATITUDE) {
				//save the state of longitude
				delta_lat = digits_to_num(location);
				for (loopindex=0;loopindex<4;loopindex++) {
					location[loopindex] = BLANK;
				}
				cur_location = 0;
				input_type = LONGITUDE;
			} else { //longitude
				//save the state of the latitude
				delta_lon = digits_to_num(location);
				//done getting input, don't listen for key presses anymore
				RBIE = OFF;
			}
		}
	} else if (key == NE_KEY) {
		if (input_type == LONGITUDE) {
			location[DIRECTION] = EAST;
		} else {
			location[DIRECTION] = NORTH;
		}	
	} else if (key == SW_KEY) {
		if (input_type == LONGITUDE) {
			location[DIRECTION] = WEST;
		} else {
			location[DIRECTION] = SOUTH;
		}
	} else { // a number
		if ((cur_location < NUM_DIGITS -1) || (location[2] == BLANK))  {
			switch (key) {
				case KEY_DIGIT_1:
					value = '1';
					break;
				case KEY_DIGIT_2:
					value = '2';
					break;
				case KEY_DIGIT_3:
					value = '3';
					break;
				case KEY_DIGIT_4:
					value = '4';
					break;
				case KEY_DIGIT_5:
					value = '5';
					break;
				case KEY_DIGIT_6:
					value = '6';
					break;
				case KEY_DIGIT_7:
					value = '7';
					break;
				case KEY_DIGIT_8:
					value = '8';
					break;
				case KEY_DIGIT_9:
					value = '9';
					break;
				case KEY_DIGIT_0:
					value = '0';
					break;
				default:
					value = BLANK;
			}					
			location[cur_location] = value;
			cur_location++;
		}
	}
}

//
//	Updates the LCD while input is being entered
//
void update_LCD() {
	write_char_to_LCD(0, START_LINE_2);	// start on line 2
	write_char_to_LCD(1, location[0]);
	write_char_to_LCD(1, location[1]);
	write_char_to_LCD(1, location[2]);
	write_char_to_LCD(1, ' ');	
	write_char_to_LCD(1, location[DIRECTION]);	
	
}

//
//converts an interger to an array of characters
//
void num_to_digits(unsigned int num, char* digits) {
	unsigned int factor = 10000;
	char i = 0;
	digits[5] = '\0';
	
	while (factor > 0) {
		digits[i] = num/factor + '0';
		num %= factor;
		i++;
		factor /=10;
	}
}

//
//converts a character array to 3 numbers to its corresponding integer value
//
char digits_to_num(char *digits) {
	int i;
	char factor = 1;
	char retval=0;
	
	for (i=2;i>=0;i--) {
		if (digits[i] != BLANK) {
			retval+= (digits[i]-'0') * factor;
			factor *=10;
		} 
	}
	return retval;
}

//
//	Adds the offset provided by the user to the current location, so that we know where we need to stop at
//
void gps_add(unsigned int *ret_lat, unsigned int *ret_lon) {
	while(cur_lat == 0);
	*ret_lat = cur_lat + (delta_lat/LAT_FOOT_PER_MINUTE);
	*ret_lon = cur_lon + (delta_lon/LON_FOOT_PER_MINUTE);
}

// check_position: Compare the current position to the desired position.  Adjust course accordingly.
//   Uses _position_updated flag to determine if the position has been updated since the last time it
//   was called.  This prevents the function from continually correcting course based on a previous location.
void check_position(void)
{

  int ns_diff; // difference between source and destination
  int ew_diff; // difference between source and destination
  int bearing_dir;  // the direction for the bearing
  int cur_dir; // the current direction
  static char turn_count = 0;
  if (_good_data == CLEAR)
  {
    // We don't have a solid location. Stop the motors.
    stop_motors();
  } else
  {
    // We have a solid location.  Start the motors.
    start_motors();
  }

  // Only check the position if it has changed since last time
  if (_position_updated == SET)
  {
    // Find the angle between the direction we're going and the direction we need to go
    // First, find the bearing from the current location to the destination
    ns_diff = dest_lat - cur_lat; // amount north we need to go
    ew_diff = dest_lon - cur_lon; // amount east we need to go
    
    // Check if we are at our destination
    if (ns_diff == ZERO && ew_diff == ZERO)
    {
      // If we made it, we're done.
      done();
    }

    if (ew_diff != ZERO) // we don't want to divide by zero
    {
      bearing_dir = arctan(ns_diff,ew_diff);
    }

    // Second, find the current direction (this parallels the above as long as we're relatively
    // close to the equator [things basically square])
    if (_ew_speed != ZERO)
    {
      cur_dir = arctan(_ns_speed,_ew_speed);
    }

    if (++turn_count % NUM_LOOPS_PER_TURN == 0) {
	    turn_count = 0;
	    // Finally, turn the car to the desired angle by turning it an amount equal to
	    // the difference between the two angles
	    turn_car(cur_dir - bearing_dir);
	}
  }

  // Clear the _position_updated flag so that we don't run this again unnecessarily
  _position_updated = CLEAR;

}

// done: the car has found its destination
void done (void)
{
  // Found the destination

  // Display "Done" on the LCD
  write_char_to_LCD(0, CLEAR_DISPLAY);
  write_string_to_LCD(DESTINATION_REACHED);
  
  // Stop.
  stop_motors();

  // Loop forever
  while (ON) {};

}

// stop_motors: All stop.
void stop_motors(void)
{
  // stop the motors (both of them)
	MOTOR_PORT &=MOTORS_OFF;
}

// start_motors: All start.
void start_motors(void)
{
	// start the motors (both of them)
	MOTOR_PORT |=MOTORS_ON;
}

// turn_car: turn the car the specified number of degrees
void turn_car(int angle)
{
  unsigned int i;
  // Relate the angle to a time
  // Use the MS_PER_DEGREE constant to determine the number of miliseconds to be turning

  LEFT_MOTOR = OFF;
  for(i=0;i<MS_PER_DEGREE*angle;i++) {
	  wait_15us();
  }
  LEFT_MOTOR = ON;
    
}

// arctan: Returns a rough approximation of the arctangent of the parameter, in degrees.
int arctan (int numerator, int denominator)
{
  int work = 0;
  int ratio = 0;
  int need90shift = CLEAR; // set if the result must be shifted by 90 degrees

  if (denominator != ZERO ) // We don't want to divide by zero
  {
    if ( my_abs(denominator) > my_abs(numerator) )
    {
      ratio = my_abs(denominator / numerator);
      need90shift = SET;
    } else
    {
      ratio = my_abs(numerator / denominator);
    }

    // This algorithm is based off a PASCAL routine from http://www.convict.lu/Jeunes/Math/arctan.htm
    work = ((-150 + 310*ratio - (ratio*ratio/2) - (ratio*ratio/3))/50 + 5)/10;

    if (need90shift == SET)  // If the angle is above 45 degrees, it needs special treatment
    {
      work = 90 - work;
    }
    
    if (numerator >= 0 && denominator > 0)
    {
      // first quadrant
      work = 90 - work;
    } else if (numerator >= 0 && denominator < 0 )
    {
      // In second quadrant
      work = 270 + work;
    } else if (numerator < 0 && denominator < 0 )
    {
      // third quadrant
      work = 270 - work;
    } else if (numerator < 0 && denominator > 0 )
    {
      // fourth quadrant
      work = 90 + work;
    }

  } else {
    // Case if the denominator is zero (the angle is 0 or 180 degrees (compass direction)
    
    if (numerator >= 0) // Or equal, so that it goes straight
    {
      // Straight ahead
      work = 0;
    } else
    {
      // Otherwise, must be straight behind
      work = 180;
    }

  }
  return work;

}

// my_abs: cheap absolute value function
int my_abs(int input)
{
  if (input >= 0)
  {
    return input;
  } else
  {
    return -input;
  }
}

// serial_buffer: Read in a sentence of the Garmin "simple text" position data
void serial_buffer(void)
{
  // Local static variables
  // sentence_position: Current character being worked on in the Simple Text sentence.  One-index.
  // A value of zero means that the position is not yet known (as when the device first starts up)
  // The value is also set to zero when the null position character is received from the GPS ('_')
  // indicating that the GPS has lost its position.
  static unsigned char sentence_position = ZERO;

  // These position variables are kept private until a complete new location is known, at
  // which point, they are copied into the global position variables
  static unsigned int flip_lat_minutes;
  static unsigned int flip_lon_minutes;
  static char flip_ns_speed; // Signed north/south velocity (negative number => south)
  static char flip_ns_speed_dir;
  static char flip_ew_speed; // Signed east/west velocity (negative number => west)
  static char flip_ew_speed_dir;

  // Normal local variables
  char cwork; // Character pulled from serial port that has yet to be processed

  // Get the byte of data from the serial port
  receive_byte(&cwork);
 // write_char_to_LCD(1, cwork);

  // Check if there is a new sentence
  if (cwork == SENTENCE_START)
  {
    _good_data = POSITION_KNOWN;
    sentence_position = FIRST_CHAR;
  }

  // Check if the position is lost
  if (cwork == NULL_DATA)
  {
    // First, set the flag so that the motors stop
    _good_data = POSITION_LOST;

    // Next, set the sentence position to zero
    sentence_position = ZERO;
  }

  // Convert ASCII to a number
  cwork = ASCIItoNum(cwork);

  // Switch based on sentence position
  switch (sentence_position)
  {
    case LAT_MIN_START:
         flip_lat_minutes = 10000*cwork;
         break;
    case LAT_MIN_START+1:
         flip_lat_minutes += 1000*cwork;
         break;
    case LAT_MIN_START+2:
         flip_lat_minutes += 100*cwork;
         break;
    case LAT_MIN_START+3:
         flip_lat_minutes += 10*cwork;
         break;
    case LAT_MIN_START+4:
         flip_lat_minutes += cwork;
         break;
    case LON_MIN_START:
         flip_lon_minutes = 10000*cwork;
         break;
    case LON_MIN_START+1:
         flip_lon_minutes += 1000*cwork;
         break;
    case LON_MIN_START+2:
         flip_lon_minutes += 100*cwork;
         break;
    case LON_MIN_START+3:
         flip_lon_minutes += 10*cwork;
         break;
    case LON_MIN_START+4:
         flip_lon_minutes += cwork;
         break;
    case EW_SPEED_DIR:
         flip_ew_speed_dir = cwork;
         break;
    case EW_SPEED_START:
         flip_ew_speed = 10 * cwork;
         break;
    case EW_SPEED_START+1:
         flip_ew_speed += cwork;
         break;
    case NS_SPEED_DIR:
         flip_ns_speed_dir = cwork;
         break;
    case NS_SPEED_START:
         flip_ns_speed = 10 * cwork;
         break;
    case NS_SPEED_START+1:
         flip_ns_speed += cwork;
         break;
    case END_OF_SENTENCE:
         // If we are at the end of the sentence, copy the temporary position values into the
         // the global position variables
         cur_lat = flip_lat_minutes;
         cur_lon = flip_lon_minutes;
         if (flip_ew_speed_dir == SET)
         {
           // East
           _ew_speed = flip_ew_speed;
         } else
         {
           // West
           _ew_speed = -flip_ew_speed;
         }
         if (flip_ns_speed_dir == SET)
         {
           // North
           _ns_speed = flip_ns_speed;
         } else
         {
           // South
           _ns_speed = -flip_ns_speed;
         }
         _position_updated = SET;
    default:
         break;
  }

  // Increment the sentence position
  sentence_position++;

}

// ASCIItoNum:  Converts the ASCII representation of a number to a binary representation
// Note: no boundary checking
char ASCIItoNum(char input)
{
  char localWork;

  localWork = input - ASCII_NUM_OFFSET;

  return localWork;
}

// initialize_serial: Sets up the serial port to be able to receive
//
void initialize_serial(void)
{

	// Set the flags
	SPEN = ENABLED;	// serial port
	SYNC = DISABLED;
	BRGH = ENABLED; // high speed
	RX9 = DISABLED;
	CREN = ENABLED;	// continuous mode
	RCIE = ENABLED; // enable interrupts

	// NOTE: No data direction need be specified, as PORTC
	// defaults to being an input

	// Set up the baud rate
	SPBRG = (1000000/SERIAL_RATE_BPS*CLOCK_SPEED_MHZ - 16)/16;

	PEIE = ON; 		// peripheral interrupts

	return;
}

// receive_byte: Returns a byte from the serial port in the
// 					function parameter pointer
//
void receive_byte(char* pSerialByte)
{
	// Wait for a byte to be received
	while (SERIAL_FLAG == NO_DATA_WAITING) {};

	// When there's a byte ready, copy it
	*pSerialByte = SERIAL_DATA_REG;
	PORTD = SERIAL_DATA_REG;
	// Clear the byte-received flag
	SERIAL_FLAG = NO_DATA_WAITING;

	return;

} //receive_byte()