#include "main.h"
#include "time.h"
#include "rtklib.h"

/* 在rtklib 中定义结构体  */



#define MAXFIELD 64 /* max number of fields in a record */

typedef enum fsm_err
{
	FSM_EOK = 0,
	FSM_ERROR,
	FSM_EINVAL,
} fsm_err_t;

enum
{
	FSM_STATE_INIT = 0,
	FSM_STATE_ABORT,
	FSM_STATE_COMPLETE,
	FSM_STATE_NAME_RECV,
	FSM_STATE_ARGS_RECV,
	FSM_STATE_CSUM_RECV,
	FSM_STATE_CRLF_RECV,
	FSM_STATE_MAX,
};

typedef enum
{
	FSM_EVENT_DOLLAR_CHAR = 0,
	FSM_EVENT_ORDINARY_CHAR,
	FSM_EVENT_COMMA_CHAR,
	FSM_EVENT_ASTERISK_CHAR,
	FSM_EVENT_CR_CHAR,
	FSM_EVENT_LF_CHAR,
	FSM_EVENT_NUL_CHAR,
	FSM_EVENT_MAX,
	FSM_EVENT_G_CHAR,
	FSM_EVENT_DATA,
	FSM_EVENT_CHEAK1,
	FSM_EVENT_CHEAK2,
	FSM_EVENT_FINISH,
	FSM_EVENT_IDLE,
} fsm_event_t;

enum
{
	FSM_ERROR_DOLLAR_REPEAT = 1,
	FSM_ERROR_ORDINARY_EARLY,
	FSM_ERROR_ORDINARY_REPEAT,
	FSM_ERROR_COMMA_EARLY,
	FSM_ERROR_COMMA_REPEAT,
	FSM_ERROR_ASTERISK_EARLY,
	FSM_ERROR_ASTERISK_REPEAT,
	FSM_ERROR_CR_EARLY,
	FSM_ERROR_CR_REPEAT,
	FSM_ERROR_LF_EARLY,
	FSM_ERROR_LF_REPEAT,
	FSM_ERROR_NUL_EARLY,
};

static int nmea_xor(uint8_t *buff, int len)
{
	int i = 0;
	int result = buff[1];
	int ans = 0;
	for (i = 2; buff[i] != '*' && i < len - 2; i++)
	{
		result ^= buff[i];
	}
	if (buff[++i] > '9')
	{
		ans += 10 + buff[i] - 'A';
	}
	else
	{
		ans += buff[i] - '0';
	}
	ans *= 16;
	if (buff[++i] > '9')
	{
		ans += 10 + buff[i] - 'A';
	}
	else
	{
		ans += buff[i] - '0';
	}
	if (ans == result)
		return 1;
	else
		return 0;
}
/* convert calendar day/time to time -------------------------------------------
* convert calendar day/time to gtime_t struct
* args   : double *ep       I   day/time {year,month,day,hour,min,sec}
* return : gtime_t struct
* notes  : proper in 1970-2037 or 1970-2099 (64bit time_t)
*-----------------------------------------------------------------------------*/
extern gtime_t epoch2time(const double *ep)
{
    const int doy[]={1,32,60,91,121,152,182,213,244,274,305,335};
    gtime_t time={0};
    int days,sec,year=(int)ep[0],mon=(int)ep[1],day=(int)ep[2];
    
    if (year<1970||2099<year||mon<1||12<mon) return time;
    
    /* leap year if year%4==0 in 1901-2099 */
    days=(year-1970)*365+(year-1969)/4+doy[mon-1]+day-2+(year%4==0&&mon>=3?1:0);
    sec=(int)floor(ep[5]);
    time.time=(time_t)days*86400+(int)ep[3]*3600+(int)ep[4]*60+sec;
    time.sec=ep[5]-sec;
    return time;
}

/* convert time in nmea format to time ---------------------------------------*/
static void septime(double t, double *t1, double *t2, double *t3)
{
	*t1 = floor(t / 10000.0);
	t -= *t1 * 10000.0;
	*t2 = floor(t / 100.0);
	*t3 = t - *t2 * 100.0;
}

/* convert ddmm.mm in nmea format to deg -------------------------------------*/
static double dmm2deg(double dmm)
{
	return floor(dmm / 100.0) + fmod(dmm, 100.0) / 60.0;
}

