//************************************************************************
// NMEA LOAD FROM rx GPS

#include <stdint.h>
#include <math.h>
#include <stdlib.h>
#include "nmea_scan.h"
#include "gps.h"

enum {ID_RX_GGA,ID_RX_GSA,ID_RX_GSV,ID_RX_RMC,ID_RX_VTG,ID_RX_LOAD,ID_RX_FIND};	

uint8_t nmea_start_load(char data)
{
	enum {NM_G,NM_P,NM_GP,NM_GPG,NM_GPR,NM_GPV,NM_GPGG,NM_GPGS,NM_GPRM,NM_GPVT};
	static uint8_t ptr = NM_G;
	
	switch (ptr)
	{
		case NM_G: if (data == 'G') ptr = NM_P;					//G
				else { ptr = NM_G; return ID_RX_FIND;}
				break; 
		case NM_P: if (data == 'P') ptr = NM_GP;				//P
				else { ptr = NM_G; return ID_RX_FIND;}
				break; 
		case NM_GP: switch (data)								//GPG || GPR || GPV
				{	
					case 'G': ptr=NM_GPG;break;
					case 'R': ptr=NM_GPR;break;
					case 'V': ptr=NM_GPV;break;
					default : ptr = NM_G; return ID_RX_FIND;
				}
				break;
		case NM_GPG: switch(data)								//GPGG || GPGS
				{	
					case 'G': ptr=NM_GPGG;break;
					case 'S': ptr=NM_GPGS;break;
					default : ptr = NM_G; return ID_RX_FIND;
				}
				break;
		case NM_GPR: if (data == 'M') ptr = NM_GPRM;			//GPRM
				else { ptr = NM_G; return ID_RX_FIND;}
				break; 			
		case NM_GPV: if (data == 'T') ptr = NM_GPVT;			//GPVT
				else { ptr = NM_G; return ID_RX_FIND;}
				break;
		case NM_GPGG: ptr = NM_G;								//GPGGA
				if (data == 'A') return ID_RX_GGA;			
				return ID_RX_FIND;
		case NM_GPGS: ptr = NM_G;
				switch(data)								//GPGG || GPGS
				{	
					case 'A': return ID_RX_GSA;
					case 'V': return ID_RX_GSV;
				}
				return ID_RX_FIND;
		case NM_GPRM: ptr = NM_G;								//GPRMC
				if (data == 'C') return ID_RX_RMC;			
				return ID_RX_FIND;
		case NM_GPVT: ptr = NM_G;								//GPVTG
				if (data == 'G') return ID_RX_VTG;			
				return ID_RX_FIND;
		default: ptr = NM_G; return ID_RX_FIND;
				
	}
	return ID_RX_LOAD;
}

uint8_t  load_nmea(uint8_t rx_shift, char *rx_buf,char *scan_buf)
{
	static uint8_t id_rx_msg = ID_RX_FIND;
	static uint8_t rx_now = 0;
	static uint8_t ptr = 0;
	char data;

	while (1)
	{
	if (rx_shift == rx_now) return RETURN_RX;
	if(++rx_now >= MAX_RX_BUF) rx_now = 0;	
	data = *(rx_buf + rx_now);

	switch (id_rx_msg)
	{
		case ID_RX_GGA:		*(scan_buf+ptr++) = data;
							if (data == '*') {id_rx_msg = ID_RX_FIND; return RETURN_GGA;}
							if (ptr >= MAX_NMEA_LOAD) id_rx_msg = ID_RX_FIND;
							break;
		case ID_RX_GSA:		*(scan_buf+ptr++) = data;
							if (data == '*') {id_rx_msg = ID_RX_FIND; return RETURN_GSA;}
							if (ptr >= MAX_NMEA_LOAD) id_rx_msg = ID_RX_FIND;
							break;
		case ID_RX_GSV: 	*(scan_buf+ptr++) = data;
							if (data == '*') {id_rx_msg = ID_RX_FIND; return RETURN_GSV;}
							if (ptr >= MAX_NMEA_LOAD) id_rx_msg = ID_RX_FIND;
							break;	
		case ID_RX_RMC: 	*(scan_buf+ptr++) = data;
							if (data == '*') {id_rx_msg = ID_RX_FIND; return RETURN_RMC;}
							if (ptr >= MAX_NMEA_LOAD) id_rx_msg = ID_RX_FIND;
							break;	
		case ID_RX_VTG: 	*(scan_buf+ptr++) = data;
							if (data == '*') {id_rx_msg = ID_RX_FIND;return RETURN_VTG;}
							if (ptr >= MAX_NMEA_LOAD) id_rx_msg = ID_RX_FIND;
							break;
		case ID_RX_LOAD:	id_rx_msg = nmea_start_load(data);	break;	
		case ID_RX_FIND:
		default: 			ptr=0;if (data == '$') id_rx_msg = ID_RX_LOAD;
			
	}
	}
	return 0 ;
}

