#include "fusion/fusion_engine.h"

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

#define TIMEBASE_HZ 1000000u
#define DURATION_S 50.0
#define PI 3.14159265358979323846264338327950288

typedef enum {
	SCENARIO_STATIONARY = 0,
	SCENARIO_PEDESTRIAN,
	SCENARIO_CAR,
	SCENARIO_AIRCRAFT,
	SCENARIO_COUNT
} scenario_kind_t;

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

typedef struct {
	double forward_accel_mps2;
	double vertical_accel_mps2;
	double yaw_rate_rps;
} command_t;

typedef struct {
	uint32_t state;
} rng_t;

typedef struct {
	double horizontal_rmse_m;
	double final_horizontal_error_m;
	double maximum_horizontal_error_m;
	double maximum_burst_state_step_m;
	uint64_t gps_accepted;
	uint64_t gps_rejected;
	uint64_t position_recoveries;
	uint64_t zero_updates;
	uint64_t zero_updates_after_startup;
	bool finite;
	bool recovered_after_burst;
} metrics_t;

static const char* scenario_name(scenario_kind_t scenario)
{
	static const char* names[] = {"stationary", "pedestrian", "car", "aircraft"};
	return names[(unsigned int)scenario];
}

static bool parse_scenario(const char* text, scenario_kind_t* scenario_out)
{
	for(unsigned int index = 0u; index < SCENARIO_COUNT; index++)
	{
		if(strcmp(text, scenario_name((scenario_kind_t)index)) == 0)
		{
			*scenario_out = (scenario_kind_t)index;
			return true;
		}
	}
	return false;
}

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

static double rng_uniform(rng_t* rng)
{
	rng->state = 1664525u * rng->state + 1013904223u;
	return ((double)(rng->state >> 8u) + 0.5) / 16777216.0;
}

static double rng_normal(rng_t* rng)
{
	double u1 = fmax(rng_uniform(rng), 1.0e-12);
	double u2 = rng_uniform(rng);
	return sqrt(-2.0 * log(u1)) * cos(2.0 * PI * u2);
}

static command_t command_at(scenario_kind_t scenario, double time_s)
{
	command_t command = {0.0, 0.0, 0.0};
	if(scenario == SCENARIO_PEDESTRIAN)
	{
		if((time_s >= 4.0) && (time_s < 6.0)) command.forward_accel_mps2 = 0.75;
		else if((time_s >= 14.0) && (time_s < 16.0)) command.forward_accel_mps2 = -0.75;
		else if((time_s >= 21.0) && (time_s < 23.0)) command.forward_accel_mps2 = 0.60;
		else if((time_s >= 31.0) && (time_s < 33.0)) command.forward_accel_mps2 = -0.60;
		if((time_s >= 24.0) && (time_s < 29.0)) command.yaw_rate_rps = 0.10;
	}
	else if(scenario == SCENARIO_CAR)
	{
		if((time_s >= 4.0) && (time_s < 9.0)) command.forward_accel_mps2 = 4.0;
		else if((time_s >= 15.0) && (time_s < 18.0)) command.forward_accel_mps2 = -20.0 / 3.0;
		else if((time_s >= 25.0) && (time_s < 29.0)) command.forward_accel_mps2 = 5.0;
		else if((time_s >= 39.0) && (time_s < 42.0)) command.forward_accel_mps2 = -20.0 / 3.0;
		if((time_s >= 31.0) && (time_s < 36.0)) command.yaw_rate_rps = 0.12;
	}
	else if(scenario == SCENARIO_AIRCRAFT)
	{
		if((time_s >= 4.0) && (time_s < 14.0)) command.forward_accel_mps2 = 8.0;
		else if((time_s >= 26.0) && (time_s < 29.0)) command.forward_accel_mps2 = 4.0;
		else if((time_s >= 42.0) && (time_s < 47.0)) command.forward_accel_mps2 = -6.0;
		if((time_s >= 6.0) && (time_s < 10.0)) command.vertical_accel_mps2 = 2.0;
		else if((time_s >= 18.0) && (time_s < 22.0)) command.vertical_accel_mps2 = -2.0;
		if((time_s >= 20.0) && (time_s < 25.0)) command.yaw_rate_rps = 0.20;
		else if((time_s >= 34.0) && (time_s < 36.0)) command.yaw_rate_rps = -0.35;
	}
	return command;
}

