//#define GNSSprint 1 //uncomment this line to turn on the diagnostic print statements
/******************************************************
  P R O  M I C R O  U S A R T  C O N N E C T I O N 
*********************************************************
Arduino
Physical logical  	description
	2		D0		RXI

/*********************************************************
V A R I A B L E S
**********************************************************/
byte GNSS_RetryCounterByte = 1;//counter advanced each time we try to access the GNSS and it fails. RGS1.5
byte GNSS_RetryCountLimitByte = 2;//number of times we will retry the GNSS when it has not sent any data
String BufferText = "";
byte count = 0;	
byte EndOfField = 0;
byte CountPlusOne = 0;
byte CountPlusTwo = 0;
boolean AtEndOfNeededSentenceBool = false;
byte pointer = 0;
byte FieldCountByte = 0;
unsigned long GNSS_WaitingForDataTimeLimitMsULong = 5000;
unsigned long GNSS_StartOfWaitingForDataMsULong = millis();
float FractionalMinutesFloat = 0;
byte StartOfAltitudeByte = 0;
byte EndOfFieldByte = 0;
boolean DiagPrintBool = true;
boolean GNSS_TimeToRetryReadingDataQBool = false;//init to don't retry access to GNSS
boolean GNSS_TimedOutWaitingForDataBool = false;//init is that GPS has been detected
unsigned int TimeNowMsUInt = millis();//time since power up.
unsigned int GNSS_DelayBeforeRetryOfReadingDataMsUInt = 60000;//Retry talking to GNSS after 60000 ms.   RGS1.5
unsigned int LastTimeWeDidA_RetryOfReadingDataMsUInt = TimeNowMsUInt - GNSS_DelayBeforeRetryOfReadingDataMsUInt - 10;//initialized so GNSS_Timing() will think it has been more than GNSS_DelayBeforeRetryOfReadingDataMsUInt since last retry
boolean IncompleteSentenceBool = false;
byte NavigationDataByte[16];
unsigned long AltitudeULong=0;
byte i = 0; //used in for loops
char character;
/**********************************************************/
void setup()
{
	Serial1.begin(19200); //set up communications path between Arduino and GPS; Default of 8 data bits, 1 stop bit, no parity or flow control is what the GPS needs.
	
	InitializeNavigationDataByteArray();	
}

void loop()
{
GNSS_Timing();
}

