#include <assert.h>
#include <math.h>
#include <stdio.h>

#include "gps/gps_pps_quality.h"

static gps_pps_quality_metrics_t precision_metrics(void)
{
	return (gps_pps_quality_metrics_t){
		.tdop = 1.7,
		.gdop = 3.2,
		.clock_bias_sigma_m = 18.0,
		.mean_cn0_dbhz = 38.0,
		.minimum_cn0_dbhz = 29.0,
		.chi_square_ratio = 0.35,
		.velocity_residual_rms_mps = 0.25,
		.used_satellites = 7u,
		.solution_valid = true,
		.residual_test_valid = true,
		.velocity_valid = true,
		.fde_applied = false
	};
}

static void test_hysteretic_profile_transitions(void)
{
	gps_pps_quality_selector_t selector;
	gps_pps_quality_selector_init(&selector);
	gps_pps_quality_metrics_t metrics = precision_metrics();
	bool changed = false;

	for(unsigned int update = 0u; update < 7u; update++)
	{
		assert(gps_pps_quality_selector_update(
			&selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER);
		assert(!changed);
	}
	assert(gps_pps_quality_selector_update(
		&selector, &metrics, &changed) == GPS_PPS_PROFILE_PRECISION);
	assert(changed);

	/* One DOP/CN0 excursion must not chatter the active loop profile. */
	metrics.tdop = 4.5;
	metrics.gdop = 7.0;
	metrics.mean_cn0_dbhz = 27.0;
	assert(gps_pps_quality_selector_update(
		&selector, &metrics, &changed) == GPS_PPS_PROFILE_PRECISION);
	assert(!changed);
	metrics = precision_metrics();
	assert(gps_pps_quality_selector_update(
		&selector, &metrics, &changed) == GPS_PPS_PROFILE_PRECISION);

	/* Sustained FDE operation degrades to the robust profile. */
	metrics.fde_applied = true;
	for(unsigned int update = 0u; update < 2u; update++)
	{
		assert(gps_pps_quality_selector_update(
			&selector, &metrics, &changed) == GPS_PPS_PROFILE_PRECISION);
	}
	assert(gps_pps_quality_selector_update(
		&selector, &metrics, &changed) == GPS_PPS_PROFILE_ROBUST);
	assert(changed);

	/* One invalid WLS epoch is skipped; sustained invalidity holds over. */
	metrics.solution_valid = false;
	for(unsigned int update = 0u; update < 2u; update++)
	{
		assert(gps_pps_quality_selector_update(
			&selector, &metrics, &changed) == GPS_PPS_PROFILE_ROBUST);
		assert(!changed);
	}
	assert(gps_pps_quality_selector_update(
		&selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER);
	assert(changed);
}

static void test_alternating_quality_does_not_upgrade(void)
{
	gps_pps_quality_selector_t selector;
	gps_pps_quality_selector_init(&selector);
	gps_pps_quality_metrics_t good = precision_metrics();
	gps_pps_quality_metrics_t poor = good;
	poor.tdop = 9.0;

	for(unsigned int update = 0u; update < 40u; update++)
	{
		const gps_pps_quality_metrics_t* metrics =
			(update & 1u) ? &poor : &good;
		bool changed = false;
		assert(gps_pps_quality_selector_update(
			&selector, metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER);
		assert(!changed);
	}
}

static void test_profile_parameters_are_ordered(void)
{
	gps_pps_loop_profile_t robust =
		gps_pps_loop_profile(GPS_PPS_PROFILE_ROBUST);
	gps_pps_loop_profile_t precision =
		gps_pps_loop_profile(GPS_PPS_PROFILE_PRECISION);
	gps_pps_loop_profile_t acquire =
		gps_pps_loop_profile(GPS_PPS_PROFILE_ACQUIRE);
	assert(robust.natural_frequency_hz < precision.natural_frequency_hz);
	assert(precision.natural_frequency_hz < acquire.natural_frequency_hz);
	assert(robust.lock_acquire_updates > precision.lock_acquire_updates);
	assert(robust.lock_release_ticks >= precision.lock_release_ticks);
}

static void test_quality_boundaries_and_recovery(void)
{
	gps_pps_quality_selector_t selector;
	gps_pps_quality_selector_init(&selector);
	gps_pps_quality_metrics_t metrics = precision_metrics();
	bool changed = false;

	/* Exactly-on-threshold values remain usable in robust mode. */
	metrics.used_satellites = 5u;
	metrics.tdop = 8.0;
	metrics.gdop = 12.0;
	metrics.clock_bias_sigma_m = 100.0;
	metrics.mean_cn0_dbhz = 22.0;
	metrics.minimum_cn0_dbhz = 14.0;
	metrics.chi_square_ratio = 1.0;
	metrics.velocity_residual_rms_mps = 3.0;
	for(unsigned int update = 0u; update < 8u; update++)
	{
		gps_pps_quality_selector_update(&selector, &metrics, &changed);
	}
	assert(selector.profile == GPS_PPS_PROFILE_ROBUST);

	/* Non-finite metrics and loss below four satellites use fast hysteresis. */
	metrics.tdop = NAN;
	for(unsigned int update = 0u; update < 2u; update++)
	{
		assert(gps_pps_quality_selector_update(
			&selector, &metrics, &changed) == GPS_PPS_PROFILE_ROBUST);
	}
	assert(gps_pps_quality_selector_update(
		&selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER);
	assert(changed);

	/* A single good epoch is not enough to leave holdover. */
	metrics = precision_metrics();
	assert(gps_pps_quality_selector_update(
		&selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER);
	metrics.used_satellites = 3u;
	for(unsigned int update = 0u; update < 3u; update++)
	{
		assert(gps_pps_quality_selector_update(
			&selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER);
	}

	/* Recovery cannot expose PPS until eight consecutive good epochs. */
	metrics = precision_metrics();
	for(unsigned int update = 0u; update < 7u; update++)
	{
		assert(gps_pps_quality_selector_update(
			&selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER);
	}
	assert(gps_pps_quality_selector_update(
		&selector, &metrics, &changed) == GPS_PPS_PROFILE_PRECISION);
}

int main(void)
{
	test_hysteretic_profile_transitions();
	test_alternating_quality_does_not_upgrade();
	test_profile_parameters_are_ordered();
	test_quality_boundaries_and_recovery();
	puts("gps_pps_quality tests passed");
	return 0;
}