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

#include "gps/gps_acq_manager.h"

static gps_acq_manager_t make_manager(
	uint64_t retry_delay,
	uint64_t hot_retry_delay,
	uint64_t hot_window,
	unsigned int max_hot_streak)
{
	gps_acq_manager_t manager;
	const gps_acq_manager_config_t config = {
		.retry_delay_ticks = retry_delay,
		.hot_retry_delay_ticks = hot_retry_delay,
		.hot_window_ticks = hot_window,
		.max_hot_streak = max_hot_streak
	};
	gps_acq_manager_init(&manager, &config);
	return manager;
}

static unsigned int start_next(
	gps_acq_manager_t* manager,
	uint64_t now,
	bool expected_hot)
{
	unsigned int prn = GPS_ACQ_MANAGER_INVALID_PRN;
	bool hot = !expected_hot;
	assert(gps_acq_manager_start_next(manager, now, &prn, &hot));
	assert(hot == expected_hot);
	return prn;
}

static void test_cold_round_robin_and_retry_from_tick_zero(void)
{
	gps_acq_manager_t manager =
		make_manager(5u, 5u, 100u, 4u);
	assert(gps_acq_manager_enable_cold(&manager, 0u));
	assert(gps_acq_manager_enable_cold(&manager, 1u));
	assert(gps_acq_manager_enable_cold(&manager, 2u));

	assert(start_next(&manager, 0u, false) == 0u);
	/* A tick-zero attempt is real and must still observe retry delay. */
	gps_acq_manager_mark_failed(&manager, 0u, 0u);
	assert(start_next(&manager, 0u, false) == 1u);
	gps_acq_manager_mark_failed(&manager, 1u, 0u);
	assert(start_next(&manager, 0u, false) == 2u);
	gps_acq_manager_mark_failed(&manager, 2u, 0u);

	unsigned int ignored;
	assert(!gps_acq_manager_start_next(
		&manager, 4u, &ignored, NULL));
	assert(start_next(&manager, 5u, false) == 0u);
}

static void test_hot_priority_round_robin_and_cold_fairness(void)
{
	gps_acq_manager_t manager =
		make_manager(1u, 1u, 100u, 2u);
	assert(gps_acq_manager_enable_cold(&manager, 0u));
	assert(gps_acq_manager_enable_cold(&manager, 1u));
	gps_acq_manager_mark_lost(&manager, 7u, 10u);
	gps_acq_manager_mark_lost(&manager, 8u, 10u);

	assert(start_next(&manager, 10u, true) == 7u);
	gps_acq_manager_mark_failed(&manager, 7u, 11u);
	assert(start_next(&manager, 11u, true) == 8u);
	gps_acq_manager_mark_failed(&manager, 8u, 12u);

	/* Two hot attempts force one cold discovery attempt. */
	assert(start_next(&manager, 12u, false) == 0u);
	gps_acq_manager_mark_failed(&manager, 0u, 13u);
	assert(start_next(&manager, 13u, true) == 7u);
}

static void test_expired_hot_becomes_cold_and_tracking_is_excluded(void)
{
	gps_acq_manager_t manager =
		make_manager(1u, 1u, 10u, 4u);
	assert(gps_acq_manager_enable_cold(&manager, 2u));
	gps_acq_manager_mark_lost(&manager, 9u, 20u);

	unsigned int prn = start_next(&manager, 31u, false);
	assert(prn == 2u);
	gps_acq_manager_mark_tracking(&manager, prn);

	prn = start_next(&manager, 31u, false);
	assert(prn == 9u);
	gps_acq_manager_mark_acquired(&manager, prn);
	unsigned int ignored;
	assert(!gps_acq_manager_start_next(
		&manager, 100u, &ignored, NULL));

	gps_acq_manager_mark_tracking(&manager, 9u);
	gps_acq_manager_mark_stale(&manager, 9u);
	assert(gps_acq_manager_state(&manager, 9u) ==
	       GPS_ACQ_PRN_STALE);
	gps_acq_manager_mark_lost(&manager, 9u, 101u);
	assert(gps_acq_manager_state(&manager, 9u) ==
	       GPS_ACQ_PRN_HOT);
}

static void test_invalid_inputs_are_safe(void)
{
	gps_acq_manager_t manager =
		make_manager(1u, 1u, 10u, 0u);
	assert(!gps_acq_manager_enable_cold(
		&manager, GPS_ACQ_MANAGER_PRN_COUNT));
	assert(gps_acq_manager_state(
		&manager, GPS_ACQ_MANAGER_PRN_COUNT) ==
	       GPS_ACQ_PRN_DISABLED);
	assert(!gps_acq_manager_start_next(NULL, 0u, NULL, NULL));
	assert(gps_acq_manager_count_searchable(NULL) == 0u);
}

