/****************************************************************************
GPS.c - Created by Peter Mauger 20/06/01
Last Modified 12/10/01

GPS contains all of the functions required to communicate with the GPS and 
extract information from the messages.
****************************************************************************/
#include "GPS.h"

/* Init_GPS initialises the GPS communications channel and makes sure data
* has been received
* returns: FALSE if the initialisation failed
* 	   TRUE if the initialisation was successful
*/
bool Init_GPS()
{
	int result = 0;		
	int timeout = 0;
	char buf = '\0';

	result = OSInitRS232( GPS_BAUDRATE, NONE, GPS_PORTNUM );	/* initialise gps port */
	if( result != 0 ) 						/* returns 0 when successful */
	{
	    LCDPrintf("RS232 init fail");
	    return FALSE;
	}
	
	result = OSFlushInRS232( GPS_PORTNUM );			/* clear the receive buffer on the Eyebot */
	if( result != 0 ) 					/* returns 0 when successful */
	{
	    LCDPrintf("RS232 flush fail");
	    return FALSE;
	}
	/* receive data on the specified port until the first character is encountered, to test the link */
	do {
		result = OSRecvRS232( &buf, GPS_PORTNUM );
		timeout++;
	}
	while( result == 1 && timeout < GPS_TIMEOUT );		/* result = 1 is RS232 timeout. Keep checking */
								/* for GPS_TIMEOUT number of timeouts before failing */
	if( result != 0 )					/* result = 0 means successfully received a char */
	{
	    LCDPrintf("RS232 recv fail error %d", result);
	    return FALSE;
	}

	return TRUE;						/* gps has been correctly initialized */
}

/* Test_GPS checks for a message to make sure consistent data is coming
* through the link
* returns: FALSE if no message was found
*	   TRUE if a message was found
*/
bool Test_GPS()
{
	message mesg;
	error error_type;
	
	LCDPrintf("\nTesting GPS\n");
	mesg.complete = FALSE;
	mesg.valid = FALSE;
	mesg.mesg_ptr = 0;
	while(mesg.complete == FALSE)
	{
		LCDPrintf(".");
		error_type = Obtain_Message("$GPGLL", &mesg);
		if(error_type != NOERROR) return FALSE;
	}
	return TRUE;
}

/* Obtain_GPS_Position determines whether a message has been completed or not and
* gets the position if it has been, or gets more of the message if it hasn't
* inputs:  plane->used to log an error if it occurs
*	   header->contains the header of the message to be retrieved
* 	   mesg->contains the current message fragment
* returns: FALSE if the position was not updated
*	   TRUE if the position has been updated
*/
bool Obtain_GPS_Position(planestate *plane)
{
	error error_type;
	position pos;
	bool valid;
	message mesg = Get_GPS_Message(*plane);
	
	if(mesg.complete == FALSE)
	{
		error_type = Obtain_Message("$GPGLL", &mesg);
		if(error_type == NOERROR)
		{
			if(mesg.complete == TRUE)
			{
				valid = Obtain_Position_From_Mesg(&mesg, &pos);
				Set_GPS_Message(plane, mesg);		/* message reset by Get_Position, update stored */
				if(valid == TRUE)
				{
					Set_Curr_Pos(plane, pos);	/* if the position was found then store it */
					return TRUE;
				}
				else return FALSE;
			}
			else
			{		
				Set_GPS_Message(plane, mesg);	/* update the currently stored message fragment */
				return FALSE;
			}
		}
		else 
		{
			Set_GPS_Message(plane, mesg);	/* message reset by Find_Message, update stored */
			Log_Event(plane, error_type);	/* log the error */
			return FALSE;
		}
	}
	else
	{
		valid = Obtain_Position_From_Mesg(&mesg, &pos);
		Set_GPS_Message(plane, mesg);		/* message reset by Get_Position, update stored */
		if(valid == TRUE)
		{
			Set_Curr_Pos(plane, pos);	/* if the position was found then store it */
			return TRUE;
		}
		else return FALSE;
	}
}

