#include "fusion/fusion_engine.h"

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

#define TIMEBASE_HZ 1000000u
#define IMU_RATE_HZ 52u
#define GPS_PERIOD_TICKS 300000u
#define MAX_GPS_EVENTS 256u
#define PI 3.14159265358979323846

/*
 * Timing/noise scale comes from logs/gps_live_5min_20260713.csv:
 * 988 fixes in 299.697 s (3.297 Hz), 300 ms median interval, 1.803 s maximum
 * interval, and about 8.6 m stationary horizontal RMS scatter.  The 6 m
 * per-axis covariance below reproduces that horizontal RMS scale.
 */
typedef enum {
	SCENARIO_NORMAL = 0,
	SCENARIO_MOVING_OUTAGE,
	SCENARIO_INTERMITTENT,
	SCENARIO_BURST_RETURN,
	SCENARIO_BAD_RESTART
} scenario_kind_t;

typedef struct {
	fusion_position_measurement_t measurement;
	uint64_t delivery_ticks;
} gps_event_t;

typedef struct {
	const char* name;
	double horizontal_rmse_m;
	double maximum_outage_error_m;
	double error_at_outage_end_m;
	double final_horizontal_error_m;
	double maximum_gps_state_jump_m;
	double speed_at_outage_start_mps;
	double speed_at_outage_end_mps;
	double horizontal_sigma_at_outage_end_m;
	uint64_t gps_accepted;
	uint64_t gps_rejected;
	uint64_t gps_recoveries;
	uint64_t stale_rejected;
	uint64_t replay_count;
	unsigned int generated;
	unsigned int dropped;
	bool finite;
} scenario_metrics_t;

static uint32_t random_state;

static double uniform_random(void)
{
	random_state = 1664525u * random_state + 1013904223u;
	return ((double)((random_state >> 8) & 0x00ffffffu) + 0.5) /
	       16777216.0;
}

static double gaussian_random(void)
{
	double first = fmax(uniform_random(), 1.0e-12);
	double second = uniform_random();
	return sqrt(-2.0 * log(first)) * cos(2.0 * PI * second);
}

static void truth_at(double time_s, double position[3],
	                 double velocity[3], double acceleration[3])
{
	memset(position, 0, 3u * sizeof(double));
	memset(velocity, 0, 3u * sizeof(double));
	memset(acceleration, 0, 3u * sizeof(double));
	if(time_s < 3.0)
	{
		return;
	}
	if(time_s < 5.0)
	{
		double elapsed = time_s - 3.0;
		acceleration[0] = 1.5;
		velocity[0] = 1.5 * elapsed;
		position[0] = 0.75 * elapsed * elapsed;
		return;
	}
	if(time_s < 50.0)
	{
		velocity[0] = 3.0;
		position[0] = 3.0 + 3.0 * (time_s - 5.0);
		return;
	}
	if(time_s < 52.0)
	{
		double elapsed = time_s - 50.0;
		acceleration[0] = -1.5;
		velocity[0] = 3.0 - 1.5 * elapsed;
		position[0] = 138.0 + 3.0 * elapsed - 0.75 * elapsed * elapsed;
		return;
	}
	position[0] = 141.0;
}

static fusion_imu_measurement_t make_imu(
	uint64_t ticks, unsigned int sequence, double gravity_mps2)
{
	double time_s = (double)ticks / (double)TIMEBASE_HZ;
	double position[3];
	double velocity[3];
	double acceleration[3];
	truth_at(time_s, position, velocity, acceleration);
	(void)position;
	(void)velocity;

	fusion_imu_measurement_t imu;
	memset(&imu, 0, sizeof(imu));
	imu.timestamp_ticks = ticks;
	imu.accel_mps2[0] = acceleration[0] + 0.030;
	imu.accel_mps2[1] = -0.020;
	imu.accel_mps2[2] = gravity_mps2 + 0.050;
	imu.gyro_rps[0] = 0.0008;
	imu.gyro_rps[1] = -0.0005;
	imu.gyro_rps[2] = 0.0006;
	if(time_s >= 3.0)
	{
		/* Small temperature-like residual drift after startup calibration. */
		imu.accel_mps2[0] += 0.012 * (time_s - 3.0) / 62.0;
	}
	for(unsigned int axis = 0u ; axis < 3u ; axis++)
	{
		double phase = 0.37 * (double)sequence + 1.7 * (double)axis;
		imu.accel_mps2[axis] += 0.025 * sin(phase);
		imu.gyro_rps[axis] += 0.0007 * cos(1.31 * phase);
	}
	imu.temperature_c = 25.0 + 0.02 * time_s;
	imu.sequence = sequence;
	imu.vehicle_at_rest = (time_s < 2.9) || (time_s >= 52.5);
	return imu;
}

