/*
 * nmea.c
 *
 *  Created on: 6 Sep 2020
 *      Author: mike
 */

#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <memory.h>

#include "libNMEA/nmea.h"

#include "libSerial/serial.h"

char linebuff[80];
unsigned linePos = 0;

nmeaCallback rmcCallback = NULL;

typedef enum
{
	SEARCH,
	READING
} NmeaState_t;

NmeaState_t lineState = SEARCH;

static bool
decodePacket(char *linebuff, int linePos, Location *loc);

bool updateLocation(Location *loc, usart_ctl *uc)
{
	unsigned chars = SerialCharsReceived(uc);
		if (!chars)
			return false; // nothing to read, return immediately

		for (int i = 0; i < chars; i++)
		{
			char c = GetCharSerial(uc);
			switch (lineState)
			{
			case SEARCH:
				if (c != '$')
					break;
				lineState = READING;
				linePos = 0;
			case READING:
				if (c == '\r')
				{
					// handle the read code
					bool success = decodePacket(linebuff, linePos, loc);
					lineState = SEARCH;

					linePos = 0;
					return success;
				}
				if (linePos < sizeof(linebuff))
					linebuff[linePos++] = c;
				else
				{
					lineState = SEARCH;
					// search for the $  in any unread string
					int i;

					for (i = 0; i < linePos; i++)
						if (linebuff[i] == '$')
						{
							memcpy(linebuff, linebuff + i, linePos - i);
							linePos = 1;
							linebuff[linePos++] = c;
							lineState = READING;
						}

					if (lineState == SEARCH)
						linePos = 0;
				}
				break;
			}
	}
	return false;
}

static int8_t
decodeDec(char c)
{
	if (c > '9' && c < '0')
		return -1;
	return c - '0';
}

static uint8_t
decodeHex(char c)
{
	int8_t v = decodeDec(c);
	if (v >= 0)
		return v;
	c = tolower(c);
	if (c >= 'a' && c <= 'f')
		return c - 'a' + 10;
	return 0;
}

// lat/long decoder
static float
decodeLL(char *ptr, int msDigitWeight)
{
	float digitWeight = msDigitWeight;
	float result = 0;
	int i = 0;
	while (1)
	{
		char c = ptr[i++];
		if (c == '.')
			continue;
		int8_t v = decodeDec(c);
		if (v > 0)
		{
			result += digitWeight * v;
			if (fabs(digitWeight - 1) < 0.01)
				digitWeight = 1 / 6.0;
			else
				digitWeight = digitWeight / 10;

			continue;
		}
		break;
	}

	return result;
}

static float
decodeFP(char *ptr)
{
	return strtof(ptr, NULL);
}

static int
decodeDecimal(char *ptr)
{
	int i;
	int res = 0;
	int const width = 2;
	for (i = 0; i < width; i++)
	{
		int8_t v = decodeDec(*ptr++);
		if (v < 0)
			return 0;
		res *= 10;
		res += v;
	}
	return res;
}

bool decodePacket(char *linebuff, int linePos, Location *loc)
{

	uint8_t checksum = 0;
	for (int i = 1; i < linePos - 3; i++)
		checksum ^= linebuff[i];
	uint8_t givenSum = (decodeHex(linebuff[linePos - 2]) << 4) + decodeHex(linebuff[linePos - 1]);
	if (givenSum != checksum)
		return false;

	char *fieldPos[20];
	int fieldCnt = 0;
	// split fields
	for (int i = 6; i < linePos - 3; i++)
	{
		if (linebuff[i] == ',')
		{
			fieldPos[fieldCnt++] = linebuff + i + 1;
		}
	}

	// decode RMC
	if (linebuff[3] == 'R' && linebuff[4] == 'M' && linebuff[5] == 'C')

	{

		// decode the fields
		loc->valid = *fieldPos[1];
		if (loc->valid == 'A')
		{
			memcpy(loc->time, fieldPos[0], 6);
			loc->lat = decodeLL(fieldPos[2], 10);
			loc->ns = *fieldPos[3];
			loc->lon = decodeLL(fieldPos[4], 100);
			loc->ew = *fieldPos[5];
			loc->speed = decodeFP(fieldPos[6]);
			loc->heading = decodeFP(fieldPos[7]);
			memcpy(loc->date, fieldPos[1], 6);

			loc->tv.tm_sec = decodeDecimal(&loc->time[4]);
			loc->tv.tm_min = decodeDecimal(&loc->time[2]);
			loc->tv.tm_hour = decodeDecimal(&loc->time[0]);

			loc->tv.tm_mday = decodeDecimal(&loc->date[0]);
			loc->tv.tm_mon = decodeDecimal(&loc->date[2]) - 1;
			loc->tv.tm_year = decodeDecimal(&loc->date[4]) + 100; //

			loc->tv.tm_isdst = 0;
			loc->utc = mktime(&loc->tv);

			loc->good = true;
		}
		if (rmcCallback)
		{
			linebuff[linePos++] = '\r';
			linebuff[linePos++] = '\n';
			linebuff[linePos++] = 0;
			rmcCallback((uint8_t *)linebuff, linePos);
		}
	}

	return true;
}

void setRmcCallback(nmeaCallback callback)
{
	rmcCallback = callback;
}