static void attitude_at(scenario_kind_t scenario, double time_s,
	                    const truth_t* truth, double euler_rad[3],
	                    double body_rate_rps[3])
{
	memset(euler_rad, 0, 3u * sizeof(double));
	memset(body_rate_rps, 0, 3u * sizeof(double));
	euler_rad[2] = truth->yaw_rad;
	command_t command = command_at(scenario, time_s);
	body_rate_rps[2] = command.yaw_rate_rps;
	if(scenario != SCENARIO_STATIONARY)
	{
		return;
	}
	const double roll_max = 20.0 * PI / 180.0;
	const double pitch_max = 12.0 * PI / 180.0;
	if((time_s >= 10.0) && (time_s < 11.0))
	{
		euler_rad[0] = roll_max * (time_s - 10.0);
		body_rate_rps[0] = roll_max;
	}
	else if((time_s >= 11.0) && (time_s < 13.0)) euler_rad[0] = roll_max;
	else if((time_s >= 13.0) && (time_s < 14.0))
	{
		euler_rad[0] = roll_max * (14.0 - time_s);
		body_rate_rps[0] = -roll_max;
	}
	if((time_s >= 18.0) && (time_s < 19.0))
	{
		euler_rad[1] = pitch_max * (time_s - 18.0);
		body_rate_rps[1] = pitch_max;
	}
	else if((time_s >= 19.0) && (time_s < 21.0)) euler_rad[1] = pitch_max;
	else if((time_s >= 21.0) && (time_s < 22.0))
	{
		euler_rad[1] = pitch_max * (22.0 - time_s);
		body_rate_rps[1] = -pitch_max;
	}
}

static void rotation_body_to_nav(const double euler[3], double rotation[3][3])
{
	double cr = cos(euler[0]);
	double sr = sin(euler[0]);
	double cp = cos(euler[1]);
	double sp = sin(euler[1]);
	double cy = cos(euler[2]);
	double sy = sin(euler[2]);
	rotation[0][0] = cy * cp;
	rotation[0][1] = cy * sp * sr - sy * cr;
	rotation[0][2] = cy * sp * cr + sy * sr;
	rotation[1][0] = sy * cp;
	rotation[1][1] = sy * sp * sr + cy * cr;
	rotation[1][2] = sy * sp * cr - cy * sr;
	rotation[2][0] = -sp;
	rotation[2][1] = cp * sr;
	rotation[2][2] = cp * cr;
}

static void propagate_truth(truth_t* truth, scenario_kind_t scenario,
	                        double start_s, double dt_s)
{
	command_t command = command_at(scenario, start_s + 0.5 * dt_s);
	double speed = hypot(truth->velocity_mps[0], truth->velocity_mps[1]);
	double yaw_mid = truth->yaw_rad + 0.5 * command.yaw_rate_rps * dt_s;
	double acceleration[3] = {
		command.forward_accel_mps2 * cos(yaw_mid) - speed * command.yaw_rate_rps * sin(yaw_mid),
		command.forward_accel_mps2 * sin(yaw_mid) + speed * command.yaw_rate_rps * cos(yaw_mid),
		command.vertical_accel_mps2
	};
	for(unsigned int axis = 0u; axis < 3u; axis++)
	{
		truth->position_m[axis] += truth->velocity_mps[axis] * dt_s +
		                            0.5 * acceleration[axis] * dt_s * dt_s;
		truth->velocity_mps[axis] += acceleration[axis] * dt_s;
	}
	truth->yaw_rad += command.yaw_rate_rps * dt_s;
}

static bool externally_at_rest(scenario_kind_t scenario, double time_s)
{
	if(time_s < 3.5) return true;
	if(scenario != SCENARIO_STATIONARY) return false;
	/* An external landed/rest supervisor owns this decision.  Attitude changes
	 * and the vibration interval revoke the constraint; quiet IMU alone never
	 * invents rest for the moving scenarios. */
	return !(((time_s >= 9.8) && (time_s < 15.0)) ||
	         ((time_s >= 17.8) && (time_s < 23.0)) ||
	         ((time_s >= 27.0) && (time_s < 30.0)));
}

