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

#include "fusion/fusion_engine.h"

#define SYNTH_PI 3.14159265358979323846264338327950288
#define SYNTH_TIMEBASE_HZ 1000000u
#define SYNTH_IMU_HZ 200u
#define SYNTH_DURATION_S 60u
#define SYNTH_GPS_EVENT_CAPACITY 96u

typedef struct {
	uint32_t state;
} synth_rng_t;

typedef struct {
	double position_m[3];
	double velocity_mps[3];
	double yaw_rad;
} synth_truth_t;

typedef struct {
	fusion_position_measurement_t measurement;
	uint64_t delivery_ticks;
	bool injected_outlier;
} synth_gps_event_t;

typedef struct {
	double horizontal_rmse_m;
	double vertical_rmse_m;
	double final_horizontal_error_m;
	double final_vertical_error_m;
	double final_speed_mps;
	double max_rejected_state_jump_m;
	double covariance_symmetry_error;
	uint64_t accepted_gps;
	uint64_t rejected_gps;
	uint64_t replay_count;
	uint64_t replayed_imu_count;
	uint64_t zero_updates;
	unsigned int injected_outliers;
	unsigned int rejected_injected_outliers;
	unsigned int false_stationary_samples;
	double moving_detection_time_s;
	double stationary_reentry_time_s;
	bool covariance_positive_semidefinite;
	bool state_finite;
} synth_metrics_t;

typedef struct {
	double final_horizontal_error_m;
	double final_vertical_error_m;
	double final_speed_mps;
	uint64_t accepted_gps;
	uint64_t zero_updates;
	unsigned int nonstationary_samples;
	bool covariance_positive_semidefinite;
	bool state_finite;
} stationary_metrics_t;

static int failures;

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

static double square(double value)
{
	return value * value;
}

static double norm2(double first, double second)
{
	return sqrt(first * first + second * second);
}

static double norm3(const double value[3])
{
	return sqrt(square(value[0]) + square(value[1]) + square(value[2]));
}

static uint32_t rng_next(synth_rng_t* rng)
{
	uint32_t value = rng->state;
	value ^= value << 13;
	value ^= value >> 17;
	value ^= value << 5;
	rng->state = value;
	return value;
}

static double rng_uniform(synth_rng_t* rng)
{
	return ((double)rng_next(rng) + 0.5) / 4294967296.0;
}

static double rng_gaussian(synth_rng_t* rng)
{
	double first = fmax(rng_uniform(rng), 1.0e-12);
	double second = rng_uniform(rng);
	return sqrt(-2.0 * log(first)) * cos(2.0 * SYNTH_PI * second);
}

static void trajectory_input(double time_s,
	                         double acceleration_nav_mps2[3],
	                         double* yaw_rate_rps)
{
	memset(acceleration_nav_mps2, 0, 3u * sizeof(double));
	*yaw_rate_rps = 0.0;
	if((time_s >= 5.0) && (time_s < 7.0))
	{
		acceleration_nav_mps2[0] = 1.0;
	}
	else if((time_s >= 10.0) && (time_s < 12.0))
	{
		acceleration_nav_mps2[0] = -1.0;
	}
	else if((time_s >= 15.0) && (time_s < 17.0))
	{
		*yaw_rate_rps = 0.25 * SYNTH_PI;
	}
	else if((time_s >= 17.0) && (time_s < 19.0))
	{
		acceleration_nav_mps2[1] = 0.8;
	}
	else if((time_s >= 22.0) && (time_s < 24.0))
	{
		acceleration_nav_mps2[1] = -0.8;
	}
}

static void truth_step(synth_truth_t* truth,
	                   double dt_s,
	                   const double acceleration_nav_mps2[3],
	                   double yaw_rate_rps)
{
	for(unsigned int axis = 0u ; axis < 3u ; axis++)
	{
		truth->position_m[axis] += truth->velocity_mps[axis] * dt_s +
		                            0.5 * acceleration_nav_mps2[axis] * dt_s * dt_s;
		truth->velocity_mps[axis] += acceleration_nav_mps2[axis] * dt_s;
	}
	truth->yaw_rad += yaw_rate_rps * dt_s;
}

