#ifndef NAVIGATION_FUSION_H
#define NAVIGATION_FUSION_H

#include <stdbool.h>
#include <stdint.h>

#include "fusion/fusion_engine.h"
#include "fusion/fusion_measurement.h"
#include "fusion/fusion_motion.h"
#include "imu/imu_sample.h"

typedef struct {
	uint64_t timestamp_ticks;
	uint64_t last_accepted_gps_ticks;
	double position_enu_m[3];
	/* ESKF velocity state.  An IMU cannot measure speed -- this is the
	 * integrated result, corrected by GNSS, and is the only meaningful
	 * "inertial speed" the system has. */
	double velocity_enu_mps[3];
	/*
	 * Last GNSS Doppler velocity in the same ENU frame, held so telemetry can
	 * difference it against the state above.  Characterising the receiver's
	 * velocity noise needs the raw observation, not the filtered estimate.
	 */
	double gps_velocity_enu_mps[3];
	double gps_speed_mps;
	double gps_velocity_residual_rms_mps;
	uint64_t gps_velocity_sample_ticks;
	bool gps_velocity_valid;
	double latitude_deg;
	double longitude_deg;
	double height_m;
	double position_std_m[3];
	double speed_mps;
	double linear_acceleration_norm_mps2;
	double last_position_nis;
	double last_velocity_nis;
	fusion_motion_state_t motion_state;
	uint64_t gps_accepted;
	uint64_t gps_nis_rejected;
	uint64_t gps_position_recoveries;
	uint64_t gps_velocity_accepted;
	uint64_t gps_velocity_nis_rejected;
	uint64_t zero_update_count;
	uint64_t replay_count;
	uint64_t stale_gps_count;
	uint64_t imu_processed_count;
	unsigned int calibration_sample_count;
	fusion_engine_state_t engine_state;
	bool local_valid;
	bool global_valid;
	bool vehicle_at_rest;
	/* Compatibility alias for UART publishers: valid means global LLH valid. */
	bool valid;
} navigation_fusion_output_t;

/* Initialize the sensor-independent 15-state ESKF and GPS-to-ENU adapter. */
bool navigation_fusion_init(
	uint64_t local_timebase_hz,
	const fusion_imu_quality_profile_t* imu_quality,
	bool platform_landed);

/* PX4-style external platform fact. For this bench prototype `true` means
 * unarmed/on-ground; a mobile integration must drive it from its own
 * arm/contact/land supervisor. */
void navigation_fusion_set_platform_landed(
	bool platform_landed,
	uint64_t timestamp_ticks);

/* Prediction input: every timestamped IMU sample is integrated; it is not an
 * ESKF observation and is never rate-selected in this layer. */
bool navigation_fusion_push_imu(const imu_sample_t* measurement);

/* Observation input: convert a valid LLH fix and optional Doppler velocity to
 * local ENU, apply both at the captured GPS epoch, then replay newer IMU
 * history to current time. */
bool navigation_fusion_push_gps(const fusion_gps_measurement_t* measurement);

/* local_valid can exist without GPS. global_valid/valid require a GPS origin. */
bool navigation_fusion_get_output(navigation_fusion_output_t* output);
const char* navigation_fusion_state_name(fusion_engine_state_t state);

#endif