/* Obtain_Message searches through one burst of data from the GPS for
* the message header passed to it
* inputs:  header->the header of the message required
*	   mesg->returns the current part of the message in a message structure
* returns: MESGTOOLONG if the message was longer than MAX_MESG_LENGTH
*	   NOERROR if either the message was found (mesg->complete == TRUE) 
*		or if the maximum read for this call was reached (mesg->complete == FALSE)
*/
error Obtain_Message(char header[HDR_LENGTH], message *mesg)
{	
	int burst_counter = 0, timeout = 0, result = 0, i = 0;

	while(burst_counter < MAX_BURST_LENGTH)		/* keep trying until looked through a full burst */
	{
		if(mesg->valid == FALSE)
		{
			if(mesg->mesg_ptr == 0)
			{
				mesg->buffer[0] = '\0';
				while( mesg->buffer[0] != '$')		/* $ is the first character in a header */
				{
					if(burst_counter < MAX_BURST_LENGTH && result != 1)
					{
						result = OSRecvRS232( &(mesg->buffer[0]), GPS_PORTNUM );
						burst_counter++;
					}
					else return NOERROR;			/* searched too much buffer, or had to wait for data. Try again later */
				}
				/* found $ in buffer, increment message pointer so that future calls skip first bit */ 
				mesg->mesg_ptr = 1;
			}
			
			if(mesg->mesg_ptr < HDR_LENGTH)
			{
				while(mesg->mesg_ptr < HDR_LENGTH)			/* grab the next HDR_LENGTH of characters so that */
				{							/* the whole header is retrieved */
					if(burst_counter < MAX_BURST_LENGTH)
					{
						result = OSRecvRS232( &(mesg->buffer[mesg->mesg_ptr]), GPS_PORTNUM );
						if(result == 0)
						{
							burst_counter++;
							mesg->mesg_ptr++;
						}
						else return NOERROR;			/* an error or timeout occured in receiving data */
					}
					else return NOERROR;				/* Searched too much buffer. Try again later */
				}
				if( strncmp( mesg->buffer, header, HDR_LENGTH ) == 0 ) mesg->valid = TRUE;	/* compare the header against the desired one */
				else									/* else wrong message header, reset the message and try again */
				{	
					while(i < mesg->mesg_ptr)			
					{
						mesg->buffer[mesg->mesg_ptr] = '\0';
						i++;
					}
					mesg->mesg_ptr = 0;
				}
			}
		}		
		else
		{
			do
			{
				if(burst_counter < MAX_BURST_LENGTH) 
				{
					if(mesg->mesg_ptr < MAX_MESSAGE_LENGTH)	
					{
						result = OSRecvRS232( &(mesg->buffer[mesg->mesg_ptr]), GPS_PORTNUM );
						if(result == 0)
						{
							mesg->mesg_ptr++;
							burst_counter++;
						}
						else return NOERROR;
					}
					else			/* message was too long, reset the mesg and return error */
					{	
						while(i < mesg->mesg_ptr)			
						{
							mesg->buffer[mesg->mesg_ptr] = '\0';
							i++;
						}
						mesg->mesg_ptr = 0;
						return MESGTOOLONG;	
					}
				}
				else return NOERROR;
			}while((mesg->buffer[mesg->mesg_ptr-1] != '*') && (mesg->buffer[mesg->mesg_ptr-1] != '\n'));	/* grab the rest of the message */
												/* a star denotes the beginning of the checksum */
			i=0;
			while(i < 3)					/* so grab the remaining two characters of the checksum */
			{
				if(burst_counter < MAX_BURST_LENGTH)
				{
					if(mesg->mesg_ptr < MAX_MESSAGE_LENGTH)
					{
						timeout = 0;
						do
						{
							result = OSRecvRS232( &(mesg->buffer[mesg->mesg_ptr]), GPS_PORTNUM );
							timeout++;
						}while(result == 1 && timeout < GPS_TIMEOUT );
						i++;
						mesg->mesg_ptr++;
						burst_counter++;
					}
					else			/* message was too long, reset the mesg and return error */
					{
						while(i < mesg->mesg_ptr)			
						{
							mesg->buffer[mesg->mesg_ptr] = '\0';
							i++;
						}
						mesg->mesg_ptr = 0;
						return MESGTOOLONG;	
					}
				}
				else return NOERROR;			/* read too much. try again later */
			}
			
			/* Whole correct message is now received!!! */
			mesg->complete = TRUE;				/* tag the message as complete */
			mesg->buffer[mesg->mesg_ptr] = '\0';		/* set the last character to null */
			mesg->mesg_length = mesg->mesg_ptr-1;		/* buff_ptr was incremented one too many */
			mesg->mesg_ptr = 0;				/* reset pointer */
			mesg->valid = FALSE;				/* reset header verification field */
			return NOERROR;
		}
	}
	return NOERROR;					/* read too much. try again later */
}

