#include <stdio.h>
#include "gps_ip_defines.h"
#include "linked_list.h"
#include <stddef.h>
#include <float.h>
#include "matrixLib.h"
#include "gps_macros.h"
#include "imu_sensor.h"
#include "navigation_fusion.h"

int main()
{
	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;
	bool imu_initialized = imu_sensor_init(&imu_config);
	bool fusion_initialized = navigation_fusion_init(BSP_CLINT_TIMEBASE_HZ);

	gps_startup_initialization(GPS_NAV_SOLVE_TIMER_TICKS);
	bool imu_running = imu_initialized && fusion_initialized && imu_sensor_start();
	(void)imu_running;
	imu_sensor_diagnostics_t imu_diagnostics;
	imu_sensor_get_diagnostics(&imu_diagnostics);
	GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
	        "init imu=%u sensor=%s who=0x%02x running=%u odr_hz=%lu "
	        "poll=%u timebase_hz=%lu\r\n",
	        imu_initialized ? 1u : 0u,
	        imu_sensor_backend_name(imu_diagnostics.backend),
	        (unsigned int)imu_diagnostics.who_am_i,
	        imu_running ? 1u : 0u,
	        (unsigned long)imu_diagnostics.configured_odr_hz,
	        imu_diagnostics.foreground_polling ? 1u : 0u,
	        (unsigned long)BSP_CLINT_TIMEBASE_HZ);

	acq_start_loop();
	uint64_t last_fusion_status_tick = 0u;

	while(1)
	{
		/* GPS and either IMU backend wake the shared machine interrupt path. */
		asm volatile("wfi");
		gps_background_task();
		calculate_position();
		if(fusion_initialized)
		{
			(void)navigation_fusion_service();
			uint64_t now = gps_get_systick();
			if((now - last_fusion_status_tick) >= GPS_SYSTICK_HZ)
			{
				navigation_fusion_output_t output;
				if(navigation_fusion_get_output(&output))
				{
					GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
					        "state=%s lat=%.10lf lon=%.10lf height=%.3lf "
					        "speed=%.3lf nis=%.3lf gps=%lu/%lu sigma=%.2lf,%.2lf,%.2lf\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,
					        (unsigned long)output.gps_accepted,
					        (unsigned long)output.gps_nis_rejected,
					        output.position_std_m[0], output.position_std_m[1],
					        output.position_std_m[2]);
				}
				last_fusion_status_tick = now;
			}
		}
	}

	return 0;
}