void GlobalNavigationSatelliteSystem()
{
/*******************************************
If a GNSS device is connected to the UART, this subroutine will parce its GPGGA sentence into bytes:

NavigationDataByte[16] which consists of:

BYTE	DESCRIPTION
0		hours (UTC)
1		minutes (UTC)
2		seconds (UTC)
3		degrees (latitude)
4		minutes (latitude)
5		seconds (latitude)
6		0 for north, 1 for south
7		degrees (longitude)
8		minutes (longitude)
9		seconds (longitude)
10		0 for east, 1 for west
11		altitude MSB (byte 3)   See also Alititude()
12		altitude (byte 2)
13		altitude (byte 1)
14		altitude LSB(byte 0)
15		units: 0 for meters, 1 for feet

The user must look at UTC in order to know if this is new data.

Array is returned with 200 for any field that is null. If $GPGGA sentence not found, all fields set to 200. 

Hours = 201 means we timed out waiting for data from GNSS.
north/south and east/west fields set to 202 means unexpected value read.

Hours = 203 means we have failed to get a response from the GNSS GNSS_RetryCountLimitByte times since power up so have given up on it.

These values were chosen because they are out of range for all bytes and is also not a symptom of a hardware fault which can occur with the value 255.

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

	if (GNSS_RetryCounterByte > GNSS_RetryCountLimitByte)
	{//give up trying to get data from the GNSS. It has failed to respond GNSS_RetryCountLimitByte times since power up. GNSS_RetryCounterByte is initialized to 1 and incremented each time we retry reading data from the GNSS. RGS1.5
		NavigationDataByte[0] = 203;//indicates software will no longer try to access the GNSS because it does not respond. 
		return;
	}
	
	BufferText = "";
	
	#ifdef GNSSprint
	Serial.print(F("line number "));			
	Serial.print(__LINE__);
	Serial.print(F(".    Time Stamp  "));	
	Serial.println(millis() - StartForTimeStampULong);
	Serial.print(F(" BufferText = "));
	Serial.print( BufferText );
	Serial.println("*");
	#endif

	if(GNSS_TimedOutWaitingForDataBool)//means we previously timed out waiting for GNSS sentence's start symbol or any characters 
	{
		
		#ifdef GNSSprint
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		#endif
		
		
		if(GNSS_TimeToRetryReadingDataQBool)
		{
			InitializeNavigationDataByteArray();//all elements set to 200
			GNSS_TimedOutWaitingForDataBool = false;//this will cause us to look for $ again.
			GNSS_TimeToRetryReadingDataQBool = false;//used up flag so clear it.
			GNSS_StartOfWaitingForDataMsULong = millis();//init timer

			#ifdef GNSSprint
			Serial.print(F("line number "));			
			Serial.print(__LINE__);
			Serial.print(F(".    Time Stamp  "));	
			Serial.println(millis() - StartForTimeStampULong);
			#endif
			
			return;//with GNSS_TimedOutWaitingForDataBool cleared, we will look for a sentence the next time around
		}
		
		NavigationDataByte[0] = 201;//tells user not to trust the array because we did not see the sentence 
		
		#ifdef DiagPrint1
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		#endif
		
		return;//we previously timed out looking for start of sentence delimiter("$") so just return.
	}

		
		#ifdef DiagPrint3
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		#endif
	
	FindNeededSentence(); //it resets timer too
	
	if (GNSS_TimedOutWaitingForDataBool)
	{
		
		#ifdef DiagPrint1
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		#endif

		return; //timed out
	}
	
	//to get here, we must have correct sentence's characters

		
		#ifdef DiagPrint3
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		#endif
	
	InitializeNavigationDataByteArray();//initialize array to 200. Parcing code should be setting each element to either received data, 200, or 201.
	
/************************************************************
D A T A  S T R U C T U R E  O F  S E N T E N C E  
*************************************************************
Sample Characters	Quantity	Count value or variable
	,				delimiter		0
	15  			hours			1 & 2  or "," if UTC is null 
	01				minutes			3 & 4
	16				seconds			5 & 6
	.				decimal			7
	00				fractional seconds	8 thorugh A-1
	,				delimiter 		A
	33				degrees lat		A+1 & A+2			
	17				minutes lat		A+3 & A+4
	.				decimal			A+5
	56410			fractional minutes lat A+6 through B-1
	,				delimiter		B
	N				north/south		B+1
	,				delimiter 		C			
	112				degrees lon		C+1 thorugh C+3
	05 				minutes lon		C+4 & C+5
	.				decimal			C+6
	05327			fractional minutes lon C+7 through D-1		
	,				delimiter		D
	W				east/west		D+1
	,				delimiter		E
	2,09,1 			ignored fields	
	.				decimal		
	02				ignored fields	
	,				delimiter		F
	362 			altitude		F+1 through G-1 or ","
	.				decimal			G   can be "." or ","
	2				fractional (ignore)	H-1
	,				delimiter		H
	M				altitude units	H+1
,-28.1,M,,0000*62	ignored fields

all empty fields will cause 200 to be put in corresponding output array element.
*******************************************************/
	#ifdef DiagPrint4
	Serial.print(F("#"));			
	Serial.println(__LINE__);
	Serial.print(F(" sentence read in is "));
	Serial.println( BufferText );
	#endif
	
	IncompleteSentenceBool = false;
	ParceTime();
	if (IncompleteSentenceBool)return;
	ParceLatitude();
	if (IncompleteSentenceBool)return;	
	ParceLongitude();
	if (IncompleteSentenceBool)return;
	SkipFourFields();
	if (IncompleteSentenceBool)return;
	ParceAltitude();
	if (IncompleteSentenceBool)return;
	
	#ifdef DiagPrint1
	Serial.print(F("line number "));			
	Serial.print(__LINE__);
	Serial.print(F(".    Time Stamp  "));	
	Serial.println(millis() - StartForTimeStampULong);
	for(i = 0;i<16;i++)
	{
	Serial.print( NavigationDataByte[i] );
	Serial.print(F(","));
	}
	Serial.println();
	#endif
		
}//At end of GlobalNavigationSatelliteSystem()

void GNSS_Timing()
{
//Set GNSS_TimeToRetryReadingDataQBool true after GNSS_DelayBeforeRetryOfReadingDataMsUInt last lapsed since the last time it was set false. It is used to test for presence of GNSS receiver after it has timed out. This is necessary if connection to device is intermittent and we don't want to just give up right away. After 4 retries, we do give up.


	#ifdef DiagPrint3
	Serial.print(F("line number "));			
	Serial.print(__LINE__);
	Serial.print(F(".    Time Stamp  "));	
	Serial.println(millis() - StartForTimeStampULong);
	#endif
	
	if (GNSS_TimeToRetryReadingDataQBool == false)
	{//flag says it is not time to retry accessing GNSS but will check timer to see if it is now time to do it

		#ifdef DiagPrint1
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		#endif
			
		if((millis() - LastTimeWeDidA_RetryOfReadingDataMsUInt) >  GNSS_DelayBeforeRetryOfReadingDataMsUInt)
		{//we have waited long enough, try accessing the GNSS again

			#ifdef DiagPrint1
			Serial.print(F("line number "));			
			Serial.print(__LINE__);
			Serial.print(F(".    Time Stamp  "));	
			Serial.println(millis() - StartForTimeStampULong);
			#endif
				
			GNSS_TimeToRetryReadingDataQBool = true; //set flag to retry GNSS
			LastTimeWeDidA_RetryOfReadingDataMsUInt = millis();//mark the time of this last retry
			GNSS_RetryCounterByte = GNSS_RetryCounterByte +1;//increment counter; When GlobalNavigationSatelliteSystem() called, it will test count and if > GNSS_RetryCountLimitByte, give up trying to retry GNSS access. RGS1.5
		}
	}
}	

