#include "gps/gps_nav_epoch.h"

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

#define SAMPLE_WINDOW 409200u
#define TOW_TOLERANCE 0.001

static int failures;

#define CHECK(condition, message) do { \
	if(!(condition)) { \
		fprintf(stderr, "FAIL: %s\n", message); \
		failures++; \
	} \
} while(0)

static gps_nav_epoch_selection_t select_epoch(
	const gps_nav_epoch_key_t* candidates,
	unsigned int count)
{
	gps_nav_epoch_selection_t selection;
	memset(&selection, 0, sizeof(selection));
	CHECK(gps_nav_epoch_select(candidates, count, SAMPLE_WINDOW,
	                           TOW_TOLERANCE, &selection),
	      "epoch selection call failed");
	return selection;
}

static unsigned int mask_count(uint32_t mask)
{
	unsigned int count = 0u;
	while(mask != 0u)
	{
		count += mask & 1u;
		mask >>= 1u;
	}
	return count;
}

static void epoch_id_rounding_and_week_wrap(void)
{
	uint64_t before_wrap;
	uint64_t after_wrap;
	uint64_t same_epoch_a;
	uint64_t same_epoch_b;
	CHECK(gps_nav_epoch_id(2300, 604799.9999, 200u, &before_wrap),
	      "epoch id rejected a valid pre-wrap TOW");
	CHECK(gps_nav_epoch_id(2301, 0.0001, 200u, &after_wrap),
	      "epoch id rejected a valid post-wrap TOW");
	CHECK(before_wrap == after_wrap,
	      "week rollover split one rounded navigation epoch");
	CHECK(gps_nav_epoch_id(2301, 123.3997, 200u, &same_epoch_a) &&
	      gps_nav_epoch_id(2301, 123.4003, 200u, &same_epoch_b) &&
	      (same_epoch_a == same_epoch_b),
	      "sub-millisecond TOW skew split one navigation epoch");
	CHECK(!gps_nav_epoch_id(2301, NAN, 200u, &same_epoch_a),
	      "non-finite TOW produced an epoch id");
	CHECK(!gps_nav_epoch_id(2301, 1.0, 333u, &same_epoch_a),
	      "non-integral epochs per GPS week were accepted");
}

static void assembly_waits_for_every_ready_channel(void)
{
	gps_nav_epoch_assembly_t assembly;
	gps_nav_epoch_assembly_reset(&assembly);
	const uint32_t ready_mask = 0xafu;
	const unsigned int order[] = {7u, 0u, 5u, 3u, 2u, 1u};
	for(unsigned int i = 0u ; i < sizeof(order) / sizeof(order[0]) ; i++)
	{
		gps_nav_epoch_event_result_t result =
			gps_nav_epoch_assembly_push(
				&assembly, 1000u, order[i], ready_mask);
		bool last = i + 1u == sizeof(order) / sizeof(order[0]);
		CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) == last,
		      "assembly completed before every ready channel arrived");
	}
	CHECK(assembly.expected_mask == ready_mask,
	      "assembly changed its frozen ready mask");
	CHECK(assembly.received_mask == ready_mask,
	      "assembly lost a ready-channel interrupt");
}

static void assembly_handles_duplicates_and_epoch_order(void)
{
	gps_nav_epoch_assembly_t assembly;
	gps_nav_epoch_assembly_reset(&assembly);
	CHECK(gps_nav_epoch_assembly_push(&assembly, 2000u, 0u, 0x0fu) ==
	      GPS_NAV_EPOCH_EVENT_ACCEPTED,
	      "first channel did not start assembly");
	CHECK(gps_nav_epoch_assembly_push(&assembly, 2000u, 0u, 0x0fu) ==
	      GPS_NAV_EPOCH_EVENT_DUPLICATE,
	      "duplicate channel interrupt was not idempotent");
	CHECK(gps_nav_epoch_assembly_push(&assembly, 1999u, 1u, 0x0fu) ==
	      GPS_NAV_EPOCH_EVENT_STALE,
	      "stale epoch interrupt was not rejected");
	CHECK(gps_nav_epoch_assembly_push(&assembly, 2001u, 1u, 0x0fu) ==
	      GPS_NAV_EPOCH_EVENT_RESTARTED,
	      "newer epoch did not replace the incomplete epoch");
	CHECK((assembly.epoch_id == 2001u) &&
	      (assembly.expected_mask == 0x0fu) &&
	      (assembly.received_mask == 0x02u),
	      "newer epoch restart retained measurements from the old epoch");
}

