/*********************************************************
YY add
Parse u-blox GPS nmea data
*/
#include "parse_gps.h"
#include "../jni_log.h"

#include <ctype.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#define DEBUG(fmt, args...)     LOGD("<parse_gps> <%s>: " fmt, __func__, ##args)

typedef const uint8_t *ptr;

static ptr skip1(ptr s, ptr e)
{
	if(s)
		while(s != e && (*s == ' ' || *s == '\t' || *s == ',' || *s == ';'))
			++s;
	return s;
}

static ptr match2(ptr s, ptr e)
{
	return s && s != e && (*s == '\r' || *s == '\n') ? s+1 : 0;
}

static ptr matchChar(ptr s, ptr e, uint8_t c)
{
	return s && s != e && toupper(*s) == (int) c ? s+1 : 0;
}

static ptr skipQuote(ptr p, ptr e)
{
	return p && p != e && *p == '"' ? p+1 : p;
}

static ptr getString(ptr p, ptr e, struct nmea_sequence *res)
{
	p = skipQuote(p, e);
	if(p)
	{
		ptr s = p;
		while(p != e && (isalnum(*p) || *p==' ' || *p=='+' || *p=='^' || *p=='-' || *p=='_' || *p=='#' || *p=='.' || *p=='/' || *p==':')) ++p;
		if(p != s)
		{
			res->data = s;
			res->length = p - s;
			return skipQuote(p, e);
		}
	}
	res->data = 0;
	res->length = 0;

	return (p != 0 && (*p == ',' || *p == '*')) ? p : 0;
}

static ptr getHexNumber(ptr p, ptr e, ptr start)
{
	if(p)
	{
		ptr s = p;
		uint16_t r = 0;
		uint8_t check = 0;

		while(p != e && isalnum(*p)) {
			if(isdigit(*p)) {
				r = (r << 4) + (*p++ - '0');
			}
			else {
				r = (r << 4) + (toupper(*p++) - 'A' + 10);
			}
		}
		if(p != s) {

			while(start != s-1) {
				check ^= *start++;
			}

			if (check != r) DEBUG("crc error exp %02X but %02X", check, r);
		}

		return ( p != s && check == r )? p : 0;
	}
	return 0;
}

static ptr getHexNumber2(ptr p, ptr e, ptr start)
{
	if(p)
	{
		ptr s = p;
		uint32_t r = 0;

		while(p != e && isalnum(*p)) {
			if(isdigit(*p)) {
				r = (r << 4) + (*p++ - '0');
			}
			else {
				r = (r << 4) + (toupper(*p++) - 'A' + 10);
			}
		}
		if(p != s) {

		}

		return ( p != s )? p : 0;
	}
	return 0;
}

static ptr findEndOfPacket(ptr s, ptr e)
{
	if(s == e) return 0;

	if(*s == '\r' || *s == '\n')
	{
		return s+1;
	}
	else
	{
		/* expecting ... <cr> */
		ptr p = s;
		while(p != e && (*p == ' ' || *p == '\t')) ++p;
		while(p != e && *p != '\r') ++p;
		return p == e ? 0 : p+1;
	}
}

static ptr getNMEA(ptr s, ptr e, struct nmea *res)
{
	res->nmea_num = 0;
	while(s && s != e && *s != '\r' && *s != '\n' && *s != '*') {
		if(s = matchChar(s, e, ',')) {
			struct nmea_sequence dat;

//			LOGD("g = %d", res->nmea_num);
			s = getString(s, e, &dat);
			if(res->nmea_num < MAX_NAEA_NUM) {
				res->nmea_value[res->nmea_num] = dat;
				res->nmea_num += 1;
			}
		}
		else {
			s = 0;
		}
	}
	return s;
}

static ptr getPARAMS(ptr s, ptr e, struct nmea *res)
{
	res->nmea_num = 0;

	while (s && s != e && *s != '\r' && *s != '\n') {
		while(s != e && (*s == ' ' || *s == '\t')) ++s;

		struct nmea_sequence dat;
		s = getString(s, e, &dat);
		if(res->nmea_num < MAX_NAEA_NUM) {
			res->nmea_value[res->nmea_num] = dat;
			res->nmea_num += 1;
		}

		while (s != e && (*s == ',')) ++s;
	}

	return s;
}

static ptr getNMEA2(ptr s, ptr e, struct nmea *res)
{
    bool first = true;

	res->nmea_num = 0;

	while(s && s != e && *s != '\r' && *s != '\n' && *s != '*') {
		if((first && (s=matchChar(s, e, ';'))) || (s=matchChar(s, e, ','))) {
		    first = false;

			struct nmea_sequence dat;
//			LOGD("g = %d", res->nmea_num);
			s = getString(s, e, &dat);

			if(res->nmea_num < MAX_NAEA_NUM) {
				res->nmea_value[res->nmea_num] = dat;
				res->nmea_num += 1;
			}
		}
		else {
			s = 0;
		}
	}
	return s;
}

static ptr findSemicolon(ptr s, ptr e)
{
	while (s && s != e && *s != ';') {
		s++;
	}
	return s;
}

