/*
 * Copyright (c) 2020 Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <math.h>

#include "gnss-galileo.h"

#include <jeffpc/types.h>
#include <jeffpc/error.h>

static inline uint32_t tow2gst(uint32_t wn, uint32_t tow)
{
	return wn * 7 * 24 * 3600 + tow;
}

static inline bool verify_svid(unsigned int sv, bool almanac)
{
	/* For almanac entries, we accept sv 0 - which means unused entry */
	if ((sv >= (almanac ? 0 : 1)) && (sv <= GALILEO_MAX_SV_ID))
		return true;

	/*
	 * Ideally we could just return false here, but it is safer to catch
	 * problems early with a panic then to wonder why everything ended
	 * up corrupted.
	 */
	panic("Got a %s SV id %u", sv ? "reserved" : "invalid", sv);
}

void galileo_state_init(struct galileo_state *state,
			const struct galileo_state_ops *ops)
{
	int i, j;

	memset(state, 0, sizeof(struct galileo_state));

	state->ops = ops;

	/* use IOD that looks wrong */
	for (i = 0; i < ARRAY_LEN(state->sv); i++) {
		state->sv[i].eph.sv = i;
		state->sv[i].eph.iod = ~0;
	}

	/* use IOD that looks wrong */
	for (i = 0; i < ARRAY_LEN(state->wip); i++) {
		state->wip[i].nav.eph.sv = ~0;
		state->wip[i].nav.eph.iod = ~0;

		for (j = 0; j < ARRAY_LEN(state->wip[0].nav.iod); j++)
			state->wip[i].nav.iod[j] = ~0;
	}
}

static void galileo_state_sync_eph(struct galileo_state *state,
				   const unsigned int sv,
				   const uint32_t gst)
{
	const bool prev_wn = state->wip[sv].nav.eph.t0.raw > state->time.tow;
	const uint16_t ephwn = state->time.wn - (prev_wn ? 1 : 0);
	struct galileo_ephemeris prev;

	if ((state->wip[sv].nav.iod[0] != state->wip[sv].nav.iod[1]) ||
	    (state->wip[sv].nav.iod[1] != state->wip[sv].nav.iod[2]) ||
	    (state->wip[sv].nav.iod[2] != state->wip[sv].nav.iod[3]))
		return;

	if (state->wip[sv].nav.iod[0] == 0xffff)
		return;

	/*
	 * Compare the timestamps we stashed when receiving the ephemeris
	 * data with the just received timestamp (@gst) to decide if we may
	 * have received some of the ephemeris pages more than a week ago.
	 *
	 * This is not a 100% accurate check.  While it will *never* tell us
	 * incorrectly that the ephemeris is < 1 week old, it may tell us
	 * that the ephemeris is >= 1 week old when it isn't.  (Because we
	 * happened not to receive any GST messages but received the
	 * ephemeris data just fine.)  This is acceptable.
	 *
	 * Once we know that the ephemeris is < 1 week old, we can try to
	 * assign it a week number.  If more than a week passed, we have no
	 * idea which week the t0e belongs to.
	 *
	 * Put another way, we are looking at the last GST message from
	 * before the ephemeris messages and the current GST message.
	 * Visually:
	 *
	 *     <GST><ephemeris data><GST>
	 *
	 * We compare these "bookends" to figure out if more than a week has
	 * passed.
	 *
	 * Therefore,
	 *
	 * (1) if it has been less than a week, we assign a week number to
	 *     the emphemeris and start using it, and
	 * (2) if it has been more than a week, we simply invalidate the
	 *     ephemeris data.
	 *
	 * This still has its problems.  If for example we receive:
	 *
	 *	<GST><ephemeris data>[long delay]<GST>
	 *
	 * We will delay using the new ephemeris data for the duration of
	 * the delay.  This is however highly unlikely, since we check the
	 * ephemeris data for freshness whenever we receive a GST message
	 * from *any* sv.  Therefore, the "long delay" would have to involve
	 * not receiving *any* GST message from *all* sv.
	 */
	if (((gst - state->wip[sv].nav.gst[0]) >= (7 * 24 * 3600)) ||
	    ((gst - state->wip[sv].nav.gst[1]) >= (7 * 24 * 3600)) ||
	    ((gst - state->wip[sv].nav.gst[2]) >= (7 * 24 * 3600)) ||
	    ((gst - state->wip[sv].nav.gst[3]) >= (7 * 24 * 3600)))
		return; /* more than a week for at least one page */

	prev = state->sv[sv].eph;
	state->sv[sv].eph = state->wip[sv].nav.eph;
	state->sv[sv].eph.sv = sv;
	state->sv[sv].eph.iod = state->wip[sv].nav.iod[0];
	state->sv[sv].eph.valid = true;
	state->sv[sv].eph.t0.gst = tow2gst(ephwn, state->sv[sv].eph.t0.raw);
	state->sv[sv].valid_eph = true;

	if (state->ops->eph_update)
		state->ops->eph_update(state,
				       (prev.iod == 0xffff) ? NULL : &prev,
				       &state->sv[sv].eph);
}

