/*
* gpsMKa.c
*
* Created: 11/15/2012 12:02:38 PM
*  Author: mkanning
*/

#include <stdbool.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "gps.h"
#include "serial.h"
#include "../config.h"
#include "led.h"



// Circular buffer for incoming data
uint8_t nmeaBuffer[NMEABUFFER_SIZE];

// Location of parser in the buffer
uint8_t nmeaBufferParsePosition = 0;

// Location of receive byte interrupt in the buffer
volatile uint16_t nmeaBufferDataPosition = 0;


/*
volatile uint8_t charTest;
ISR(USART1_RX_vect)
{
	serial0_sendChar(UDR1);
	
}*/

// holds the byte ALREADY PARSED. includes starting character
int bytesReceived = 0;

//data (and checksum) of most recent transmission
char data[16];

//used to skip over bytes during parse
int skipBytes = 0;

//used to index data arrays during data collection
int numBytes = 0;

//variables to store data from transmission
//least significant digit is stored at location 0 of arrays
char tramsmissionType[7];

char timestamp[12];	//hhmmss.ss
char* get_timestamp() {
	return timestamp;
}
	
char latitude[14];	//lllll.lla
char* get_latitude() {
	return latitude;
}

char longitude[14];	//yyyyy.yyb
char* get_longitude() {
	return longitude;
}


char quality;		//quality for GGA and validity for RMC
char numSatellites[4];
char* getNumSatelites() {
	return numSatellites;
}

char hdop[6];		//xx.x
char* get_hdop() {
	return hdop;
}

char altitude[10];	//xxxxxx.x
char wgs84Height[8];	//sxxx.x
char lastUpdated[8];	//blank - included for testing
char stationID[8];	//blank - included for testing
char checksum[3];	//xx

char knots[8];		//xxx.xx
char* get_speedKnots() {
	return knots;
}

char course[8];		//xxx.x
char* get_course() {
	return course;
}
	
char dayofmonth[9];	//ddmmyy
char* get_dayofmonth() {
	return dayofmonth;
}

char variation[9];	//xxx.xb
int calculatedChecksum;
int receivedChecksum;

char convertBuf1[15];
char convertBuf2[15];

// transmission state machine
enum decodeState {
	//shared fields
	INITIALIZE=0,
	GET_TYPE,
	GPS_CHECKSUM,	//XOR of all the bytes between the $ and the * (not including the delimiters themselves), written in hexadecimal
	//GGA data fields
	GGA_TIME,
	GGA_LATITUDE,
	GGA_LONGITUDE,
	GGA_QUALITY,
	GGA_SATELLITES,
	GGA_HDOP,
	GGA_ALTITUDE,
	GGA_WGS84,
	GGA_LAST_UPDATE,
	GGA_STATION_ID,
	//RMC data fields
	RMC_TIME,
	RMC_VALIDITY,
	RMC_LATITUDE,
	RMC_LONGITUDE,
	RMC_KNOTS,
	RMC_COURSE,
	RMC_DATE,
	RMC_MAG_VARIATION,
	
}decodeState;


char debugBuff[128];

ISR(USART1_RX_vect)
{
	nmeaBuffer[nmeaBufferDataPosition % NMEABUFFER_SIZE] = UDR1;
	nmeaBufferDataPosition = (nmeaBufferDataPosition + 1) % NMEABUFFER_SIZE;
	//serial0_sendChar(UDR1);
	//snprintf(debugBuff, 32, "GPS: bdp: %d, bpp: %d decodestate: %u \r\n", nmeaBufferDataPosition, nmeaBufferParsePosition, decodeState);
	//serial0_sendString((debugBuff));
}


// Could inline if program space available
static void setParserState(uint8_t state)
{
	decodeState = state;

	
	// If resetting, clear vars
	if(state == INITIALIZE)
	{
		calculatedChecksum = 0;
	}
	
	// Every time we change state, we have parsed a byte
	nmeaBufferParsePosition = (nmeaBufferParsePosition + 1) % NMEABUFFER_SIZE;
}