static void assembly_disconnect_and_reconnect(void)
{
	gps_nav_epoch_assembly_t assembly;
	gps_nav_epoch_assembly_reset(&assembly);
	CHECK(gps_nav_epoch_assembly_push(&assembly, 3000u, 0u, 0x1fu) ==
	      GPS_NAV_EPOCH_EVENT_ACCEPTED,
	      "disconnect scenario did not start");
	CHECK(gps_nav_epoch_assembly_push(&assembly, 3000u, 1u, 0x1fu) ==
	      GPS_NAV_EPOCH_EVENT_ACCEPTED,
	      "second interrupt unexpectedly completed");
	CHECK(!gps_nav_epoch_assembly_remove_channel(&assembly, 4u),
	      "pending-channel disconnect completed too early");
	CHECK(gps_nav_epoch_assembly_push(&assembly, 3000u, 2u, 0x0fu) ==
	      GPS_NAV_EPOCH_EVENT_ACCEPTED,
	      "post-disconnect interrupt was rejected");
	CHECK(gps_nav_epoch_assembly_push(&assembly, 3000u, 3u, 0x0fu) ==
	      GPS_NAV_EPOCH_EVENT_COMPLETE,
	      "unlock did not release the removed channel from the epoch");

	gps_nav_epoch_assembly_reset(&assembly);
	CHECK(gps_nav_epoch_assembly_push(&assembly, 3001u, 0u, 0x1fu) ==
	      GPS_NAV_EPOCH_EVENT_ACCEPTED,
	      "reconnect epoch did not start");
	for(unsigned int channel = 1u ; channel < 5u ; channel++)
	{
		gps_nav_epoch_event_result_t result =
			gps_nav_epoch_assembly_push(
				&assembly, 3001u, channel, 0x1fu);
		CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) ==
		      (channel == 4u),
		      "reconnected channel was not required by the next epoch");
	}
}

static void channel_join_does_not_expand_active_epoch(void)
{
	gps_nav_epoch_assembly_t assembly;
	gps_nav_epoch_assembly_reset(&assembly);
	CHECK(gps_nav_epoch_assembly_push(&assembly, 4000u, 0u, 0x0fu) ==
	      GPS_NAV_EPOCH_EVENT_ACCEPTED,
	      "join scenario did not start");
	CHECK(gps_nav_epoch_assembly_push(&assembly, 4000u, 4u, 0x1fu) ==
	      GPS_NAV_EPOCH_EVENT_INVALID,
	      "joining channel changed the active epoch");
	CHECK(assembly.expected_mask == 0x0fu,
	      "active expected mask expanded after a channel joined");
	for(unsigned int channel = 1u ; channel < 4u ; channel++)
	{
		gps_nav_epoch_event_result_t result =
			gps_nav_epoch_assembly_push(
				&assembly, 4000u, channel, 0x1fu);
		CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) ==
		      (channel == 3u),
		      "original ready set did not complete after a join");
	}
}

static uint32_t deterministic_random(uint32_t* state)
{
	uint32_t value = *state;
	value ^= value << 13;
	value ^= value >> 17;
	value ^= value << 5;
	*state = value;
	return value;
}

static void randomized_interrupt_and_unlock_model(void)
{
	uint32_t random_state = 0x6a09e667u;
	for(unsigned int trial = 0u ; trial < 10000u ; trial++)
	{
		unsigned int channel_count =
			4u + (deterministic_random(&random_state) % 6u);
		uint32_t ready_mask = (1u << channel_count) - 1u;
		unsigned int order[GPS_NAV_EPOCH_MAX_CANDIDATES];
		for(unsigned int i = 0u ; i < channel_count ; i++)
		{
			order[i] = i;
		}
		for(unsigned int i = channel_count ; i > 1u ; i--)
		{
			unsigned int swap =
				deterministic_random(&random_state) % i;
			unsigned int temporary = order[i - 1u];
			order[i - 1u] = order[swap];
			order[swap] = temporary;
		}

		bool disconnect =
			(deterministic_random(&random_state) & 3u) == 0u;
		unsigned int dropped_channel = disconnect ?
			(deterministic_random(&random_state) % channel_count) :
			GPS_NAV_EPOCH_MAX_CANDIDATES;
		uint32_t final_mask = ready_mask;
		if(disconnect)
		{
			final_mask &= ~(1u << dropped_channel);
		}

		gps_nav_epoch_assembly_t assembly;
		gps_nav_epoch_assembly_reset(&assembly);
		bool complete = false;
		for(unsigned int i = 0u ; i < channel_count ; i++)
		{
			unsigned int channel = order[i];
			if(channel == dropped_channel)
			{
				complete = gps_nav_epoch_assembly_remove_channel(
					&assembly, channel);
				continue;
			}
			gps_nav_epoch_event_result_t result =
				gps_nav_epoch_assembly_push(
					&assembly, 5000u + trial, channel, ready_mask);
			if((deterministic_random(&random_state) & 7u) == 0u)
			{
				CHECK(gps_nav_epoch_assembly_push(
					      &assembly, 5000u + trial, channel,
					      ready_mask) ==
				      GPS_NAV_EPOCH_EVENT_DUPLICATE,
				      "random duplicate interrupt was not idempotent");
			}
			complete = result == GPS_NAV_EPOCH_EVENT_COMPLETE;
		}
		if(disconnect &&
		   ((assembly.expected_mask & (1u << dropped_channel)) != 0u))
		{
			complete = gps_nav_epoch_assembly_remove_channel(
				&assembly, dropped_channel);
		}
		CHECK(mask_count(final_mask) < 4u ||
		      (complete && gps_nav_epoch_assembly_complete(&assembly)),
		      "random interrupt/unlock sequence did not complete");
		CHECK(assembly.expected_mask == final_mask,
		      "random unlock changed the wrong expected channel");
		CHECK(assembly.received_mask == final_mask,
		      "random model lost or invented an interrupt");
	}
}

