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

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

#include "console.h"
#include "gps.h"
#include "gpsdo.h"
#include "misc.h"

#include "ice1usb_proto.h"

#include "config.h"


struct {
	/* Configuration */
	enum gpsdo_vctxo_model vctxo;

	/* Current tuning */
	struct {
		uint16_t coarse;
		uint16_t fine;
	} tune;

	/* Measurements */
	struct {
		uint32_t tick_prev;	/* Previous tick value */
		uint32_t last;		/* Last measurement */
		int      skip;		/* Number of measurement left to discard */
		int      invalid;	/* Number of consecutive invalid measurements */
	} meas;

	/* FSM */
	enum {
		STATE_DISABLED,		/* Forced to manual */
		STATE_HOLD_OVER,	/* GPS invalid data, hold */
		STATE_TUNE_COARSE,	/* Adjust coarse tuning until we're +- 3 Hz */
		STATE_TUNE_FINE,	/* Fine tracking */
	} state;

	/* Coarse tuning */
	struct {
		int step;
	} coarse;

	/* Fine tracking */
	struct {
		int acc;		/* Accumulated error */
		int integral;		/* PID Integral term */
	} fine;

} g_gpsdo;


/*
 * VCXTO parameters
 *
 * - iKv is reciprocal sensitivity vs 'coarse' count for fast initial acquisition
 * - Kp, Ki, Kd are params for the fine-tracking PID loop
 *
 * Note that the spec if often a guaranteed minimum range and goes
 * from ~0.1V to 3.2V instead of 0-3.3V so actual sensitivity is
 * higher than the "theoritical value". We boost it by ~ 10% here.
 */
static const struct {
	int iKv;  /* hi-count / Hz         (.8  fixed point) */
	int Kp;   /* PID proportional term (.12 fixed point) */
	int Ki;   /* PID integral term     (.12 fixed point) */
	int Kd;   /* PID differential term (.12 fixed point) */
} vctxo_params[] = {
	[VCTXO_TAITIEN_VT40] = {
		.iKv =   300, /* +-  50 ppm pull range => ~ 0.75 Hz / hi-count (set to 0.85) */
		.Kp  = 14336,
		.Ki  =   410,
		.Kd  = -6144,
	},

	[VCTXO_SITIME_SIT3808_E] = {
		.iKv =   160, /* +- 100 ppm pull range => ~ 1.50 Hz / hi-count (set to 1.6) */
		.Kp  =  7168,
		.Ki  =   205,
		.Kd  = -4096,
	},
};

/* Tuning params */
#define TARGET		30720000
#define MAX_DEV_VALID	1000
#define MAX_DEV_FINE	3
#define MAX_INVALID	5


static void _gpsdo_coarse_start(void);


void
gpsdo_get_status(struct e1usb_gpsdo_status *status)
{
	const uint8_t state_map[] = {
		[STATE_DISABLED]    = ICE1USB_GPSDO_STATE_DISABLED,
		[STATE_HOLD_OVER]   = ICE1USB_GPSDO_STATE_HOLD_OVER,
		[STATE_TUNE_COARSE] = ICE1USB_GPSDO_STATE_TUNE_COARSE,
		[STATE_TUNE_FINE]   = ICE1USB_GPSDO_STATE_TUNE_FINE,
	};
	const uint8_t ant_map[] = {
		[ANT_UNKNOWN] = ICE1USB_GPSDO_ANT_UNKNOWN,
		[ANT_OK]      = ICE1USB_GPSDO_ANT_OK,
		[ANT_OPEN]    = ICE1USB_GPSDO_ANT_OPEN,
		[ANT_SHORT]   = ICE1USB_GPSDO_ANT_SHORT,
	};

	status->state          = state_map[g_gpsdo.state];
	status->antenna_state  = ant_map[gps_antenna_status()];
	status->valid_fix      = gps_has_valid_fix();
	status->mode           = (g_gpsdo.state == STATE_DISABLED) ? ICE1USB_GPSDO_MODE_DISABLED : ICE1USB_GPSDO_MODE_AUTO;
	status->tune.coarse    = g_gpsdo.tune.coarse;
	status->tune.fine      = g_gpsdo.tune.fine;
	status->freq_est       = g_gpsdo.meas.last;
	status->err_acc        = (g_gpsdo.state == STATE_TUNE_FINE) ? g_gpsdo.fine.acc : 0;
}

