#include "imu_sensor.h"

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

#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
#include "tdk_icm42688.h"
#endif

#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
#include "st_lsm6dsox.h"
#endif

#if !defined(IMU_SENSOR_BUILD_AUTO) && !defined(IMU_SENSOR_BUILD_TDK) && \
    !defined(IMU_SENSOR_BUILD_ST)
#error "Select IMU_SENSOR_BUILD_AUTO, IMU_SENSOR_BUILD_TDK or IMU_SENSOR_BUILD_ST"
#endif

static imu_sensor_backend_t active_backend;
static imu_calibration_t active_calibration;
static uint32_t active_odr_hz;
static uint64_t calibration_rejected_count;

void imu_sensor_default_config(imu_sensor_config_t* config)
{
	if(config == NULL)
	{
		return;
	}
	memset(config, 0, sizeof(*config));
	for(unsigned int backend = 0u ; backend < IMU_SENSOR_BACKEND_COUNT ; backend++)
	{
		imu_calibration_default(&config->calibration_by_backend[backend]);
	}
}

#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
static bool tdk_accel_range(unsigned int full_scale_g,
	                        tdk_accel_full_scale_t* range)
{
	if(range == NULL)
	{
		return false;
	}
	switch(full_scale_g)
	{
		case 2u: *range = TDK_ACCEL_FS_2G; return true;
		case 4u: *range = TDK_ACCEL_FS_4G; return true;
		case 8u: *range = TDK_ACCEL_FS_8G; return true;
		case 16u: *range = TDK_ACCEL_FS_16G; return true;
		default: return false;
	}
}

static bool tdk_gyro_range(unsigned int full_scale_dps,
	                       tdk_gyro_full_scale_mdps_t* range)
{
	if(range == NULL)
	{
		return false;
	}
	switch(full_scale_dps)
	{
		case 125u: *range = TDK_GYRO_FS_125; return true;
		case 250u: *range = TDK_GYRO_FS_250; return true;
		case 500u: *range = TDK_GYRO_FS_500; return true;
		case 1000u: *range = TDK_GYRO_FS_1000; return true;
		case 2000u: *range = TDK_GYRO_FS_2000; return true;
		default: return false;
	}
}

static bool try_tdk(const imu_sensor_config_t* config)
{
	if(!imu_calibration_is_valid(
		&config->calibration_by_backend[IMU_SENSOR_TDK_ICM42688]))
	{
		return false;
	}
	tdk_icm42688_config_t backend_config;
	tdk_icm42688_default_config(&backend_config);
	if(config->odr_hz != 0u)
	{
		backend_config.odr_hz = config->odr_hz;
	}
	if((config->accel_full_scale_g != 0u) &&
	   !tdk_accel_range(config->accel_full_scale_g,
	                    &backend_config.accel_full_scale_g))
	{
		return false;
	}
	if((config->gyro_full_scale_dps != 0u) &&
	   !tdk_gyro_range(config->gyro_full_scale_dps,
	                   &backend_config.gyro_full_scale_mdps))
	{
		return false;
	}
	if(!tdk_icm42688_init(&backend_config))
	{
		return false;
	}
	active_backend = IMU_SENSOR_TDK_ICM42688;
	active_calibration =
		config->calibration_by_backend[IMU_SENSOR_TDK_ICM42688];
	active_odr_hz = backend_config.odr_hz;
	return true;
}
#endif

#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
static bool try_st(const imu_sensor_config_t* config)
{
	if(!imu_calibration_is_valid(
		&config->calibration_by_backend[IMU_SENSOR_ST_LSM6DSOX]))
	{
		return false;
	}
	st_lsm6dsox_config_t backend_config;
	st_lsm6dsox_default_config(&backend_config);
	if(config->odr_hz != 0u)
	{
		backend_config.odr_hz = config->odr_hz;
	}
	if(config->accel_full_scale_g != 0u)
	{
		backend_config.accel_full_scale_g = config->accel_full_scale_g;
	}
	if(config->gyro_full_scale_dps != 0u)
	{
		backend_config.gyro_full_scale_dps = config->gyro_full_scale_dps;
	}
	if(!st_lsm6dsox_init(&backend_config))
	{
		return false;
	}
	active_backend = IMU_SENSOR_ST_LSM6DSOX;
	active_calibration =
		config->calibration_by_backend[IMU_SENSOR_ST_LSM6DSOX];
	active_odr_hz = backend_config.odr_hz;
	return true;
}
#endif

bool imu_sensor_init(const imu_sensor_config_t* config)
{
	active_backend = IMU_SENSOR_NONE;
	active_odr_hz = 0u;
	calibration_rejected_count = 0u;
	if(config == NULL)
	{
		return false;
	}

#if defined(IMU_SENSOR_BUILD_TDK)
	return try_tdk(config);
#elif defined(IMU_SENSOR_BUILD_ST)
	return try_st(config);
#else
	/* WHO_AM_I probing is read-only until a matching backend is found. */
	if(try_tdk(config))
	{
		return true;
	}
	return try_st(config);
#endif
}