ptr parseGPS(ptr s, ptr e)
{
	ptr p;
	for(; (p = findEndOfPacket(s, e)) != 0; s = p)
	{
		if(p == s+1)
		{
			/* Silently discard one character; no packets are that short */
			continue;
		}
		else
		{//GPGGA   GPGSA   GPGSV    GPRMC    GPVTG    GPGLL    GPZDA
			ptr start = s;
			struct nmea nmeas;

			if(match2(getHexNumber(matchChar(getNMEA(matchChar(matchChar(matchChar(matchChar(matchChar(start = matchChar(skip1(s, e), e, '$'), e, 'G'), e, 'P'), e, 'G'), e, 'G'), e, 'A'), e, &nmeas), e, '*'), e, start), e)) {
				handleGPGGA(&nmeas);
				continue;
			}
			if(match2(getHexNumber(matchChar(getNMEA(matchChar(matchChar(matchChar(matchChar(matchChar(start = matchChar(skip1(s, e), e, '$'), e, 'G'), e, 'P'), e, 'G'), e, 'S'), e, 'A'), e, &nmeas), e, '*'), e, start), e)) {
				handleGPGSA(&nmeas);
				continue;
			}
			if(match2(getHexNumber(matchChar(getNMEA(matchChar(matchChar(matchChar(matchChar(matchChar(start = matchChar(skip1(s, e), e, '$'), e, 'G'), e, 'P'), e, 'G'), e, 'S'), e, 'V'), e, &nmeas), e, '*'), e, start), e)) {
				handleGPGSV(&nmeas);
				continue;
			}
			if(match2(getHexNumber(matchChar(getNMEA(matchChar(matchChar(matchChar(matchChar(matchChar(start = matchChar(skip1(s, e), e, '$'), e, 'G'), e, 'P'), e, 'R'), e, 'M'), e, 'C'), e, &nmeas), e, '*'), e, start), e)) {
				handleGPRMC(&nmeas);
				continue;
			}
			if(match2(getHexNumber(matchChar(getNMEA(matchChar(matchChar(matchChar(matchChar(matchChar(start = matchChar(skip1(s, e), e, '$'), e, 'G'), e, 'P'), e, 'V'), e, 'T'), e, 'G'), e, &nmeas), e, '*'), e, start), e)) {
				handleGPVTG(&nmeas);
				continue;
			}
			if(match2(getHexNumber(matchChar(getNMEA(matchChar(matchChar(matchChar(matchChar(matchChar(start = matchChar(skip1(s, e), e, '$'), e, 'G'), e, 'P'), e, 'G'), e, 'L'), e, 'L'), e, &nmeas), e, '*'), e, start), e)) {
				handleGPGLL(&nmeas);
				continue;
			}
			if(match2(getHexNumber(matchChar(getNMEA(matchChar(matchChar(matchChar(matchChar(matchChar(start = matchChar(skip1(s, e), e, '$'), e, 'G'), e, 'P'), e, 'Z'), e, 'D'), e, 'A'), e, &nmeas), e, '*'), e, start), e)) {
				handleGPZDA(&nmeas);
				continue;
			}
			if(match2(getHexNumber(matchChar(getNMEA(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(start = matchChar(skip1(s, e), e, '$'), e, 'P'), e, 'T'), e, 'N'), e, 'L'), e, ','), e, 'P'), e, 'J'), e, 'K'), e, &nmeas), e, '*'), e, start), e)) {
				handlePJK(&nmeas);
				continue;
			}
			if(match2(getHexNumber(matchChar(getNMEA(matchChar(matchChar(matchChar(matchChar(matchChar(start = matchChar(skip1(s, e), e, '$'), e, 'G'), e, 'P'), e, 'T'), e, 'R'), e, 'A'), e, &nmeas), e, '*'), e, start), e)) {
				handleGPTRA(&nmeas);
				continue;
			}
			if (match2(getHexNumber2(matchChar(getNMEA2(findSemicolon(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(start = matchChar(skip1(s, e), e, '#'), e, 'B'), e, 'E'), e, 'S'), e, 'T'), e, 'P'), e, 'O'), e, 'S'), e, 'A'), e), e, &nmeas), e, '*'), e, start), e)) {
				handleBESTPOSA(&nmeas);
				continue;
			}
			if (match2(getPARAMS(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(skip1(s, e), e, 'P'), e, 'J'), e, 'K'), e, ' '), e, 'P'), e, 'A'), e, 'R'), e, 'A'), e, 'M'), e, 'E'), e, 'T'), e, 'E'), e, 'R'), e, ':'), e, &nmeas), e)) {
				handlePJKParam(&nmeas);
				continue;
			}
			if (match2(getPARAMS(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(skip1(s, e), e, 'S'), e, 'T'), e, 'A'), e, 'R'), e, 'T'), e, 'T'), e, 'I'), e, 'M'), e, 'E'), e, ':'), e, &nmeas), e)) {
				handleRTKRebootComp(&nmeas);
				continue;
			}
			if(match2(getHexNumber(matchChar(getNMEA(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(matchChar(start = matchChar(skip1(s, e), e, '$'), e, 'C'), e, 'A'), e, 'R'), e, 'I'), e, 'N'), e, 'F'), e, 'O'), e, &nmeas), e, '*'), e, start), e)) {
				handleCarinfo(&nmeas);
				continue;
			}
			if(match2(getHexNumber(matchChar(getNMEA(matchChar(matchChar(matchChar(matchChar(start = matchChar(skip1(s, e), e, '$'), e, 'K'), e, 'S'), e, 'X'), e, 'T'), e, &nmeas), e, '*'), e, start), e)) {
				handleKsxt(&nmeas);
				continue;
			}
			handleUnrecognisedNMEA(s, p-s);
		}
	}

	return s;
}