void InitializeNavigationDataByteArray()
{
	for(i = 0;i < 16;i++)NavigationDataByte[i] = 205;
}

void ParceTime()
{
	//parce hour UTC
	count = 0;//delimiter position before hours UTC 
	
	#ifdef DiagPrintGNSS
	Serial.print(F("line number "));	
	Serial.print(__LINE__);
	Serial.println(F(" BufferText = "));
	Serial.println( BufferText );
	#endif
	
	if(BufferText[1] == ',')//UTC field is null
	{
		NavigationDataByte[0] = 200;//null UTC so corresponding output array element for hours is set to 200
		NavigationDataByte[1] = 200;//null UTC so corresponding output array element for minutes is set to 200
		NavigationDataByte[2] = 200;//null UTC so corresponding output array element for minutes is set to 200
		IncompleteSentenceBool = true;
		return;
	}else{
		//UTC is not null so process it
		
		#ifdef DiagPrint3
		Serial.print(F("line number "));		
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		#endif
		
		NavigationDataByte[0] = (((BufferText[1] - 0x30)*10) + (BufferText[2] - 0x30));//convert each ASCII character to a number and then use it to build the hour value
		
		//do range check to protect against unexpected field values
		if(NavigationDataByte[0] > 23)NavigationDataByte[0] = 200;
		
		//parce minutes UTC

		NavigationDataByte[1] = ((BufferText[3] - 0x30)*10) + (BufferText[4] - 0x30);
		//do range check to protect against unexpected field values
		if(NavigationDataByte[1] > 59)NavigationDataByte[1] = 200;
		
		//parce seconds UTC
		NavigationDataByte[2] = ((BufferText[5] - 0x30)*10) + (BufferText[6] - 0x30);
		//do range check to protect against unexpected field values
		
		#ifdef DiagPrint3
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		Serial.print(F(" BufferText[5] = "));
		Serial.println( BufferText[5] );
		Serial.print(F(" BufferText[6] = "));
		Serial.println( BufferText[6] );
		Serial.print(F(" NavigationDataByte[2] = "));
		Serial.println( NavigationDataByte[2] );
		#endif
		
		if(NavigationDataByte[2] > 59)NavigationDataByte[2] = 200;
	
		count = 7;//puts us at either "." if just before fractional seconds or "," for field delimiter
			
		#ifdef DiagPrint3
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		Serial.print(F(" seconds = "));
		Serial.println( NavigationDataByte[2] );
		#endif

		//parce fractions of a second UTC. I don't use it but need to advance count to next delimiter
		
		if(BufferText[count] == ',')
		{//if there is no fractional part, we see "," right away so "A" is 7.
	
		#ifdef DiagPrint1
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		#endif
	
		}else{
		//there is a fractional part
		
			while(BufferText[count] != ',')
			{
		
				#ifdef DiagPrintGNSS
				Serial.print(F("line number "));			
				Serial.print(__LINE__);
				Serial.print(F(".    Time Stamp  "));	
				Serial.println(millis() - StartForTimeStampULong);
				Serial.print(F("count = "));
				Serial.println( count );
				Serial.print(F(" BufferText[count] = "));
				Serial.println( BufferText[count] );*/
				#endif
				
				count++;//search for delimiter and advance count along field. Exit just before ',' with count at A-1
			}
			//we exit while() at delimter which is count of "A"
		}
	}
}//end of ParceTime()

