#include "stdafx.h"
#include "cphidgetgps.h"
#include "cusb.h"
#include "csocket.h"
#include "cthread.h"
#include <math.h>

// === Internal Functions === //
static int checkcrc(char *data, int crc);
static int parse_NMEA_data(char *data, CPhidgetGPSInfo *phid);
static int parse_GPS_packets(CPhidgetGPSInfo *phid);

//clearVars - sets all device variables to unknown state
CPHIDGETCLEARVARS(GPS)
	TESTPTR(phid);

	phid->fix = PUNI_BOOL;

	phid->heading = PUNK_DBL;
	phid->velocity = PUNK_DBL;
	phid->altitude = PUNK_DBL;
	phid->latitude = PUNK_DBL;
	phid->longitude = PUNK_DBL;

	phid->haveTime = PUNK_BOOL;
	phid->haveDate = PUNK_BOOL;

	return EPHIDGET_OK;
}

//initAfterOpen - sets up the initial state of an object, reading in packets from the device if needed
//				  used during attach initialization - on every attach
CPHIDGETINIT(GPS)
	unsigned char buffer[8] = { 0 };
	int readtries;
	TESTPTR(phid);

	phid->sckbuf_read=0;
	phid->sckbuf_write=0;
	ZEROMEM(phid->sckbuf, 256);

	phid->lastFix = PUNK_BOOL;
	phid->lastLatitude = PUNK_DBL;
	phid->lastLongitude = PUNK_DBL;
	phid->lastAltitude = PUNK_DBL;
	
	phid->fix = PUNK_BOOL;
	phid->heading = PUNK_DBL;
	phid->velocity = PUNK_DBL;
	phid->altitude = PUNK_DBL;
	phid->latitude = PUNK_DBL;
	phid->longitude = PUNK_DBL;

	phid->haveTime = PUNK_BOOL;
	phid->haveDate = PUNK_BOOL;

	//clean out the NMEA sentences
	ZEROMEM(&phid->GPSData, sizeof(NMEAData));

	//Examples for sending data. buffer[0] is dataLength, other spots are data
	//  continue in multiple packets if needed. Wait for ACK between sends
	//set factory defaults and reboot
	/*buffer[0]=0x02;buffer[1]=0x04;buffer[2]=0x01;
	CUSBSendPacket((CPhidgetHandle)phid, buffer);*/
	//set 10Hz update rate
	/*buffer[0]=0x03;buffer[1]=0x0e;buffer[2]=0x0a;buffer[3]=0x00;
	CUSBSendPacket((CPhidgetHandle)phid, buffer);*/
	//Get Ephemeris
	/*buffer[0]=0x02;buffer[1]=0x30;buffer[2]=0x00;
	CUSBSendPacket((CPhidgetHandle)phid, buffer);*/
	//Enable WAAS
	buffer[0]=0x03;buffer[1]=0x37;buffer[2]=0x01;buffer[2]=0x01;
	CUSBSendPacket((CPhidgetHandle)phid, buffer);
	//Query WAAS
	/*buffer[0]=0x01;buffer[1]=0x38;
	CUSBSendPacket((CPhidgetHandle)phid, buffer);*/

	//read some initial data - rate is 10Hz so we shouldn't have to wait long
	//This ensures we have received at least one GGA and one RMC
	readtries = 30; //250ms
	while((phid->fix==PUNK_BOOL || phid->haveTime==PUNK_BOOL || phid->haveDate==PUNK_BOOL) && readtries)
	{
		CPhidget_read((CPhidgetHandle)phid);
		readtries--;
	}
	if(phid->fix==PUNK_BOOL)
		phid->fix=PFALSE;

	return EPHIDGET_OK;
}

//dataInput - parses device packets
CPHIDGETDATA(GPS)
	int i=0;

	if (length < 0) return EPHIDGET_INVALIDARG;
	TESTPTR(phid);
	TESTPTR(buffer);

	//LOG(PHIDGET_LOG_DEBUG, "GPS Packet size: %d", buffer[0]);

	/* stick it in a buffer */
	for (i=0; i<buffer[0]; i++)
	{
		phid->sckbuf[phid->sckbuf_write++] = buffer[i+1];
	}
	parse_GPS_packets(phid);

	return EPHIDGET_OK;
}