static void test_failed_hot_search_is_backed_off(void)
{
	gps_acq_manager_t manager =
		make_manager(1u, 30u, 100u, 4u);
	assert(gps_acq_manager_enable_cold(&manager, 0u));
	gps_acq_manager_mark_lost(&manager, 20u, 0u);

	assert(start_next(&manager, 0u, true) == 20u);
	gps_acq_manager_mark_failed(&manager, 20u, 5u);
	assert(start_next(&manager, 5u, false) == 0u);
	gps_acq_manager_mark_failed(&manager, 0u, 6u);
	assert(start_next(&manager, 7u, false) == 0u);
	gps_acq_manager_mark_failed(&manager, 0u, 8u);

	assert(start_next(&manager, 35u, true) == 20u);
}

/*
 * Root cause found on the board: a satellite tracked seconds ago was served the
 * 30 s hot backoff from its very first retry, while a never-seen cold PRN was
 * retried every 0.5 s.  The priority was inverted - the best candidate was
 * penalised sixty times harder than the worst.  Recency must buy fast retries
 * that decay with failures, not be discarded immediately.
 */
static void a_freshly_lost_prn_is_retried_fast_before_backing_off(void)
{
	gps_acq_manager_t manager;
	const gps_acq_manager_config_t config = {
		.retry_delay_ticks = 5u,       /* fast cadence */
		.hot_retry_delay_ticks = 300u, /* long backoff */
		.hot_window_ticks = 7200u,
		.max_hot_streak = 4u,
		.hot_fast_attempts = 3u
	};
	gps_acq_manager_init(&manager, &config);
	assert(gps_acq_manager_enable_cold(&manager, 7u));

	uint64_t now = 1000u;
	unsigned int prn = 0u;
	bool hot = false;

	/* Establish it as tracked, then lose it. */
	assert(gps_acq_manager_start_next(&manager, now, &prn, &hot));
	gps_acq_manager_mark_acquired(&manager, prn);
	gps_acq_manager_mark_tracking(&manager, prn);
	gps_acq_manager_mark_lost(&manager, 7u, now);

    /* The first few retries must come at the fast cadence, not after 300. */
	for(unsigned int attempt = 0u; attempt < config.hot_fast_attempts;
	    attempt++)
	{
		assert(gps_acq_manager_start_next(&manager, now, &prn, &hot));
		assert(prn == 7u);
		assert(hot);
		gps_acq_manager_mark_failed(&manager, 7u, now);
		now += config.retry_delay_ticks;
	}

	/* Allowance spent: it must now observe the long backoff. */
	bool served_early = false;
	for(unsigned int i = 0u; i < 5u; i++)
	{
		if(gps_acq_manager_start_next(&manager, now, &prn, &hot) &&
		   (prn == 7u) && hot)
		{
			served_early = true;
		}
		now += config.retry_delay_ticks;
	}
	assert(!served_early);

	/* After the backoff it becomes a candidate again. */
	now += config.hot_retry_delay_ticks;
	bool served_late = false;
	for(unsigned int i = 0u; i < 40u; i++)
	{
		if(gps_acq_manager_start_next(&manager, now, &prn, &hot) &&
		   (prn == 7u))
		{
			served_late = true;
			break;
		}
	}
	assert(served_late);
}

/*
 * Almanac-directed acquisition.  With one acquisition engine, sweeping PRNs
 * that are below the horizon is what starves the satellites that are actually
 * up: 75% of failed searches on the board were against below-horizon PRNs.
 * The visibility prediction must bias the scheduler hard, yet remain purely
 * advisory - it can never make a real satellite unreachable.
 */