static void multi_epoch_hardware_interrupt_model(void)
{
	gps_nav_epoch_assembly_t assembly;
	uint32_t random_state = 0xbb67ae85u;
	uint32_t ready_mask = 0x7fu;
	unsigned int completed_epochs = 0u;
	unsigned int dropped_epochs = 0u;
	unsigned int joined_epochs = 0u;

	for(unsigned int epoch = 0u ; epoch < 2000u ; epoch++)
	{
		bool drop_before_interrupt = (epoch == 250u);
		bool drop_after_interrupt = (epoch == 900u);
		bool join_during_epoch = (epoch == 500u) || (epoch == 1200u);
		unsigned int changing_channel =
			(epoch == 1200u) ? 6u : 5u;

		if(epoch == 400u)
		{
			ready_mask |= 1u << 2;
		}
		if(epoch == 1050u)
		{
			ready_mask |= 1u << 4;
		}
		if(join_during_epoch)
		{
			ready_mask &= ~(1u << changing_channel);
		}

		uint32_t epoch_ready_mask = ready_mask;
		unsigned int order[GPS_NAV_EPOCH_MAX_CANDIDATES];
		unsigned int order_count = 0u;
		for(unsigned int channel = 0u ;
		    channel < GPS_NAV_EPOCH_MAX_CANDIDATES ; channel++)
		{
			if((epoch_ready_mask & (1u << channel)) != 0u)
			{
				order[order_count++] = channel;
			}
		}
		for(unsigned int i = order_count ; i > 1u ; i--)
		{
			unsigned int swap =
				deterministic_random(&random_state) % i;
			unsigned int temporary = order[i - 1u];
			order[i - 1u] = order[swap];
			order[swap] = temporary;
		}

		gps_nav_epoch_assembly_reset(&assembly);
		bool completed = false;
		for(unsigned int event_index = 0u ;
		    event_index < order_count ; event_index++)
		{
			unsigned int channel = order[event_index];
			if(drop_before_interrupt && (channel == 2u))
			{
				ready_mask &= ~(1u << channel);
				(void)gps_nav_epoch_assembly_remove_channel(
					&assembly, channel);
				dropped_epochs++;
				continue;
			}

			gps_nav_epoch_event_result_t result =
				gps_nav_epoch_assembly_push(
					&assembly, 100000u + epoch, channel,
					epoch_ready_mask);
			if(result == GPS_NAV_EPOCH_EVENT_COMPLETE)
			{
				completed = true;
			}

			if(drop_after_interrupt && (channel == 4u))
			{
				ready_mask &= ~(1u << channel);
				if(gps_nav_epoch_assembly_remove_channel(
					   &assembly, channel))
				{
					completed = true;
				}
				dropped_epochs++;
			}

			if(join_during_epoch && (event_index == 0u))
			{
				ready_mask |= 1u << changing_channel;
				CHECK(gps_nav_epoch_assembly_push(
					      &assembly, 100000u + epoch,
					      changing_channel, ready_mask) ==
				      GPS_NAV_EPOCH_EVENT_INVALID,
				      "mid-epoch connection expanded the frozen mask");
				joined_epochs++;
			}

			if((event_index == 0u) && (epoch != 0u))
			{
				CHECK(gps_nav_epoch_assembly_push(
					      &assembly, 99999u + epoch,
					      channel, epoch_ready_mask) ==
				      GPS_NAV_EPOCH_EVENT_STALE,
				      "delayed previous-epoch interrupt was accepted");
			}
		}

		CHECK(completed &&
		      gps_nav_epoch_assembly_complete(&assembly),
		      "hardware stream did not complete a live epoch");
		CHECK(assembly.received_mask == assembly.expected_mask,
		      "hardware stream completed with a missing interrupt");
		completed_epochs++;
	}

	CHECK(completed_epochs == 2000u,
	      "hardware stream produced more or fewer than one completion per epoch");
	CHECK(dropped_epochs == 2u,
	      "hardware stream did not exercise both unlock positions");
	CHECK(joined_epochs == 2u,
	      "hardware stream did not exercise both reconnect positions");
}