static fusion_imu_measurement_t make_imu(scenario_kind_t scenario,
	                                    const truth_t* truth, double time_s,
	                                    uint64_t ticks, double gravity,
	                                    unsigned int sequence, rng_t* rng)
{
	command_t command = command_at(scenario, time_s);
	double speed = hypot(truth->velocity_mps[0], truth->velocity_mps[1]);
	double acceleration_nav[3] = {
		command.forward_accel_mps2 * cos(truth->yaw_rad) -
			speed * command.yaw_rate_rps * sin(truth->yaw_rad),
		command.forward_accel_mps2 * sin(truth->yaw_rad) +
			speed * command.yaw_rate_rps * cos(truth->yaw_rad),
		command.vertical_accel_mps2 + gravity
	};
	double euler[3];
	double body_rate[3];
	double rotation[3][3];
	attitude_at(scenario, time_s, truth, euler, body_rate);
	rotation_body_to_nav(euler, rotation);
	fusion_imu_measurement_t imu;
	memset(&imu, 0, sizeof(imu));
	imu.timestamp_ticks = ticks;
	for(unsigned int body = 0u; body < 3u; body++)
	{
		for(unsigned int nav = 0u; nav < 3u; nav++)
		{
			imu.accel_mps2[body] += rotation[nav][body] * acceleration_nav[nav];
		}
		static const double accel_bias[3] = {0.035, -0.025, 0.055};
		static const double gyro_bias[3] = {0.0008, -0.0006, 0.0007};
		imu.accel_mps2[body] += accel_bias[body] + 0.012 * rng_normal(rng);
		imu.gyro_rps[body] = body_rate[body] + gyro_bias[body] +
		                     0.00035 * rng_normal(rng);
	}
	if((scenario == SCENARIO_STATIONARY) &&
	   (time_s >= 27.0) && (time_s < 29.0))
	{
		imu.accel_mps2[0] += 0.80 * sin(2.0 * PI * 7.0 * time_s);
		imu.accel_mps2[1] += 0.55 * sin(2.0 * PI * 11.0 * time_s);
		imu.gyro_rps[2] += 0.035 * sin(2.0 * PI * 5.0 * time_s);
	}
	imu.temperature_c = 25.0 + 0.5 * sin(0.08 * time_s);
	imu.sequence = sequence;
	imu.vehicle_at_rest = externally_at_rest(scenario, time_s);
	return imu;
}

static fusion_position_measurement_t make_gps(const truth_t* truth,
	                                          double time_s, uint64_t ticks,
	                                          rng_t* rng, double gps_position[3])
{
	double horizontal_std = time_s < 8.0 ? 8.0 : 4.0;
	double vertical_std = time_s < 8.0 ? 12.0 : 7.0;
	for(unsigned int axis = 0u; axis < 3u; axis++)
	{
		double std = axis < 2u ? horizontal_std : vertical_std;
		gps_position[axis] = truth->position_m[axis] + std * rng_normal(rng);
	}
	if((time_s >= 32.0) && (time_s < 34.0))
	{
		gps_position[0] += 130.0;
		gps_position[1] -= 90.0;
		gps_position[2] += 55.0;
	}
	fusion_position_measurement_t gps;
	memset(&gps, 0, sizeof(gps));
	gps.timestamp_ticks = ticks;
	memcpy(gps.position_enu_m, gps_position, sizeof(gps.position_enu_m));
	gps.covariance_enu_m2[0] = square(horizontal_std);
	gps.covariance_enu_m2[4] = square(horizontal_std);
	gps.covariance_enu_m2[8] = square(vertical_std);
	/* Exactly four satellites have zero residual degrees of freedom.  This is
	 * therefore a valid WLS output with covariance but no redundant FDE test. */
	gps.source_quality = 100.0;
	gps.source_valid = true;
	return gps;
}

static void local_to_geodetic(const double position[3], double* latitude_deg,
	                          double* longitude_deg, double* height_m)
{
	const double origin_latitude_deg = 40.78925034214207;
	const double origin_longitude_deg = 29.46277530841728;
	const double origin_height_m = 100.0;
	const double latitude_rad = origin_latitude_deg * PI / 180.0;
	*latitude_deg = origin_latitude_deg + position[1] / 6378137.0 * 180.0 / PI;
	*longitude_deg = origin_longitude_deg +
		position[0] / (6378137.0 * cos(latitude_rad)) * 180.0 / PI;
	*height_m = origin_height_m + position[2];
}

static void write_location(FILE* log_file, const char* source,
	                       scenario_kind_t scenario, double time_s,
	                       const double position[3], unsigned int gps_ms,
	                       unsigned int imu_ms)
{
	if(log_file == NULL) return;
	double latitude;
	double longitude;
	double height;
	local_to_geodetic(position, &latitude, &longitude, &height);
	fprintf(log_file,
	        "[LOCATION] source=%s scenario=%s t=%.3f lat=%.12f lon=%.12f "
	        "height=%.3f gdop=3.2 pdop=2.6 hdop=2.0 vdop=1.7 tdop=1.8 "
	        "tracked=4 used=4 gps_ms=%u imu_ms=%u\n",
	        source, scenario_name(scenario), time_s, latitude, longitude,
	        height, gps_ms, imu_ms);
}

