#include "adapters/imu_fusion_profile.h"

#include <stddef.h>

#define PROFILE_GRAVITY_MPS2 9.80665
#define PROFILE_DEGREES_TO_RADIANS 0.017453292519943295769236907684886

static void set_all(double values[3], double value)
{
	for(unsigned int axis = 0u ; axis < 3u ; axis++)
	{
		values[axis] = value;
	}
}

bool imu_fusion_quality_profile_for_backend(
	imu_sensor_backend_t backend,
	fusion_imu_quality_profile_t* profile_out)
{
	if(profile_out == NULL)
	{
		return false;
	}
	fusion_imu_quality_default(profile_out);
	switch(backend)
	{
		case IMU_SENSOR_TDK_ICM42688:
			profile_out->name = "tdk-icm42688";
			/* ICM-42688-P: 65/65/70 ug/sqrt(Hz), 2.8 mdps/sqrt(Hz). */
			profile_out->nominal_accel_noise_density_mps2_sqrt_hz[0] =
				65.0e-6 * PROFILE_GRAVITY_MPS2;
			profile_out->nominal_accel_noise_density_mps2_sqrt_hz[1] =
				65.0e-6 * PROFILE_GRAVITY_MPS2;
			profile_out->nominal_accel_noise_density_mps2_sqrt_hz[2] =
				70.0e-6 * PROFILE_GRAVITY_MPS2;
			set_all(profile_out->nominal_gyro_noise_density_rps_sqrt_hz,
			        2.8e-3 * PROFILE_DEGREES_TO_RADIANS);
			/* Conservative fusion values; tune bias RW with long Allan logs. */
			set_all(profile_out->process_accel_noise_density_mps2_sqrt_hz, 0.025);
			set_all(profile_out->process_gyro_noise_density_rps_sqrt_hz, 0.0022);
			set_all(profile_out->accel_bias_random_walk_mps2_sqrt_s, 0.0015);
			set_all(profile_out->gyro_bias_random_walk_rps_sqrt_s,
			        0.002 * PROFILE_DEGREES_TO_RADIANS);
			profile_out->calibration_accel_std_limit_mps2 = 0.09;
			profile_out->calibration_gyro_std_limit_rps =
				0.10 * PROFILE_DEGREES_TO_RADIANS;
			/*
			 * ICM-42688-P zero-g offset is specified at +-40 mg (0.392 m/s^2).
			 * The bench unit measured 0.278 m/s^2 (+28 mg): in spec, but it
			 * consumed 93% of the old combined 0.30 tolerance and left the
			 * motion gate no usable headroom, so startup calibration stalled.
			 *
			 * 0.42 is deliberately only just above the datasheet limit.  This
			 * allowance also sets how much sustained acceleration calibration
			 * cannot distinguish from offset, so widening it further would
			 * trade startup robustness for a worse initial attitude.
			 */
			profile_out->calibration_accel_offset_allowance_mps2 = 0.42;
			set_all(profile_out->zero_gyro_measurement_std_rps,
			        0.02 * PROFILE_DEGREES_TO_RADIANS);
			/*
			 * Rest-detector limits from a stationary bench capture of this
			 * board (soc/software/tdk_noise_test, 3000 samples at the
			 * production 50 Hz / +-4 g / +-250 dps / UI filter 0x77), reported
			 * as the norms fusion_motion_update() actually gates on:
			 *
			 *   accel_norm  p99.9 = 0.014 m/s^2   max = 0.0154 m/s^2
			 *   gyro_norm   p99.9 = 0.040 deg/s   max = 0.0417 deg/s
			 *   jerk_norm   p99.9 = 0.10 m/s^3    max = 0.0915 m/s^3
			 *
			 * Gyro and jerk sit far above their noise floors here because they
			 * are measured directly: 6x p99.9 still leaves room for gyro bias
			 * drift between ESKF updates, and a static attitude error cancels
			 * in the jerk derivative.  Jerk is therefore the most sensitive
			 * motion discriminator available and is tightened hardest.
			 *
			 * The accel limit deliberately keeps far more headroom than its
			 * 0.014 m/s^2 noise floor would suggest.  linear_accel_nav carries
			 * the attitude error times gravity, so a 0.1 deg tilt error alone
			 * contributes 0.017 m/s^2 -- more than the entire sensor noise
			 * budget.  Sizing this limit to the noise floor would break rest
			 * detection whenever attitude estimation is slightly off.
			 */
			profile_out->stationary_accel_limit_mps2 = 0.1125;
			profile_out->stationary_gyro_limit_rps =
				0.25 * PROFILE_DEGREES_TO_RADIANS;
			profile_out->moving_accel_threshold_mps2 = 0.30;
			profile_out->moving_gyro_threshold_rps =
				0.75 * PROFILE_DEGREES_TO_RADIANS;
			profile_out->moving_jerk_threshold_mps3 = 0.60;
			break;

		case IMU_SENSOR_ST_LSM6DSOX:
			profile_out->name = "st-lsm6dsox";
			/* LSM6DSOX at +-2 g: 70 ug/sqrt(Hz), 3.8 mdps/sqrt(Hz). */
			set_all(profile_out->nominal_accel_noise_density_mps2_sqrt_hz,
			        70.0e-6 * PROFILE_GRAVITY_MPS2);
			set_all(profile_out->nominal_gyro_noise_density_rps_sqrt_hz,
			        3.8e-3 * PROFILE_DEGREES_TO_RADIANS);
			set_all(profile_out->process_accel_noise_density_mps2_sqrt_hz, 0.03);
			set_all(profile_out->process_gyro_noise_density_rps_sqrt_hz, 0.0030);
			set_all(profile_out->accel_bias_random_walk_mps2_sqrt_s, 0.0025);
			set_all(profile_out->gyro_bias_random_walk_rps_sqrt_s,
			        0.004 * PROFILE_DEGREES_TO_RADIANS);
			profile_out->calibration_accel_std_limit_mps2 = 0.12;
			profile_out->calibration_gyro_std_limit_rps =
				0.20 * PROFILE_DEGREES_TO_RADIANS;
			/* LSM6DSOX zero-g offset is specified at +-40 mg over temperature. */
			profile_out->calibration_accel_offset_allowance_mps2 = 0.42;
			set_all(profile_out->zero_gyro_measurement_std_rps,
			        0.04 * PROFILE_DEGREES_TO_RADIANS);
			/* Unchanged from the historical fusion_motion defaults; these are
			 * the field-tuned values this platform was validated against. */
			profile_out->stationary_accel_limit_mps2 = 0.15;
			profile_out->stationary_gyro_limit_rps =
				1.00 * PROFILE_DEGREES_TO_RADIANS;
			profile_out->moving_accel_threshold_mps2 = 0.40;
			profile_out->moving_gyro_threshold_rps =
				3.00 * PROFILE_DEGREES_TO_RADIANS;
			profile_out->moving_jerk_threshold_mps3 = 4.0;
			break;

		default:
			return false;
	}
	return fusion_imu_quality_is_valid(profile_out);
}