void ParceLatitude()
{	
	//parce degrees latitude
	CountPlusOne = count+1;	//at A+1 if field not null and B if it is null
	if(BufferText[CountPlusOne] == ',')
	{//latitude field is null so fill each byte with 200
		NavigationDataByte[3] = 200;
		NavigationDataByte[4] = 200;
		NavigationDataByte[5] = 200;
		IncompleteSentenceBool = true;
		return;
	}else{
		//process degrees lattitude field
		CountPlusOne = count+1;//most significant digit  "A+1"
		CountPlusTwo  = count+2;//least significant digit  "A+2"
		NavigationDataByte[3] = ((BufferText[CountPlusOne] - 0x30)*10) + (BufferText[CountPlusTwo] - 0x30);
		count = count + 3;//now at start of minutes latitude sub-field.
		
		//do range check to protect against unexpected field values
		if(NavigationDataByte[3] > 90)NavigationDataByte[3] = 200;
		
		#ifdef DiagPrintGNSS
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		Serial.print(F(" NavigationDataByte[3] = "));
		Serial.println( NavigationDataByte[3] );
		#endif
		
		//parce minutes latitude
		//count is most significant digit which is "A+3"
		CountPlusOne = count+1;//least significant digit "A+4"
		NavigationDataByte[4] = ((BufferText[count] - 0x30)*10) + (BufferText[CountPlusOne] - 0x30);
		count = count+2;//now at "A+5" if this is the decimal before fractional minutes latitude or at "B" if no fractional part, the delimiter before North/South field.
		
		//do range check to protect against unexpected field values
		if(NavigationDataByte[4] > 59)NavigationDataByte[4] = 200;
		

		float FractionalMinutesFloat = 0;		
		if(BufferText[count] == '.')//this means we have a fractional minute. If none, we just use FractionalMinutesFloat = 0
		{ 
			//parce fractional minutes latitude and convert to seconds
			count = count+1;//now at A+6, the most significant digit of fracctional minutes
			EndOfField = count;//prepare to search for end of fractional minutes sub-field which will be "B-1"
		
			//Serial.println(__LINE__);
			
			while(BufferText[EndOfField] != ',')EndOfField++;//search for delimiter and advance EndOfField along sub-field. Exit at "," with count at "B". The number we need to process starts at count and ends at EndOfField-1
		
			//Serial.println(__LINE__);
			
			float DividerFloat = 10;//scales each digit
		
			for(pointer = count; pointer < (EndOfField); pointer++)
			{//pointer goes from A+6 to B-1 
				FractionalMinutesFloat = FractionalMinutesFloat + float((BufferText[pointer]- 0x30)/DividerFloat);
				DividerFloat = DividerFloat*10;//prepare to scale next digit 
			}
			
			NavigationDataByte[5] = byte(FractionalMinutesFloat*60);//convert from fractional minutes to integer seconds
			
		//do range check to protect against unexpected field values
		if(NavigationDataByte[5] > 59)NavigationDataByte[5] = 200;			
		
			count = EndOfField;//update character count to delimiter just before north/south field. Now at B.
		}else{
			//there is no fractional minutes so seconds latitude is simply 0
			// character count is at delimiter just before north/south field. Now at B.
			NavigationDataByte[5] =	0;
		}
	}
	count = count+1;//advance count to N/S field  "B+1"
	
	#ifdef DiagPrintGNSS
	Serial.print(F("line number "));			
	Serial.print(__LINE__);
	Serial.print(F(".    Time Stamp  "));	
	Serial.println(millis() - StartForTimeStampULong);
	Serial.print(F(" count = "));
	Serial.println( count );
	#endif

	
	//parce north/south  
	switch(BufferText[count]){
	case ',':
		NavigationDataByte[6] = 200;//means null field
		count = count + 2;//character count set to delimiter just before degrees longitude  B+2
		break;
		
	case 'N':
		NavigationDataByte[6] = 0;//0 means north
		count = count +1;//advance to delimiter just before degrees longitude	"C"
		break;
		
	case 'S':
	NavigationDataByte[6] = 1;//1 means south
	count = count +1;//advance to delimiter just before degrees longitude	"C"
	break;
	
	default:
	NavigationDataByte[6] = 202;//means unexpected value
	count = count +1;//advance to delimiter just before degrees longitude	"C"
	break;
	}
}//end of ParceLatitude()