void
gpsdo_enable(bool enable)
{
	if (!enable)
		g_gpsdo.state = STATE_DISABLED;
	else if (g_gpsdo.state == STATE_DISABLED)
		g_gpsdo.state = STATE_HOLD_OVER;
}

bool
gpsdo_enabled(void)
{
	return g_gpsdo.state != STATE_DISABLED;
}

void
gpsdo_set_tune(uint16_t  coarse, uint16_t  fine)
{
	/* Set value */
	g_gpsdo.tune.coarse = coarse;
	g_gpsdo.tune.fine   = fine;

	pdm_set(PDM_CLK_HI, true, g_gpsdo.tune.coarse, false);
	pdm_set(PDM_CLK_LO, true, g_gpsdo.tune.fine,   false);

	/* If we were in 'fine' mode, reset to coarse */
	if (g_gpsdo.state == STATE_TUNE_FINE)
		_gpsdo_coarse_start();
}

void
gpsdo_get_tune(uint16_t *coarse, uint16_t *fine)
{
	*coarse = g_gpsdo.tune.coarse;
	*fine   = g_gpsdo.tune.fine;
}


static void
_gpsdo_coarse_start(void)
{
	/* Debug */
#ifdef GPSDO_DEBUG
	printf("[+] GPSDO Coarse Start: tune=%d:%d\n", g_gpsdo.tune.coarse, g_gpsdo.tune.fine);
#endif

	/* Set the state */
	g_gpsdo.state = STATE_TUNE_COARSE;

	/* Skip a few measurements to be safe */
	g_gpsdo.meas.skip = 3;

	/* Reset coarse tuning state */
	g_gpsdo.coarse.step = 0;

	/* Put the fine adjust back in the middle point */
	g_gpsdo.tune.coarse += ((int)g_gpsdo.tune.fine - 2048) >> 6;
	g_gpsdo.tune.fine    = 2048;

	pdm_set(PDM_CLK_HI, true, g_gpsdo.tune.coarse, false);
	pdm_set(PDM_CLK_LO, true, g_gpsdo.tune.fine,   false);
}

static void
_gpsdo_fine_start(void)
{
#ifdef GPSDO_DEBUG
	printf("[+] GPSDO Fine Start\n");
#endif

	/* Set the state */
	g_gpsdo.state = STATE_TUNE_FINE;

	/* Reset the long term error tracking */
	g_gpsdo.fine.acc = 0;
	g_gpsdo.fine.integral = 0;
}

static void
_gpsdo_coarse_tune(uint32_t tick_diff)
{
	int freq_diff = (int)tick_diff - TARGET;

	/* Is the measurement good enough to switch to fine ? */
	if ((freq_diff > -MAX_DEV_FINE) && (freq_diff < MAX_DEV_FINE)) {
		_gpsdo_fine_start();
		return;
	}

	/* Estimate correction and apply it */
	g_gpsdo.tune.coarse -= (freq_diff * vctxo_params[g_gpsdo.vctxo].iKv) >> 8;
	pdm_set(PDM_CLK_HI, true, g_gpsdo.tune.coarse, false);

	/* Skip next measurement */
	g_gpsdo.meas.skip = 1;

	/* Debug */
#ifdef GPSDO_DEBUG
	printf("[+] GPSDO Coarse: err=%d tune=%d\n", freq_diff, g_gpsdo.tune.coarse);
#endif
}

static void
_gpsdo_fine_track(uint32_t tick_diff)
{
	int freq_diff = (int)tick_diff - TARGET;

	/* Did we deviate too much ? */
	if ((freq_diff < -2*MAX_DEV_FINE) || (freq_diff > 2*MAX_DEV_FINE)) {
		_gpsdo_coarse_start();
		return;
	}

	/* Accumulate long term error and integrate it */
	g_gpsdo.fine.acc += freq_diff;
	g_gpsdo.fine.integral += g_gpsdo.fine.acc;

	/* PID */
	g_gpsdo.tune.fine = 2048 - ((
		vctxo_params[g_gpsdo.vctxo].Kp * g_gpsdo.fine.acc +
		vctxo_params[g_gpsdo.vctxo].Ki * g_gpsdo.fine.integral +
		vctxo_params[g_gpsdo.vctxo].Kd * freq_diff
	) >> 12);

	/* If fine tune is getting close to boundary, do our
	 * best to transfer part of it to coarse tuning */
	if ((g_gpsdo.tune.fine < 512) || (g_gpsdo.tune.fine > 3584))
	{
		int coarse_adj = ((int)g_gpsdo.tune.fine - 2048) >> 6;

		g_gpsdo.tune.coarse += coarse_adj;
		g_gpsdo.tune.fine   -= coarse_adj << 6;

		pdm_set(PDM_CLK_HI, true, g_gpsdo.tune.coarse, false);
	}

	/* Apply fine */
	pdm_set(PDM_CLK_LO, true, g_gpsdo.tune.fine, false);

	/* Debug */
#ifdef GPSDO_DEBUG
	printf("[+] GPSDO Fine: err=%d acc=%d tune=%d\n", freq_diff, g_gpsdo.fine.acc, g_gpsdo.tune.fine);
#endif
}