static fusion_position_measurement_t make_position(
	uint64_t timestamp_ticks,
	const synth_truth_t* truth,
	synth_rng_t* gps_rng,
	bool outlier)
{
	fusion_position_measurement_t position;
	memset(&position, 0, sizeof(position));
	position.timestamp_ticks = timestamp_ticks;
	position.position_enu_m[0] = truth->position_m[0] + 1.2 * rng_gaussian(gps_rng);
	position.position_enu_m[1] = truth->position_m[1] + 1.2 * rng_gaussian(gps_rng);
	position.position_enu_m[2] = truth->position_m[2] + 2.0 * rng_gaussian(gps_rng);
	if(outlier)
	{
		position.position_enu_m[0] += 150.0;
		position.position_enu_m[1] -= 120.0;
		position.position_enu_m[2] += 80.0;
	}
	position.covariance_enu_m2[0] = square(1.2);
	position.covariance_enu_m2[4] = square(1.2);
	position.covariance_enu_m2[8] = square(2.0);
	position.source_quality = outlier ? 0.7 : 1.0;
	position.source_valid = true;
	return position;
}

static bool covariance_is_psd(const eskf_t* filter,
	                          double* symmetry_error_out)
{
	double lower[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM] = {{0.0}};
	double max_symmetry_error = 0.0;
	for(unsigned int row = 0u ; row < ESKF_ERROR_STATE_DIM ; row++)
	{
		for(unsigned int col = 0u ; col < ESKF_ERROR_STATE_DIM ; col++)
		{
			double error = fabs(filter->covariance[row][col] -
			                    filter->covariance[col][row]);
			if(error > max_symmetry_error)
			{
				max_symmetry_error = error;
			}
		}
	}
	if(symmetry_error_out != NULL)
	{
		*symmetry_error_out = max_symmetry_error;
	}

	/* A tiny diagonal jitter makes the semidefinite check numerically robust. */
	for(unsigned int row = 0u ; row < ESKF_ERROR_STATE_DIM ; row++)
	{
		for(unsigned int col = 0u ; col <= row ; col++)
		{
			double value = filter->covariance[row][col];
			if(row == col)
			{
				value += 1.0e-10;
			}
			for(unsigned int k = 0u ; k < col ; k++)
			{
				value -= lower[row][k] * lower[col][k];
			}
			if(row == col)
			{
				if(!isfinite(value) || (value <= 0.0))
				{
					return false;
				}
				lower[row][col] = sqrt(value);
			}
			else
			{
				lower[row][col] = value / lower[col][col];
			}
		}
	}
	return true;
}

static void initialize_synthetic_engine(fusion_engine_t* engine)
{
	fusion_engine_config_t config;
	fusion_engine_default_config(&config, SYNTH_TIMEBASE_HZ);
	config.calibration_duration_s = 2.0;
	config.calibration_min_samples = SYNTH_IMU_HZ;
	CHECK(fusion_engine_init(engine, &config), "synthetic fusion engine initializes");

	fusion_position_measurement_t origin;
	memset(&origin, 0, sizeof(origin));
	origin.timestamp_ticks = 0u;
	origin.covariance_enu_m2[0] = square(1.2);
	origin.covariance_enu_m2[4] = square(1.2);
	origin.covariance_enu_m2[8] = square(2.0);
	origin.source_quality = 1.0;
	origin.source_valid = true;
	CHECK(fusion_engine_push_position(engine, &origin, NULL, NULL),
	      "initial GPS is retained during calibration");
}