void nmea_gga(char *buf,DATA_GPS *pgps)
{
	uint8_t a;

	if (*(buf++) != ',') return;								// ,
	for (a=0;a<5;a++) while (*(buf++) != ',');					// UTC,lat,N,lot,W,
	pgps->fix_position = *(buf++) - 0x30;						// fix_position
	if (*(buf++) != ',') return;								// ,
	pgps->satelites_used = 10 * (*(buf++) - 0x30) ;				// satelites
	pgps->satelites_used += *(buf++)-0x30;
	if (*(buf++) != ',') return;								// ,
	while (*(buf++) != ',');									// HDOP,
	pgps->altitude = atof(buf++);								// atlitude,
	for (a=0;a<2;a++) while (*(buf++) != ',');					// ,M,
	pgps->geoid = atof(buf++);									// geoid
	for (a=0;a<2;a++) while (*(buf++) != ',');					//,M,
	if (*buf != ',')
	{
	 	pgps->age_diff_corr = atol(buf);						// age_dif_corr,
		while (*(buf++) != ',');
	}
	else buf++;
	if (*buf != ',') pgps->diff_id = atol(buf);					// ID_diff_station
}

void nmea_gsa(char *buf,DATA_GPS *pgps)
{
	uint8_t a;

	if (*(buf++) != ',') return;								// ,
	pgps->mode1 = *(buf++);										// mode1
	if (*(buf++) != ',') return;								// ,
	pgps->mode2 = *(buf++);										// mode2
	if (*(buf++) != ',') return;								// ,
	for (a=0;a<12;a++)
	{
		if (*buf != ',')
		{
			//pgps->satelite_id[a] = 10*(*(buf++) - 0x30);
			//pgps->satelite_id[a] += *(buf++)-0x30;
			//buf++;
			pgps->satelite_id[a] = atol(buf);
			while (*(buf++) != ',');
		}
		else
		{
		 	pgps->satelite_id[a] = 0;
		 	buf++;
		 }
	}
	pgps->PDOP = atof(buf++);
	while (*(buf++) != ',');
	pgps->HDOP = atof(buf++);
	while (*(buf++) != ',');
	pgps->VDOP = atof(buf++);
	while (*(buf++) != ',');
}