/* decode NMEA RMC (Recommended Minumum Specific GNSS Data) sentence ---------*/
static int decode_nmearmc(char **val, int n, sol_t *sol)
{
	double tod = 0.0, lat = 0.0, lon = 0.0, vel = 0.0, dir = 0.0, date = 0.0, ang = 0.0, ep[6];
	double pos[3] = {0};
	char act = ' ', ns = 'N', ew = 'E', mew = 'E', mode = 'A';
	int i;

	// trace(4, "decode_nmearmc: n=%d\n", n);

	for (i = 0; i < n; i++)
	{
		switch (i)
		{
		case 0:
			tod = atof(val[i]);
			break; /* time in utc (hhmmss) */
		case 1:
			act = *val[i];
			break; /* A=active,V=void */
		case 2:
			lat = atof(val[i]);
			break; /* latitude (ddmm.mmm) */
		case 3:
			ns = *val[i];
			break; /* N=north,S=south */
		case 4:
			lon = atof(val[i]);
			break; /* longitude (dddmm.mmm) */
		case 5:
			ew = *val[i];
			break; /* E=east,W=west */
		case 6:
			vel = atof(val[i]);
			break; /* speed (knots) */
		case 7:
			dir = atof(val[i]);
			break; /* track angle (deg) */
		case 8:
			date = atof(val[i]);
			break; /* date (ddmmyy) */
		case 9:
			ang = atof(val[i]);
			break; /* magnetic variation */
		case 10:
			mew = *val[i];
			break; /* E=east,W=west */
		case 11:
			mode = *val[i];
			break; /* mode indicator (>nmea 2) */
				   /* A=autonomous,D=differential */
				   /* E=estimated,N=not valid,S=simulator */
		}
	}
	if ((act != 'A' && act != 'V') || (ns != 'N' && ns != 'S') || (ew != 'E' && ew != 'W'))
	{
		// trace(3, "invalid nmea rmc format\n");
		return 0;
	}
	pos[0] = (ns == 'S' ? -1.0 : 1.0) * dmm2deg(lat) * D2R;
	pos[1] = (ew == 'W' ? -1.0 : 1.0) * dmm2deg(lon) * D2R;
	septime(date, ep + 2, ep + 1, ep);
	septime(tod, ep + 3, ep + 4, ep + 5);
	ep[0] += ep[0] < 80.0 ? 2000.0 : 1900.0;
	sol->utctime=epoch2time(ep);
	sol->time = utc2gpst(sol->utctime);
	pos2ecef(pos, sol->rr);
	sol->stat = mode == 'D' ? SOLQ_DGPS : SOLQ_SINGLE;
	sol->ns = 0;

	sol->type = 0; /* postion type = xyz */

	//	trace(1, "decode_nmearmc: %s rr=%.3f %.3f %.3f stat=%d ns=%d vel=%.2f dir=%.0f ang=%.0f mew=%c mode=%c \r\n",
	//		  time_str(sol->time, 0), sol->rr[0], sol->rr[1], sol->rr[2], sol->stat, sol->ns,
	//		  vel, dir, ang, mew, mode);

	// trace(3, "decode_nmearmc: %s rr=%.3f %.3f %.3f stat=%d ns=%d  \r\n",
	// 	  time_str(sol->time, 0), sol->rr[0], sol->rr[1], sol->rr[2], sol->stat, sol->ns);

	return 2; /* update time */
}
static int decode_nmea(char *buff, sol_t *sol)
{
	char *p, *q, *val[MAXFIELD];
	int n = 0;

	// trace(4, "decode_nmea: buff=%s\n", buff);

	/* parse fields */
	for (p = buff; *p && n < MAXFIELD; p = q + 1)
	{
		if ((q = strchr(p, ',')) || (q = strchr(p, '*')))
		{
			val[n++] = p;
			*q = '\0';
		}
		else
			break;
	}
	if (n < 1)
	{
		return 0;
	}
	if (!strcmp(val[0] + 3, "RMC"))
	{ /* $xxRMC */
		return decode_nmearmc(val + 1, n - 1, sol);
	}
	// else if (!strcmp(val[0] + 3, "ZDA"))
	//{ /* $xxZDA */
	//     return decode_nmeazda(val + 1, n - 1, sol);
	// }
	// else if (!strcmp(val[0] + 3, "GGA"))
	//{ /* $xxGGA */
	//     return decode_nmeagga(val + 1, n - 1, sol);
	// }
	return 0;
}