static synth_metrics_t run_dynamic_scenario(uint32_t scenario_seed)
{
	static fusion_engine_t engine;
	initialize_synthetic_engine(&engine);
	synth_truth_t truth = {{0.0, 0.0, 0.0}, {0.0, 0.0, 0.0}, 0.0};
	synth_rng_t imu_rng = {0x13579bdfu ^ (0x9e3779b9u * scenario_seed)};
	synth_rng_t gps_rng = {0x2468ace1u ^ (0x85ebca6bu * scenario_seed)};
	synth_rng_t timing_rng = {0xa5a5c3c3u ^ (0xc2b2ae35u * scenario_seed)};
	const double accel_bias[3] = {0.030, -0.025, 0.060};
	const double gyro_bias[3] = {0.0010, -0.0007, 0.0009};
	const uint64_t imu_period_ticks = SYNTH_TIMEBASE_HZ / SYNTH_IMU_HZ;
	const double dt_s = 1.0 / (double)SYNTH_IMU_HZ;
	const uint64_t final_tick = (uint64_t)SYNTH_DURATION_S * SYNTH_TIMEBASE_HZ;
	uint64_t next_gps_tick = 3000000u;
	unsigned int gps_sequence = 0u;
	synth_gps_event_t events[SYNTH_GPS_EVENT_CAPACITY];
	unsigned int event_head = 0u;
	unsigned int event_count = 0u;
	double horizontal_error_sum = 0.0;
	double vertical_error_sum = 0.0;
	uint64_t error_sample_count = 0u;
	synth_metrics_t metrics;
	memset(&metrics, 0, sizeof(metrics));
	metrics.moving_detection_time_s = NAN;
	metrics.stationary_reentry_time_s = NAN;
#ifdef SYNTH_TRACE
	fusion_motion_state_t traced_state = FUSION_MOTION_BOOTSTRAP;
#endif

	for(uint64_t tick = 0u ; tick <= final_tick ; tick += imu_period_ticks)
	{
		double acceleration_nav[3] = {0.0, 0.0, 0.0};
		double yaw_rate = 0.0;
		if(tick != 0u)
		{
			double midpoint_s = ((double)tick - 0.5 * (double)imu_period_ticks) /
			                    (double)SYNTH_TIMEBASE_HZ;
			trajectory_input(midpoint_s, acceleration_nav, &yaw_rate);
			truth_step(&truth, dt_s, acceleration_nav, yaw_rate);
		}

		double midpoint_yaw = truth.yaw_rad - 0.5 * yaw_rate * dt_s;
		double cosine = cos(midpoint_yaw);
		double sine = sin(midpoint_yaw);
		double specific_force_nav[3] = {
			acceleration_nav[0], acceleration_nav[1], engine.config.eskf.gravity_mps2
		};
		fusion_imu_measurement_t imu;
		memset(&imu, 0, sizeof(imu));
		imu.timestamp_ticks = tick;
		imu.accel_mps2[0] = cosine * specific_force_nav[0] +
		                    sine * specific_force_nav[1];
		imu.accel_mps2[1] = -sine * specific_force_nav[0] +
		                     cosine * specific_force_nav[1];
		imu.accel_mps2[2] = specific_force_nav[2];
		imu.gyro_rps[2] = yaw_rate;
		for(unsigned int axis = 0u ; axis < 3u ; axis++)
		{
			imu.accel_mps2[axis] += accel_bias[axis] + 0.025 * rng_gaussian(&imu_rng);
			imu.gyro_rps[axis] += gyro_bias[axis] + 0.0008 * rng_gaussian(&imu_rng);
		}
		imu.temperature_c = 25.0 + 0.5 * sin((double)tick / 10000000.0);
		imu.sequence = (uint32_t)(tick / imu_period_ticks);
		imu.vehicle_at_rest =
			(norm3(truth.velocity_mps) < 0.05) &&
			(norm3(acceleration_nav) < 0.05) && (fabs(yaw_rate) < 0.02);
		CHECK(fusion_engine_push_imu(&engine, &imu), "dynamic synthetic IMU accepted");
		if(engine.state == FUSION_ENGINE_FAULT)
		{
			break;
		}

		if((tick >= next_gps_tick) &&
		   ((event_head + event_count) < SYNTH_GPS_EVENT_CAPACITY))
		{
			bool outlier = (gps_sequence == 8u) || (gps_sequence == 27u);
			unsigned int index = event_head + event_count;
			events[index].measurement = make_position(tick, &truth, &gps_rng, outlier);
			double delay_s = 0.18 + 0.17 * rng_uniform(&timing_rng);
			events[index].delivery_ticks = tick +
				(uint64_t)(delay_s * (double)SYNTH_TIMEBASE_HZ + 0.5);
			events[index].injected_outlier = outlier;
			event_count++;
			if(outlier)
			{
				metrics.injected_outliers++;
			}
			gps_sequence++;
			double next_period_s = 0.80 + 0.40 * rng_uniform(&timing_rng);
			next_gps_tick = tick +
				(uint64_t)(next_period_s * (double)SYNTH_TIMEBASE_HZ + 0.5);
		}

		while((event_count != 0u) && (events[event_head].delivery_ticks <= tick))
		{
			double before[3];
			memcpy(before, engine.current.filter.position_m, sizeof(before));
			eskf_update_result_t result = ESKF_UPDATE_INVALID;
			double nis = NAN;
			CHECK(fusion_engine_push_position(&engine, &events[event_head].measurement,
			                                  &result, &nis),
			      "delayed asynchronous GPS is processed");
			if(events[event_head].injected_outlier &&
			   (result == ESKF_UPDATE_REJECTED_NIS))
			{
				metrics.rejected_injected_outliers++;
				double state_jump = norm2(
					engine.current.filter.position_m[0] - before[0],
					engine.current.filter.position_m[1] - before[1]);
				if(state_jump > metrics.max_rejected_state_jump_m)
				{
					metrics.max_rejected_state_jump_m = state_jump;
				}
			}
			event_head++;
			event_count--;
		}

		if(engine.state == FUSION_ENGINE_RUNNING)
		{
			double time_s = (double)tick / (double)SYNTH_TIMEBASE_HZ;
			fusion_motion_state_t state = engine.current.motion.state;
#ifdef SYNTH_TRACE
			if(state != traced_state)
			{
				printf("TRACE t=%.3f state=%s speed=%.3f gnss=%u confirm=%u accel=%.3f jerk=%.3f\n",
				       time_s, fusion_motion_state_name(state),
				       eskf_speed_mps(&engine.current.filter),
				       (unsigned int)engine.current.gnss_evidence,
				       engine.current.gnss_moving_count,
				       engine.current.motion.last_linear_accel_norm_mps2,
				       engine.current.motion.last_jerk_norm_mps3);
				traced_state = state;
			}
#endif
			if(isnan(metrics.moving_detection_time_s) && (time_s >= 5.0) &&
			   (state == FUSION_MOTION_MOVING))
			{
				metrics.moving_detection_time_s = time_s;
			}
			if((time_s >= 7.25) && (time_s <= 9.75) &&
			   (state == FUSION_MOTION_STATIONARY))
			{
				metrics.false_stationary_samples++;
			}
			if((time_s >= 19.25) && (time_s <= 21.75) &&
			   (state == FUSION_MOTION_STATIONARY))
			{
				metrics.false_stationary_samples++;
			}
			if(isnan(metrics.stationary_reentry_time_s) && (time_s >= 24.0) &&
			   (state == FUSION_MOTION_STATIONARY))
			{
				metrics.stationary_reentry_time_s = time_s;
			}
			if(time_s >= 3.0)
			{
				double east_error = engine.current.filter.position_m[0] - truth.position_m[0];
				double north_error = engine.current.filter.position_m[1] - truth.position_m[1];
				double up_error = engine.current.filter.position_m[2] - truth.position_m[2];
				horizontal_error_sum += square(east_error) + square(north_error);
				vertical_error_sum += square(up_error);
				error_sample_count++;
			}
		}
	}

	metrics.accepted_gps = engine.diagnostics.position_accepted;
	metrics.rejected_gps = engine.diagnostics.position_nis_rejected;
	metrics.replay_count = engine.diagnostics.replay_count;
	metrics.replayed_imu_count = engine.diagnostics.replayed_imu_count;
	metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
	metrics.state_finite = eskf_state_is_finite(&engine.current.filter);
	metrics.covariance_positive_semidefinite = covariance_is_psd(
		&engine.current.filter, &metrics.covariance_symmetry_error);
	if(error_sample_count != 0u)
	{
		metrics.horizontal_rmse_m = sqrt(horizontal_error_sum / (double)error_sample_count);
		metrics.vertical_rmse_m = sqrt(vertical_error_sum / (double)error_sample_count);
	}
	metrics.final_horizontal_error_m = norm2(
		engine.current.filter.position_m[0] - truth.position_m[0],
		engine.current.filter.position_m[1] - truth.position_m[1]);
	metrics.final_vertical_error_m = fabs(
		engine.current.filter.position_m[2] - truth.position_m[2]);
	metrics.final_speed_mps = norm3(engine.current.filter.velocity_mps);
	return metrics;
}