//eventsAfterOpen - sends out an event for all valid data, used during attach initialization
CPHIDGETINITEVENTS(GPS)
	if(phid->fix != PUNK_BOOL)
		FIRE(PositionFixStatusChange, phid->fix);
	if (phid->fix == PTRUE && (phid->latitude != PUNK_DBL || phid->longitude != PUNK_DBL || phid->altitude != PUNK_DBL))
		FIRE(PositionChange, phid->latitude, phid->longitude, phid->altitude);
	return EPHIDGET_OK;
}

//getPacket - not used for GPS
CGETPACKET(GPS)
	return EPHIDGET_UNEXPECTED;
}

/* checks a CRC */
static int checkcrc(char *data, int crc) {
	unsigned int i=0;
	unsigned char check=0;
	for(i=1;i<strlen(data);i++)
		check^=data[i];
	if(check == crc)
		return 0;
	return 1;
}

/* this parses a full NMEA sentence */
static int parse_NMEA_data(char *data, CPhidgetGPSInfo *phid) {
	char *dataarray[50];
	int numfields = 0;
	int i,j,crc=0;
	double dintpart, decpart;
	int intpart;
	double tempD;

	//LOG(PHIDGET_LOG_DEBUG,"%s\n",data);

	/* fist check CRC if there is one */
	j = (int)strlen(data);
	for(i=0;i<j;i++) {
		if(data[i] == '*') {
			crc = strtol(data+i+1,NULL,16);
			data[i] = '\0';
			if(checkcrc(data, crc))
			{
				/* This will usually be caused by blocking in an event handler, which causes some packets to be dropped. 
					Safe to ignore. */
				LOG(PHIDGET_LOG_WARNING,"CRC Error parsing NMEA sentence.");
				return 1;
			}
			break;
		}
	}

	/* seperate out by commas */
	dataarray[0] = data;
	j = (int)strlen(data);
	for(i=0;i<j;i++) {
		if(data[i] == ',') {
			numfields++;
			dataarray[numfields] = data+i+1;
			data[i] = '\0';
		}
	}

	if(strlen(dataarray[0]) != 6)
	{
		LOG(PHIDGET_LOG_WARNING,"Bad sentence type.");
		return 1;
	}

	/* find the type of sentence */
	if(!strncmp("GGA",dataarray[0]+3,3)) {

		//time: HHMMSS.milliseconds
		if(strlen(dataarray[1]) >= 6) {
			decpart = modf(strtod(dataarray[1], NULL), &dintpart);
			intpart = (int)dintpart;
			phid->GPSData.GGA.time.tm_hour = (short)(intpart/10000);
			phid->GPSData.GGA.time.tm_min = (short)(intpart/100%100);
			phid->GPSData.GGA.time.tm_sec = (short)(intpart%100);
			phid->GPSData.GGA.time.tm_ms = (short)round(decpart*1000);
			phid->haveTime = PTRUE;
		}
		else
			phid->haveTime = PFALSE;

		/* convert lat/long to signed decimal degree format */
		if(strlen(dataarray[2])) {
			tempD = (int)(strtol(dataarray[2], NULL, 10) / 100) + (strtod((dataarray[2]+2),NULL) / 60);
			if(dataarray[3][0] == 'S')
				phid->GPSData.GGA.latitude = -tempD;
			else
				phid->GPSData.GGA.latitude = tempD;
		}
		else
			phid->GPSData.GGA.latitude = 0;

		if(strlen(dataarray[4])) {
			tempD = (int)(strtol(dataarray[4], NULL, 10) / 100) + (strtod((dataarray[4]+3),NULL) / 60);
			if(dataarray[5][0] == 'W')
				phid->GPSData.GGA.longitude = -tempD;
			else
				phid->GPSData.GGA.longitude = tempD;
		}
		else
			phid->GPSData.GGA.longitude = 0;

		phid->GPSData.GGA.fixQuality = (short)strtol(dataarray[6],NULL,10);
		phid->GPSData.GGA.numSatellites = (short)strtol(dataarray[7],NULL,10);
		phid->GPSData.GGA.horizontalDilution = strtod(dataarray[8],NULL);

		phid->GPSData.GGA.altitude = strtod(dataarray[9],NULL);
		phid->GPSData.GGA.heightOfGeoid = strtod(dataarray[11],NULL);

		//Set local variables for getters/events
		phid->fix = (phid->GPSData.GGA.fixQuality == 0) ? PFALSE : PTRUE;
		if(phid->fix)
		{
			phid->altitude = phid->GPSData.GGA.altitude;
			phid->latitude = phid->GPSData.GGA.latitude;
			phid->longitude = phid->GPSData.GGA.longitude;
		}
		else
		{
			phid->altitude = PUNK_DBL;
			phid->latitude = PUNK_DBL;
			phid->longitude = PUNK_DBL;
		}

		//Fix status changed
		if(phid->fix != phid->lastFix)
		{
			FIRE(PositionFixStatusChange, phid->fix);
			phid->lastFix = phid->fix;
		}

		/* only sends event if the fix is valid, and position has changed a bit */
		if (phid->fix == PTRUE
			&& (phid->latitude != phid->lastLatitude || phid->longitude != phid->lastLongitude || phid->altitude != phid->lastAltitude)
			)
		{
			FIRE(PositionChange, phid->latitude, phid->longitude, phid->altitude);
			phid->lastLatitude = phid->latitude;
			phid->lastLongitude = phid->longitude;
			phid->lastAltitude = phid->altitude;
		}
	}
	else if(!strncmp("GSA",dataarray[0]+3,3)) {
		phid->GPSData.GSA.mode = dataarray[1][0];
		phid->GPSData.GSA.fixType = (short)strtol(dataarray[2],NULL,10);
		for(i=0;i<12;i++)
			phid->GPSData.GSA.satUsed[i] = (short)strtol(dataarray[i+3],NULL,10);
		phid->GPSData.GSA.posnDilution = strtod(dataarray[15],NULL);
		phid->GPSData.GSA.horizDilution = strtod(dataarray[16],NULL);
		phid->GPSData.GSA.vertDilution = strtod(dataarray[17],NULL);
	}
	else if(!strncmp("GSV",dataarray[0]+3,3)) {
		int numSentences, sentenceNumber, numSats;

		numSentences = strtol(dataarray[1],NULL,10);
		sentenceNumber = strtol(dataarray[2],NULL,10);
		numSats = strtol(dataarray[3],NULL,10);

		phid->GPSData.GSV.satsInView = (short)numSats;
		for(i=0;i<(numSentences==sentenceNumber?numSats-(4*(numSentences-1)):4);i++) {
			phid->GPSData.GSV.satInfo[i+((sentenceNumber-1)*4)].ID = (short)strtol(dataarray[4+(i*4)],NULL,10);
			phid->GPSData.GSV.satInfo[i+((sentenceNumber-1)*4)].elevation = (short)strtol(dataarray[5+(i*4)],NULL,10);
			phid->GPSData.GSV.satInfo[i+((sentenceNumber-1)*4)].azimuth = strtol(dataarray[6+(i*4)],NULL,10);
			phid->GPSData.GSV.satInfo[i+((sentenceNumber-1)*4)].SNR = (short)strtol(dataarray[7+(i*4)],NULL,10);
		}
	}
	else if(!strncmp("RMC",dataarray[0]+3,3)) {
		if(strlen(dataarray[1])>=6) {
			decpart = modf(strtod(dataarray[1], NULL), &dintpart);
			intpart = (int)dintpart;
			phid->GPSData.RMC.time.tm_hour = (short)(intpart/10000);
			phid->GPSData.RMC.time.tm_min = (short)(intpart/100%100);
			phid->GPSData.RMC.time.tm_sec = (short)(intpart%100);
			phid->GPSData.RMC.time.tm_ms = (short)round(decpart*1000);
		}

		phid->GPSData.RMC.status = dataarray[2][0];

		/* convert lat/long to signed decimal degree format */
		if(strlen(dataarray[3])) {
			tempD = (int)(strtol(dataarray[3], NULL, 10) / 100) + (strtod((dataarray[3]+2),NULL) / 60);
			if(dataarray[4][0] == 'S')
				phid->GPSData.RMC.latitude = -tempD;
			else
				phid->GPSData.RMC.latitude = tempD;
		}
		else
			phid->GPSData.RMC.latitude = 0;

		if(strlen(dataarray[5])) {
			tempD = (int)(strtol(dataarray[5], NULL, 10) / 100) + (strtod((dataarray[5]+3),NULL) / 60);
			if(dataarray[6][0] == 'W')
				phid->GPSData.RMC.longitude = -tempD;
			else
				phid->GPSData.RMC.longitude = tempD;
		}
		else
			phid->GPSData.RMC.longitude = 0;

		phid->GPSData.RMC.speedKnots = strtod(dataarray[7],NULL);
		phid->GPSData.RMC.heading = strtod(dataarray[8],NULL);

		if(strlen(dataarray[9])>=6) {
			intpart = strtol(dataarray[9], NULL, 10);
			phid->GPSData.RMC.date.tm_mday = (short)(intpart/10000);
			phid->GPSData.RMC.date.tm_mon = (short)(intpart/100%100);
			phid->GPSData.RMC.date.tm_year = (short)(intpart%100) + 2000; //2-digit year, add 2000 years
			phid->haveDate = PTRUE;
		}
		else
			phid->haveDate = PFALSE;

		tempD = strtod(dataarray[10],NULL);
		if(dataarray[11][0] == 'W')
			phid->GPSData.RMC.magneticVariation = -tempD;
		else
			phid->GPSData.RMC.magneticVariation = tempD;

		phid->GPSData.RMC.mode = dataarray[12][0];

		if(phid->GPSData.RMC.status == 'A')
		{
			phid->velocity = phid->GPSData.RMC.speedKnots * 1.852; //convert to km/h
			phid->heading = phid->GPSData.RMC.heading;
		}
		else
		{
			phid->velocity = PUNK_DBL;
			phid->heading = PUNK_DBL;
		}
	}
	else if(!strncmp("VTG",dataarray[0]+3,3)) {
		phid->GPSData.VTG.trueHeading = strtod(dataarray[1],NULL);
		phid->GPSData.VTG.magneticHeading = strtod(dataarray[3],NULL);
		phid->GPSData.VTG.speedKnots = strtod(dataarray[5],NULL);
		phid->GPSData.VTG.speed = strtod(dataarray[7],NULL);
		phid->GPSData.VTG.mode = dataarray[9][0];
	}
	else {
		LOG(PHIDGET_LOG_INFO,"Unrecognized sentence type: %s", dataarray[0]+3);
	}

	return 0;
}