void ParceLongitude()
{
	//parce degrees longitude
	count = count+1;//advance to most significant digit  C+1
	CountPlusOne = count+1;//C+2
	CountPlusTwo  = count+2;//C+3
	
	#ifdef DiagPrint1			
	Serial.print(__LINE__);
	Serial.print(F(".    Time Stamp  "));	
	Serial.println(millis() - StartForTimeStampULong);
	Serial.print(F(" count = "));
	Serial.println( count );
	Serial.print(F(" BufferText[count] = "));
	Serial.println( BufferText[count] );
	Serial.print(F(" BufferText[CountPlusOne] = "));
	Serial.println( BufferText[CountPlusOne] );
	Serial.print(F(" BufferText[CountPlusTwo] = "));
	Serial.println( BufferText[CountPlusTwo] );
	#endif
	
	if((BufferText[count] == ',')|| (BufferText[CountPlusOne]==','))
	{
		IncompleteSentenceBool = true;
		return;
	}
	NavigationDataByte[7] = ((BufferText[count]- 0x30)*100) + ((BufferText[CountPlusOne]- 0x30)*10) + (BufferText[CountPlusTwo]- 0x30);
	
	//do range check to protect against unexpected field values
	if(NavigationDataByte[7] > 180)NavigationDataByte[7] = 200;
	
	#ifdef DiagPrintGNSS
	Serial.print(F("line number "));			
	Serial.print(__LINE__);
	Serial.print(F(".    Time Stamp  "));	
	Serial.println(millis() - StartForTimeStampULong);
	Serial.print(F(" NavigationDataByte[7] = "));
	Serial.println( NavigationDataByte[7] );
	#endif
	
	count = count + 3;//advance to most significant digit of minutes longitude. C+4
	
	//parce minutes longitude
	CountPlusOne = count+1;//C+5
	NavigationDataByte[8] = ((BufferText[count]- 0x30)*10) + (BufferText[CountPlusOne]- 0x30);
	
	//do range check to protect against unexpected field values
	if(NavigationDataByte[8] > 59)NavigationDataByte[8] = 200;
	
	//parce seconds longitude
	count = count + 2;//advance to start of fractional minutes longitude which starts with "." if there is any fraction ("C+6") or with "," to signal the end of the field ("D").
	EndOfField = count;
	if(BufferText[EndOfField] == '.')
	{//we have a fractional part of minutes longitude
		while(BufferText[EndOfField] != ',')EndOfField++;//search for delimiter and advance EndOfField along field. Exit at "," with EndOfField at "D". The number we need to process starts at count+1 ("C+7") and ends at EndOfField-1 ("D-1")
		
			//Serial.println(__LINE__);
			
		count = count+1;//at start of fractional part
		float DividerFloat = 10;//scales each number 
		FractionalMinutesFloat = 0;
		
		for(pointer = count; pointer < EndOfField; pointer++)
		{
			#ifdef DiagPrintGNSS
			Serial.print(F("line number "));			
			Serial.print(__LINE__);
			Serial.print(F(".    Time Stamp  "));	
			Serial.println(millis() - StartForTimeStampULong);
			Serial.print(F(" pointer = "));
			Serial.println( pointer );
			Serial.print(F(" BufferText[pointer] = "));
			Serial.println( BufferText[pointer] );			
			Serial.print(F(" DividerFloat = "));
			Serial.println( DividerFloat );
			#endif			
			
			FractionalMinutesFloat = FractionalMinutesFloat + float((BufferText[pointer]- 0x30)/DividerFloat);
			DividerFloat = DividerFloat*10;//prepare to scale next digit
			
			#ifdef DiagPrintGNSS
			Serial.print(F(" FractionalMinutesFloat = "));
			Serial.println( FractionalMinutesFloat );
			#endif
			
		}
		
		NavigationDataByte[9] = byte(FractionalMinutesFloat*60);//convert from fractional minutes to integer seconds
		
		//do range check to protect against unexpected field values
		if(NavigationDataByte[9] > 59)NavigationDataByte[9] = 200;
	
		count = EndOfField;//advance to delimiter just before East/West field "D"
	}else{
		//we do not have a fractional part of minutes longitude
		NavigationDataByte[9] = 0;//there is no fractional part of minutes longitude so seconds equals 0. Note that this is not null (200)
		//count is C+6 but since fractional minutes sub-field is empty, we are at the delimiter so this count is also "D". 
	}
	
	//parce east/west
	CountPlusOne = count+1;//D+1
	CountPlusTwo = count+2;
	
	#ifdef DiagPrintGNSS
	Serial.print(F("line number "));			
	Serial.print(__LINE__);
	Serial.print(F(".    Time Stamp  "));	
	Serial.println(millis() - StartForTimeStampULong);
	Serial.print(F(" count = "));
	Serial.println( count );
	Serial.print(F(" BufferText[CountPlusOne] = "));
	Serial.println( BufferText[CountPlusOne] );
	#endif
	
	switch(BufferText[CountPlusOne]){
	case ',':
		//East/West field is empty
		IncompleteSentenceBool = true;
		return;
	
	case 'E':
		NavigationDataByte[10] = 0;//0 means east
		count = count+2;//at delimeter after normal East/West   ("E")
		break;
		
	case 'W':
	NavigationDataByte[10] = 1;//1 means west
	count = count+2;//at delimeter after normal East/West   ("E")
	break;
	
	default:
	NavigationDataByte[10] = 202;//means unexpected value
	IncompleteSentenceBool = true;
	return;
	}
	
	if(BufferText[CountPlusTwo] != ',')
	{//unexpected character after E/W field. This check was added because I saw this anomoly once.
		IncompleteSentenceBool = true;
		return;
	}
	
}//end of ParceLongitude()

void SkipFourFields()
{
	//next, advance past 4 delimiter. Count is now at delimiter just after east/west field which is "E". We will increment count until we get to the 4th delimiter which is just before the altitude field.
	FieldCountByte = 0;
	while(FieldCountByte < 4)
	{
		if(BufferText[count] == ',')FieldCountByte = FieldCountByte +1;//if we find a ',', increment the count until we have traversed 4 fields which we are ignoring
		count=count+1;//advance to next character
		
		#ifdef DiagPrint1
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		Serial.print(F(" count = "));
		Serial.println( count );
		Serial.print(F(" BufferText[count] = "));
		Serial.println( BufferText[count] );
		#endif
		
	}
}//end of SkipFourFields()

