/*
 * 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;

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)
{
  while (1)
    {
      if (!SerialCharsReceived (uc))
	return false; // nothing to read, return immediately

      char c = GetCharSerial (uc);
      switch (lineState)
	{
	case SEARCH:
	  if (c == '$')
	    lineState = READING;
	  linePos = 0;
	  break;
	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] == '$')
		  {
		    int n = i + 1;
		    memcpy (linebuff, linebuff + n, linePos - i);
		    linePos = linePos - i;
		    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 = 0; 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 = 5; i < linePos - 3; i++)
    {
      if (linebuff[i] == ',')
	{
	  fieldPos[fieldCnt++] = linebuff + i + 1;
	}

    }

  // decode RMC
  if (linebuff[2] == 'R' && linebuff[3] == 'M' && linebuff[4] == '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;
	}
    }
  return true;
}