use crate::{
    ImuConfig,
    constants::{G0, OMEGA_E},
    utils::msg_func::warn_msg,
};
use nalgebra::Vector3;
use serde::Deserialize;
use std::{error::Error, f64::consts::PI, fs};

#[derive(Debug, Deserialize)]
pub struct IaConfig {
    pub delta_ribi: Vector3<f64>,
    pub delta_vibi: Vector3<f64>,
    pub delta_eul_ibi: Vector3<f64>,
    pub lat: f64,
}

impl IaConfig {
    /// Convert (deg, arc.sec, arc.sec) to radians of `delta_eul_ibi`
    fn unit_conv(&mut self) {
        //self.delta_eul_ibi = self.delta_eul_ibi.map(f64::to_radians);
        self.delta_eul_ibi[0] *= PI / 180.0;
        self.delta_eul_ibi[1] *= PI / 180.0 / 3600.0;
        self.delta_eul_ibi[2] *= PI / 180.0 / 3600.0;
        self.lat *= PI / 180.0;
    }

    /// # Example
    /// ```
    /// use navitools::{ImuConfig, IaConfig};
    ///
    /// // If you only want to use IA config from the input file:
    /// let ia_cfg_fname = "./user_space/usr_cfg/ia_cfg_dummy.toml";
    /// let ia_cfg = IaConfig::load_ia_cfg(ia_cfg_fname, None).expect("load IA cfg failed");
    ///
    /// // If you want to overwrite the attitude errors using IMU config, add the following:
    /// 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");
    /// let ia_cfg = IaConfig::load_ia_cfg(ia_cfg_fname, Some(&imu_cfg));
    /// ```
    ///
    /// # Warning
    /// The equations used to estimate the initial alignment errors are rudimentary.
    /// Use them with caution.
    ///
    pub fn load_ia_cfg(
        //cfg: &SimConfig,
        ia_fname: &str,
        imu_cfg: Option<&ImuConfig>,
    ) -> Result<Self, Box<dyn Error>> {
        let cfg_contents = fs::read_to_string(ia_fname).inspect_err(|_| {
            println!(
                "Failed to read IA config file '{}'; make sure you have the right one.",
                ia_fname
            );
        })?;
        let mut ia_cfg: IaConfig = toml::de::from_str(&cfg_contents)?;
        // unit conversion
        ia_cfg.unit_conv();

        if let Some(imu) = imu_cfg {
            //FIX: should transform from IMU body axis to NED?
            // Use the largest bias value.
            //let ba = imu.b_a[imu.b_a.iamax()];
            //let bgE = imu.b_g[imu.b_g.iamax()];
            //let bgN = bgE;

            let baE = imu.primary.b_a[1];
            let baN = imu.primary.b_a[0];
            let bgE = imu.primary.b_g[1];
            let bgN = imu.primary.b_g[0];
            let lat = ia_cfg.lat.to_radians();

            // azi = atan(bgE/(omega_e*cosL + bgN)
            // lvl = atan(ba/g)
            //let lvl = (ba / G0).atan();
            let roll = (baE / G0).atan();
            let pitch = (baN / G0).atan();
            let azi = (bgE / (OMEGA_E * lat.cos() + bgN)).atan();

            ia_cfg = IaConfig {
                delta_ribi: ia_cfg.delta_ribi,
                delta_vibi: ia_cfg.delta_vibi,
                //delta_eul_ibi: Vector3::new(azi, lvl, lvl),
                delta_eul_ibi: Vector3::new(azi, pitch, roll), //FIX: should be roll, pitch, yaw?
                lat: ia_cfg.lat,
            };
            warn_msg("IA cfg has been modified with the loaded IMU cfg.");
        }
        Ok(ia_cfg)
    }
}