static metrics_t run_case(scenario_kind_t scenario, unsigned int gps_ms,
	                      unsigned int imu_ms, FILE* log_file)
{
	static fusion_engine_t engine;
	memset(&engine, 0, sizeof(engine));
	fusion_engine_config_t config;
	fusion_engine_default_config(&config, TIMEBASE_HZ);
	config.calibration_duration_s = 2.0;
	/* Calibration is time-based for asynchronous sensors; retain enough samples
	 * to estimate variance without silently assuming a particular ODR. */
	config.calibration_min_samples = 2u * 1000u / imu_ms + 1u;
	metrics_t metrics;
	memset(&metrics, 0, sizeof(metrics));
	if(!fusion_engine_init(&engine, &config)) return metrics;

	truth_t truth;
	memset(&truth, 0, sizeof(truth));
	rng_t imu_rng = {0x7a31f20du ^ ((uint32_t)scenario << 24u) ^ imu_ms};
	rng_t gps_rng = {0xb41d93c7u ^ ((uint32_t)scenario << 20u) ^ gps_ms};
	uint64_t imu_period_ticks = (uint64_t)imu_ms * 1000u;
	uint64_t gps_period_ticks = (uint64_t)gps_ms * 1000u;
	uint64_t next_imu_ticks = 0u;
	uint64_t next_gps_ticks = 0u;
	uint64_t next_plot_ticks = 0u;
	uint64_t end_ticks = (uint64_t)(DURATION_S * TIMEBASE_HZ);
	uint64_t last_ticks = 0u;
	unsigned int imu_sequence = 0u;
	double error_squared_sum = 0.0;
	uint64_t error_count = 0u;
	uint64_t accepted_at_burst_end = 0u;
	uint64_t zero_updates_at_startup_end = 0u;
	double previous_horizontal[2] = {0.0, 0.0};
	bool have_previous = false;

	if(log_file != NULL)
	{
		fprintf(log_file, "# ESKF synthetic replay; blue=GPS red=IMU-corrected green=truth\n");
		fprintf(log_file, "# scenario=%s gps_ms=%u imu_ms=%u satellites=4\n",
		        scenario_name(scenario), gps_ms, imu_ms);
	}
	while(true)
	{
		uint64_t ticks = next_imu_ticks;
		if(next_gps_ticks < ticks) ticks = next_gps_ticks;
		if(next_plot_ticks < ticks) ticks = next_plot_ticks;
		if(ticks > end_ticks) break;
		double start_s = (double)last_ticks / TIMEBASE_HZ;
		double dt_s = (double)(ticks - last_ticks) / TIMEBASE_HZ;
		propagate_truth(&truth, scenario, start_s, dt_s);
		last_ticks = ticks;
		double time_s = (double)ticks / TIMEBASE_HZ;

		if(ticks == next_gps_ticks)
		{
			/* A 3 s dropout exercises asynchronous dead reckoning. */
			if(!((time_s >= 25.0) && (time_s < 28.0)))
			{
				double gps_position[3];
				fusion_position_measurement_t gps = make_gps(
					&truth, time_s, ticks, &gps_rng, gps_position);
				eskf_update_result_t result = ESKF_UPDATE_INVALID;
				(void)fusion_engine_push_position(&engine, &gps, &result, NULL);
				write_location(log_file, "gps", scenario, time_s, gps_position,
				               gps_ms, imu_ms);
			}
			next_gps_ticks += gps_period_ticks;
		}

		if(ticks == next_imu_ticks)
		{
			fusion_imu_measurement_t imu = make_imu(
				scenario, &truth, time_s, ticks, config.eskf.gravity_mps2,
				imu_sequence++, &imu_rng);
			if(!fusion_engine_push_imu(&engine, &imu)) break;
			next_imu_ticks += imu_period_ticks;
		}

		if(engine.state == FUSION_ENGINE_RUNNING)
		{
			if((time_s >= 4.0) && (zero_updates_at_startup_end == 0u))
				zero_updates_at_startup_end =
					engine.diagnostics.zero_velocity_update_count;
			double horizontal_error = hypot(
				engine.current.filter.position_m[0] - truth.position_m[0],
				engine.current.filter.position_m[1] - truth.position_m[1]);
			metrics.maximum_horizontal_error_m = fmax(
				metrics.maximum_horizontal_error_m, horizontal_error);
			if(time_s >= 8.0)
			{
				error_squared_sum += square(horizontal_error);
				error_count++;
			}
			if(have_previous && (time_s >= 32.0) && (time_s < 34.2))
			{
				double step = hypot(
					engine.current.filter.position_m[0] - previous_horizontal[0],
					engine.current.filter.position_m[1] - previous_horizontal[1]);
				metrics.maximum_burst_state_step_m = fmax(
					metrics.maximum_burst_state_step_m, step);
			}
			previous_horizontal[0] = engine.current.filter.position_m[0];
			previous_horizontal[1] = engine.current.filter.position_m[1];
			have_previous = true;
			if((time_s >= 34.0) && (accepted_at_burst_end == 0u))
				accepted_at_burst_end = engine.diagnostics.position_accepted;
		}

		if(ticks == next_plot_ticks)
		{
			write_location(log_file, "trajectory", scenario, time_s,
			               truth.position_m, gps_ms, imu_ms);
			if(engine.state == FUSION_ENGINE_RUNNING)
				write_location(log_file, "imu_corrected", scenario, time_s,
				               engine.current.filter.position_m, gps_ms, imu_ms);
			next_plot_ticks += 200000u;
		}
	}

	double final_error = hypot(
		engine.current.filter.position_m[0] - truth.position_m[0],
		engine.current.filter.position_m[1] - truth.position_m[1]);
	metrics.horizontal_rmse_m = error_count > 0u
		? sqrt(error_squared_sum / (double)error_count) : INFINITY;
	metrics.final_horizontal_error_m = final_error;
	metrics.gps_accepted = engine.diagnostics.position_accepted;
	metrics.gps_rejected = engine.diagnostics.position_nis_rejected;
	metrics.position_recoveries = engine.diagnostics.position_recovery_count;
	metrics.zero_updates = engine.diagnostics.zero_velocity_update_count;
	metrics.zero_updates_after_startup = metrics.zero_updates >= zero_updates_at_startup_end
		? metrics.zero_updates - zero_updates_at_startup_end : metrics.zero_updates;
	metrics.recovered_after_burst = engine.diagnostics.position_accepted >
		accepted_at_burst_end;
	metrics.finite = (engine.state == FUSION_ENGINE_RUNNING) &&
		eskf_state_is_finite(&engine.current.filter) && isfinite(final_error);
	return metrics;
}