static fusion_position_measurement_t make_gps(
	uint64_t capture_ticks, bool bad_restart_fix)
{
	double position[3];
	double velocity[3];
	double acceleration[3];
	truth_at((double)capture_ticks / (double)TIMEBASE_HZ,
	         position, velocity, acceleration);
	(void)velocity;
	(void)acceleration;

	fusion_position_measurement_t gps;
	memset(&gps, 0, sizeof(gps));
	gps.timestamp_ticks = capture_ticks;
	gps.position_enu_m[0] = position[0] + 6.0 * gaussian_random();
	gps.position_enu_m[1] = position[1] + 6.0 * gaussian_random();
	gps.position_enu_m[2] = position[2] + 10.0 * gaussian_random();
	if(bad_restart_fix)
	{
		gps.position_enu_m[0] += 80.0;
		gps.position_enu_m[1] -= 50.0;
		gps.position_enu_m[2] += 30.0;
	}
	gps.covariance_enu_m2[0] = 36.0;
	gps.covariance_enu_m2[4] = 36.0;
	gps.covariance_enu_m2[8] = 100.0;
	gps.source_quality = 80.0;
	gps.source_valid = true;
	return gps;
}

static bool event_should_drop(scenario_kind_t kind, double capture_s)
{
	if((kind == SCENARIO_MOVING_OUTAGE) &&
	   (capture_s >= 15.0) && (capture_s < 27.0))
	{
		return true;
	}
	if((kind == SCENARIO_BAD_RESTART) &&
	   (capture_s >= 15.0) && (capture_s < 23.1))
	{
		return true;
	}
	if(kind == SCENARIO_INTERMITTENT)
	{
		bool natural_long_gap =
			((capture_s >= 12.0) && (capture_s < 13.8)) ||
			((capture_s >= 31.0) && (capture_s < 32.8));
		return natural_long_gap || (uniform_random() < 0.35);
	}
	return false;
}

static unsigned int build_events(
	scenario_kind_t kind, gps_event_t events[MAX_GPS_EVENTS],
	unsigned int* dropped_out)
{
	unsigned int count = 0u;
	unsigned int dropped = 0u;
	uint64_t previous_delivery = 0u;
	for(uint64_t capture = 3300000u ; capture <= 64000000u ;
	    capture += GPS_PERIOD_TICKS)
	{
		double capture_s = (double)capture / (double)TIMEBASE_HZ;
		if(event_should_drop(kind, capture_s))
		{
			dropped++;
			continue;
		}
		if(count >= MAX_GPS_EVENTS)
		{
			break;
		}
		bool bad_restart_fix = (kind == SCENARIO_BAD_RESTART) &&
		                       (capture_s >= 23.1) && (capture_s < 24.0);
		events[count].measurement = make_gps(capture, bad_restart_fix);
		uint64_t delay = 120000u + (uint64_t)(250000.0 * uniform_random());
		uint64_t delivery = capture + delay;
		if((kind == SCENARIO_BURST_RETURN) &&
		   (capture_s >= 15.0) && (capture_s < 20.5))
		{
			delivery = 20700000u;
		}
		/* A receiver normally publishes epochs in capture order even when its
		 * processing latency jitters. Preserve that property here. */
		if(delivery < previous_delivery)
		{
			delivery = previous_delivery;
		}
		events[count].delivery_ticks = delivery;
		previous_delivery = delivery;
		count++;
	}
	*dropped_out = dropped;
	return count;
}