static void check_dynamic_metrics(const synth_metrics_t* metrics)
{
	CHECK(metrics->state_finite, "60 s synthetic state remains finite");
	CHECK(metrics->covariance_positive_semidefinite,
	      "60 s covariance remains positive semidefinite");
	CHECK(metrics->covariance_symmetry_error < 1.0e-11,
	      "60 s covariance remains symmetric");
	CHECK(isfinite(metrics->moving_detection_time_s) &&
	      (metrics->moving_detection_time_s <= 5.010),
	      "sudden acceleration is detected within two 200 Hz samples");
	CHECK(metrics->false_stationary_samples == 0u,
	      "constant-velocity legs never enable stationary updates");
	CHECK(isfinite(metrics->stationary_reentry_time_s) &&
	      (metrics->stationary_reentry_time_s >= 25.0) &&
	      (metrics->stationary_reentry_time_s <= 28.0),
	      "final stop enters stationary after hysteresis dwell");
	CHECK(metrics->injected_outliers == 2u,
	      "two synthetic multipath-like GPS outliers were injected");
	CHECK(metrics->rejected_injected_outliers == metrics->injected_outliers,
	      "NIS rejects every injected GPS outlier");
	CHECK(metrics->max_rejected_state_jump_m < 1.0e-8,
	      "rejected delayed GPS does not perturb the current state");
	CHECK(metrics->accepted_gps >= 45u,
	      "most asynchronous GPS measurements are accepted");
	CHECK(metrics->rejected_gps >= metrics->injected_outliers,
	      "GPS reject diagnostics include injected outliers");
	CHECK(metrics->replay_count >= 50u && metrics->replayed_imu_count > 1000u,
	      "delayed GPS exercises fixed-lag replay extensively");
	CHECK(metrics->zero_updates > 150u,
	      "stationary periods apply PX4-rate 5 Hz ZUPT updates");
	CHECK(metrics->horizontal_rmse_m < 2.0,
	      "bias/noise/delay trajectory horizontal RMSE is bounded");
	CHECK(metrics->vertical_rmse_m < 2.5,
	      "bias/noise/delay trajectory vertical RMSE is bounded");
	CHECK(metrics->final_horizontal_error_m < 1.5,
	      "final stationary horizontal error is bounded");
	CHECK(metrics->final_vertical_error_m < 2.0,
	      "final stationary vertical error is bounded");
	CHECK(metrics->final_speed_mps < 0.05,
	      "final stationary ZUPT speed is near zero");
}

