#include "fusion/fusion_engine.h"

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

#define TEST_TIMEBASE_HZ 1000000u
#define TEST_IMU_HZ 52u
#define TEST_DURATION_S 30u
#define TEST_PI 3.14159265358979323846264338327950288
#define GPS_PERIOD_TICKS 200000u
#define GPS_DELAY_TICKS 73000u
#define MAX_GPS_EVENTS 160u

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

typedef struct {
	const char* name;
	double peak_horizontal_error_m;
	double peak_vertical_error_m;
	double final_horizontal_error_m;
	double final_vertical_error_m;
	double final_speed_mps;
	double final_roll_error_deg;
	uint64_t imu_pushed;
	uint64_t gps_accepted;
	uint64_t gps_rejected;
	uint64_t zero_updates;
	fusion_engine_state_t engine_state;
	fusion_motion_state_t motion_state;
} scenario_metrics_t;

static double radians(double degrees)
{
	return degrees * TEST_PI / 180.0;
}

static double degrees(double radians_value)
{
	return radians_value * 180.0 / TEST_PI;
}

/* The IMU rotates about its own X axis without translating.  Calibration is
 * complete before the manoeuvre.  This is the smallest synthetic version of
 * the live hand-tilt failure: the true position and the GNSS antenna remain at
 * the origin while body-frame gravity changes with roll. */
static void truth_roll(uint64_t ticks,
	                   double final_roll_rad,
	                   double* roll_rad,
	                   double* roll_rate_rps)
{
	double time_s = (double)ticks / (double)TEST_TIMEBASE_HZ;
	double roll = 0.0;
	double rate = 0.0;
	const double maximum_roll = radians(20.0);
	/* A short hand motion is intentionally placed between the 250 ms selected
	 * samples.  Full-rate IMU observes the angular rate; sample dropping can
	 * observe only the changed gravity vector and completely miss the gyro. */
	if((time_s >= 16.05) && (time_s < 16.15))
	{
		roll = maximum_roll * (time_s - 16.05) / 0.10;
		rate = maximum_roll / 0.10;
	}
	else if((time_s >= 16.15) && (time_s < 17.05))
	{
		roll = maximum_roll;
	}
	else if((time_s >= 17.05) && (time_s < 17.15))
	{
		double fraction = (time_s - 17.05) / 0.10;
		roll = maximum_roll + fraction * (final_roll_rad - maximum_roll);
		rate = (final_roll_rad - maximum_roll) / 0.10;
	}
	else if(time_s >= 17.15)
	{
		roll = final_roll_rad;
	}
	*roll_rad = roll;
	*roll_rate_rps = rate;
}

static fusion_imu_measurement_t make_imu(uint64_t ticks,
	                                     double gravity_mps2,
	                                     double final_roll_rad,
	                                     double gyro_scale)
{
	double roll;
	double roll_rate;
	truth_roll(ticks, final_roll_rad, &roll, &roll_rate);
	fusion_imu_measurement_t imu;
	memset(&imu, 0, sizeof(imu));
	imu.timestamp_ticks = ticks;
	/* R_nb is a positive X rotation.  A stationary accelerometer measures
	 * R_nb^T [0, 0, g], hence [0, g sin(phi), g cos(phi)] in body axes. */
	imu.accel_mps2[1] = gravity_mps2 * sin(roll);
	imu.accel_mps2[2] = gravity_mps2 * cos(roll);
	imu.gyro_rps[0] = gyro_scale * roll_rate;
	imu.temperature_c = 25.0;
	imu.sequence = (uint32_t)((ticks * TEST_IMU_HZ + TEST_TIMEBASE_HZ / 2u) /
	                          TEST_TIMEBASE_HZ);
	double time_s = (double)ticks / (double)TEST_TIMEBASE_HZ;
	imu.vehicle_at_rest = (time_s < 16.05) || (time_s >= 18.15);
	return imu;
}

static fusion_position_measurement_t make_gps(uint64_t ticks)
{
	fusion_position_measurement_t gps;
	memset(&gps, 0, sizeof(gps));
	gps.timestamp_ticks = ticks;
	gps.covariance_enu_m2[0] = 4.0;
	gps.covariance_enu_m2[4] = 4.0;
	gps.covariance_enu_m2[8] = 9.0;
	gps.source_quality = 100.0;
	gps.source_valid = true;
	return gps;
}

