#ifndef FUSION_MOTION_H
#define FUSION_MOTION_H

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

typedef enum {
	FUSION_MOTION_BOOTSTRAP = 0,
	FUSION_MOTION_STATIONARY,
	FUSION_MOTION_MOVING,
	FUSION_MOTION_STATIONARY_CANDIDATE
} fusion_motion_state_t;

typedef enum {
	FUSION_GNSS_MOTION_UNKNOWN = 0,
	FUSION_GNSS_MOTION_STILL,
	FUSION_GNSS_MOTION_MOVING
} fusion_gnss_motion_evidence_t;

typedef struct {
	double stationary_accel_limit_mps2;
	double stationary_gyro_limit_rps;
	double moving_accel_threshold_mps2;
	double moving_gyro_threshold_rps;
	double moving_jerk_threshold_mps3;
	double jerk_filter_time_constant_s;
	double stationary_speed_limit_mps;
	double moving_speed_threshold_mps;
	double stationary_dwell_s;
	unsigned int weak_motion_samples;
} fusion_motion_config_t;

typedef struct {
	fusion_motion_state_t state;
	double quiet_time_s;
	double filtered_linear_accel_mps2[3];
	double previous_filtered_linear_accel_mps2[3];
	double last_linear_accel_norm_mps2;
	double last_gyro_norm_rps;
	double last_jerk_norm_mps3;
	double last_speed_mps;
	unsigned int weak_motion_count;
	uint64_t transition_count;
	bool have_previous_accel;
} fusion_motion_detector_t;

void fusion_motion_default_config(fusion_motion_config_t* config);
void fusion_motion_init(fusion_motion_detector_t* detector,
	                    bool bootstrap_stationary);

fusion_motion_state_t fusion_motion_update(
	fusion_motion_detector_t* detector,
	const fusion_motion_config_t* config,
	const double linear_accel_nav_mps2[3],
	const double unbiased_gyro_rps[3],
	double speed_mps,
	fusion_gnss_motion_evidence_t gnss_evidence,
	double dt_s);

bool fusion_motion_allows_zero_update(const fusion_motion_detector_t* detector);
const char* fusion_motion_state_name(fusion_motion_state_t state);

#endif