static double horizontal_error(const fusion_engine_t* engine, uint64_t ticks)
{
	double truth_position[3];
	double truth_velocity[3];
	double truth_acceleration[3];
	truth_at((double)ticks / (double)TIMEBASE_HZ,
	         truth_position, truth_velocity, truth_acceleration);
	(void)truth_velocity;
	(void)truth_acceleration;
	return hypot(engine->current.filter.position_m[0] - truth_position[0],
	             engine->current.filter.position_m[1] - truth_position[1]);
}

static scenario_metrics_t run_scenario(
	scenario_kind_t kind, const char* name, uint32_t seed)
{
	static fusion_engine_t engine;
	static gps_event_t events[MAX_GPS_EVENTS];
	fusion_engine_config_t config;
	scenario_metrics_t metrics;
	memset(&metrics, 0, sizeof(metrics));
	metrics.name = name;
	random_state = seed;
	unsigned int dropped = 0u;
	unsigned int event_count = build_events(kind, events, &dropped);
	metrics.generated = event_count;
	metrics.dropped = dropped;

	fusion_engine_default_config(&config, TIMEBASE_HZ);
	if(!fusion_engine_init(&engine, &config))
	{
		return metrics;
	}
	fusion_position_measurement_t origin;
	memset(&origin, 0, sizeof(origin));
	origin.timestamp_ticks = 1u;
	origin.covariance_enu_m2[0] = 36.0;
	origin.covariance_enu_m2[4] = 36.0;
	origin.covariance_enu_m2[8] = 100.0;
	origin.source_quality = 100.0;
	origin.source_valid = true;
	if(!fusion_engine_push_position(&engine, &origin, NULL, NULL))
	{
		return metrics;
	}

	unsigned int event_head = 0u;
	double squared_error_sum = 0.0;
	unsigned int error_count = 0u;
	unsigned int imu_count = 65u * IMU_RATE_HZ;
	for(unsigned int sample = 0u ; sample <= imu_count ; sample++)
	{
		uint64_t ticks =
			((uint64_t)sample * TIMEBASE_HZ + IMU_RATE_HZ / 2u) / IMU_RATE_HZ;
		fusion_imu_measurement_t imu = make_imu(
			ticks, sample, config.eskf.gravity_mps2);
		if(!fusion_engine_push_imu(&engine, &imu))
		{
			return metrics;
		}

		while((event_head < event_count) &&
		      (events[event_head].delivery_ticks <= ticks))
		{
			double before[3];
			memcpy(before, engine.current.filter.position_m, sizeof(before));
			(void)fusion_engine_push_position(
				&engine, &events[event_head].measurement, NULL, NULL);
			double jump = hypot(
				engine.current.filter.position_m[0] - before[0],
				engine.current.filter.position_m[1] - before[1]);
			if(jump > metrics.maximum_gps_state_jump_m)
			{
				metrics.maximum_gps_state_jump_m = jump;
			}
			event_head++;
		}

		if(engine.state != FUSION_ENGINE_RUNNING)
		{
			continue;
		}
		double time_s = (double)ticks / (double)TIMEBASE_HZ;
		double error = horizontal_error(&engine, ticks);
		if(time_s >= 3.0)
		{
			squared_error_sum += error * error;
			error_count++;
		}
		if((time_s >= 15.0) && (time_s <= 27.0) &&
		   (error > metrics.maximum_outage_error_m))
		{
			metrics.maximum_outage_error_m = error;
		}
		if(fabs(time_s - 27.0) < 0.5 / (double)IMU_RATE_HZ)
		{
			metrics.error_at_outage_end_m = error;
			metrics.speed_at_outage_end_mps =
				hypot(engine.current.filter.velocity_mps[0],
				      engine.current.filter.velocity_mps[1]);
			metrics.horizontal_sigma_at_outage_end_m = sqrt(
				engine.current.filter.covariance[0][0] +
				engine.current.filter.covariance[1][1]);
		}
		if(fabs(time_s - 15.0) < 0.5 / (double)IMU_RATE_HZ)
		{
			metrics.speed_at_outage_start_mps =
				hypot(engine.current.filter.velocity_mps[0],
				      engine.current.filter.velocity_mps[1]);
		}
	}

	metrics.horizontal_rmse_m =
		(error_count != 0u) ? sqrt(squared_error_sum / (double)error_count) : NAN;
	metrics.final_horizontal_error_m =
		horizontal_error(&engine, 65u * TIMEBASE_HZ);
	metrics.gps_accepted = engine.diagnostics.position_accepted;
	metrics.gps_rejected = engine.diagnostics.position_nis_rejected;
	metrics.gps_recoveries = engine.diagnostics.position_recovery_count;
	metrics.stale_rejected = engine.diagnostics.position_stale_rejected;
	metrics.replay_count = engine.diagnostics.replay_count;
	metrics.finite = (engine.state == FUSION_ENGINE_RUNNING) &&
	                 eskf_state_is_finite(&engine.current.filter) &&
	                 isfinite(metrics.horizontal_rmse_m) &&
	                 isfinite(metrics.final_horizontal_error_m);
	return metrics;
}

