use crate::constants::G0;
use nalgebra::{Matrix3, Vector3};
use serde::Deserialize;
use std::{error::Error, f64::consts::PI, fs};

#[derive(Debug, Deserialize, Clone)]
pub enum ErrMode {
    Add,
    Mul,
}

/// Primary Physical Parameters
///
/// |variable|description|user/config unit|
/// |--|--|--|
/// |b_g | Gyro Bias |deg/h |
/// |b_a | Accel Bias | mg |
/// | | Gyro Scale Factor Error | mg |
/// | | Accel Scale Factor Error | mg |
/// | | Gyro Non-orthogonality (off-diagonal) | ppm |
/// | | Accel Non-orthogonality (off-diagonal) | ppm |
/// | | Angle Random Walk (Gyro noise) | deg/sqrt(h) |
/// | | Velocity Random Walk (Accel noise) | m/s/sqrt(h) |
#[derive(Debug, Clone, Deserialize)]
pub struct ImuPrimaryParams {
    pub b_g: Vector3<f64>,
    pub b_a: Vector3<f64>,
    pub s_g: Vector3<f64>,
    pub s_a: Vector3<f64>,
    pub M_g: Matrix3<f64>,
    pub M_a: Matrix3<f64>,
    pub arw: Vector3<f64>,
    pub vrw: Vector3<f64>,
}

/// Derived Composite Conversion Matrices
#[derive(Debug, Clone, Deserialize, Default)]
pub struct ImuDerivedParams {
    pub S_g: Matrix3<f64>,
    pub S_a: Matrix3<f64>,
    pub SM_g: Matrix3<f64>,
    pub SM_a: Matrix3<f64>,
    pub SM_g_inv: Matrix3<f64>,
    pub SM_a_inv: Matrix3<f64>,
}

/// IMU Configuration Wrapper
#[derive(Debug, Clone, Deserialize)]
pub struct ImuConfig {
    pub data_rate: f64,
    pub err_mode: ErrMode,

    pub primary: ImuPrimaryParams,
    #[serde(skip)]
    pub derived: ImuDerivedParams,
}

impl ImuConfig {
    pub(crate) fn unit_conv(params: &mut ImuPrimaryParams) {
        //TODO: check the const value!
        let g0 = G0;

        // Gyro biases (deg/hour, converted to rad/sec; body axes)
        params.b_g *= PI / 180.0 / 3600.0;

        // Accelerometer biases
        // (milli-g, converted to m/s^2; body axes)
        params.b_a *= g0 * 1e-3;

        // Gyro and accelerometer cross coupling errors (ppm, converted to unitless; body axes)
        params.M_g = params.M_g.transpose() * 1e-6;
        params.M_a = params.M_a.transpose() * 1e-6;
        // Gyro and accelerometer scale factor (ppm, converted to unitless; body axes)
        params.s_g *= 1e-6;
        params.s_a *= 1e-6;

        // Gyro ARW
        // (deg per root hour, converted to rad s^-0.5)
        params.arw = params.arw.map(|x| x.to_radians()) / 60.0;
        // Accelerometer VRW
        // (milli-g per root Hz, converted to m s^-1.5)
        params.vrw *= g0 * 1e-3;
    }

    pub(crate) fn compute_derived_matrices(
        primary: &ImuPrimaryParams,
        err_mode: &ErrMode,
    ) -> Result<ImuDerivedParams, Box<dyn Error>> {
        let eye3 = Matrix3::identity();
        let S_g = eye3 + Matrix3::from_diagonal(&primary.s_g);
        let S_a = eye3 + Matrix3::from_diagonal(&primary.s_a);
        let M_g = primary.M_g;
        let M_a = primary.M_a;

        //--- The follows are pre-computed to reduce the real-time loading
        let (SM_g, SM_a) = match err_mode {
            ErrMode::Add => (S_g + M_g, S_a + M_a),
            ErrMode::Mul => (S_g * (eye3 + M_g), S_a * (eye3 + M_a)),
        };
        //--- the inversion matrices
        let (SM_g_inv, SM_a_inv) = match err_mode {
            ErrMode::Add => (
                SM_g.try_inverse().ok_or("[err] SM_g inversion failed.")?,
                SM_a.try_inverse().ok_or("[err] SM_a inversion failed.")?,
            ),
            ErrMode::Mul => (
                {
                    let sgi = Matrix3::from_diagonal(&S_g.diagonal().map(|x| 1.0 / x));
                    let mgi = (eye3 + M_g)
                        .try_inverse()
                        .ok_or("[err] (I+M_g) inversion failed.")?;
                    mgi * sgi
                },
                {
                    let sai = Matrix3::from_diagonal(&S_a.diagonal().map(|x| 1.0 / x));
                    let mai = (eye3 + M_a)
                        .try_inverse()
                        .ok_or("[err] (I+M_a) inversion failed.")?;
                    mai * sai
                },
            ),
        };
        Ok(ImuDerivedParams {
            S_g,
            S_a,
            SM_g,
            SM_a,
            SM_g_inv,
            SM_a_inv,
        })
    }

    /// This function is for user to manually assign parameters.
    /// You should give the parameters with the same units of the config file.
    pub fn update_params_user(
        &mut self,
        mut params: ImuPrimaryParams,
    ) -> Result<(), Box<dyn Error>> {
        Self::unit_conv(&mut params);
        self.primary = params;
        self.derived = Self::compute_derived_matrices(&self.primary, &self.err_mode)?;
        Ok(())
    }

    /// This function is for algorithm, such as KF and your other function in the simulator.
    pub fn update_params_algo(&mut self, params: ImuPrimaryParams) -> Result<(), Box<dyn Error>> {
        self.primary = params;
        self.derived = Self::compute_derived_matrices(&self.primary, &self.err_mode)?;
        Ok(())
    }

    /// Load the configuration file of an IMU.
    ///
    /// An example file of configuring the parameters of IMU can be found in
    /// `./user_space/usr_cfg/imu_cfg_fname.toml`.
    ///
    /// # Example
    /// ```
    /// use navitools::ImuConfig;
    ///
    /// 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");
    ///
    /// println!("Sampling rate: {}", imu_cfg.data_rate);
    ///
    /// println!("Gyroscope\nbiases: {}", imu_cfg.primary.b_g.transpose());
    /// println!("SF errors: {}", imu_cfg.primary.s_g);
    /// println!("Cross coupling errors: {}", imu_cfg.primary.M_g);
    /// println!("ARW: {}", imu_cfg.primary.arw.transpose());
    ///
    /// println!("Accelerometr\nbiases: {}", imu_cfg.primary.b_a.transpose());
    /// println!("SF errors: {}", imu_cfg.primary.s_a);
    /// println!("Cross coupling errors: {}", imu_cfg.primary.M_a);
    /// println!("VRW: {}", imu_cfg.primary.vrw.transpose());
    /// ```
    pub fn load_imu_cfg(imu_fname: &str) -> Result<Self, Box<dyn Error>> {
        let cfg_contents = fs::read_to_string(imu_fname).inspect_err(|_| {
            println!(
                "Failed to read IMU config file '{}'; make sure you have the right one.",
                imu_fname
            );
        })?;
        let mut imu_cfg: ImuConfig = toml::de::from_str(&cfg_contents)?;
        Self::unit_conv(&mut imu_cfg.primary);
        imu_cfg.derived = Self::compute_derived_matrices(&imu_cfg.primary, &imu_cfg.err_mode)?;

        Ok(imu_cfg)
    }
}