/* Obtain_Position_From_Mesg retrieves the data from GLL messages.
* It expects a certain format, and if this is not adhered to will fail
* inputs:  mesg->contains the GLL message
*	   pos->returns the data from the message
* returns: FALSE if the message is incorrect
*	   TRUE if the data has been successfully retrieved
*/
bool Obtain_Position_From_Mesg(message *mesg_in, position *pos)
{
	message mesg;
	int i;
	char lat_dir_value, lon_dir_value;
	char status_value;
	double lat_value = 0;
	double lon_value = 0;
	int n_s = 0, e_w = 0;		/* used as a multiplier for lat/long. +1 or -1 based on cardinality */
	char utc_value[11];		/* stores the utc value */
	bool valid = FALSE;
	char value[11];			/* used to store the numbers */

	strncpy(mesg.buffer, mesg_in->buffer, mesg_in->mesg_length);	/* copy mesg into temporary */
	mesg.mesg_length = mesg_in->mesg_length;
	mesg.mesg_ptr = mesg_in->mesg_ptr;
	
	for(i=0;i<mesg_in->mesg_length;i++)				/* then reset the original message */
	{	
		mesg_in->buffer[i] = '\0';
	}
	mesg_in->mesg_length = 0;
	mesg_in->mesg_ptr = 0;	
	mesg_in->complete = FALSE;
	
	/* First check for $GPGLL in message */
	if( strncmp(mesg.buffer, "$GPGLL", HDR_LENGTH) != 0 ) return FALSE;	

	/* first character the after the '$GPGLL' header should be a comma, should be in 7th position (index of 6) */
	mesg.mesg_ptr = HDR_LENGTH;
	if( !CommaCheck( &mesg ) ) return FALSE;
	
	/* now the latitude of the position */
	if( !ReadValue( value, &mesg ) ) return FALSE;
	lat_value = atof( value );
	
	/*	if( lat_value < -180 || lat_value > 180 ) return FALSE; *** lat value does not necessarily have to be between 180 and -180 */

	/* check for another comma */
	if( !CommaCheck( &mesg ) ) return FALSE;
	
	/* now the latitude direction */
	if( ((mesg.buffer[mesg.mesg_ptr] != 'N') && (mesg.buffer[mesg.mesg_ptr] != 'S')) || (mesg.mesg_ptr >= mesg.mesg_length) ) return FALSE;
	else
	{
		lat_dir_value = mesg.buffer[mesg.mesg_ptr];
		mesg.mesg_ptr++;
	}
	
	/* check for another comma */
	if( !CommaCheck( &mesg ) ) return FALSE;
	
	/* now the longitude of the position */
	if( !ReadValue( value, &mesg ) ) return FALSE;
	lon_value = atof( value );
	
	/*	if( lon_value < -180 || lon_value > 180 ) return FALSE; *** long value doesn't necessarily have to be 180 to -180 */
	
	/* check for another comma */
	if( !CommaCheck( &mesg ) ) return FALSE;
	
	/* now the longitude direction */
	if( ((mesg.buffer[mesg.mesg_ptr] != 'E') && (mesg.buffer[mesg.mesg_ptr] != 'W')) || (mesg.mesg_ptr >= mesg.mesg_length) ) return FALSE;
	else 
	{
	    lon_dir_value = mesg.buffer[mesg.mesg_ptr];
	    mesg.mesg_ptr++;
	}
	
	/* check for another comma */
	if( !CommaCheck( &mesg ) ) return FALSE;
	
	/* now the UTC/GMT time of position fix */
	if( !ReadValue( value, &mesg ) ) return FALSE;
	strncpy(utc_value, value, 11 );
	if( strlen( utc_value ) != 10 ) return FALSE;
	/* check for another comma */
	if( !CommaCheck( &mesg ) ) return FALSE;
	
	/* now the status of the fix */
	if( ((mesg.buffer[mesg.mesg_ptr] != 'A') && (mesg.buffer[mesg.mesg_ptr] != 'V')) || (mesg.mesg_ptr >= mesg.mesg_length) ) return FALSE;
	else
	{
	        if( mesg.buffer[mesg.mesg_ptr] == 'A' ) valid = TRUE;	/* 'A' means good data */
	        status_value = mesg.buffer[mesg.mesg_ptr];
		mesg.mesg_ptr++;
	}
	
	/* don't check for a comma in this message, as there shouldn't be one */
	/* just check the checksum */
	if( !ChecksumCheck( &mesg ) ) return FALSE;

	/* if we get to here, the message was in correct format, if valid, use it */
	if( valid ) {
		if( lat_dir_value == 'N' ) n_s = NORTH;		/* NORTH determines the polarity of the latitude */
		else if( lat_dir_value == 'S' ) n_s = SOUTH;	/* SOUTH determines the polarity of the latitude */
		else return FALSE;
		pos->latitude = lat_value*n_s;
		
		if( lon_dir_value == 'E' ) e_w = EAST;		/* EAST determines the polarity of the latitude */
		else if( lon_dir_value == 'W' ) e_w = WEST;	/* WEST determines the polarity of the latitude */
		else return FALSE;
		pos->longitude = lon_value*e_w;
	}
	
	return TRUE;
}

/* CommaCheck makes sure that the current character is a comma
* inputs:  mesg->contains the message and a pointer to the current character
* returns: FALSE if there wasn't a comma
*	   TRUE if there was one
*/
bool CommaCheck( message *mesg ) {

	if( ( mesg->buffer[mesg->mesg_ptr] == ',' ) && ( mesg->mesg_ptr < mesg->mesg_length ) ) {
		mesg->mesg_ptr++;
		return TRUE;
	}
	else return FALSE;
}

