#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 "navigation_fusion.h"
#include "tdk_icm42688.h"

int main()
{
	tdk_icm42688_config_t imu_config;
	tdk_icm42688_default_config(&imu_config);
	bool imu_initialized = tdk_icm42688_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 && tdk_icm42688_start();
	GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_FUSION,
	        "init imu=%u running=%u odr_hz=%u timebase_hz=%lu\r\n",
	        imu_initialized ? 1u : 0u, imu_running ? 1u : 0u,
	        imu_config.odr_hz, (unsigned long)BSP_CLINT_TIMEBASE_HZ);

	acq_start_loop();
	uint64_t last_fusion_status_tick = 0u;

	while(1)
	{
		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;
}