static void __sync_time(struct galileo_state_time *time,
			uint32_t gst, uint16_t wn, uint32_t tow,
			bool allow_equal)
{
	/* sanity check */
	if (time->valid) {
		if (allow_equal) {
			ASSERT3U(time->gst, <=, gst);
			ASSERT3U(time->wn, <=, wn);
			if (time->wn == wn)
				ASSERT3U(time->tow, <=, tow);
		} else {
			ASSERT3U(time->gst, <, gst);
			ASSERT3U(time->wn, <=, wn);
			if (time->wn == wn)
				ASSERT3U(time->tow, <, tow);
		}
	}

	/* set */
	time->gst = gst;
	time->wn = wn;
	time->tow = tow;
	time->valid = true;
}

static void galileo_state_sync_time(struct galileo_state *state,
				    unsigned int sv, uint16_t wn,
				    uint32_t tow)
{
	const struct galileo_state_time prev_sv = state->sv[sv].last_time;
	const struct galileo_state_time prev_global = state->time;
	const uint32_t gst = tow2gst(wn, tow);
	int i;

	__sync_time(&state->sv[sv].last_time, gst, wn, tow, false);

	/*
	 * Use <= for the global time instead of < because another sv may
	 * have already updated the global system time values.
	 */
	__sync_time(&state->time, gst, wn, tow, true);

	/*
	 * If this is *not* the first time message from the constellation,
	 * we were able to tag all ephemeris pages received since then with
	 * that GST timestamp.  This timestamp is required to sync the
	 * ephemeris.
	 */
	if (prev_global.valid)
		for (i = 0; i < ARRAY_LEN(state->sv); i++)
			galileo_state_sync_eph(state, i, gst);

	if (state->ops->time)
		state->ops->time(state, sv, &state->time, &prev_sv);
}

bool galileo_state_apply_page(struct galileo_state *state,
			      const unsigned int sv,
			      const struct galileo_inav_page *pg)
{
	if (!verify_svid(sv, false))
		return false;

	if (!pg->page_nominal)
		return true; /* nothing to do for alerts */

	if (pg->nominal.type == 63)
		return true; /* nothing to do for dummy pages */

	if (pg->nominal.type == 1) {
		/* this SV's ephemeris */
		struct galileo_ephemeris *eph = &state->wip[sv].nav.eph;

		state->wip[sv].nav.gst[0] = state->time.gst;
		state->wip[sv].nav.iod[0] = pg->nominal.w1.iod;
		eph->t0.raw = pg->nominal.w1.t0e * 60;
		eph->m0 = ldexp(pg->nominal.w1.m0 * M_PI, -31);
		eph->e = ldexp(pg->nominal.w1.e, -33);
		eph->sqrt_a = ldexp(pg->nominal.w1.sqrt_a, -19);
	} else if (pg->nominal.type == 2) {
		/* this SV's ephemeris */
		struct galileo_ephemeris *eph = &state->wip[sv].nav.eph;

		state->wip[sv].nav.gst[1] = state->time.gst;
		state->wip[sv].nav.iod[1] = pg->nominal.w2.iod;
		eph->omega0 = ldexp(pg->nominal.w2.omega0 * M_PI, -31);
		eph->i0 = ldexp(pg->nominal.w2.i0 * M_PI, -31);
		eph->omega = ldexp(pg->nominal.w2.omega * M_PI, -31);
		eph->idot = ldexp(pg->nominal.w2.idot * M_PI, -43);
	} else if (pg->nominal.type == 3) {
		/* this SV's ephemeris */
		struct galileo_ephemeris *eph = &state->wip[sv].nav.eph;

		state->wip[sv].nav.gst[2] = state->time.gst;
		state->wip[sv].nav.iod[2] = pg->nominal.w3.iod;
		eph->omegadot = ldexp(pg->nominal.w3.omegadot * M_PI, -43);
		eph->delta_n = ldexp(pg->nominal.w3.delta_n * M_PI, -43);
		eph->cuc = ldexp(pg->nominal.w3.cuc, -29);
		eph->cus = ldexp(pg->nominal.w3.cus, -29);
		eph->crc = ldexp(pg->nominal.w3.crc, -5);
		eph->crs = ldexp(pg->nominal.w3.crs, -5);
	} else if (pg->nominal.type == 4) {
		/* this SV's ephemeris */
		struct galileo_ephemeris *eph = &state->wip[sv].nav.eph;

		ASSERT3U(sv, ==, pg->nominal.w4.svid);

		state->wip[sv].nav.gst[3] = state->time.gst;
		state->wip[sv].nav.iod[3] = pg->nominal.w4.iod;
		eph->cic = ldexp(pg->nominal.w4.cic, -29);
		eph->cis = ldexp(pg->nominal.w4.cis, -29);
	} else if (pg->nominal.type == 5) {
		/* GST */
		galileo_state_sync_time(state, sv,
					pg->nominal.w5.wn,
					pg->nominal.w5.tow);
	} else {
		return false; /* we don't know how to interpret this page */
	}

	return true;
}