/* this parses a full sentence */
static int parse_SkyTraq_response(unsigned char *data, CPhidgetGPSInfo *phid) {
	int msgLength = data[3];
	int crc = data[msgLength + 4];
	int i, crccheck=0;
	char buffer[256];
	
	for(i=0;i<msgLength;i++)
	{
		crccheck^=data[i+4];
	}
	if(crc != crccheck)
	{
		LOG(PHIDGET_LOG_WARNING,"CRC Error parsing SkyTraq response.");
		return -1;
	}

	switch(data[4])
	{
		case 0x83: //ACK
			LOG(PHIDGET_LOG_INFO, "SkyTraq ACK: 0x%02x", data[5]);
			break;
		case 0x84: //NACK
			LOG(PHIDGET_LOG_INFO, "SkyTraq NACK: 0x%02x", data[5]);
			break;
		default:
			LOG(PHIDGET_LOG_INFO, "Got a SkyTraq message: 0x%02x", data[4]);
			buffer[0]=0;
			for(i=0;i<msgLength-1;i++)
			{
				if(i%8==0 && i!=0)
				{
					LOG(PHIDGET_LOG_INFO, "%s", buffer);
					buffer[0]=0;
				}
				sprintf(buffer+strlen(buffer)," 0x%02x", data[i+5]);
			}
			if(buffer[0])
				LOG(PHIDGET_LOG_INFO, "%s", buffer);
	}

	return 0;
}