void ParceAltitude()
{
	StartOfAltitudeByte = count;//count is now at the first digit of the altitude field which is "F+1"
	//start including digits until we reach either "." or ","
	EndOfFieldByte = StartOfAltitudeByte;// "F+1"
	
	#ifdef DiagPrint1
	Serial.print(F("line number "));			
	Serial.print(__LINE__);
	Serial.print(F(".    Time Stamp  "));	
	Serial.println(millis() - StartForTimeStampULong);
	Serial.print(F(" StartOfAltitudeByte = "));
	Serial.println( StartOfAltitudeByte );
	Serial.print(F(" BufferText[StartOfAltitudeByte] = "));
	Serial.println( BufferText[StartOfAltitudeByte] );	
	#endif
	
	if(BufferText[StartOfAltitudeByte] == ',')//if true, we are at H and altitude field is null
	{
		IncompleteSentenceBool = true;
		return;
	}else{
		//altitude field is not null but may not have a fractinal part 
		while((BufferText[EndOfFieldByte] != '.') && (BufferText[EndOfFieldByte] != ','))
		{	
			EndOfFieldByte++;//search for decimal and comma and if neither seen, advance EndOfField along field. Exit at "G" if decimal found or "H" if "," found. The numbers we need to process starts at count (F+1) and ends at EndOfFieldByte-1.
		
			#ifdef DiagPrint1
			Serial.print(F("line number "));			
			Serial.print(__LINE__);
			Serial.print(F(".    Time Stamp  "));	
			Serial.println(millis() - StartForTimeStampULong);
			Serial.print(F(" EndOfFieldByte = "));
			Serial.println( EndOfFieldByte );
			Serial.print(F(" BufferText[EndOfFieldByte] = "));
			Serial.println( BufferText[EndOfFieldByte] );
			#endif
			
		}

		EndOfFieldByte = EndOfFieldByte -1;//must back up one count because the above logic takes us to the decimal or delimiter
		
			#ifdef DiagPrint1
			Serial.print(F("line number "));			
			Serial.print(__LINE__);
			Serial.print(F(".    Time Stamp  "));	
			Serial.println(millis() - StartForTimeStampULong);
			Serial.print(F(" EndOfFieldByte = "));
			Serial.println( EndOfFieldByte );
			Serial.print(F(" BufferText[EndOfFieldByte] = "));
			Serial.println( BufferText[EndOfFieldByte] );
			#endif
			
		AltitudeULong = 0;
		long MultiplierLong = 1;//scales each number
		
		for(pointer = EndOfFieldByte; pointer >= StartOfAltitudeByte; pointer--)
		{//move from right (G-1) to left (F+1)
		
			#ifdef DiagPrint1
			Serial.print(F("line number "));			
			Serial.print(__LINE__);
			Serial.print(F(".    Time Stamp  "));	
			Serial.println(millis() - StartForTimeStampULong);
			Serial.print(F(" pointer = "));
			Serial.println( pointer );
			Serial.print(F(" BufferText[pointer] = "));
			Serial.println( BufferText[pointer] );
			Serial.print(F(" MultiplierLong = "));
			Serial.println( MultiplierLong );
			#endif
			
			AltitudeULong = AltitudeULong + long((BufferText[pointer] - 0x30) * MultiplierLong);
			MultiplierLong = MultiplierLong * 10;//advance to next highest number
			
			#ifdef DiagPrint1
			Serial.print(F("line number "));			
			Serial.print(__LINE__);
			Serial.print(F(".    Time Stamp  "));	
			//Serial.println(millis() - StartForTimeStampULong);
			Serial.print(F(" AltitudeULong = "));
			Serial.println( AltitudeULong );
			#endif
			
		}
		
		#ifdef DiagPrint1
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		Serial.print(F(" AltitudeULong = "));
		Serial.println( AltitudeULong );
		#endif
		
		/*******************************
		transfer AltitudeULong into array as 4 bytes
		
		byte	description
		11		altitude MSB (byte 3)
		12		altitude (byte 2)
		13		altitude (byte 1)
		14		altitude LSB(byte 0)
		*********************************/
		
		#ifdef DiagPrint1
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		#endif
		
		NavigationDataByte[11] = byte(AltitudeULong >> 24);//MSB: byte 3
		NavigationDataByte[12] = byte(AltitudeULong >> 16);//byte 2
		NavigationDataByte[13] = byte(AltitudeULong >> 8);//  byte 1
		NavigationDataByte[14] = byte(AltitudeULong);//LSB: byte 0
	}
	
	count = EndOfFieldByte + 1;//advance to character after integer alititude  "G" or "H"
	
	//ignore fractional altitude and advance count to next field
	
	#ifdef DiagPrint1
	Serial.print(F("line number "));			
	Serial.print(__LINE__);
	Serial.print(F(".    Time Stamp  "));	
	Serial.println(millis() - StartForTimeStampULong);
	Serial.print(F(" count = "));
	Serial.println( count );
	#endif
	
	GNSS_StartOfWaitingForDataMsULong = millis();
	while(BufferText[count] != ',')
	{
		#ifdef DiagPrint1
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		Serial.print(F(" count = "));
		Serial.println( count );
		Serial.print(F(" BufferText[count] = "));
		Serial.println( BufferText[count] );
		#endif
		
		count = count +1;
		if((millis()-GNSS_StartOfWaitingForDataMsULong) > GNSS_WaitingForDataTimeLimitMsULong)
			{
				IncompleteSentenceBool = true;
				return;
			}
	}
	
	//now we are at the delimeter preceeding units field: "H"
	
	#ifdef DiagPrint1
	Serial.print(F("line number "));			
	Serial.print(__LINE__);
	Serial.print(F(".    Time Stamp  "));	
	Serial.println(millis() - StartForTimeStampULong);
	#endif
		
	CountPlusOne  = count+1;//H+1
	
	#ifdef DiagPrint1
	Serial.print(F("line number "));			
	Serial.print(__LINE__);
	Serial.print(F(".    Time Stamp  "));	
	Serial.println(millis() - StartForTimeStampULong);
	Serial.print(F(" CountPlusOne = "));
	Serial.println( CountPlusOne );
	Serial.print(F(" BufferText[CountPlusOne] = "));
	Serial.println( BufferText[CountPlusOne] );
	#endif
		
	switch(BufferText[CountPlusOne]){//units field
		case ',':	
		
		#ifdef DiagPrint1
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		#endif
		
		IncompleteSentenceBool = true;
		return;

		case 'M':
		NavigationDataByte[15] = 0;//0 means metric
		
		#ifdef DiagPrint1
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		#endif
		
		break;
		
		case 'f':
		NavigationDataByte[15] = 1;//1 means feet but we this is not in the standards for the GPGGA sentence
		
		#ifdef DiagPrint1
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		#endif
		
		break;
		
		default:
		NavigationDataByte[15] = 202;//means unexpected value
		
		#ifdef DiagPrint1
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		#endif
		
		IncompleteSentenceBool = true;
		return;
	}
	
	#ifdef DiagPrint1
	Serial.print(F("line number "));			
	Serial.print(__LINE__);
	Serial.print(F(".    Time Stamp  "));	
	Serial.println(millis() - StartForTimeStampULong);
	Serial.print(F(" NavigationDataByte[15] = "));
	Serial.println( NavigationDataByte[15] );
	#endif
		
}//end of ParceAltitude()