void nmea_gsv(char *buf,DATA_GPS *pgps)
{
	uint8_t a,i,b;

	if (*(buf++) != ',') return;									// ,
	pgps->gsv_num_msg = (*(buf++) - 0x30);							// num of msg
	if (*(buf++) != ',') return;									//,
	pgps->gsv_msg = (*(buf++) - 0x30);								// num msg
	if (*(buf++) != ',') return;									//,
	pgps->gsv_satelites_view = 10*(*(buf++) - 0x30);				// satelites view
	pgps->gsv_satelites_view +=(*(buf++) - 0x30);
	if (*(buf++) != ',') return;									// ,
	a = 4 * (pgps->gsv_msg - 1);
	for (i= 0; i<4; i++)
	{
		/*pgps->satelit_detail[a].id = 10*(*(buf++) - 0x30);			
		 pgps->satelit_detail[a].id += *(buf++) - 0x30; 			
		buf++;
		pgps->satelit_detail[a].elevation = 10*(*(buf++) - 0x30);
		 pgps->satelit_detail[a].elevation += *(buf++) - 0x30;
		buf++;
		pgps->satelit_detail[a].azimut = 100*(*(buf++) - 0x30);
		 pgps->satelit_detail[a].azimut += 10*(*(buf++) - 0x30);
		 pgps->satelit_detail[a].azimut += *(buf++) - 0x30;
		buf++;*/
		pgps->satelit_detail[a].id = atol(buf);
 		 while (*(buf++) != ',');
		pgps->satelit_detail[a].elevation = atol(buf);
 		 while (*(buf++) != ',');
		pgps->satelit_detail[a].azimut = atol(buf);
 		 while (*(buf++) != ',');

		if ((*buf) != ',')
		{
			if (*(buf) == '*')
			{
				pgps->satelit_detail[a].SNR =0;
				return;
			}
			//pgps->satelit_detail[a].SNR = 10*(*(buf++) - 0x30);
		 	 //pgps->satelit_detail[a].SNR += *(buf++) - 0x30;
			 pgps->satelit_detail[a].SNR = atol(buf);
 		 	  b= *(buf++);
			  while ((b != ',') && (b != '*') ) b=*(buf++);
			 if (b == '*') return;
		}
		else 
		{
			pgps->satelit_detail[a].SNR =0;
			buf++;
		}
		if ( ++a >= pgps->gsv_satelites_view ) return;
		
	}								
}

//$GPRMC,161229.487,A,3723.2475,N,12158.3416,W,0.13,309.62,120598, ,*10
void nmea_rmc(char *buf,DATA_GPS *pgps)
{
	uint8_t a;	

	if (*(buf++) != ',') return;									// ,
	pgps->hour = (10* (*(buf++) - 0x30));							//hour
	 pgps->hour += (*(buf++) - 0x30);
	pgps->minute = 10* (*(buf++) - 0x30);							//minute
	 pgps->minute += (*(buf++) - 0x30);
	pgps->second = 10* (*(buf++) - 0x30);							//second
	 pgps->second += (*(buf++) - 0x30);
	while (*(buf++) != ',');										//.xxx	
	pgps->status = *(buf++);
	if (*(buf++) != ',') return;									//A,					
	pgps->latitude =10.0 * (*(buf++) - 0x30);
	 pgps->latitude += 1.0 * (*(buf++) - 0x30);
	 pgps->latitude += atof(buf)/60.0;
	 while (*(buf++) != ',');										//latitude,
	pgps->ns_indicator =  *(buf++);									// N/S
	if (*(buf++) != ',') return;									//,
	pgps->longitude = 100.0 * (*(buf++) - 0x30);
	 pgps->longitude += 10.0 * (*(buf++) - 0x30);
	 pgps->longitude += 1.0 * (*(buf++) - 0x30);
	 pgps->longitude += atof(buf)/60.0;
	while (*(buf++) != ',');										//longitude,
	pgps->we_indicator =  *(buf++);									// E/W
	for (a=0;a<3;a++) while (*(buf++) != ',');						// ,speed,course,
	pgps->day = 10* (*(buf++) - 0x30);	
	 pgps->day += *(buf++) - 0x30;									// day
	pgps->month = 10* (*(buf++) - 0x30);	
	 pgps->month += *(buf++) - 0x30;								// month
	pgps->year = 10* (*(buf++) - 0x30);
	 pgps->year += *(buf) - 0x30;									// year
		
}

void nmea_vtg(char *buf,DATA_GPS *pgps)
{
	uint8_t a;

	if (*(buf++) != ',') return;							// ,
	pgps->course = atof(buf++);								// course
	for (a=0;a<6;a++) while (*(buf++) != ',');				// ,T,course,M,speed,N,
	pgps->speed = atof(buf++);
}