static void all_coherent_across_sample_wrap(void)
{
	gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
		{2300, 604799.9998, UINT32_MAX - 100u},
		{2301, 0.0001, 50u},
		{2301, 0.0000, 1200u},
		{2300, 604799.9999, UINT32_MAX - 2000u}
	};
	gps_nav_epoch_selection_t selection = select_epoch(candidates, 4u);
	CHECK(selection.selected_count == 4u,
	      "sample/week wrap split a coherent epoch");
	CHECK(selection.selected_mask == 0x0fu,
	      "sample/week wrap selected the wrong rows");
}

static void tow_outlier_does_not_block_good_cluster(void)
{
	gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
		{2300, 100.0000, 1000000u},
		{2300, 100.0001, 1010000u},
		{2300,  99.9999, 1020000u},
		{2300, 100.0002, 1030000u},
		{2300, 100.0000, 1040000u},
		{2300, 100.0001, 1050000u},
		{2300, 100.2000, 1060000u}
	};
	gps_nav_epoch_selection_t selection = select_epoch(candidates, 7u);
	CHECK(selection.selected_count == 6u,
	      "single TOW outlier blocked the six-satellite cluster");
	CHECK(selection.selected_mask == 0x3fu,
	      "TOW outlier was not excluded");
}

static void newest_sample_outlier_does_not_define_the_cluster(void)
{
	gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
		{2300, 200.0000, 2000000u},
		{2300, 200.0000, 2010000u},
		{2300, 200.0000, 2020000u},
		{2300, 200.0000, 2030000u},
		{2300, 200.0000, 2040000u},
		{2300, 200.0000, 2050000u},
		{2300, 200.0000, 3000000u}
	};
	gps_nav_epoch_selection_t selection = select_epoch(candidates, 7u);
	CHECK(selection.selected_count == 6u,
	      "newest raw-sample outlier displaced the largest cluster");
	CHECK(selection.selected_mask == 0x3fu,
	      "newest raw-sample outlier was not excluded");
}

static void equal_clusters_prefer_newer_epoch(void)
{
	gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
		{2300, 300.0000, 1000000u},
		{2300, 300.0000, 1010000u},
		{2300, 300.0000, 1020000u},
		{2300, 300.0000, 1030000u},
		{2300, 300.2000, 5000000u},
		{2300, 300.2000, 5010000u},
		{2300, 300.2000, 5020000u},
		{2300, 300.2000, 5030000u}
	};
	gps_nav_epoch_selection_t selection = select_epoch(candidates, 8u);
	CHECK(selection.selected_count == 4u,
	      "equal epoch clusters returned the wrong size");
	CHECK(selection.selected_mask == 0xf0u,
	      "equal epoch clusters did not prefer the newer epoch");
}

static void nonfinite_candidate_is_excluded(void)
{
	gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
		{2300, 400.0000, 1000000u},
		{2300, 400.0000, 1010000u},
		{2300, 400.0000, 1020000u},
		{2300, 400.0000, 1030000u},
		{2300, NAN,      1040000u}
	};
	gps_nav_epoch_selection_t selection = select_epoch(candidates, 5u);
	CHECK(selection.selected_count == 4u,
	      "invalid TOW candidate blocked valid measurements");
	CHECK(selection.selected_mask == 0x0fu,
	      "invalid TOW candidate was selected");
}

int main(void)
{
	epoch_id_rounding_and_week_wrap();
	assembly_waits_for_every_ready_channel();
	assembly_handles_duplicates_and_epoch_order();
	assembly_disconnect_and_reconnect();
	channel_join_does_not_expand_active_epoch();
	randomized_interrupt_and_unlock_model();
	multi_epoch_hardware_interrupt_model();
	all_coherent_across_sample_wrap();
	tow_outlier_does_not_block_good_cluster();
	newest_sample_outlier_does_not_define_the_cluster();
	equal_clusters_prefer_newer_epoch();
	nonfinite_candidate_is_excluded();

	if(failures == 0)
	{
		puts("gps_nav_epoch tests passed");
	}
	return failures == 0 ? 0 : 1;
}