static void print_metrics(scenario_kind_t scenario, unsigned int gps_ms,
	                      unsigned int imu_ms, const metrics_t* metrics)
{
	printf("%-10s GPS=%4ums IMU=%3ums rmse=%7.2fm final=%7.2fm max=%7.2fm "
	       "burst-step=%6.2fm gps=%3llu/%2llu recovery=%llu zupt=%llu(+%llu) finite=%u\n",
	       scenario_name(scenario), gps_ms, imu_ms, metrics->horizontal_rmse_m,
	       metrics->final_horizontal_error_m, metrics->maximum_horizontal_error_m,
	       metrics->maximum_burst_state_step_m,
	       (unsigned long long)metrics->gps_accepted,
	       (unsigned long long)metrics->gps_rejected,
	       (unsigned long long)metrics->position_recoveries,
	       (unsigned long long)metrics->zero_updates,
	       (unsigned long long)metrics->zero_updates_after_startup,
	       metrics->finite ? 1u : 0u);
}

static int run_matrix(void)
{
	static const unsigned int gps_periods_ms[] = {200u, 500u, 1000u};
	static const unsigned int imu_periods_ms[] = {19u, 20u, 100u, 200u, 300u};
	bool passed = true;
	printf("Four-satellite asynchronous platform matrix; 3 s outage and 2 s coherent GNSS fault\n");
	for(unsigned int scenario = 0u; scenario < SCENARIO_COUNT; scenario++)
	{
		for(unsigned int gps_index = 0u; gps_index < 3u; gps_index++)
		{
			for(unsigned int imu_index = 0u; imu_index < 5u; imu_index++)
			{
				metrics_t metrics = run_case((scenario_kind_t)scenario,
					gps_periods_ms[gps_index], imu_periods_ms[imu_index], NULL);
				print_metrics((scenario_kind_t)scenario, gps_periods_ms[gps_index],
				              imu_periods_ms[imu_index], &metrics);
				bool common_ok = metrics.finite && metrics.recovered_after_burst &&
					(metrics.position_recoveries == 0u) &&
					(metrics.maximum_burst_state_step_m < 40.0);
				bool rest_ok = (scenario == SCENARIO_STATIONARY)
					? (metrics.zero_updates_after_startup > 0u)
					: (metrics.zero_updates_after_startup == 0u);
				bool production_rate_ok = (imu_periods_ms[imu_index] > 20u) ||
					(metrics.final_horizontal_error_m < 25.0);
				passed = passed && common_ok && rest_ok && production_rate_ok;
			}
		}
	}
	if(!passed)
	{
		fprintf(stderr, "FAIL: platform scenario matrix contract.\n");
		return 1;
	}
	printf("PASS: 19/20 ms production rates stayed bounded; 100/200/300 ms coarse IMU "
	       "rates remained finite and their dynamic accuracy loss is reported, not hidden.\n");
	return 0;
}