void FindNeededSentence()
{
	GNSS_StartOfWaitingForDataMsULong = millis();
	
	#ifdef DiagPrint3
	Serial.print(F("line number "));			
	Serial.print(__LINE__);
	Serial.print(F(".    Time Stamp  "));	
	Serial.println(millis() - StartForTimeStampULong);
	#endif
	
	AtEndOfNeededSentenceBool = false;
	
	while(AtEndOfNeededSentenceBool == false)
	{//keep reading characters from GNSS until $GPGGA is detected or we time out
			
		while(Serial1.available() < 1)
		{//Buffer is empty but we will wait up to GNSS_WaitingForDataTimeLimitMsULong (5 seconds)
		
			if((millis()-GNSS_StartOfWaitingForDataMsULong) > GNSS_WaitingForDataTimeLimitMsULong)
			{
				
				#ifdef DiagPrint1
				Serial.print(F("line number "));			
				Serial.print(__LINE__);
				Serial.print(F(".    Time Stamp  "));	
				Serial.println(millis() - StartForTimeStampULong);
				#endif
				
				GNSS_TimedOutWaitingForDataBool = true;//timed out waiting for characters
				NavigationDataByte[0] = 201;//flags that this array should not be trusted because header not found			
				return;
			}
		}//when UART buffer has data, we continue.
		
		#ifdef DiagPrint3
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		#endif

		if((millis()-GNSS_StartOfWaitingForDataMsULong) > GNSS_WaitingForDataTimeLimitMsULong)
		{//timed out waiting for "$"
						
			#ifdef DiagPrint1
			Serial.print(F("line number "));			
			Serial.print(__LINE__);
			Serial.print(F(".    Time Stamp  "));	
			Serial.println(millis() - StartForTimeStampULong);
			#endif
		
			GNSS_TimedOutWaitingForDataBool = true;
			NavigationDataByte[0] = 201;//flags that this array should not be trusted because header not found
			
			#ifdef DiagPrint3
			Serial.print(F("line number "));			
			Serial.print(__LINE__);
			Serial.print(F(".    Time Stamp  "));	
			Serial.println(millis() - StartForTimeStampULong);
			#endif
				
			return;
		}

		character = Serial1.read();//only get here when buffer has at least one character
		
		#ifdef DiagPrint3	
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		Serial.print(F("+"));
		Serial.println( character );
		#endif
		
		if (character == '$')
		{//at start of a sentence. Now see if it is the one we need
			GNSS_StartOfWaitingForDataMsULong = millis();//initialize timer to protect against hanging due to no inputs from GPS
						
			#ifdef DiagPrint3
			Serial.print(F("line number "));			
			Serial.print(__LINE__);
			Serial.print(F(".    Time Stamp  "));	
			Serial.println(millis() - StartForTimeStampULong);
			#endif
			
			GNSS_TimedOutWaitingForDataBool = false;//because we found $ but it will change to true if we can't find right header 
			ReadHeader();
			
			#ifdef DiagPrint3
			Serial.print(F("line number "));			
			Serial.print(__LINE__);
			Serial.print(F(".    Time Stamp  "));	
			Serial.println(millis() - StartForTimeStampULong);
			#endif
			
			if(GNSS_TimedOutWaitingForDataBool)return;
			
			#ifdef DiagPrint3
			Serial.print(F("line number "));			
			Serial.print(__LINE__);
			Serial.print(F(".    Time Stamp  "));	
			Serial.println(millis() - StartForTimeStampULong);
			#endif
			
			if (BufferText.indexOf("GPGGA") == 0)
			{//found needed header 
			
				#ifdef DiagPrint3
				Serial.print(F("line number "));			
				Serial.print(__LINE__);
				Serial.print(F(".    Time Stamp  "));	
				Serial.println(millis() - StartForTimeStampULong);
				#endif

				ReadSentence();//it controls AtEndOfNeededSentenceBool flag so controls the while() around it. End of realtime critical code
							
				#ifdef DiagPrint3
				Serial.print(F("line number "));			
				Serial.print(__LINE__);
				Serial.print(F(".    Time Stamp  "));	
				Serial.println(millis() - StartForTimeStampULong);
				#endif
			
				if(GNSS_TimedOutWaitingForDataBool)return;
			
				#ifdef DiagPrint3
				Serial.print(F("line number "));			
				Serial.print(__LINE__);
				Serial.print(F(".    Time Stamp  "));	
				Serial.println(millis() - StartForTimeStampULong);
				#endif
				
			}
			
			#ifdef DiagPrint3
			Serial.print(F("line number "));			
			Serial.print(__LINE__);
			Serial.print(F(".    Time Stamp  "));	
			Serial.println(millis() - StartForTimeStampULong);
			#endif
			
		}
			
			#ifdef DiagPrint3
			Serial.print(F("line number "));			
			Serial.print(__LINE__);
			Serial.print(F(".    Time Stamp  "));	
			Serial.println(millis() - StartForTimeStampULong);
			#endif
		
	}//if correct sentence found, we leave while() loop. Otherwise, we keep scanning for $GPGGA until we time out.
	
	#ifdef DiagPrint5
	Serial.print(F("#"));			
	Serial.println(__LINE__);
	Serial.print(F(" sentence read in is "));
	Serial.println( BufferText );
	#endif
}//end of FindNeededSentence()