// extern int input_nmea(nmea_t *nmea, uint8_t data)
// {
// 	static uint8_t state = 0;
// 	int8_t ans = 0;
// 	trace(5, "input_nmea: data=%02x\n", data);

// 	if (nmea->nbyte == 0 && data == '$')
// 	{
// 		state = 1;
// 	}
// 	else if (nmea->nbyte != 0 && data == '*')
// 	{
// 		state = 2;
// 	}
// 	else if (nmea->len != 0)
// 	{
// 		state = 3;
// 	}

// 	if (state == 1)
// 	{
// 		nmea->buff[nmea->nbyte++] = data;
// 		if(nmea->nbyte == 2 && nmea->buff[1] != 'G')
// 		{
// 			ans = -1;
// 		}
// 	}
// 	else if (state == 2)
// 	{
// 		nmea->buff[nmea->nbyte++] = data;
// 		nmea->len = nmea->nbyte + 2;
// 	}
// 	else if (state == 3)
// 	{
// 		nmea->buff[nmea->nbyte++] = data;
// 		if (nmea->nbyte == nmea->len) /* �����ռ���� ����У�� */
// 		{
// 			if (0 == nmea_xor(nmea->buff, nmea->len))
// 			{
// 				trace(1, "nmea parity error: len=%d \n", nmea->len);
// 				ans = -1;
// 			}
// 			else
// 			{
// 				ans = 1;
// 			}
// 		}
// 	}
// 	if (ans == 1)
// 	{
// 		return decode_nmea((char *)nmea->buff, &(nmea->sol));
// 	}
// 	if(nmea->nbyte >=255) /* Խ�� */
// 	{
// 		ans = -1;
// 	}
// 	if (ans != 0)
// 	{
// 		nmea->len = 0;
// 		nmea->nbyte = 0;
// 		state = 0;
// 	}

// 	return ans;
// }

extern int input_nmea(nmea_t *nmea, const uint8_t data)
{
	static fsm_event_t event = 0;
	int8_t ans = 0;
	// trace(5, "input_nmea: data=%02x\n", data);

	switch (data)
	{
	case '$':
		event = FSM_EVENT_DOLLAR_CHAR;
		break;
	case '*':
		if(event == FSM_EVENT_DATA)
		event = FSM_EVENT_ASTERISK_CHAR;
		else
		event = FSM_EVENT_IDLE;
		break;
	default:
		break;
	}

	switch (event)
	{
	case FSM_EVENT_DOLLAR_CHAR:
		nmea->nbyte = 0;
		nmea->len = 0;
		nmea->buff[nmea->nbyte++] = data;
		event = FSM_EVENT_G_CHAR;
		break;
	case FSM_EVENT_G_CHAR:
		if (data == 'G')
		{
			nmea->buff[nmea->nbyte++] = data;
			event = FSM_EVENT_DATA;
		}
		else
		{
			event = FSM_EVENT_IDLE;
		}
		break;
	case FSM_EVENT_DATA:
		if (nmea->nbyte > 168)
		{
			event = FSM_EVENT_IDLE;
		}
		else
		{
			nmea->buff[nmea->nbyte++] = data;
		}
		break;
	case FSM_EVENT_ASTERISK_CHAR:
		nmea->buff[nmea->nbyte++] = data;
		nmea->len = nmea->nbyte + 2;
		event = FSM_EVENT_CHEAK1;
		break;
	case FSM_EVENT_CHEAK1:
		nmea->buff[nmea->nbyte++] = data;
		event = FSM_EVENT_CHEAK2;
		break;
	case FSM_EVENT_CHEAK2:
		nmea->buff[nmea->nbyte++] = data;
		event = FSM_EVENT_FINISH;
		break;
	default:
		break;
	}
	if (event == FSM_EVENT_FINISH)
	{
		event = FSM_EVENT_IDLE;
		if (0 == nmea_xor(nmea->buff, nmea->len))
		{
			// trace(3, "nmea parity error: len=%d \r\n", nmea->len);
			ans = -1;
		}
		else
		{
			ans = 1;
			return decode_nmea((char *)nmea->buff, &(nmea->sol));
		}
	}

	return ans;
}