static stationary_metrics_t run_stationary_500hz_scenario(void)
{
	static fusion_engine_t engine;
	fusion_engine_config_t config;
	fusion_engine_default_config(&config, SYNTH_TIMEBASE_HZ);
	config.calibration_duration_s = 2.0;
	config.calibration_min_samples = 500u;
	CHECK(fusion_engine_init(&engine, &config), "500 Hz stationary engine initializes");

	fusion_position_measurement_t origin;
	memset(&origin, 0, sizeof(origin));
	origin.covariance_enu_m2[0] = square(0.8);
	origin.covariance_enu_m2[4] = square(0.8);
	origin.covariance_enu_m2[8] = square(1.5);
	origin.source_quality = 1.0;
	origin.source_valid = true;
	CHECK(fusion_engine_push_position(&engine, &origin, NULL, NULL),
	      "500 Hz stationary origin accepted");

	const uint64_t imu_period_ticks = SYNTH_TIMEBASE_HZ / 500u;
	const uint64_t final_tick = 30000000u;
	uint64_t next_gps_tick = 3000000u;
	uint64_t delivery_tick = 0u;
	bool gps_pending = false;
	fusion_position_measurement_t pending_gps;
	synth_rng_t imu_rng = {0x10203040u};
	synth_rng_t gps_rng = {0x55667788u};
	const double accel_bias[3] = {0.020, -0.015, 0.050};
	const double gyro_bias[3] = {0.0008, -0.0005, 0.0006};
	stationary_metrics_t metrics;
	memset(&metrics, 0, sizeof(metrics));

	for(uint64_t tick = 0u ; tick <= final_tick ; tick += imu_period_ticks)
	{
		fusion_imu_measurement_t imu;
		memset(&imu, 0, sizeof(imu));
		imu.timestamp_ticks = tick;
		imu.accel_mps2[2] = config.eskf.gravity_mps2;
		for(unsigned int axis = 0u ; axis < 3u ; axis++)
		{
			imu.accel_mps2[axis] += accel_bias[axis] + 0.020 * rng_gaussian(&imu_rng);
			imu.gyro_rps[axis] = gyro_bias[axis] + 0.0006 * rng_gaussian(&imu_rng);
		}
		imu.temperature_c = 24.0;
		imu.sequence = (uint32_t)(tick / imu_period_ticks);
		imu.vehicle_at_rest = true;
		CHECK(fusion_engine_push_imu(&engine, &imu), "500 Hz stationary IMU accepted");

		if((tick >= next_gps_tick) && !gps_pending)
		{
			synth_truth_t stationary_truth = {{0.0, 0.0, 0.0},
			                                    {0.0, 0.0, 0.0}, 0.0};
			pending_gps = make_position(tick, &stationary_truth, &gps_rng, false);
			pending_gps.position_enu_m[0] *= 0.8 / 1.2;
			pending_gps.position_enu_m[1] *= 0.8 / 1.2;
			pending_gps.position_enu_m[2] *= 1.5 / 2.0;
			pending_gps.covariance_enu_m2[0] = square(0.8);
			pending_gps.covariance_enu_m2[4] = square(0.8);
			pending_gps.covariance_enu_m2[8] = square(1.5);
			delivery_tick = tick + 250000u;
			gps_pending = true;
			next_gps_tick += 1000000u;
		}
		if(gps_pending && (tick >= delivery_tick))
		{
			eskf_update_result_t result = ESKF_UPDATE_INVALID;
			CHECK(fusion_engine_push_position(&engine, &pending_gps, &result, NULL),
			      "500 Hz delayed stationary GPS processed");
			CHECK(result == ESKF_UPDATE_ACCEPTED,
			      "500 Hz stationary GPS passes NIS");
			gps_pending = false;
		}
		if((tick >= 3000000u) && (engine.state == FUSION_ENGINE_RUNNING) &&
		   (engine.current.motion.state != FUSION_MOTION_STATIONARY))
		{
			metrics.nonstationary_samples++;
		}
	}

	metrics.accepted_gps = engine.diagnostics.position_accepted;
	metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
	metrics.state_finite = eskf_state_is_finite(&engine.current.filter);
	metrics.covariance_positive_semidefinite = covariance_is_psd(
		&engine.current.filter, NULL);
	metrics.final_horizontal_error_m = norm2(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 = norm3(engine.current.filter.velocity_mps);
	return metrics;
}

static void check_stationary_metrics(const stationary_metrics_t* metrics)
{
	CHECK(metrics->state_finite, "30 s / 500 Hz stationary state remains finite");
	CHECK(metrics->covariance_positive_semidefinite,
	      "30 s / 500 Hz stationary covariance remains PSD");
	CHECK(metrics->nonstationary_samples == 0u,
	      "30 s / 500 Hz noise never creates a false moving state");
	CHECK(metrics->accepted_gps >= 26u,
	      "30 s / 500 Hz delayed stationary GPS remains accepted");
	CHECK(metrics->zero_updates >= 120u,
	      "30 s / 500 Hz stationary run keeps PX4-rate ZUPT active");
	CHECK(metrics->final_horizontal_error_m < 1.0,
	      "30 s / 500 Hz stationary horizontal error is bounded");
	CHECK(metrics->final_vertical_error_m < 1.0,
	      "30 s / 500 Hz stationary vertical error is bounded");
	CHECK(metrics->final_speed_mps < 0.02,
	      "30 s / 500 Hz stationary speed remains near zero");
}

int main(void)
{
	synth_metrics_t metrics = run_dynamic_scenario(0u);
	check_dynamic_metrics(&metrics);
	double worst_horizontal_rmse_m = metrics.horizontal_rmse_m;
	double worst_final_horizontal_error_m = metrics.final_horizontal_error_m;
	double latest_stationary_reentry_s = metrics.stationary_reentry_time_s;
	for(uint32_t scenario = 1u ; scenario < 8u ; scenario++)
	{
		synth_metrics_t trial = run_dynamic_scenario(scenario);
		check_dynamic_metrics(&trial);
		worst_horizontal_rmse_m = fmax(worst_horizontal_rmse_m,
		                                trial.horizontal_rmse_m);
		worst_final_horizontal_error_m = fmax(worst_final_horizontal_error_m,
		                                       trial.final_horizontal_error_m);
		latest_stationary_reentry_s = fmax(latest_stationary_reentry_s,
		                                      trial.stationary_reentry_time_s);
	}
	stationary_metrics_t stationary = run_stationary_500hz_scenario();
	check_stationary_metrics(&stationary);
	printf("Synthetic 60 s asynchronous ESKF scenario\n");
	printf("  position RMSE     horizontal=%6.3f m vertical=%6.3f m\n",
	       metrics.horizontal_rmse_m, metrics.vertical_rmse_m);
	printf("  final error       horizontal=%6.3f m vertical=%6.3f m speed=%7.4f m/s\n",
	       metrics.final_horizontal_error_m, metrics.final_vertical_error_m,
	       metrics.final_speed_mps);
	printf("  motion timing     moving=%6.3f s stationary-again=%6.3f s false-still=%u\n",
	       metrics.moving_detection_time_s, metrics.stationary_reentry_time_s,
	       metrics.false_stationary_samples);
	printf("  GPS/replay        accepted=%llu rejected=%llu replay=%llu replayed-imu=%llu\n",
	       (unsigned long long)metrics.accepted_gps,
	       (unsigned long long)metrics.rejected_gps,
	       (unsigned long long)metrics.replay_count,
	       (unsigned long long)metrics.replayed_imu_count);
	printf("  outlier isolation injected=%u rejected=%u max-state-jump=%.3e m\n",
	       metrics.injected_outliers, metrics.rejected_injected_outliers,
	       metrics.max_rejected_state_jump_m);
	printf("  covariance        PSD=%d symmetry-error=%.3e zero-updates=%llu\n",
	       metrics.covariance_positive_semidefinite ? 1 : 0,
	       metrics.covariance_symmetry_error,
	       (unsigned long long)metrics.zero_updates);
	printf("  8-seed robustness worst-h-rmse=%6.3f m worst-final-h=%6.3f m latest-stop=%6.3f s\n",
	       worst_horizontal_rmse_m, worst_final_horizontal_error_m,
	       latest_stationary_reentry_s);
	printf("Synthetic 30 s stationary / 500 Hz scenario\n");
	printf("  final error       horizontal=%6.3f m vertical=%6.3f m speed=%7.4f m/s\n",
	       stationary.final_horizontal_error_m,
	       stationary.final_vertical_error_m,
	       stationary.final_speed_mps);
	printf("  decisions         false-moving-samples=%u GPS-accepted=%llu zero-updates=%llu\n",
	       stationary.nonstationary_samples,
	       (unsigned long long)stationary.accepted_gps,
	       (unsigned long long)stationary.zero_updates);
	if(failures != 0)
	{
		fprintf(stderr, "%d synthetic fusion check(s) failed\n", failures);
		return 1;
	}
	printf("PASS: production bare-metal ESKF sources passed the Linux/GCC scenario.\n");
	return 0;
}