#ifdef GNSStest
void PrintOutGNSS_Array()
{//diag 
		#ifdef DiagPrint5
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		#endif
		
		for(i = 0;i<16;i++)
		{
		Serial.print( NavigationDataByte[i] );
		Serial.print(F(","));
		}
		Serial.println();
		
		InitializeNavigationDataByteArray();//done with array so initialize it.
}
#endif

void ReadHeader()
{
	count = 0;
	BufferText = "";
	
		#ifdef DiagPrint3
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		Serial.print(F(" BufferText = "));
		Serial.print( BufferText );
		Serial.println("*");
		#endif
	
	while(count < 5)
	{//record header which is 5 characters
		while(Serial1.available() < 1)
		{
			if((millis()-GNSS_StartOfWaitingForDataMsULong) > GNSS_WaitingForDataTimeLimitMsULong)
			{
				GNSS_TimedOutWaitingForDataBool = true;//timed out waiting for characters
				NavigationDataByte[0] = 201;//flags that this array should not be trusted because header not found
				
				#ifdef DiagPrint3
				Serial.print(F("line number "));			
				Serial.print(__LINE__);
				Serial.print(F(".    Time Stamp  "));	
				Serial.println(millis() - StartForTimeStampULong);
				#endif
				
				return;
			}
		}
		//when buffer has data, we continue.
		character = Serial1.read();
		
		#ifdef DiagPrint3
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		Serial.print(F("                 character = "));
		Serial.println( character );
		#endif
		
		BufferText.concat(character);
		count++;
	}
	
		#ifdef DiagPrint3
		Serial.print(F("line number "));			
		Serial.print(__LINE__);
		Serial.print(F(".    Time Stamp  "));	
		Serial.println(millis() - StartForTimeStampULong);
		Serial.print(F(" BufferText = "));
		Serial.println( BufferText );
		#endif
		
}

void ReadSentence()
{
	//Serial.println(__LINE__);

	BufferText = "";//prepare to record all characters starting after header
	count = 0;
	while(AtEndOfNeededSentenceBool == false)
	{//record all characters after header until carriage return

		while(Serial1.available() < 1)
		{
			if((millis()-GNSS_StartOfWaitingForDataMsULong) > GNSS_WaitingForDataTimeLimitMsULong)
			{
				GNSS_TimedOutWaitingForDataBool = true;//timed out waiting for characters
				NavigationDataByte[0] = 201;//flags that this array should not be trusted because header not found
				
				#ifdef DiagPrint1
				Serial.print(F("line number "));			
				Serial.print(__LINE__);
				Serial.print(F(".    Time Stamp  "));	
				Serial.println(millis() - StartForTimeStampULong);
				#endif
				
				return;
			}
		}
	//only get here when buffer has data
	character = Serial1.read();		
	
	  if (character == '\r')
	  {
		  AtEndOfNeededSentenceBool = true;//found carriage return so at end of the sentence. Setting this flag releases us from the while()
		  
		  //Serial.println();
		  //Serial.print(F("line number "));			
		  //Serial.print(__LINE__);
		  //Serial.print(F(".    Time Stamp  "));	
		  //Serial.println(millis() - StartForTimeStampULong);
		  
	  }else{
		  BufferText.concat(character);//if not at end of sentence, keep recording
		  
		  /*Serial.print(F("line number "));			
		  Serial.print(__LINE__);
		  Serial.print(F(".    Time Stamp  "));	
		  Serial.println(millis() - StartForTimeStampULong);*/
		  
	  }
		
	}//now have all characters in needed sentence
}