int main(int argc, char** argv)
{
	if(argc == 1) return run_matrix();
	if((argc == 6) && (strcmp(argv[1], "--benchmark") == 0))
	{
		scenario_kind_t scenario;
		if(!parse_scenario(argv[2], &scenario))
		{
			fprintf(stderr, "unknown scenario: %s\n", argv[2]);
			return 2;
		}
		unsigned int gps_ms = (unsigned int)strtoul(argv[3], NULL, 10);
		unsigned int imu_ms = (unsigned int)strtoul(argv[4], NULL, 10);
		unsigned int iterations = (unsigned int)strtoul(argv[5], NULL, 10);
		if((iterations == 0u) ||
		   !((gps_ms == 200u) || (gps_ms == 500u) || (gps_ms == 1000u)) ||
		   !((imu_ms == 19u) || (imu_ms == 20u) || (imu_ms == 100u) ||
		      (imu_ms == 200u) || (imu_ms == 300u)))
		{
			fprintf(stderr, "supported GPS: 200/500/1000 ms; "
			                "IMU: 19/20/100/200/300 ms; iterations > 0\n");
			return 2;
		}
		metrics_t metrics;
		for(unsigned int iteration = 0u; iteration < iterations; iteration++)
			metrics = run_case(scenario, gps_ms, imu_ms, NULL);
		print_metrics(scenario, gps_ms, imu_ms, &metrics);
		printf("completed %u benchmark iterations\n", iterations);
		return metrics.finite ? 0 : 1;
	}
	if((argc == 6) && (strcmp(argv[1], "--log") == 0))
	{
		scenario_kind_t scenario;
		if(!parse_scenario(argv[2], &scenario))
		{
			fprintf(stderr, "unknown scenario: %s\n", argv[2]);
			return 2;
		}
		unsigned int gps_ms = (unsigned int)strtoul(argv[3], NULL, 10);
		unsigned int imu_ms = (unsigned int)strtoul(argv[4], NULL, 10);
		if(!((gps_ms == 200u) || (gps_ms == 500u) || (gps_ms == 1000u)) ||
		   !((imu_ms == 19u) || (imu_ms == 20u) || (imu_ms == 100u) ||
		      (imu_ms == 200u) || (imu_ms == 300u)))
		{
			fprintf(stderr, "supported GPS: 200/500/1000 ms; IMU: 19/20/100/200/300 ms\n");
			return 2;
		}
		FILE* log_file = fopen(argv[5], "w");
		if(log_file == NULL)
		{
			fprintf(stderr, "cannot create %s: %s\n", argv[5], strerror(errno));
			return 2;
		}
		metrics_t metrics = run_case(scenario, gps_ms, imu_ms, log_file);
		if(fclose(log_file) != 0)
		{
			fprintf(stderr, "cannot close %s\n", argv[5]);
			return 2;
		}
		print_metrics(scenario, gps_ms, imu_ms, &metrics);
		printf("wrote replay log: %s\n", argv[5]);
		return metrics.finite ? 0 : 1;
	}
	fprintf(stderr, "usage: %s [--log stationary|pedestrian|car|aircraft "
	                "200|500|1000 19|20|100|200|300 output.log]\n"
	                "       %s [--benchmark stationary|pedestrian|car|aircraft "
	                "200|500|1000 19|20|100|200|300 iterations]\n",
	        argv[0], argv[0]);
	return 2;
}