/* this parses out the packets */
static int parse_GPS_packets(CPhidgetGPSInfo *phid) {

	unsigned char current_queuesize, msgsize, temp;
	int result, i=0;

	do {
		result = 0;
		/* Not known if packetsize is valid yet... */

		/* advance read ptr to '$' or 0xa0 */
		i=0;
		while ((i < 255) && (phid->sckbuf_read != phid->sckbuf_write))
		{
			if(phid->sckbuf[phid->sckbuf_read] == '$' || phid->sckbuf[phid->sckbuf_read] == 0xa0)
				break;
			i++;
			phid->sckbuf_read++;
		}
		
		current_queuesize = phid->sckbuf_write - phid->sckbuf_read;

		//response msg from skytraq - size is in posn 3
		if(phid->sckbuf[phid->sckbuf_read] == 0xa0)
		{
			unsigned char tempbuffer[256];
			if(current_queuesize < 4)
				break;
			msgsize = 7 + phid->sckbuf[(unsigned char)(phid->sckbuf_read + 3)];
			if(current_queuesize < msgsize)
				break;

			for (i = 0; i<msgsize; i++)
				tempbuffer[i] = phid->sckbuf[phid->sckbuf_read++];

			/* We know that we have at least a full sentence here... look for another */
			result=1;

			/* here we'll actually parse this sentence */
			if(parse_SkyTraq_response(tempbuffer, phid))
			{
				LOG(PHIDGET_LOG_WARNING,"Error parsing SkyTraq response.");
			}
		}
		else //NMEA/Other
		{
			char tempbuffer[256];
			/* find the end of the sentence */
			temp = phid->sckbuf_read;
			msgsize = 0;
			for (i=0; i < current_queuesize; i++,temp++)
			{
				if(phid->sckbuf[temp] == '\n') {
					msgsize = i;
					break;
				}
			}
			if(!msgsize) break; //couldn't find it

			for (i = 0; i<msgsize; i++)
				tempbuffer[i] = phid->sckbuf[phid->sckbuf_read++];
			tempbuffer[i] = 0;

			/* We know that we have at least a full sentence here... look for another */
			result=1;

			//NMEA - always starts with '$GP'
			if (current_queuesize >= 6 && tempbuffer[1] == 'G' && tempbuffer[2] == 'P')
			{

				/* here we'll actually parse this sentence */
				if(parse_NMEA_data(tempbuffer, phid))
				{
					LOG(PHIDGET_LOG_WARNING,"Error parsing NMEA sentence.");
				}
			}
			else //Something else that starts with a '$'
			{
				LOG(PHIDGET_LOG_INFO, "GPS Message: %s", tempbuffer);
			}
		}
	}
	while(result);

	return 0;
}