void
gpsdo_poll(void)
{
	uint32_t tick_now, tick_diff;
	bool valid;

	/* If more than 3 sec elapsed since last PPS, go to hold-over */
	if ((g_gpsdo.state != STATE_DISABLED) &&
	    time_elapsed(g_gpsdo.meas.tick_prev, 3 * SYS_CLK_FREQ)) {
		g_gpsdo.state = STATE_HOLD_OVER;
		g_gpsdo.meas.invalid = 0;
		g_gpsdo.meas.skip = 0;
		return;
	}

	/* Get current tick and check if there was a PPS and estimate frequency */
	tick_now = time_pps_read();

	if (tick_now == g_gpsdo.meas.tick_prev)
		return;

	g_gpsdo.meas.last = tick_diff = tick_now - g_gpsdo.meas.tick_prev;
	g_gpsdo.meas.tick_prev = tick_now;

	/* If we're currently discarding samples, skip it */
	if (g_gpsdo.meas.skip) {
		g_gpsdo.meas.skip--;
		return;
	}

	/* If we're disabled, nothing else to do */
	if (g_gpsdo.state == STATE_DISABLED)
		return;

	/* Check GPS state */
	if (!gps_has_valid_fix()) {
		/* No GPS fix, go to hold-over */
		g_gpsdo.state = STATE_HOLD_OVER;
		g_gpsdo.meas.invalid = 0;
		g_gpsdo.meas.skip = 0;
		return;
	}

	/* Check measurement plausibility */
	valid = (tick_diff > (TARGET - MAX_DEV_VALID)) && (tick_diff < (TARGET + MAX_DEV_VALID));

	if (valid) {
		/* If we're in hold-over, switch to active */
		if (g_gpsdo.state == STATE_HOLD_OVER) {
			_gpsdo_coarse_start();
			return;
		}
	} else {
		/* Count invalid measurements */
		if (++g_gpsdo.meas.invalid >= MAX_INVALID) {
			if (g_gpsdo.state != STATE_HOLD_OVER) {
				/* We go back to hold-over */
				g_gpsdo.state = STATE_HOLD_OVER;
				g_gpsdo.meas.invalid = 0;
			} else {
				/* We're in hold-over, with valid fix, and
				 * still get a bunch of invalid. Reset tuning */
				g_gpsdo.tune.coarse = 2048;
				g_gpsdo.tune.fine   = 2048;

				pdm_set(PDM_CLK_HI, true, g_gpsdo.tune.coarse, false);
				pdm_set(PDM_CLK_LO, true, g_gpsdo.tune.fine,   false);
			}
		}

		/* In all cases, invalid measurements are not used */
		return;
	}

	g_gpsdo.meas.invalid = 0;

	/* If we reach here, we have a valid fix, valid measurement and
	 * we're not in hold-over or disabled. Feed the correct loop */
	if (g_gpsdo.state == STATE_TUNE_COARSE)
		_gpsdo_coarse_tune(tick_diff);
	else if (g_gpsdo.state == STATE_TUNE_FINE)
		_gpsdo_fine_track(tick_diff);
}


void
gpsdo_init(enum gpsdo_vctxo_model vctxo)
{
	/* State */
	memset(&g_gpsdo, 0x00, sizeof(g_gpsdo));

	/* Default tune to middle range */
	g_gpsdo.tune.coarse = 2048;
	g_gpsdo.tune.fine   = 2048;

	pdm_set(PDM_CLK_HI, true, g_gpsdo.tune.coarse, false);
	pdm_set(PDM_CLK_LO, true, g_gpsdo.tune.fine,   false);

	/* Initial state and config */
	g_gpsdo.state = STATE_HOLD_OVER;
	g_gpsdo.vctxo = vctxo;
}