#ifndef ESKF_H
#define ESKF_H

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

#define ESKF_VECTOR_DIM 3u
#define ESKF_QUATERNION_DIM 4u
#define ESKF_ERROR_STATE_DIM 15u

typedef enum {
	ESKF_UPDATE_INVALID = 0,
	ESKF_UPDATE_ACCEPTED,
	ESKF_UPDATE_REJECTED_NIS
} eskf_update_result_t;

typedef struct {
	double accel_noise_density[ESKF_VECTOR_DIM];
	double gyro_noise_density[ESKF_VECTOR_DIM];
	double accel_bias_random_walk[ESKF_VECTOR_DIM];
	double gyro_bias_random_walk[ESKF_VECTOR_DIM];
	double initial_position_std_m[ESKF_VECTOR_DIM];
	double initial_velocity_std_mps[ESKF_VECTOR_DIM];
	double initial_attitude_std_rad[ESKF_VECTOR_DIM];
	double initial_accel_bias_std_mps2[ESKF_VECTOR_DIM];
	double initial_gyro_bias_std_rps[ESKF_VECTOR_DIM];
	double gravity_mps2;
	double max_propagation_step_s;
	double position_nis_gate;
	double covariance_diagonal_floor;
} eskf_config_t;

typedef struct {
	double position_m[ESKF_VECTOR_DIM];
	double velocity_mps[ESKF_VECTOR_DIM];
	double body_to_nav_quaternion[ESKF_QUATERNION_DIM];
	double accel_bias_mps2[ESKF_VECTOR_DIM];
	double gyro_bias_rps[ESKF_VECTOR_DIM];
	double covariance[ESKF_ERROR_STATE_DIM][ESKF_ERROR_STATE_DIM];
	double last_linear_accel_nav_mps2[ESKF_VECTOR_DIM];
	double last_unbiased_gyro_rps[ESKF_VECTOR_DIM];
	double last_position_nis;
	uint64_t propagation_count;
	uint64_t position_update_accepted_count;
	uint64_t position_update_rejected_count;
	bool initialized;
} eskf_t;

void eskf_default_config(eskf_config_t* config);

bool eskf_quaternion_align_vectors(const double source[ESKF_VECTOR_DIM],
	                               const double target[ESKF_VECTOR_DIM],
	                               double quaternion_out[ESKF_QUATERNION_DIM]);

bool eskf_initialize(eskf_t* filter,
	                 const eskf_config_t* config,
	                 const double initial_position_m[ESKF_VECTOR_DIM],
	                 const double body_to_nav_quaternion[ESKF_QUATERNION_DIM],
	                 const double accel_bias_mps2[ESKF_VECTOR_DIM],
	                 const double gyro_bias_rps[ESKF_VECTOR_DIM]);

bool eskf_propagate(eskf_t* filter,
	                const eskf_config_t* config,
	                const double accel_mps2[ESKF_VECTOR_DIM],
	                const double gyro_rps[ESKF_VECTOR_DIM],
	                double dt_s);

/* Midpoint inertial integration over two timestamped raw samples.  The
 * constant-input eskf_propagate() entry point remains available for callers
 * that already provide an interval-integrated/held measurement. */
bool eskf_propagate_interval(
	eskf_t* filter,
	const eskf_config_t* config,
	const double start_accel_mps2[ESKF_VECTOR_DIM],
	const double start_gyro_rps[ESKF_VECTOR_DIM],
	const double end_accel_mps2[ESKF_VECTOR_DIM],
	const double end_gyro_rps[ESKF_VECTOR_DIM],
	double dt_s);

eskf_update_result_t eskf_position_update(
	eskf_t* filter,
	const eskf_config_t* config,
	const double position_m[ESKF_VECTOR_DIM],
	const double covariance_m2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM],
	double* nis_out);

/* Re-anchor position after a higher-level fusion timeout has established that
 * repeated, valid position observations are mutually coherent.  Velocity,
 * attitude and IMU biases are preserved; position cross-covariances are
 * cleared because they are no longer valid after the state discontinuity. */
bool eskf_reset_position(
	eskf_t* filter,
	const double position_m[ESKF_VECTOR_DIM],
	const double covariance_m2[ESKF_VECTOR_DIM * ESKF_VECTOR_DIM]);

bool eskf_zero_velocity_update(eskf_t* filter, double velocity_std_mps);

bool eskf_zero_angular_rate_update(
	eskf_t* filter,
	const double measured_gyro_rps[ESKF_VECTOR_DIM],
	const double measurement_std_rps[ESKF_VECTOR_DIM]);

double eskf_speed_mps(const eskf_t* filter);
bool eskf_state_is_finite(const eskf_t* filter);

#endif