use crate::ImuConfig;
use crate::utils::rand_func::{SensorRng, gen_randn_vec3};
use nalgebra::Vector3;
use serde::Serialize;

#[derive(Debug, Clone, Serialize)]
pub struct ImuData {
    /// Angular rate (rad/s)
    pub ang_rate: Vector3<f64>,
    /// Specific force (m/s^2)
    pub sp_force: Vector3<f64>,
}

impl ImuData {
    pub fn new(ang_rate: &Vector3<f64>, sp_force: &Vector3<f64>) -> Self {
        ImuData {
            ang_rate: *ang_rate,
            sp_force: *sp_force,
        }
    }
}

/// Add errors and noises onto the ideal IMU data.
///
/// The implemented error sources are:
/// - Biases
/// - Scale factor errors
/// - Misalignment errors[^1]
/// - White noises
/// - ~~Gyro g-dependent biases (mainly for MEMS)~~
///
/// [^1]: NOTE: In Groves book, they are "the cross coupling errors."
///     The difference will be clarified in the future...
///
/// # Example
/// ```
/// use nalgebra::Vector3;
/// use navitools::{imu_model, ImuData, ImuConfig, get_rng, ErrMode};
///
/// // Load your IMU config file
/// let imu_cfg_fname = "./user_space/usr_cfg/imu_cfg_dummy.toml";
/// let imu_cfg = ImuConfig::load_imu_cfg(imu_cfg_fname).expect("load IMU cfg failed");
///
/// // Create an RNG
/// let mut imu_rng = get_rng(Some(42));
///
/// // Prepare your own ideal data.
/// let angular_rate = Vector3::from_element(0.2);
/// let specific_force = Vector3::from_element(1.0);
/// let dummy_data = ImuData::new(&angular_rate, &specific_force);
/// let imu_ideal_data_arr = vec![dummy_data; 3];
///
/// // (you don't need this; it's for doc test.)
/// let ans_arr = match imu_cfg.err_mode {
///     ErrMode::Add => (
///         vec![
///         ImuData::new(
///             &Vector3::new(0.20057928839308875, 0.2006143410444089, 0.20068589118285346),
///             &Vector3::new(1.0140513183786324, 1.0187782075797498, 1.0284268237047733)),
///         ImuData::new(
///             &Vector3::new(0.20041662121276574, 0.2001743696074018, 0.20042209112709056),
///             &Vector3::new(0.9921154673349378, 0.9594475676405574, 0.9928530913941555)),
///         ImuData::new(
///             &Vector3::new(0.20064799997648644, 0.20135583615849578, 0.19981793033516507),
///             &Vector3::new(1.0233171521835536, 1.1187696523194919, 0.9113813334660845)),
///         ]
///     ),
///     ErrMode::Mul => (
///         vec![
///         ImuData::new(
///             &Vector3::new(0.20057961239308875, 0.2006146650444089, 0.20068621518285346),
///             &Vector3::new(1.0140533183786327, 1.01878020757975, 1.0284288237047734)),
///         ImuData::new(
///             &Vector3::new(0.20041694521276573, 0.20017469360740178, 0.20042241512709055),
///             &Vector3::new(0.9921174673349381, 0.9594495676405577, 0.9928550913941555)),
///         ImuData::new(
///             &Vector3::new(0.20064832397648644, 0.20135616015849578, 0.19981825433516506),
///             &Vector3::new(1.0233191521835538, 1.1187716523194922, 0.9113833334660846)),
///         ]
///     ),
/// };
///
/// // Run `imu_model()` in a loop to create simulated measurements.
/// for (idx, imu_ideal_data) in imu_ideal_data_arr.iter().enumerate() {
///     let imu_meas = imu_model(&imu_ideal_data, &imu_cfg, &mut imu_rng);
///
///     assert!(imu_meas.ang_rate == ans_arr[idx].ang_rate);
///     assert!(imu_meas.sp_force == ans_arr[idx].sp_force);
/// }
/// ```
///
pub fn imu_model(true_sensor: &ImuData, imu_cfg: &ImuConfig, imu_rng: &mut SensorRng) -> ImuData {
    let tor_i = 1.0 / imu_cfg.data_rate;
    if tor_i <= 0.0 {
        panic!("[err] imu_model(): non-positive time interval.");
    }

    let true_omgibb = true_sensor.ang_rate;
    let true_fibb = true_sensor.sp_force;

    // Generate noise
    // ref: Groves 4.4.3 Random Noise
    let randn = gen_randn_vec3(imu_rng, 0.0, 1.0);
    let gyro_noise = randn.component_mul(&(imu_cfg.primary.arw / tor_i.sqrt()));
    let accel_noise = randn.component_mul(&(imu_cfg.primary.vrw / tor_i.sqrt()));

    // Add errors and noise onto the ideal data
    // ref of "Add" error model: Groves (4.16) and (4.17)
    // TODO: G_g is not used now...
    //let uq_omgibb = imu_cfg.b_g + imu_cfg.SM_g * true_omgibb + imu_cfg.G_g * true_fibb + gyro_noise;
    let uq_omgibb = imu_cfg.primary.b_g + imu_cfg.derived.SM_g * true_omgibb + gyro_noise;
    let uq_fibb = imu_cfg.primary.b_a + imu_cfg.derived.SM_a * true_fibb + accel_noise;

    // TODO: add quantized errors
    let meas_omgibb = uq_omgibb;
    let meas_fibb = uq_fibb;

    ImuData::new(&meas_omgibb, &meas_fibb)
}