bool imu_sensor_start(void)
{
	switch(active_backend)
	{
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
		case IMU_SENSOR_TDK_ICM42688:
			return tdk_icm42688_start();
#endif
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
		case IMU_SENSOR_ST_LSM6DSOX:
			return st_lsm6dsox_start();
#endif
		default:
			return false;
	}
}

void imu_sensor_stop(void)
{
	switch(active_backend)
	{
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
		case IMU_SENSOR_TDK_ICM42688:
			tdk_icm42688_stop();
			break;
#endif
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
		case IMU_SENSOR_ST_LSM6DSOX:
			st_lsm6dsox_stop();
			break;
#endif
		default:
			break;
	}
}

void imu_sensor_irq_handler(void)
{
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
	if(active_backend == IMU_SENSOR_TDK_ICM42688)
	{
		tdk_icm42688_irq_handler();
	}
#endif
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
	if(active_backend == IMU_SENSOR_ST_LSM6DSOX)
	{
		st_lsm6dsox_irq_handler();
	}
#endif
}

void imu_sensor_service(void)
{
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
	if(active_backend == IMU_SENSOR_ST_LSM6DSOX)
	{
		st_lsm6dsox_service();
	}
#endif
}

bool imu_sensor_requires_foreground_service(void)
{
	return false;
}

bool imu_sensor_take_measurement(fusion_imu_measurement_t* measurement_out)
{
	if(measurement_out == NULL)
	{
		return false;
	}
	bool have_measurement = false;
	switch(active_backend)
	{
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
		case IMU_SENSOR_TDK_ICM42688:
			have_measurement =
				tdk_icm42688_take_measurement(measurement_out);
			break;
#endif
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
		case IMU_SENSOR_ST_LSM6DSOX:
			have_measurement =
				st_lsm6dsox_take_measurement(measurement_out);
			break;
#endif
		default:
			break;
	}
	if(!have_measurement)
	{
		return false;
	}
	if(!imu_calibration_apply(&active_calibration, measurement_out))
	{
		calibration_rejected_count++;
		return false;
	}
	return true;
}

void imu_sensor_get_diagnostics(imu_sensor_diagnostics_t* diagnostics_out)
{
	if(diagnostics_out == NULL)
	{
		return;
	}
	memset(diagnostics_out, 0, sizeof(*diagnostics_out));
	diagnostics_out->backend = active_backend;
	diagnostics_out->configured_odr_hz = active_odr_hz;
	diagnostics_out->foreground_polling =
		imu_sensor_requires_foreground_service();

	switch(active_backend)
	{
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_TDK)
		case IMU_SENSOR_TDK_ICM42688:
		{
			tdk_icm42688_diagnostics_t source;
			tdk_icm42688_get_diagnostics(&source);
			diagnostics_out->irq_count = source.irq_count;
			diagnostics_out->sample_count = source.sample_count;
			diagnostics_out->invalid_sample_count =
				source.invalid_sample_count + calibration_rejected_count;
			diagnostics_out->spurious_irq_count = source.spurious_irq_count;
			diagnostics_out->queue_overflow_count = source.queue_overflow_count;
			diagnostics_out->estimated_missed_sample_count =
				source.estimated_missed_sample_count;
			diagnostics_out->last_timestamp_ticks = source.last_timestamp_ticks;
			diagnostics_out->who_am_i = source.who_am_i;
			diagnostics_out->initialized = source.initialized;
			diagnostics_out->running = source.running;
			break;
		}
#endif
#if defined(IMU_SENSOR_BUILD_AUTO) || defined(IMU_SENSOR_BUILD_ST)
		case IMU_SENSOR_ST_LSM6DSOX:
		{
			st_lsm6dsox_diagnostics_t source;
			st_lsm6dsox_get_diagnostics(&source);
			diagnostics_out->irq_count = source.irq_count;
			diagnostics_out->sample_count = source.sample_count;
			diagnostics_out->invalid_sample_count =
				source.invalid_sample_count + calibration_rejected_count;
			diagnostics_out->spurious_irq_count = source.spurious_irq_count;
			diagnostics_out->queue_overflow_count = source.queue_overflow_count;
			diagnostics_out->estimated_missed_sample_count =
				source.estimated_missed_sample_count;
			diagnostics_out->last_timestamp_ticks = source.last_timestamp_ticks;
			diagnostics_out->who_am_i = source.who_am_i;
			diagnostics_out->initialized = source.initialized;
			diagnostics_out->running = source.running;
			break;
		}
#endif
		default:
			diagnostics_out->invalid_sample_count = calibration_rejected_count;
			break;
	}
}

imu_sensor_backend_t imu_sensor_active_backend(void)
{
	return active_backend;
}

bool imu_sensor_active_quality_profile(
	fusion_imu_quality_profile_t* profile_out)
{
	return imu_sensor_quality_profile_for_backend(active_backend, profile_out);
}

const char* imu_sensor_backend_name(imu_sensor_backend_t backend)
{
	switch(backend)
	{
		case IMU_SENSOR_TDK_ICM42688: return "tdk-icm42688";
		case IMU_SENSOR_ST_LSM6DSOX: return "st-lsm6dsox";
		default: return "none";
	}
}