//// MKa GPS transmission parser START
void parse_gps_transmission(void){
	
	// Pull byte off of the buffer
	char byte;
	
	
	
	while(nmeaBufferDataPosition != nmeaBufferParsePosition) {
	led_on(LED_ACTIVITY);
		
		byte = nmeaBuffer[nmeaBufferParsePosition];
		
		//snprintf(debugBuff, 64, "GPSParse: byte [%c] decodestate: %u bp: %u pp: %u\r\n",  byte, decodeState, nmeaBufferDataPosition, nmeaBufferParsePosition);
		//serial0_sendString((debugBuff));
		
		if(decodeState == INITIALIZE) //start of transmission sentence
		{
			if(byte == '$') {
				#ifdef DEBUG_NMEA
				serial0_sendString("found $\r\n");
				#endif
				
				setParserState(GET_TYPE);
				numBytes = 0; //prep for next phases
				skipBytes = 0;
				calculatedChecksum = 0;
			}		
			
			else {
				setParserState(INITIALIZE);
			}
		}
	
		//parse transmission type
		else if (decodeState == GET_TYPE)
		{
			tramsmissionType[numBytes] = byte;
			numBytes++;
			
			#ifdef DEBUG_NMEA
			serial0_sendString("stored a type byte\r\n");
			#endif
			
			if(byte == ',') //end of this data type
			{
				tramsmissionType[5] = 0x00;
				
				if (tramsmissionType[2] == 'G' &&
				tramsmissionType[3] == 'G' &&
				tramsmissionType[4] == 'A')
				{
					setParserState(GGA_TIME);
					numBytes = 0;
				}
				else if (tramsmissionType[2] == 'R' &&
				tramsmissionType[3] == 'M' &&
				tramsmissionType[4] == 'C')
				{
					setParserState(RMC_TIME);
					numBytes = 0;
				}
				else //this is an invalid transmission type
				{
					setParserState(INITIALIZE);
				}
			}
			else {
				// continue
				setParserState(GET_TYPE);
			}
			
		}
	
		///parses GGA transmissions START
		/// $--GGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*xx
		//timestamp
		else if (decodeState == GGA_TIME)
		{
			if (byte == ',') //end of this data type
			{
				timestamp[4] = 0x00; // Cut off at 4 (no seconds) for APRS
				setParserState(GGA_LATITUDE);
				skipBytes = 0; //prep for next phase of parse
				numBytes = 0;
			}
			else //store data
			{
				#ifdef DEBUG_NMEA
				serial0_sendString("found GGA time byte\r\n");
				#endif
				
				setParserState(GGA_TIME);
				timestamp[numBytes] = byte; //byte; //adjust number of bytes to fit array
				numBytes++;
			}
		}
	
		//latitude
		else if (decodeState == GGA_LATITUDE)
		{
			if (byte == ',' && skipBytes == 0) //discard this byte
			{
				#ifdef DEBUG_NMEA
				serial0_sendString("found lat skip byte\r\n");
				#endif
				
				skipBytes = 1;
				setParserState(GGA_LATITUDE);
			}
			else if (byte == ',') //end of this data type
			{
				
				latitude[7] = 0x00; // null terminate
				
				
				// First 2 bytes
				//convertBuf1[0] = latitude[0];
				//convertBuf1[1] = latitude[1];
				//convertBuf1[2] = '\0';
				//strncpy(convertBuf2, latitude, 7);
				//convertBuf2[7] = '\0';
				
				
				setParserState(GGA_LONGITUDE);
				skipBytes = 0; //prep for next phase of parse
				numBytes = 0;
			}
			else //store data
			{
				#ifdef DEBUG_NMEA
				serial0_sendString("found lat byte\r\n");
				#endif
				
				latitude[numBytes] = byte; //adjust number of bytes to fit array
				numBytes++;
				setParserState(GGA_LATITUDE);
			}
		}
	
		//longitude
		else if (decodeState == GGA_LONGITUDE)
		{
			if (byte == ',' && skipBytes == 0) //discard this byte
			{
				#ifdef DEBUG_NMEA
				serial0_sendString("found long skip byte\r\n");
				#endif
				
				skipBytes = 1;
				setParserState(GGA_LONGITUDE);
			}
			else if (byte == ',') //end of this data type
			{
				longitude[8] = 0x00;
				setParserState(GGA_QUALITY);
				numBytes = 0; //prep for next phase of parse
				skipBytes = 0;
			}
			else //store data
			{
				#ifdef DEBUG_NMEA
				serial0_sendString("found long byte\r\n");
				#endif
				
				longitude[numBytes] = byte; //adjust number of bytes to fit array
				numBytes++;
				setParserState(GGA_LONGITUDE);
			}
		}
	
		//GGA quality
		else if (decodeState == GGA_QUALITY)
		{
			if (byte == ',') //end of this data type
			{
				setParserState(GGA_SATELLITES);
				numBytes = 0; //prep for next phase of parse
			}
			else //store data
			{
				#ifdef DEBUG_NMEA
				serial0_sendString("found quality byte\r\n");
				#endif
				
				quality = byte; //maybe reset if invalid data ??
				setParserState(GGA_QUALITY);
			}
		}
	
		//number of satellites
		else if (decodeState == GGA_SATELLITES)
		{
			if (byte == ',') //end of this data type
			{
				numSatellites[numBytes] = 0x00;
				setParserState(GGA_HDOP);
				numBytes = 0; //prep for next phase of parse
			}
			else //store data
			{
				numSatellites[numBytes] = byte; //adjust number of bytes to fit array
				numBytes++;
				setParserState(GGA_SATELLITES);
			}
		}
	
		//HDOP
		else if (decodeState == GGA_HDOP)
		{
			if (byte == ',' ) //end of this data type
			{
				hdop[numBytes] = 0x00;
				setParserState(GGA_ALTITUDE);
				numBytes = 0; //prep for next phase of parse
				skipBytes = 0;
			}
			else //store data
			{
				hdop[numBytes] = byte; //adjust number of bytes to fit array
				numBytes++;
				setParserState(GGA_HDOP);
			}
		}
	
		//altitude
		else if (decodeState == GGA_ALTITUDE)
		{
			if (byte == ',' && skipBytes == 0) //discard this byte
			{
				skipBytes = 1;
				setParserState(GGA_ALTITUDE);
			}
			else if(byte == ',') //end of this data type
			{
				altitude[numBytes] = 0x00;
				setParserState(GGA_WGS84);
				numBytes = 0; //prep for next phase of parse
			}
			else //store data
			{
				altitude[numBytes] = byte; //adjust number of bytes to fit array
				numBytes++;
				setParserState(GGA_ALTITUDE);
			}
		}
	
		//WGS84 Height
		else if (decodeState == GGA_WGS84)
		{
			if (byte == ',' && skipBytes == 0) //discard this byte
			{
				skipBytes = 1;
				setParserState(GGA_WGS84);
			}
			else if(byte == ',') //end of this data type
			{
				wgs84Height[numBytes] = 0x00;
				setParserState(GGA_LAST_UPDATE);
				skipBytes = 0; //prep for next phase of parse
				numBytes = 0;
			}
			else //store data
			{
				wgs84Height[numBytes] = byte; //adjust number of bytes to fit array
				numBytes++;
				setParserState(GGA_WGS84);
			}
		}
	
		//last GGA DGPS update
		else if (decodeState == GGA_LAST_UPDATE)
		{
			if (byte == ',') //end of this data type
			{
				lastUpdated[numBytes] = 0x00;
				setParserState(GGA_STATION_ID);
				numBytes = 0; //prep for next phase of parse
			}
			else //store data - this should be blank
			{
				lastUpdated[numBytes] = byte; //adjust number of bytes to fit array
				numBytes++;
				setParserState(GGA_LAST_UPDATE);
			}
		}
	
		//GGA DGPS station ID
		else if (decodeState == GGA_STATION_ID)
		{
			if (byte == ',' || byte == '*') //end of this data type
			{
				stationID[numBytes] = 0x00;
				setParserState(GPS_CHECKSUM);
				numBytes = 0; //prep for next phase of parse
			}
			else //store data - this should be blank
			{
				stationID[numBytes] = byte; //adjust number of bytes to fit array
				numBytes++;
				setParserState(GGA_STATION_ID);
			}
		}
		///parses GGA transmissions END
	
		/// $GPRMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,ddmmyy,x.x,a*hh
		///parses RMC transmissions
		//time
		// emz: commented setter, GMC time better?
		else if(decodeState == RMC_TIME)
		{
			if (byte == ',') //end of this data type
			{
				//timestamp[numBytes] = 0x00;
				setParserState(RMC_VALIDITY);
				numBytes = 0; //prep for next phase of parse
			}
			else //store data
			{
				#ifdef DEBUG_NMEA
				serial0_sendString("found time byte\r\n");
				#endif
				
				//timestamp[numBytes] = byte; //adjust number of bytes to fit array
				numBytes++;
				setParserState(RMC_TIME);
			}
		}
	
		//validity
		// not needed? dupe gga
		else if(decodeState == RMC_VALIDITY)
		{
			if (byte == ',') //end of this data type
			{
				setParserState(RMC_LATITUDE);
				skipBytes = 0; //prep for next phase of parse
				numBytes = 0;
			}
			else //store data
			{
				#ifdef DEBUG_NMEA
				serial0_sendString("found valid byte\r\n");
				#endif
				
				//quality = byte;
				numBytes++;
				setParserState(RMC_VALIDITY);
			}
		}
	
		//latitude RMC (we don't need this, commented out setter)
		else if(decodeState == RMC_LATITUDE)
		{
			if (byte == ',' && skipBytes == 0) //discard this byte
			{
				#ifdef DEBUG_NMEA
				serial0_sendString("found lat skip byte\r\n");
				#endif
				
				skipBytes = 1; 
				setParserState(RMC_LATITUDE);
			}
			else if (byte == ',') //end of this data type
			{
				setParserState(RMC_LONGITUDE);
				skipBytes = 0; //prep for next phase of parse
				numBytes = 0;
			}
			else //store data
			{
				#ifdef DEBUG_NMEA
				serial0_sendString("found lat byte\r\n");
				#endif
				
				//latitude[numBytes]= byte; //adjust number of bytes to fit array
				numBytes++;
				setParserState(RMC_LATITUDE);
			}
		}
	
		//longitude RMC (we don't need this, commented out setter)
		else if(decodeState == RMC_LONGITUDE)
		{
			if (byte == ',' && skipBytes == 0) //discard this byte
			{
				#ifdef DEBUG_NMEA
				serial0_sendString("found long byte\r\n");
				#endif
				
				skipBytes = 1; 
				setParserState(RMC_LONGITUDE);
			}
			else if (byte == ',') //end of this data type
			{
				setParserState(RMC_KNOTS);
				skipBytes = 0;
				numBytes = 0;
			}
			else //store data
			{
				#ifdef DEBUG_NMEA
				serial0_sendString("found long byte\r\n");
				#endif
				
				//longitude[numBytes]= byte; //adjust number of bytes to fit array
				numBytes++;
				setParserState(RMC_LONGITUDE);
			}
		}
	
		//knots
		else if(decodeState == RMC_KNOTS)
		{
			if (byte == ',') //end of this data type
			{
				knots[numBytes] = 0x00;
				setParserState(RMC_COURSE);
				numBytes = 0; //prep for next phase of parse
			}
			else //store data
			{
				setParserState(RMC_KNOTS);
				knots[numBytes]= byte; //adjust number of bytes to fit array
				numBytes++;
			}
		}
	
		//course
		else if(decodeState == RMC_COURSE)
		{
			if (byte == ',') //end of this data type
			{
				course[numBytes] = 0x00;
				setParserState(RMC_DATE);
				numBytes = 0; //prep for next phase of parse
			}
			else //store data
			{
				setParserState(RMC_COURSE);
				course[numBytes] = byte; //adjust number of bytes to fit array
				numBytes++;
			}
		}
	
		//date
		else if(decodeState == RMC_DATE)
		{
			if (byte == ',') //end of this data type
			{
				// Cut it off at day of month. Also has month and year if we ever need it.
				dayofmonth[2] = 0x00;
				setParserState(RMC_MAG_VARIATION);
				skipBytes = 0; //prep for next phase of parse
				numBytes = 0;
			}
			else //store data
			{
				setParserState(RMC_DATE);
				dayofmonth[numBytes] = byte; //adjust number of bytes to fit array
				numBytes++;
			}
		}
	
		//magnetic variation
		else if(decodeState == RMC_MAG_VARIATION)
		{
			if (byte == '*') //end of this data type
			{
				variation[numBytes] = 0x00;
				setParserState(GPS_CHECKSUM);
				numBytes = 0; //prep for next phase of parse
			}
			else //store data
			{
				setParserState(RMC_MAG_VARIATION);
				variation[numBytes] = byte; //adjust number of bytes to fit array
				numBytes++;
			}
		}
		///parses RMC transmissions END
	
	
		//checksum
		else if (decodeState == GPS_CHECKSUM)
		{
			if (numBytes == 2) //end of data - terminating character ??
			{
				//checksum calculator for testing http://www.hhhh.org/wiml/proj/nmeaxor.html
				//TODO: must determine what to do with correct and incorrect messages
				receivedChecksum = checksum[0] + (checksum[1]*16);	//convert bytes to int
				if(calculatedChecksum==receivedChecksum)
				{
				
				}
				else
				{
				
				}
				#ifdef DEBUG_NMEA
				serial0_sendString("OMG GOT TO CHECKSUM!\r\n");
				#endif
				
				serial0_sendString((debugBuff));
				setParserState(INITIALIZE);
				numBytes = 0; //prep for next phase of parse
			}
			else //store data
			{
				setParserState(GPS_CHECKSUM);
				checksum[numBytes] = byte; //adjust number of bytes to fit array
				numBytes++;
			}
		}
		else {
			setParserState(INITIALIZE);
		}
	
		if (decodeState!=GPS_CHECKSUM && decodeState!=INITIALIZE)	//want bytes between '$' and '*'
		{
			//input byte into running checksum
			XORbyteWithChecksum(byte);
		}
		led_off(LED_ACTIVITY);
	}	
	

}
/// MKa GPS transmission parser END

void XORbyteWithChecksum(uint8_t byte){
	calculatedChecksum ^= (int)byte; //this may need to be re-coded
}