static unsigned int build_gps_events(gps_event_t events[MAX_GPS_EVENTS])
{
	unsigned int count = 0u;
	/* Deliberately offset GNSS epochs from both the 52 Hz IMU samples and
	 * integer seconds. */
	for(uint64_t ticks = 123457u;
	    ticks <= (uint64_t)TEST_DURATION_S * TEST_TIMEBASE_HZ;
	    ticks += GPS_PERIOD_TICKS)
	{
		events[count].measurement = make_gps(ticks);
		events[count].arrival_ticks = ticks + GPS_DELAY_TICKS;
		count++;
	}
	return count;
}

static double quaternion_roll(const double quaternion[4])
{
	double w = quaternion[0];
	double x = quaternion[1];
	double y = quaternion[2];
	double z = quaternion[3];
	return atan2(2.0 * (w * x + y * z),
	             1.0 - 2.0 * (x * x + y * y));
}

static scenario_metrics_t run_scenario(const char* name,
	                                   bool emulate_250_ms_sample_drop,
	                                   bool provide_gps,
	                                   double final_roll_deg,
	                                   double gyro_scale)
{
	static fusion_engine_t engine;
	memset(&engine, 0, sizeof(engine));
	fusion_engine_config_t config;
	fusion_engine_default_config(&config, TEST_TIMEBASE_HZ);
	if(!fusion_engine_init(&engine, &config))
	{
		return (scenario_metrics_t){.name = name, .engine_state = FUSION_ENGINE_FAULT};
	}

	/* Give calibration an absolute origin, just as the product does when GNSS
	 * becomes available before initial tilt/bias calibration completes. */
	if(provide_gps)
	{
		fusion_position_measurement_t origin = make_gps(1u);
		(void)fusion_engine_push_position(&engine, &origin, NULL, NULL);
	}

	gps_event_t events[MAX_GPS_EVENTS];
	unsigned int event_count = provide_gps ? build_gps_events(events) : 0u;
	unsigned int event_head = 0u;
	uint64_t last_forwarded_ticks = 0u;
	bool have_forwarded = false;
	scenario_metrics_t metrics;
	memset(&metrics, 0, sizeof(metrics));
	metrics.name = name;

	const uint64_t sample_count =
		(uint64_t)TEST_DURATION_S * TEST_IMU_HZ;
	for(uint64_t sample = 0u ; sample <= sample_count ; sample++)
	{
		uint64_t ticks = (sample * TEST_TIMEBASE_HZ + TEST_IMU_HZ / 2u) /
		                 TEST_IMU_HZ;
		fusion_imu_measurement_t imu = make_imu(
			ticks, config.eskf.gravity_mps2, radians(final_roll_deg), gyro_scale);
		bool forward = true;
		if(emulate_250_ms_sample_drop && have_forwarded &&
		   ((ticks - last_forwarded_ticks) < 250000u))
		{
			forward = false;
		}
		if(forward)
		{
			if(!fusion_engine_push_imu(&engine, &imu))
			{
				break;
			}
			last_forwarded_ticks = ticks;
			have_forwarded = true;
			metrics.imu_pushed++;
		}

		while((event_head < event_count) &&
		      (events[event_head].arrival_ticks <= ticks))
		{
			(void)fusion_engine_push_position(
				&engine, &events[event_head].measurement, NULL, NULL);
			event_head++;
		}

		if(engine.state == FUSION_ENGINE_RUNNING)
		{
			double horizontal = hypot(engine.current.filter.position_m[0],
			                          engine.current.filter.position_m[1]);
			metrics.peak_horizontal_error_m =
				fmax(metrics.peak_horizontal_error_m, horizontal);
			metrics.peak_vertical_error_m =
				fmax(metrics.peak_vertical_error_m,
				     fabs(engine.current.filter.position_m[2]));
		}
	}

	metrics.engine_state = engine.state;
	if(engine.state == FUSION_ENGINE_RUNNING)
	{
		metrics.final_horizontal_error_m =
			hypot(engine.current.filter.position_m[0],
			      engine.current.filter.position_m[1]);
		metrics.final_vertical_error_m = fabs(engine.current.filter.position_m[2]);
		metrics.final_speed_mps = eskf_speed_mps(&engine.current.filter);
		metrics.final_roll_error_deg =
			fabs(degrees(quaternion_roll(engine.current.filter.body_to_nav_quaternion)) -
			     final_roll_deg);
		metrics.motion_state = engine.current.motion.state;
	}
	metrics.gps_accepted = engine.diagnostics.position_accepted;
	metrics.gps_rejected = engine.diagnostics.position_nis_rejected;
	metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
	return metrics;
}

