#include "adapters/gps_system_uart.h"

#include <bsp/soc.h>
#include <stddef.h>

#include "gps/gps_macros.h"
#include "gps/gps_receiver.h"
#include "imu/imu_sensor.h"

#define GPS_AIDED_MAX_AGE_MS 1000u

#ifndef GPS_BUILD_ID
#define GPS_BUILD_ID "unknown"
#endif

void gps_system_uart_publish_location(
	const char* source,
	double latitude_deg,
	double longitude_deg,
	double height_m,
	const gps_nav_solution_t* gps)
{
	if((source == NULL) || (gps == NULL))
	{
		return;
	}
	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_LOCATION,
	        "source=%s lat=%.20lf lon=%.20lf height=%.3lf "
	        "gdop=%.10lf pdop=%.10lf hdop=%.10lf vdop=%.10lf tdop=%.10lf "
	        "tracked=%u used=%u velocity_valid=%u speed=%.3lf "
	        "velocity_rms=%.3lf\r\n",
	        source, latitude_deg, longitude_deg, height_m,
	        gps->gdop, gps->pdop, gps->hdop, gps->vdop, gps->tdop,
	        gps->tracked_satellite_count, gps->used_satellite_count,
	        gps->velocity_valid ? 1u : 0u,
	        gps->speed_mps, gps->velocity_residual_rms_mps);
}

void gps_system_uart_publish_fused_location(
	const navigation_fusion_output_t* output)
{
	if((output == NULL) || !output->global_valid)
	{
		return;
	}

	uint64_t age_ticks = output->timestamp_ticks >= output->last_accepted_gps_ticks
		? output->timestamp_ticks - output->last_accepted_gps_ticks : 0u;
	uint64_t age_ms_wide =
		age_ticks / (BSP_CLINT_TIMEBASE_HZ / 1000u);
	uint32_t age_ms = age_ms_wide > UINT32_MAX
		? UINT32_MAX : (uint32_t)age_ms_wide;
	const char* mode =
		(output->gps_accepted > 0u) && (age_ms <= GPS_AIDED_MAX_AGE_MS)
		? "gps_aided" : "dead_reckoning";

	/* The application rate-limits this compact record by IMU sample count,
	 * leaving UART headroom without reducing the ESKF prediction rate. */
	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_LOCATION,
	        "source=imu_corrected mode=%s lat=%.9lf lon=%.9lf "
	        "height=%.2lf gps_age_ms=%lu\r\n",
	        mode, output->latitude_deg, output->longitude_deg,
	        output->height_m, (unsigned long)age_ms);
}

void gps_system_uart_publish_startup(
	bool fusion_enabled,
	bool fusion_initialized,
	bool imu_initialized,
	bool imu_running,
	unsigned int fused_publish_every_samples)
{
	imu_sensor_diagnostics_t imu;
	imu_sensor_get_diagnostics(&imu);
	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION,
	        "init build=%s eskf_enabled=%u eskf_ready=%u imu=%u sensor=%s who=0x%02x "
	        "running=%u target_hz=%lu actual_hz=%lu publish_every=%u "
	        "nav_ms=%u timebase_hz=%lu\r\n",
	        GPS_BUILD_ID,
	        fusion_enabled ? 1u : 0u,
	        fusion_initialized ? 1u : 0u,
	        imu_initialized ? 1u : 0u,
	        imu_sensor_backend_name(imu.backend),
	        (unsigned int)imu.who_am_i,
	        imu_running ? 1u : 0u,
	        (unsigned long)imu.requested_odr_hz,
	        (unsigned long)imu.configured_odr_hz,
	        fused_publish_every_samples,
	        GPS_NAV_SOLVE_PERIOD_MS,
	        (unsigned long)BSP_CLINT_TIMEBASE_HZ);
}

void gps_system_uart_publish_periodic_fusion_status(
	navigation_fusion_output_t* output,
	uint64_t* last_status_tick)
{
	if((output == NULL) || (last_status_tick == NULL))
	{
		return;
	}
	uint64_t now = gps_receiver_status_ticks();
	if((now - *last_status_tick) < gps_receiver_status_ticks_per_second())
	{
		return;
	}
	*last_status_tick = now;

	imu_sensor_diagnostics_t imu;
	imu_sensor_get_diagnostics(&imu);
	if(!navigation_fusion_get_output(output))
	{
		return;
	}

	if(output->global_valid)
	{
		GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
		        "state=%s lat=%.10lf lon=%.10lf height=%.3lf "
		        "speed=%.3lf pos_nis=%.3lf vel_nis=%.3lf "
		        "gps=%lu/%lu velocity=%lu/%lu recoveries=%lu "
		        "zupt=%lu replay=%lu "
		        "sigma=%.2lf,%.2lf,%.2lf imu=%lu fused_imu=%lu "
		        "overflow=%lu missed=%lu\r\n",
		        fusion_motion_state_name(output->motion_state),
		        output->latitude_deg, output->longitude_deg, output->height_m,
		        output->speed_mps, output->last_position_nis,
		        output->last_velocity_nis,
		        (unsigned long)output->gps_accepted,
		        (unsigned long)output->gps_nis_rejected,
		        (unsigned long)output->gps_velocity_accepted,
		        (unsigned long)output->gps_velocity_nis_rejected,
		        (unsigned long)output->gps_position_recoveries,
		        (unsigned long)output->zero_update_count,
		        (unsigned long)output->replay_count,
		        output->position_std_m[0], output->position_std_m[1],
		        output->position_std_m[2],
		        (unsigned long)imu.sample_count,
		        (unsigned long)output->imu_processed_count,
		        (unsigned long)imu.queue_overflow_count,
		        (unsigned long)imu.estimated_missed_sample_count);
		return;
	}

	GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
	        "state=%s calibration_samples=%u gps=%lu/%lu recoveries=%lu "
	        "imu=%lu fused_imu=%lu overflow=%lu missed=%lu\r\n",
	        navigation_fusion_state_name(output->engine_state),
	        output->calibration_sample_count,
	        (unsigned long)output->gps_accepted,
	        (unsigned long)output->gps_nis_rejected,
	        (unsigned long)output->gps_position_recoveries,
	        (unsigned long)imu.sample_count,
	        (unsigned long)output->imu_processed_count,
	        (unsigned long)imu.queue_overflow_count,
	        (unsigned long)imu.estimated_missed_sample_count);
}