static void test_invisible_prn_is_slowed_but_never_excluded(void)
{
	const uint64_t retry = 10u;
	const uint64_t unlikely = 500u;
	gps_acq_manager_t manager;
	const gps_acq_manager_config_t config = {
		.retry_delay_ticks = retry,
		.hot_retry_delay_ticks = 1000u,
		.hot_window_ticks = 100u,
		.max_hot_streak = 4u,
		.hot_fast_attempts = 1u,
		.unlikely_retry_delay_ticks = unlikely
	};
	gps_acq_manager_init(&manager, &config);
	assert(gps_acq_manager_search_mask(&manager) == 0xffffffffu);

	for(unsigned int prn = 0u; prn < GPS_ACQ_MANAGER_PRN_COUNT; prn++)
	{
		assert(gps_acq_manager_enable_cold(&manager, prn));
	}

	/* Only PRN index 3 is predicted visible. */
	gps_acq_manager_set_search_mask(&manager, 1u << 3);

	/*
	 * Early on, every search must go to the one visible PRN rather than round
	 * robin over the whole constellation.
	 */
	for(unsigned int attempt = 0u; attempt < 5u; attempt++)
	{
		uint64_t now = 1u + (attempt * retry);
		unsigned int prn = GPS_ACQ_MANAGER_INVALID_PRN;
		bool hot = true;
		assert(gps_acq_manager_start_next(&manager, now, &prn, &hot));
		assert(prn == 3u);
		gps_acq_manager_mark_failed(&manager, prn, now);
	}

	/*
	 * But an invisible PRN must still come round eventually, so a stale
	 * almanac or a stale position self-corrects instead of blinding the
	 * receiver permanently.
	 */
	bool saw_invisible = false;
	for(uint64_t now = unlikely; now <= (unlikely * 6u); now += retry)
	{
		unsigned int prn = GPS_ACQ_MANAGER_INVALID_PRN;
		bool hot = true;
		if(!gps_acq_manager_start_next(&manager, now, &prn, &hot))
		{
			continue;
		}
		gps_acq_manager_mark_failed(&manager, prn, now);
		if(prn != 3u)
		{
			saw_invisible = true;
			break;
		}
	}
	assert(saw_invisible);
	puts("  invisible PRNs are slowed but still revisited");
}

static void test_empty_mask_falls_back_to_searching_everything(void)
{
	gps_acq_manager_t manager = make_manager(10u, 1000u, 100u, 4u);
	for(unsigned int prn = 0u; prn < GPS_ACQ_MANAGER_PRN_COUNT; prn++)
	{
		assert(gps_acq_manager_enable_cold(&manager, prn));
	}
	/*
	 * A prediction that nothing is visible is always wrong, and honouring it
	 * would stall acquisition completely.  Degrade to the blind sweep.
	 */
	gps_acq_manager_set_search_mask(&manager, 0u);
	assert(gps_acq_manager_search_mask(&manager) == 0xffffffffu);

	unsigned int prn = GPS_ACQ_MANAGER_INVALID_PRN;
	bool hot = true;
	assert(gps_acq_manager_start_next(&manager, 1u, &prn, &hot));
	puts("  empty visibility mask falls back to searching everything");
}

static void test_visibility_does_not_disturb_a_visible_prn(void)
{
	const uint64_t retry = 10u;
	gps_acq_manager_t manager;
	const gps_acq_manager_config_t config = {
		.retry_delay_ticks = retry,
		.hot_retry_delay_ticks = 1000u,
		.hot_window_ticks = 100u,
		.max_hot_streak = 4u,
		.hot_fast_attempts = 4u,
		.unlikely_retry_delay_ticks = 500u
	};
	gps_acq_manager_init(&manager, &config);
	assert(gps_acq_manager_enable_cold(&manager, 5u));
	gps_acq_manager_set_search_mask(&manager, 1u << 5);

	/* A visible PRN keeps the fast cold cadence exactly as before. */
	unsigned int prn = start_next(&manager, 1u, false);
	assert(prn == 5u);
	gps_acq_manager_mark_failed(&manager, prn, 1u);
	prn = GPS_ACQ_MANAGER_INVALID_PRN;
	bool hot = true;
	assert(!gps_acq_manager_start_next(&manager, 1u + retry - 1u, &prn, &hot));
	assert(gps_acq_manager_start_next(&manager, 1u + retry, &prn, &hot));
	assert(prn == 5u);
	puts("  visible PRNs keep the fast cadence");
}

int main(void)
{
	test_invisible_prn_is_slowed_but_never_excluded();
	test_empty_mask_falls_back_to_searching_everything();
	test_visibility_does_not_disturb_a_visible_prn();
	a_freshly_lost_prn_is_retried_fast_before_backing_off();
	test_cold_round_robin_and_retry_from_tick_zero();
	test_hot_priority_round_robin_and_cold_fairness();
	test_expired_hot_becomes_cold_and_tracking_is_excluded();
	test_invalid_inputs_are_safe();
	test_failed_hot_search_is_backed_off();
	puts("gps acquisition manager tests passed");
	return 0;
}