// === Exported Functions === //

//create and initialize a device structure
CCREATE(GPS, PHIDCLASS_GPS)

//event setup functions
CFHANDLE(GPS, PositionChange, double, double, double)
CFHANDLE(GPS, PositionFixStatusChange, int)

CGET(GPS,Latitude,double)
	TESTPTRS(phid,pVal) 
	TESTDEVICETYPE(PHIDCLASS_GPS)
	TESTATTACHED
	TESTMASGN(latitude, PUNK_DBL)

	MASGN(latitude)
}

CGET(GPS,Longitude,double)
	TESTPTRS(phid,pVal) 
	TESTDEVICETYPE(PHIDCLASS_GPS)
	TESTATTACHED
	TESTMASGN(longitude, PUNK_DBL)

	MASGN(longitude)
}

CGET(GPS,Altitude,double)
	TESTPTRS(phid,pVal) 
	TESTDEVICETYPE(PHIDCLASS_GPS)
	TESTATTACHED
	TESTMASGN(altitude, PUNK_DBL)

	MASGN(altitude)
}

CGET(GPS,Time,GPSTime)
	TESTPTRS(phid,pVal) 
	TESTDEVICETYPE(PHIDCLASS_GPS)
	TESTATTACHED
	if(phid->haveTime == PUNK_BOOL)
		return EPHIDGET_UNKNOWNVAL;

	MASGN(GPSData.GGA.time)
}

CGET(GPS,Date,GPSDate)
	TESTPTRS(phid,pVal) 
	TESTDEVICETYPE(PHIDCLASS_GPS)
	TESTATTACHED
	if(phid->haveDate == PUNK_BOOL)
		return EPHIDGET_UNKNOWNVAL;

	MASGN(GPSData.RMC.date)
}

CGET(GPS,NMEAData,NMEAData)
	TESTPTRS(phid,pVal) 
	TESTDEVICETYPE(PHIDCLASS_GPS)
	TESTATTACHED

	//Not uspported over webservice
	if(CPhidget_statusFlagIsSet(phid->phid.status, PHIDGET_REMOTE_FLAG))
		return EPHIDGET_UNSUPPORTED;

	MASGN(GPSData)
}

CGET(GPS,Heading,double)
	TESTPTRS(phid,pVal) 
	TESTDEVICETYPE(PHIDCLASS_GPS)
	TESTATTACHED
	TESTMASGN(heading, PUNK_DBL)

	MASGN(heading)
}

CGET(GPS,Velocity,double)
	TESTPTRS(phid,pVal) 
	TESTDEVICETYPE(PHIDCLASS_GPS)
	TESTATTACHED
	TESTMASGN(velocity, PUNK_DBL)

	MASGN(velocity)
}

CGET(GPS,PositionFixStatus,int)
	TESTPTRS(phid,pVal) 
	TESTDEVICETYPE(PHIDCLASS_GPS)
	TESTATTACHED
	TESTMASGN(fix, PUNK_BOOL)

	MASGN(fix)
}