/*
 * gps.c
 *
 * Copyright (C) 2019-2022  Sylvain Munaut <tnt@246tNt.com>
 * SPDX-License-Identifier: GPL-3.0-or-later
 */

#include <stddef.h>
#include <stdbool.h>
#include <stdint.h>
#include <string.h>

#include "console.h"
#include "gps.h"
#include "misc.h"
#include "rs422.h"
#include "usb_gps.h"
#include "utils.h"

#include "config.h"


struct gps_uart {
	uint32_t data;
	uint32_t csr;
} __attribute__((packed,aligned(4)));

#define GPS_UART_CSR_DIV(baud)	((SYS_CLK_FREQ+(baud)/2)/(baud)-2)
#define GPS_UART_CSR_TX_EMPTY	(1 << 29)
#define GPS_UART_DATA_EMPTY	(1 << 31)

static volatile struct gps_uart * const gps_uart_regs = (void*)(AUX_UART_BASE);


static struct {
	/* NMEA rx state */
	struct {
		enum {
			GNS_WAIT   = 0,
			GNS_READ   = 1,
			GNS_CK_HI  = 2,
			GNS_CK_LO  = 3,
			GNS_END_CR = 4,
			GNS_END_LF = 5,
		} state;

		int     len;
		uint8_t cksum;
		char    buf[95];	/* Max length is actually 82 + 1 (\0) */
	} nmea;

	/* Current lock state */
	struct {
		bool valid;
	} fix;

	/* Reported antenna state */
	enum gps_antenna_state antenna;
} g_gps;


static void
_gps_empty(bool wait_eol)
{
	bool eol = false;

	uint32_t c;
	while (1) {
		c = gps_uart_regs->data;
		if (c & GPS_UART_DATA_EMPTY) {
			if (!wait_eol || eol)
				break;
		} else {
			eol = (c == '\n');
		}
	}
}

static void
_gps_send(const char *s)
{
	char cksum = 0;

	/* Start sentence */
	gps_uart_regs->data = '$';

	/* Send payload */
	while (*s)
		cksum ^= (gps_uart_regs->data = *s++);

	/* Send checksum */
	gps_uart_regs->data = '*';

	s = hexstr(&cksum, 1, false);
	gps_uart_regs->data = *s++;
	gps_uart_regs->data = *s++;

	gps_uart_regs->data = '\r';
	gps_uart_regs->data = '\n';
}

static const char *
_gps_query(void)
{
	uint32_t c;
	while (1) {
		/* Get next char */
		c = gps_uart_regs->data;
		if (c & GPS_UART_DATA_EMPTY)
			break;

		/* Always store it */
		if (g_gps.nmea.len == sizeof(g_gps.nmea.buf))
			g_gps.nmea.state = GNS_WAIT;
		else
			g_gps.nmea.buf[g_gps.nmea.len++] = c;

		/* State */
		if (c == '$') {
			/* '$' always triggers reset */
			g_gps.nmea.state  = GNS_READ;
			g_gps.nmea.cksum  = 0;
			g_gps.nmea.len    = 1;
			g_gps.nmea.buf[0] = '$';
		} else {
			switch (g_gps.nmea.state) {
			case GNS_READ:
				if (c == '*')
					g_gps.nmea.state = GNS_CK_HI;
				else
					g_gps.nmea.cksum ^= c;
				break;
			case GNS_CK_HI:
				g_gps.nmea.cksum ^= hexval(c) << 4;
				g_gps.nmea.state = GNS_CK_LO;
				break;
			case GNS_CK_LO:
				g_gps.nmea.cksum ^= hexval(c);
				g_gps.nmea.state = GNS_END_CR;
				break;
			case GNS_END_CR:
				g_gps.nmea.state = (c == '\r') ? GNS_END_LF : GNS_WAIT;
				break;
			case GNS_END_LF:
				g_gps.nmea.state = GNS_WAIT;
				g_gps.nmea.buf[g_gps.nmea.len] = 0x00;
				if (c == '\n')
					return g_gps.nmea.buf;
				break;
			default:
				g_gps.nmea.state = GNS_WAIT;
				g_gps.nmea.len   = 0;
				break;
			}
		}
	}
	return NULL;
}

void
_gps_parse_nmea(const char *nmea)
{
	/* Very basic parsing, we just look at $PERC,GPsts message for
	 * state 1 (survey mode) and 2 (position-hold) */
	if (!strncmp(nmea, "$PERC,GPsts,", 12))
	{
		g_gps.fix.valid = (nmea[12] == '1') || (nmea[12] == '2');
	}
}


bool
gps_has_valid_fix(void)
{
	return g_gps.fix.valid;
}

enum gps_antenna_state
gps_antenna_status(void)
{
	return g_gps.antenna;
}

void
gps_poll(void)
{
	const char *nmea;

	/* Check if we have anything from the module */
	nmea = _gps_query();
	if (!nmea)
		return;

	/* If we do, process it locally to update our state */
	_gps_parse_nmea(nmea);

	/* And queue it for USB */
	usb_gps_puts(nmea);
}

void
gps_init(void)
{
	uint32_t start_time = time_now_read();
	bool init_ok;

	/* State init */
	memset(&g_gps, 0x00, sizeof(g_gps));

	/* Hold onboard GPS in reset */
	gpio_out(3, false);
	gpio_dir(3, true);

	/* Init RS422 board */
	rs422_init();

	/* Configure uart and empty buffer */
	gps_uart_regs->csr = GPS_UART_CSR_DIV(9600);
	_gps_empty(false);

	/* Wait for first line of output as sign it's ready, timeout after 10 s */
	while (!time_elapsed(start_time, 10 * SYS_CLK_FREQ))
		if ((init_ok = (_gps_query() != NULL)))
			break;

	if (init_ok) {
		printf("[+] GPS ok\n");
	} else {
		printf("[!] GPS init failed\n");
	}
}