#include <bsp/soc.h>
#include <stdbool.h>
#include <stdint.h>

#include "adapters/gps_fusion_publisher.h"
#include "adapters/gps_system_uart.h"
#include "adapters/imu_fusion_profile.h"
#include "fusion/navigation_fusion.h"
#include "gps/gps_receiver.h"
#include "gps_system.h"
#include "imu/imu_sensor.h"

#ifndef GPS_ESKF_ENABLE
#define GPS_ESKF_ENABLE 1
#endif

#if (GPS_ESKF_ENABLE != 0) && (GPS_ESKF_ENABLE != 1)
#error "GPS_ESKF_ENABLE must be 0 or 1"
#endif

#ifndef IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES
#define IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES 5u
#endif

#if (IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES < 1)
#error "IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES must be at least one"
#endif

#define IMU_SAMPLES_BEFORE_GPS_SERVICE 1u
#define IMU_SAMPLES_AFTER_GPS_SERVICE  3u

static void update_eskf_doppler_bandwidth(
	const navigation_fusion_output_t* output)
{
	if((output == NULL) || !output->local_valid)
	{
		return;
	}
	double alpha = 0.05;
	if(output->motion_state == FUSION_MOTION_MOVING)
	{
		alpha = (output->linear_acceleration_norm_mps2 >= 2.5) ?
			0.40 : 0.20;
	}
	gps_receiver_set_eskf_doppler_alpha(alpha);
}

static void initialize_sensor_fusion(gps_system_state_t* system)
{
	system->fusion_enabled = (GPS_ESKF_ENABLE != 0);
	if(!system->fusion_enabled)
	{
		return;
	}

	imu_sensor_config_t imu_config;
	imu_sensor_default_config(&imu_config);
	imu_config.odr_hz = IMU_ODR_HZ;
	imu_config.accel_full_scale_g = IMU_ACCEL_FS_G;
	imu_config.gyro_full_scale_dps = IMU_GYRO_FS_DPS;
	system->imu_initialized = imu_sensor_init(&imu_config);

	fusion_imu_quality_profile_t imu_quality;
	bool quality_available = system->imu_initialized &&
		imu_fusion_quality_profile_for_backend(
			imu_sensor_active_backend(), &imu_quality);
	system->fusion_initialized = quality_available &&
		navigation_fusion_init(
			BSP_CLINT_TIMEBASE_HZ, &imu_quality,
			true); /* Current prototype is the PX4-equivalent unarmed bench mode. */
}

static unsigned int feed_pending_imu_to_eskf(
	gps_system_state_t* system,
	unsigned int max_samples)
{
	if(!system->fusion_initialized || (max_samples == 0u))
	{
		return 0u;
	}

	unsigned int processed = 0u;
	imu_sensor_service();
	while((processed < max_samples) &&
	      imu_sensor_take_measurement(&system->imu_sample))
	{
		bool accepted = navigation_fusion_push_imu(&system->imu_sample);
		bool output_available = accepted &&
			navigation_fusion_get_output(&system->fused_solution);
		if(output_available)
		{
			update_eskf_doppler_bandwidth(&system->fused_solution);
		}
		if(output_available && system->fused_solution.global_valid &&
		   (system->fused_solution.timestamp_ticks !=
		    system->last_fused_location_tick))
		{
			system->fused_publish_sample_count++;
			bool first_location =
				(system->last_fused_location_tick == 0u);
			bool publish_due =
				(system->fused_publish_sample_count >=
				 IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES);
			if(first_location || publish_due)
			{
				gps_system_uart_publish_fused_location(
					&system->fused_solution);
				system->last_fused_location_tick =
					system->fused_solution.timestamp_ticks;
				system->fused_publish_sample_count = 0u;
			}
		}
		processed++;
	}
	return processed;
}

int main(void)
{
	gps_system_state_t system = {0};

	/* GPS and IMU are initialized independently.  The application owns their
	 * interrupt connection and decides whether the ESKF path is enabled. */
	initialize_sensor_fusion(&system);
	gps_receiver_initialize(GPS_NAV_SOLVE_TIMER_TICKS);
	bool imu_irq_attached = system.fusion_initialized &&
		gps_receiver_attach_external_irq(BSP_PMOD_IRQ,
		                                 imu_sensor_irq_handler);
	system.imu_running = imu_irq_attached && system.fusion_initialized &&
	                     imu_sensor_start();
	gps_receiver_start_acquisition();
	gps_system_uart_publish_startup(
		system.fusion_enabled, system.fusion_initialized,
		system.imu_initialized, system.imu_running,
		IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES);

	bool imu_backlog_may_remain = false;
	for(;;)
	{
		/* Skip WFI only while draining a bounded IMU backlog.  Every batch
		 * still services GPS before more prediction work, so a continuously
		 * non-empty IMU queue cannot starve navigation epochs. */
		if(!imu_backlog_may_remain)
		{
			/* GPS, IMU data-ready and the maintenance timer wake this loop. */
			asm volatile("wfi");
		}

		/* Limit pre-GPS work to one sample for low event latency. */
		(void)feed_pending_imu_to_eskf(
			&system, IMU_SAMPLES_BEFORE_GPS_SERVICE);

		/* GPS remains an independent receiver: events first, then coherent WLS. */
		gps_receiver_process_pending_events();
		system.gps_epoch_ready = gps_receiver_solve_ready_epoch(
			&system.gps_solution, &system.gps_measurement_ticks);

		/* WLS may take long enough for more IMU samples to arrive.  Drain a
		 * small batch, then return to GPS service before continuing. */
		unsigned int imu_after_gps = feed_pending_imu_to_eskf(
			&system, IMU_SAMPLES_AFTER_GPS_SERVICE);
		imu_backlog_may_remain =
			(imu_after_gps == IMU_SAMPLES_AFTER_GPS_SERVICE);

		if(system.gps_epoch_ready && system.gps_solution.solution_valid)
		{
			/* Raw WLS latitude/longitude remains visible even with ESKF disabled. */
			gps_system_uart_publish_location("gps",
			                 system.gps_solution.latitude_deg,
			                 system.gps_solution.longitude_deg,
			                 system.gps_solution.height_m,
			                 &system.gps_solution);

			/* This is the only GPS -> sensor-fusion hand-off in the application. */
			bool gps_measurement_ready = system.fusion_initialized &&
				gps_fusion_build_measurement(
					&system.gps_solution,
					system.gps_measurement_ticks,
					&system.gps_for_fusion);
			if(gps_measurement_ready)
			{
				/* Delayed GPS update is applied at its capture timestamp; newer
				 * IMU history is replayed to produce the current fused state. */
				(void)navigation_fusion_push_gps(&system.gps_for_fusion);
			}
		}

		if(system.fusion_initialized)
		{
			gps_system_uart_publish_periodic_fusion_status(
				&system.fused_solution, &system.last_status_tick);
		}
	}
}