#include "fusion/navigation_fusion.h"

#include "fusion/fusion_gps_adapter.h"
#include "fusion/fusion_rest_detector.h"

#include <math.h>
#include <stddef.h>
#include <string.h>

static fusion_gps_adapter_t gps_adapter;
static fusion_engine_t fusion_engine;
static fusion_rest_detector_t rest_detector;
static bool fusion_initialized;
static uint64_t imu_processed_count;

bool navigation_fusion_init(
	uint64_t local_timebase_hz,
	const fusion_imu_quality_profile_t* imu_quality,
	bool platform_landed)
{
	fusion_engine_config_t engine_config;
	fusion_gps_adapter_config_t adapter_config;
	fusion_rest_detector_config_t rest_config;
	if((local_timebase_hz == 0u) ||
	   !fusion_imu_quality_is_valid(imu_quality))
	{
		return false;
	}
	fusion_engine_default_config(&engine_config, local_timebase_hz);
	if(!fusion_engine_apply_imu_quality(&engine_config, imu_quality))
	{
		return false;
	}
	fusion_gps_adapter_default_config(&adapter_config);
	fusion_rest_detector_default_config(&rest_config);
	if(!fusion_engine_init(&fusion_engine, &engine_config) ||
	   !fusion_gps_adapter_init(&gps_adapter, &adapter_config) ||
	   !fusion_rest_detector_init(
		&rest_detector, &rest_config, local_timebase_hz, platform_landed))
	{
		return false;
	}
	imu_processed_count = 0u;
	fusion_initialized = true;
	return true;
}

void navigation_fusion_set_platform_landed(
	bool platform_landed,
	uint64_t timestamp_ticks)
{
	if(fusion_initialized)
	{
		fusion_rest_detector_set_landed(
			&rest_detector, platform_landed, timestamp_ticks);
	}
}

bool navigation_fusion_push_imu(const imu_sample_t* measurement)
{
	if(!fusion_initialized || (measurement == NULL))
	{
		return false;
	}
	if(!fusion_rest_detector_update(&rest_detector, measurement))
	{
		return false;
	}
	fusion_imu_measurement_t fusion_measurement;
	memset(&fusion_measurement, 0, sizeof(fusion_measurement));
	fusion_measurement.timestamp_ticks = measurement->timestamp_ticks;
	memcpy(fusion_measurement.accel_mps2, measurement->accel_mps2,
	       sizeof(fusion_measurement.accel_mps2));
	memcpy(fusion_measurement.gyro_rps, measurement->gyro_rps,
	       sizeof(fusion_measurement.gyro_rps));
	fusion_measurement.temperature_c = measurement->temperature_c;
	fusion_measurement.sequence = measurement->sequence;
	fusion_measurement.vehicle_at_rest = rest_detector.at_rest;
	if(!fusion_engine_push_imu(&fusion_engine, &fusion_measurement))
	{
		return false;
	}
	imu_processed_count++;
	return true;
}

bool navigation_fusion_push_gps(const fusion_gps_measurement_t* measurement)
{
	if(!fusion_initialized)
	{
		return false;
	}
	fusion_position_measurement_t position;
	if(!fusion_gps_adapter_convert(&gps_adapter, measurement, &position))
	{
		return false;
	}
	return fusion_engine_push_position(&fusion_engine, &position, NULL, NULL);
}

bool navigation_fusion_get_output(navigation_fusion_output_t* output)
{
	if((output == NULL) || !fusion_initialized)
	{
		return false;
	}
	memset(output, 0, sizeof(*output));
	output->engine_state = fusion_engine.state;
	output->calibration_sample_count = fusion_engine.calibration.sample_count;
	output->gps_accepted = fusion_engine.diagnostics.position_accepted;
	output->gps_nis_rejected = fusion_engine.diagnostics.position_nis_rejected;
	output->gps_position_recoveries =
		fusion_engine.diagnostics.position_recovery_count;
	output->zero_update_count =
		fusion_engine.diagnostics.zero_velocity_update_count;
	output->replay_count = fusion_engine.diagnostics.replay_count;
	output->stale_gps_count =
		fusion_engine.diagnostics.position_stale_rejected;
	output->imu_processed_count = imu_processed_count;
	if(fusion_engine.state != FUSION_ENGINE_RUNNING)
	{
		return true;
	}
	const fusion_runtime_t* runtime = &fusion_engine.current;
	output->timestamp_ticks = runtime->filter_timestamp_ticks;
	output->last_accepted_gps_ticks =
		runtime->last_accepted_position_ticks;
	memcpy(output->position_enu_m, runtime->filter.position_m,
	       sizeof(output->position_enu_m));
	memcpy(output->velocity_enu_mps, runtime->filter.velocity_mps,
	       sizeof(output->velocity_enu_mps));
	for(unsigned int axis = 0 ; axis < 3u ; axis++)
	{
		output->position_std_m[axis] =
			sqrt(fmax(0.0, runtime->filter.covariance[axis][axis]));
	}
	output->speed_mps = eskf_speed_mps(&runtime->filter);
	output->last_position_nis = runtime->filter.last_position_nis;
	output->motion_state = runtime->motion.state;
	output->vehicle_at_rest = runtime->vehicle_at_rest;
	output->local_valid = true;
	if(!fusion_gps_adapter_local_to_geodetic(
		&gps_adapter, runtime->filter.position_m,
		&output->latitude_deg, &output->longitude_deg, &output->height_m))
	{
		/* IMU propagation is still a valid local dead-reckoning state.  Without
		 * an externally established origin it must not be labelled as LLH. */
		return true;
	}
	output->global_valid = true;
	output->valid = true;
	return true;
}

const char* navigation_fusion_state_name(fusion_engine_state_t state)
{
	switch(state)
	{
		case FUSION_ENGINE_CALIBRATING: return "calibrating";
		case FUSION_ENGINE_RUNNING: return "running";
		case FUSION_ENGINE_FAULT: return "fault";
		default: return "unknown";
	}
}