#include "fusion/fusion_rest_detector.h"

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

static int failures;

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

static imu_sample_t sample_at(uint64_t ticks, uint64_t timebase_hz,
	                          double gyro_x_rps, unsigned int sequence)
{
	imu_sample_t sample;
	memset(&sample, 0, sizeof(sample));
	sample.timestamp_ticks = ticks;
	double time_s = (double)ticks / (double)timebase_hz;
	sample.accel_mps2[0] = 0.004 * sin(11.0 * time_s);
	sample.accel_mps2[1] = 0.004 * cos(13.0 * time_s);
	sample.accel_mps2[2] = 9.80665 + 0.004 * sin(17.0 * time_s);
	sample.gyro_rps[0] = gyro_x_rps;
	sample.gyro_rps[1] = 0.0002 * sin(7.0 * time_s);
	sample.gyro_rps[2] = 0.0002 * cos(9.0 * time_s);
	sample.temperature_c = 25.0;
	sample.sequence = sequence;
	return sample;
}

static void test_rate(unsigned int rate_hz)
{
	const uint64_t timebase_hz = 1000000u;
	fusion_rest_detector_config_t config;
	fusion_rest_detector_t detector;
	fusion_rest_detector_default_config(&config);
	CHECK(fusion_rest_detector_init(
		&detector, &config, timebase_hz, true),
	      "rest detector initializes in unarmed/landed mode");

	bool initial_rest_seen = false;
	bool impulse_cleared_rest = false;
	double reentry_time_s = NAN;
	uint64_t sample_count = 6u * rate_hz;
	for(uint64_t index = 0u ; index <= sample_count ; index++)
	{
		uint64_t ticks = (index * timebase_hz + rate_hz / 2u) / rate_hz;
		double time_s = (double)ticks / (double)timebase_hz;
		double gyro_x = ((time_s >= 2.0) &&
		                 (time_s < 2.0 + 2.0 / (double)rate_hz)) ? 4.0 : 0.0;
		imu_sample_t sample = sample_at(
			ticks, timebase_hz, gyro_x, (unsigned int)index);
		CHECK(fusion_rest_detector_update(&detector, &sample),
		      "monotonic IMU sample updates rest detector");
		if((time_s >= 1.0) && (time_s < 2.0) && detector.at_rest)
		{
			initial_rest_seen = true;
		}
		if((time_s >= 2.0) && (time_s < 2.2) && !detector.at_rest)
		{
			impulse_cleared_rest = true;
		}
		if((time_s > 2.0) && !isfinite(reentry_time_s) && detector.at_rest)
		{
			reentry_time_s = time_s;
		}
	}
	CHECK(initial_rest_seen, "quiet landed platform reaches at-rest after dwell");
	CHECK(impulse_cleared_rest, "high-frequency gyro impulse clears at-rest");
	CHECK(isfinite(reentry_time_s) && (reentry_time_s >= 3.0) &&
	      (reentry_time_s <= 5.5),
	      "rest returns only after vibration decay and one-second dwell");
	printf("rest-detector %3u Hz: reentry=%.3f s accel-vibe=%.6f gyro-vibe=%.6f\n",
	       rate_hz, reentry_time_s,
	       detector.accel_vibration_metric_mps2,
	       detector.gyro_vibration_metric_rps);
}

static void test_platform_gate_and_timestamps(void)
{
	const uint64_t timebase_hz = 1000000u;
	fusion_rest_detector_config_t config;
	fusion_rest_detector_t detector;
	fusion_rest_detector_default_config(&config);
	CHECK(fusion_rest_detector_init(
		&detector, &config, timebase_hz, false),
	      "rest detector initializes in mobile/not-landed mode");
	for(unsigned int index = 0u ; index <= 150u ; index++)
	{
		imu_sample_t sample = sample_at(
			(uint64_t)index * 10000u, timebase_hz, 0.0, index);
		CHECK(fusion_rest_detector_update(&detector, &sample),
		      "mobile quiet samples remain valid");
	}
	CHECK(!detector.at_rest,
	      "quiet IMU cannot authorize ZUPT while platform is not landed");

	imu_sample_t duplicate = sample_at(1500000u, timebase_hz, 0.0, 151u);
	CHECK(!fusion_rest_detector_update(&detector, &duplicate),
	      "duplicate timestamp is rejected");
	fusion_rest_detector_set_landed(&detector, true, 1500000u);
	for(unsigned int index = 151u ; index <= 260u ; index++)
	{
		imu_sample_t sample = sample_at(
			(uint64_t)index * 10000u, timebase_hz, 0.0, index);
		CHECK(fusion_rest_detector_update(&detector, &sample),
		      "landed quiet samples update rest detector");
	}
	CHECK(detector.at_rest,
	      "external landed fact plus quiet dwell authorizes at-rest");
}

int main(void)
{
	test_rate(52u);
	test_rate(104u);
	test_rate(208u);
	test_platform_gate_and_timestamps();
	if(failures != 0)
	{
		fprintf(stderr, "%d rest-detector test(s) failed\n", failures);
		return 1;
	}
	printf("PX4-style platform rest detector tests passed.\n");
	return 0;
}