static void print_metrics(const scenario_metrics_t* metrics)
{
	printf("%-20s imu=%4llu state=%u motion=%-20s peak-h=%9.3f m peak-v=%9.3f m "
	       "final-h=%9.3f m final-v=%9.3f m speed=%8.3f m/s "
	       "roll=%7.3f deg gps=%llu/%llu zupt=%llu\n",
	       metrics->name,
	       (unsigned long long)metrics->imu_pushed,
	       (unsigned int)metrics->engine_state,
	       fusion_motion_state_name(metrics->motion_state),
	       metrics->peak_horizontal_error_m,
	       metrics->peak_vertical_error_m,
	       metrics->final_horizontal_error_m,
	       metrics->final_vertical_error_m,
	       metrics->final_speed_mps,
	       metrics->final_roll_error_deg,
	       (unsigned long long)metrics->gps_accepted,
	       (unsigned long long)metrics->gps_rejected,
	       (unsigned long long)metrics->zero_updates);
}

int main(void)
{
	scenario_metrics_t full_rate = run_scenario(
		"52 Hz full IMU", false, true, 0.0, 1.0);
	scenario_metrics_t imu_only = run_scenario(
		"52 Hz, no GNSS", false, false, 0.0, 1.0);
	scenario_metrics_t residual = run_scenario(
		"1 deg residual tilt", false, true, 10.0, 0.9);
	scenario_metrics_t sparse = run_scenario(
		"250 ms sample-drop", true, true, 0.0, 1.0);
	printf("Asynchronous stationary tilt: 52 Hz IMU, 5 Hz GNSS, 73 ms GNSS delay\n");
	print_metrics(&full_rate);
	print_metrics(&imu_only);
	print_metrics(&residual);
	print_metrics(&sparse);

	bool full_rate_ok = (full_rate.engine_state == FUSION_ENGINE_RUNNING) &&
	                    (full_rate.peak_horizontal_error_m < 1.0) &&
	                    (full_rate.peak_vertical_error_m < 1.0) &&
	                    (full_rate.final_speed_mps < 0.05);
	bool imu_only_ok = (imu_only.engine_state == FUSION_ENGINE_RUNNING) &&
	                   (imu_only.gps_accepted == 0u) &&
	                   (imu_only.peak_horizontal_error_m < 1.0) &&
	                   (imu_only.peak_vertical_error_m < 1.0) &&
	                   (imu_only.final_speed_mps < 0.05);
	bool residual_recovered =
		(residual.engine_state == FUSION_ENGINE_RUNNING) &&
		(residual.motion_state == FUSION_MOTION_STATIONARY) &&
		(residual.final_speed_mps < 0.05) &&
		(residual.final_horizontal_error_m < 1.0) &&
		(residual.final_roll_error_deg < 0.5);
	bool sparse_aliasing_detected_and_recovered =
		(sparse.engine_state == FUSION_ENGINE_RUNNING) &&
		(sparse.peak_horizontal_error_m > 3.0) &&
		(sparse.final_horizontal_error_m < 1.0) &&
		(sparse.final_speed_mps < 0.10) &&
		(sparse.motion_state == FUSION_MOTION_STATIONARY);
	if(!full_rate_ok || !imu_only_ok || !residual_recovered ||
	   !sparse_aliasing_detected_and_recovered)
	{
		fprintf(stderr,
		        "FAIL: full-rate propagation and GNSS-aided rest recovery must "
		        "remain bounded; sparse sampling must expose a large transient but "
		        "recover after an external at-rest decision.\n");
		return 1;
	}
	printf("PASS: full-rate and residual-tilt cases remained bounded; sparse "
	       "sampling exposed the transient and external at-rest recovered it.\n");
	return 0;
}