/* ReadValue reads a string from before a comma (or the end of the message)
* from the message
* inputs:  value->the string read
*	   mesg->the mesg (which will be reset)
* returns: FALSE if no string was found (ie up to a comma or end of message)
*	   TRUE if a string was returned
*/
bool ReadValue( char *value, message *mesg ) {
        int curr_val_ptr = 0;   /* pointer for tracking length of the current value */
	char curr_value[11];	/* string for storage of current value, max length 10 */

	/* read until we find the next comma or a '*' (latter is for last value in some messages) */
	while( ( mesg->buffer[mesg->mesg_ptr] != ',' ) && ( mesg->buffer[mesg->mesg_ptr] != '*' ) && ( mesg->mesg_ptr < mesg->mesg_length ) ) {
	  if( curr_val_ptr < 10 ) {
	    	curr_value[curr_val_ptr] = mesg->buffer[mesg->mesg_ptr];
		curr_val_ptr++;
		mesg->mesg_ptr++;
	  }
	  else return FALSE;
	}
	curr_value[curr_val_ptr] = '\0';
	curr_val_ptr++;
	strncpy( value, curr_value, curr_val_ptr );
	return TRUE;
}

/* ChecksumCheck determines whether the checksum is correct for that message
* inputs:  mesg->contains the message data and a pointer to the checksum (hopefully)
* returns: FALSE if the checksum was incorrect (or not correct format)
*	   TRUE if the message was correctly received
*/
bool ChecksumCheck( message *mesg ) {
	int i;
	int start_pos = mesg->mesg_ptr;		/* integer for storing position of the '*' */
	char csum_read_str[3];			/* temporary string for storing the checksum read from the line */
	int csum_calc = 0, csum_read = 0;			/* integer for storing the calculated checksum */

	/* next character should be the '*' at the start of the checksum */
	if( mesg->buffer[mesg->mesg_ptr] != '*' ) 
	{
	  LCDPrintf("* missing in csum ");
	  return FALSE;
	}
	mesg->mesg_ptr++;
	
	/* now the checksum should be the next two characters, put them into a string */
	for( i = 0; i < 2; i++ ) {
		csum_read_str[i] = mesg->buffer[mesg->mesg_ptr];
		mesg->mesg_ptr++;
	}
	csum_read_str[2] = '\0';
	/* calculate what the checksum (8-bit XOR of all characters between the '$' and the '*') should be */
	for( i = 1; i < start_pos; i++ ) csum_calc ^= mesg->buffer[i];
	
	/* check that this number is inside the acceptable range */
	if( csum_calc < 0 || csum_calc > 255 ) return FALSE;

	/* convert the checksum read from the string (in hexadecimal format) to a decimal format integer */
	csum_read = Convert_HexStringToDecInt( csum_read_str );
	/* check if the checksum read from the string and the calculated one is equal */
	if( csum_calc != csum_read ) return FALSE;

	/* finally, check that the end of the string is next (should follow checksum) */
	if( mesg->mesg_ptr != mesg->mesg_length ) 
	{
	    LCDPrintf("EOS not found ");
	    return FALSE;
	}
	/* if we get here, checksum was correct and was the last thing in the message */
	return TRUE;
}

/* Convert_HexStringToDecInt simply converts a two character hexadecimal string into
* an decimal integer value
* inputs:  csum_read_str->contains the hex value
* returns: the decimal value of the hex string
*/
int Convert_HexStringToDecInt( char csum_read_str[3] ) {
  int csum_read = 0, i;  
  
  for(i=0;i<2;i++)
  {
    switch(csum_read_str[i])
    {
    case 'F': csum_read += 15;
      break;
    case 'E': csum_read += 14;
      break;
    case 'D': csum_read += 13;
      break;
    case 'C': csum_read += 12;
      break;
    case 'B': csum_read += 11;
      break;
    case 'A': csum_read += 10;
      break;
    case '9': csum_read += 9;
      break;
    case '8': csum_read += 8;
      break;
    case '7': csum_read += 7;
      break;
    case '6': csum_read += 6;
      break;
    case '5': csum_read += 5;
      break;
    case '4': csum_read += 4;
      break;
    case '3': csum_read += 3;
      break;
    case '2': csum_read += 2;
      break;
    case '1': csum_read += 1;
      break;
    case '0': csum_read += 0;
      break;
    default: LCDPrintf("Bad char HEX2INT\n");
      return -1;
    }
    if(i == 0) csum_read = csum_read * 16;	/* multiply first char by 16 */ 
  }						/* second char added as is */
  return csum_read;
}