static void print_metrics(const scenario_metrics_t* metrics)
{
	printf("%-18s fixes=%3u drop=%2u accepted/rejected=%3llu/%2llu "
	       "recovery=%llu stale=%llu rmse=%5.2fm outage-max/end=%5.2f/%5.2fm "
	       "speed15/27=%4.2f/%4.2f sigma27=%5.1fm jump=%5.2fm final=%5.2fm finite=%u\n",
	       metrics->name, metrics->generated, metrics->dropped,
	       (unsigned long long)metrics->gps_accepted,
	       (unsigned long long)metrics->gps_rejected,
	       (unsigned long long)metrics->gps_recoveries,
	       (unsigned long long)metrics->stale_rejected,
	       metrics->horizontal_rmse_m,
	       metrics->maximum_outage_error_m,
	       metrics->error_at_outage_end_m,
	       metrics->speed_at_outage_start_mps,
	       metrics->speed_at_outage_end_mps,
	       metrics->horizontal_sigma_at_outage_end_m,
	       metrics->maximum_gps_state_jump_m,
	       metrics->final_horizontal_error_m,
	       metrics->finite ? 1u : 0u);
}

int main(void)
{
	scenario_metrics_t normal = run_scenario(
		SCENARIO_NORMAL, "normal", 0x10203040u);
	scenario_metrics_t outage = run_scenario(
		SCENARIO_MOVING_OUTAGE, "12s-moving-outage", 0x20304050u);
	scenario_metrics_t intermittent = run_scenario(
		SCENARIO_INTERMITTENT, "intermittent", 0x30405060u);
	scenario_metrics_t burst = run_scenario(
		SCENARIO_BURST_RETURN, "burst-return", 0x40506070u);
	scenario_metrics_t restart = run_scenario(
		SCENARIO_BAD_RESTART, "bad-restart", 0x50607080u);

	puts("Real-log-scaled GNSS failure scenarios: 3.3 Hz, 6 m horizontal / 10 m vertical sigma");
	print_metrics(&normal);
	print_metrics(&outage);
	print_metrics(&intermittent);
	print_metrics(&burst);
	print_metrics(&restart);

	bool common_ok =
		normal.finite && outage.finite && intermittent.finite && burst.finite &&
		restart.finite &&
		(normal.horizontal_rmse_m < 5.0) &&
		/* Position-only dead reckoning is allowed to drift, but the error must
		 * remain statistically covered and recover once GNSS returns. */
		(outage.maximum_outage_error_m <
		 2.0 * outage.horizontal_sigma_at_outage_end_m) &&
		(outage.horizontal_sigma_at_outage_end_m > 10.0) &&
		(outage.final_horizontal_error_m < 5.0) &&
		(intermittent.final_horizontal_error_m < 6.0) &&
		(burst.stale_rejected == 0u) &&
		(burst.replay_count > 0u);
	bool restart_safe =
		(restart.gps_recoveries == 0u) &&
		(restart.gps_rejected >= 3u) &&
		(restart.maximum_gps_state_jump_m < 50.0) &&
		(restart.final_horizontal_error_m < 6.0);
	if(!common_ok || !restart_safe)
	{
		fprintf(stderr,
		        "FAIL: GNSS outage/reacquisition safety regression "
		        "(common=%u restart-safe=%u)\n",
		        common_ok ? 1u : 0u, restart_safe ? 1u : 0u);
		return 1;
	}
	puts("PASS: realistic GNSS loss, intermittent delivery and reacquisition stayed bounded.");
	return 0;
}