/// Compensate the systematic errors of the IMU data.
///
pub fn imu_compensator(real_sensor: &ImuData, imu_cfg: &ImuConfig) -> ImuData {
    let comp_ang_rate = imu_cfg.derived.SM_g_inv * (real_sensor.ang_rate - imu_cfg.primary.b_g);
    let comp_sp_force = imu_cfg.derived.SM_a_inv * (real_sensor.sp_force - imu_cfg.primary.b_a);

    ImuData::new(&comp_ang_rate, &comp_sp_force)
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::{ErrMode, ImuConfig, ImuPrimaryParams, get_rng};
    use nalgebra::Matrix3;

    #[test]
    fn test_imu_dual_path_and_error_modes() {
        let cfg_fname = "./user_space/usr_cfg/imu_cfg_dummy.toml";
        let true_data = ImuData::new(&Vector3::new(0.5, 0.5, 0.5), &Vector3::new(1.0, 9.8, -1.0));
        let mut imu_rng = get_rng(Some(42));

        // 1. Shared raw dataset initialized using human-friendly engineering units (TOML format)
        let user_params = ImuPrimaryParams {
            b_g: Vector3::new(0.02, -0.01, 0.05),    // deg/h
            b_a: Vector3::new(15.0, 25.0, -10.0),    // mg
            s_g: Vector3::new(150.0, -200.0, 100.0), // ppm
            s_a: Vector3::new(-100.0, 300.0, 250.0), // ppm
            M_g: Matrix3::new(0.0, 900.0, 900.0, 900.0, 0.0, 900.0, 900.0, 900.0, 0.0), // ppm
            M_a: Matrix3::new(
                0.0, 1000.0, 1000.0, 1000.0, 0.0, 1000.0, 1000.0, 1000.0, 0.0,
            ), // ppm
            // Set noise to zero vectors for exact deterministic compensation checking
            arw: Vector3::zeros(), // deg/sqrt(h)
            vrw: Vector3::zeros(), // m/s/sqrt(h)
        };

        // 2. Generate a valid, corresponding SI dataset using your pub(crate) conversion helper
        let mut algo_params = user_params.clone();
        ImuConfig::unit_conv(&mut algo_params);

        // =====================================================================
        // PART 1: VERIFY ADD MODE (SM = I + s + M)
        // =====================================================================
        {
            let mut cfg_user = ImuConfig::load_imu_cfg(cfg_fname).expect("Failed to load");
            let mut cfg_algo = ImuConfig::load_imu_cfg(cfg_fname).expect("Failed to load");

            cfg_user.err_mode = ErrMode::Add;
            cfg_algo.err_mode = ErrMode::Add;

            cfg_user.update_params_user(user_params.clone()).unwrap();
            cfg_algo.update_params_algo(algo_params.clone()).unwrap();

            // Validate user-loaded vs algo-updated derived fields align perfectly
            assert!((cfg_user.derived.SM_g_inv - cfg_algo.derived.SM_g_inv).norm() < 1e-15);
            assert!((cfg_user.derived.SM_a_inv - cfg_algo.derived.SM_a_inv).norm() < 1e-15);

            // Validate that without noise, the compensator reconstructs the exact raw inputs
            let real_data = imu_model(&true_data, &cfg_user, &mut imu_rng);
            let comp_data = imu_compensator(&real_data, &cfg_user);
            assert!((true_data.ang_rate - comp_data.ang_rate).norm() < 1e-12);
            assert!((true_data.sp_force - comp_data.sp_force).norm() < 1e-12);
        }

        // =====================================================================
        // PART 2: VERIFY MUL MODE (SM = (I + s) * (I + M))
        // =====================================================================
        {
            let mut cfg_user = ImuConfig::load_imu_cfg(cfg_fname).expect("Failed to load");
            let mut cfg_algo = ImuConfig::load_imu_cfg(cfg_fname).expect("Failed to load");

            cfg_user.err_mode = ErrMode::Mul;
            cfg_algo.err_mode = ErrMode::Mul;

            cfg_user.update_params_user(user_params).unwrap();
            cfg_algo.update_params_algo(algo_params).unwrap();

            // Validate user-loaded vs algo-updated derived fields align perfectly
            assert!((cfg_user.derived.SM_g_inv - cfg_algo.derived.SM_g_inv).norm() < 1e-15);
            assert!((cfg_user.derived.SM_a_inv - cfg_algo.derived.SM_a_inv).norm() < 1e-15);

            // Validate that without noise, the compensator reconstructs the exact raw inputs
            let real_data = imu_model(&true_data, &cfg_user, &mut imu_rng);
            let comp_data = imu_compensator(&real_data, &cfg_user);
            assert!((true_data.ang_rate - comp_data.ang_rate).norm() < 1e-12);
            assert!((true_data.sp_force - comp_data.sp_force).norm() < 1e-12);
        }
    }
}
