#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_period_scaled_hysteresis_and_settling(void)
{
	gps_pps_quality_selector_t selector;
	gps_pps_quality_selector_init_period(&selector, 0.5);
	assert(selector.upgrade_updates == 4u);
	assert(selector.degrade_updates == 2u);
	gps_pps_loop_profile_t precision =
		gps_pps_loop_profile_for_period(GPS_PPS_PROFILE_PRECISION, 0.5);
	gps_pps_loop_profile_t robust =
		gps_pps_loop_profile_for_period(GPS_PPS_PROFILE_ROBUST, 0.5);
	assert(precision.lock_acquire_updates == 2u);
	assert(precision.software_settle_updates == 5u);
	assert(robust.lock_acquire_updates == 4u);
	assert(robust.software_settle_updates == 7u);

	gps_pps_quality_selector_init_period(&selector, 1.0);
	assert(selector.upgrade_updates == 2u);
	assert(selector.degrade_updates == 2u);
	precision = gps_pps_loop_profile_for_period(
		GPS_PPS_PROFILE_PRECISION, 1.0);
	robust = gps_pps_loop_profile_for_period(
		GPS_PPS_PROFILE_ROBUST, 1.0);
	assert(precision.lock_acquire_updates == 2u);
	assert(precision.software_settle_updates == 3u);
	assert(robust.lock_acquire_updates == 2u);
	assert(robust.software_settle_updates == 4u);
}

static void assert_coefficients(
	gps_pps_profile_t profile,
	double period_sec,
	int64_t expected_kp,
	int64_t expected_ki)
{
	int64_t kp = 0;
	int64_t ki = 0;
	uint32_t max_slew = 0u;
	gps_pps_loop_profile_t parameters =
		gps_pps_loop_profile_for_period(profile, period_sec);
	assert(gps_pps_loop_coefficients(
		parameters, period_sec, 125000000.0,
		&kp, &ki, &max_slew));
	assert(kp == expected_kp);
	assert(ki == expected_ki);
	assert(max_slew == 85899u);
}

static void test_period_scaled_discrete_coefficients(void)
{
	/* The production 200 ms coefficients remain bit-for-bit unchanged. */
	assert_coefficients(GPS_PPS_PROFILE_ACQUIRE, 0.2, 764018, 64006);
	assert_coefficients(GPS_PPS_PROFILE_PRECISION, 0.2, 222838, 5445);
	assert_coefficients(GPS_PPS_PROFILE_ROBUST, 0.2, 84891, 640);

	/* Other cadences exponentiate those proven discrete poles in time. */
	assert_coefficients(GPS_PPS_PROFILE_ACQUIRE, 0.5, 615780, 127492);
	assert_coefficients(GPS_PPS_PROFILE_PRECISION, 0.5, 209783, 12804);
	assert_coefficients(GPS_PPS_PROFILE_ROBUST, 0.5, 82980, 1564);
	assert_coefficients(GPS_PPS_PROFILE_ACQUIRE, 1.0, 447388, 178038);
	assert_coefficients(GPS_PPS_PROFILE_PRECISION, 1.0, 190239, 23149);
	assert_coefficients(GPS_PPS_PROFILE_ROBUST, 1.0, 79923, 3011);
}

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_period_scaled_hysteresis_and_settling();
	test_period_scaled_discrete_coefficients();
	test_quality_boundaries_and_recovery();
	puts("gps_pps_quality tests passed");
	return 0;
}