//! Here are the functions used to convert the attitudes among different representations.
//!
//! The supported attitude representations are:
//! - CTM [^1]: A Coordsinate Transformation Matrix, used to describe a transformation from beta to alpha. [^groves_ctm]
//! - Quaternion: We use the form of `w + xi + yi +zk`.
//! - Euler angles: We use the form of [roll, pitch, yaw]<sup>T</sup> in radians,
//!   and consider only the `zyx` or `aerospace` sequence as defined in p.135 of Kuipers (1999) [^2].
//!   It can be used to describe the rotation from beta to alpha, or the attitude of alpha relative to beta. [^groves_eul]
//!
//! In the above description:
//! - alpha means the object frame, and
//! - beta means the reference frame
//!
//! [^1]: This term is extensively used in Groves' book, and I believe it is what others called a DCM (Direct Cosine Matrix).
//! [^2]: J.B. Kuipers (1999), Quaternions and Rotation Sequences. (This is a paper [available online](https://projecteuclid.org/download/pdf_1/euclid.pgiq/1433524882), not the book.)
//! [^groves_eul]: See Groves' section 2.2.1 `Euler Attitude`.
//! [^groves_ctm]: See Groves' section 2.2.2 `Coordinate Transformation Matrix`.

use nalgebra::{Matrix3, Quaternion, Vector3};

/// Convert CTM to Quaternion. [^motekew_2014]
///
/// # Example
///
/// ```
/// use nalgebra::{Matrix3, Quaternion};
/// use navitools::ctm2quat;
///
/// let tol = 1e-15;
///
/// let ctm = Matrix3::new(0.0, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, -1.0);
/// let quat = ctm2quat(&ctm);
/// let ans = Quaternion::new(0.0, 0.707106781186548, 0.707106781186548, 0.0);
///
/// assert!((quat.w - ans.w).abs() < tol);
/// assert!((quat.i - ans.i).abs() < tol);
/// assert!((quat.j - ans.j).abs() < tol);
/// assert!((quat.k - ans.k).abs() < tol);
/// ```
///
///
/// [^motekew_2014]: Kurt A. Motekew (2014), [Quaternion to DCM and Back Again](https://motoq.github.io/doc/tnotes/dcmq.pdf).
///
pub fn ctm2quat(ctm: &Matrix3<f64>) -> Quaternion<f64> {
    let kappa = 0.25;
    if 1.0 + ctm[(0, 0)] + ctm[(1, 1)] + ctm[(2, 2)] > kappa {
        let tmp = 1.0 + ctm[(0, 0)] + ctm[(1, 1)] + ctm[(2, 2)];
        let q0 = (0.25 * tmp).sqrt();
        let q1 = 0.25 * (ctm[(1, 2)] - ctm[(2, 1)]) / q0;
        let q2 = 0.25 * (ctm[(2, 0)] - ctm[(0, 2)]) / q0;
        let q3 = 0.25 * (ctm[(0, 1)] - ctm[(1, 0)]) / q0;
        Quaternion::new(q0, q1, q2, q3)
    } else if 1.0 + ctm[(0, 0)] - ctm[(1, 1)] - ctm[(2, 2)] > kappa {
        let tmp = 1.0 + ctm[(0, 0)] - ctm[(1, 1)] - ctm[(2, 2)];
        let q1 = (0.25 * tmp).sqrt();
        let q0 = 0.25 * (ctm[(1, 2)] - ctm[(2, 1)]) / q1;
        let q2 = 0.25 * (ctm[(0, 1)] + ctm[(1, 0)]) / q1;
        let q3 = 0.25 * (ctm[(2, 0)] + ctm[(0, 2)]) / q1;
        Quaternion::new(q0, q1, q2, q3)
    } else if 1.0 - ctm[(0, 0)] + ctm[(1, 1)] - ctm[(2, 2)] > kappa {
        let tmp = 1.0 - ctm[(0, 0)] + ctm[(1, 1)] - ctm[(2, 2)];
        let q2 = (0.25 * tmp).sqrt();
        let q0 = 0.25 * (ctm[(2, 0)] - ctm[(0, 2)]) / q2;
        let q1 = 0.25 * (ctm[(0, 1)] + ctm[(1, 0)]) / q2;
        let q3 = 0.25 * (ctm[(1, 2)] + ctm[(2, 1)]) / q2;
        Quaternion::new(q0, q1, q2, q3)
    } else if 1.0 - ctm[(0, 0)] - ctm[(1, 1)] + ctm[(2, 2)] > kappa {
        let tmp = 1.0 - ctm[(0, 0)] - ctm[(1, 1)] + ctm[(2, 2)];
        let q3 = (0.25 * tmp).sqrt();
        let q0 = 0.25 * (ctm[(0, 1)] - ctm[(1, 0)]) / q3;
        let q1 = 0.25 * (ctm[(2, 0)] + ctm[(0, 2)]) / q3;
        let q2 = 0.25 * (ctm[(1, 2)] + ctm[(2, 1)]) / q3;
        Quaternion::new(q0, q1, q2, q3)
    } else {
        panic!("[err] something wrong when converting CTM to quat");
    }
}

/// Convert CTM to Quaternion. [^1]
///
/// NOTE: Do NOT use this function. It's incomplete.
/// This is left in the code base as a reminder.
///
/// [^1]: Based on Groves' Eq.(2.35) and (2.36).
///
fn _ctm2quat(c: Matrix3<f64>) -> Quaternion<f64> {
    //TODO: do we need to perform renormalization on CTM before the conversion?
    let mut q0;
    if 1.0 + c.trace() < 0.0 {
        q0 = 0.0;
    } else {
        q0 = 0.5 * (1.0 + c[(0, 0)] + c[(1, 1)] + c[(2, 2)]).sqrt();
    }
    let q1;
    let q2;
    let q3;

    let tol = 1.0e-12;
    if q0 < tol {
        q1 = 0.5 * (1.0 + c[(0, 0)] - c[(1, 1)] - c[(2, 2)]).sqrt();
        q0 = 0.25 * (c[(1, 2)] - c[(2, 1)]) / q1;
        q2 = 0.25 * (c[(1, 0)] + c[(0, 1)]) / q1;
        q3 = 0.25 * (c[(2, 0)] + c[(0, 2)]) / q1;
        //q0 = 0.25 * (c[(2, 1)] - c[(1, 2)]) / q1;
        //q2 = 0.25 * (c[(0, 1)] + c[(1, 0)]) / q1;
        //q3 = 0.25 * (c[(0, 2)] + c[(2, 0)]) / q1;
    } else {
        q1 = 0.25 * (c[(1, 2)] - c[(2, 1)]) / q0;
        q2 = 0.25 * (c[(2, 0)] - c[(0, 2)]) / q0;
        q3 = 0.25 * (c[(0, 1)] - c[(1, 0)]) / q0;
    }
    Quaternion::new(q0, q1, q2, q3)
}

/// Convert a Quaternion to a CTM. [^1][^2]
///
/// # Example
///
/// ```
/// use nalgebra::{Matrix3, Quaternion};
/// use navitools::quat2ctm;
///
/// let quat = Quaternion::new(1.0, 0.0, 1.0, 0.0);
/// let ctm = quat2ctm(&quat);
/// let ctm_ans = Matrix3::new(0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0);
///
/// let tol = 1e-15;
/// for i in 0..3 {
///     for j in 0..3 {
///         assert!(
///             (ctm[(i, j)] - ctm_ans[(i, j)]).abs() < tol,
///             "Matrices are not equal at position ({}, {}): {} != {}",
///             i, j, ctm[(i, j)], ctm_ans[(i, j)]
///         );
///     }
/// }
/// ```
///
/// [^1]: Modified from Eq.(4) in [Motekew (2014)](https://motoq.github.io/doc/tnotes/dcmq.pdf).
/// You may also check p.168 of [Kuipers (1999)](
/// https://press.princeton.edu/books/paperback/9780691102986/quaternions-and-rotation-sequences
/// ) if it's available to you.
/// [^2]: Check [Euler-Rodrigues formula](https://en.wikipedia.org/wiki/Euler%E2%80%93Rodrigues_formula) (transposed).
///
pub fn quat2ctm(q: &Quaternion<f64>) -> Matrix3<f64> {
    let qn = q.norm();
    let (w, x, y, z) = (q.w / qn, q.i / qn, q.j / qn, q.k / qn);
    Matrix3::new(
        2.0 * (w * w + x * x) - 1.0,
        2.0 * (x * y + w * z),
        2.0 * (x * z - w * y),
        2.0 * (x * y - w * z),
        2.0 * (w * w + y * y) - 1.0,
        2.0 * (y * z + w * x),
        2.0 * (x * z + w * y),
        2.0 * (y * z - w * x),
        2.0 * (w * w + z * z) - 1.0,
    )
}

/// Converts a CTM to the corresponding set of Euler angles. [^1]
///
/// The `ctm` describing transformation from beta to alpha,
/// and the `eul` describing rotation from beta to alpha in the order of roll, pitch, yaw (rad).
///
/// # Example
/// TODO...
///
/// [^1]: Groves' Eqn. (2.23)
///
pub fn ctm2eul(ctm: &Matrix3<f64>) -> Vector3<f64> {
    Vector3::new(
        ctm[(1, 2)].atan2(ctm[(2, 2)]), // roll
        -ctm[(0, 2)].asin(),            // pitch
        ctm[(0, 1)].atan2(ctm[(0, 0)]), // yaw
    )
}

/// Converts a CTM to the corresponding set of Euler angles with small angle approximation. [^1]
///
/// # Example
/// TODO...
///
/// [^1]: Groves' Eqn. (2.26)
///
pub fn ctm2eul_approx(ctm: &Matrix3<f64>) -> Vector3<f64> {
    Vector3::new(
        ctm[(1, 2)],  // roll
        -ctm[(0, 2)], // pitch
        ctm[(0, 1)],  // yaw
    )
}

/// Converts a set of Euler angles to the corresponding CTM. [^1]
///
/// The `ctm` describing transformation from beta to alpha,
/// and the `eul` describing rotation from beta to alpha in the order of roll, pitch, yaw (rad).
///
/// # Example
/// TODO...
///
/// [^1]: Groves' Eqn. (2.22)
///
pub fn eul2ctm(eul: &Vector3<f64>) -> Matrix3<f64> {
    // Precalculate sines and cosines of the Euler angles
    let sin_phi = eul[0].sin();
    let cos_phi = eul[0].cos();
    let sin_theta = eul[1].sin();
    let cos_theta = eul[1].cos();
    let sin_psi = eul[2].sin();
    let cos_psi = eul[2].cos();

    Matrix3::new(
        cos_theta * cos_psi,
        cos_theta * sin_psi,
        -sin_theta,
        -cos_phi * sin_psi + sin_phi * sin_theta * cos_psi,
        cos_phi * cos_psi + sin_phi * sin_theta * sin_psi,
        sin_phi * cos_theta,
        sin_phi * sin_psi + cos_phi * sin_theta * cos_psi,
        -sin_phi * cos_psi + cos_phi * sin_theta * sin_psi,
        cos_phi * cos_theta,
    )
}

/// Convert a Quaternion to the corresponding set of Euler angles. [^1]
///
/// Equivalent to MATLAB's default output with a reversed angle sequence.
/// My reasoning here [^2] is that the `ZYX` in MATLAB make it output
/// in the order of [yaw (Z), pitch (Y), roll (X)], but in `navitools`
/// the output of `eul` always in the order of [roll, pitch, yaw].
/// This results in a reversed order compared to MATLAB's output.
///
/// # Example
/// ```
/// use nalgebra::Quaternion;
/// use navitools::quat2eul;
///
/// let tol = 1e-15;
/// let q = Quaternion::new(0.7071, 0.7071, 0.0, 0.0);
/// let eul = quat2eul(&q);
/// let ans = [1.570796326794897, 0.0, 0.0];
///
/// assert!((eul.x - ans[0]).abs() < tol);
/// assert!((eul.y - ans[1]).abs() < tol);
/// assert!((eul.z - ans[2]).abs() < tol);
/// ```
///
/// [^1]: Groves' book Eqn. (2.37).
/// [^2]: The Euler angles are really confusing. My interpretation here might be wrong.
///
pub fn quat2eul(q: &Quaternion<f64>) -> Vector3<f64> {
    let qn = q.norm();
    let (w, x, y, z) = (q.w / qn, q.i / qn, q.j / qn, q.k / qn);
    Vector3::new(
        (2.0 * (w * x + y * z)).atan2(1.0 - 2.0 * (x * x + y * y)), // roll; phi
        (2.0 * (w * y - x * z)).asin(),                             // pitch; theta
        (2.0 * (w * z + x * y)).atan2(1.0 - 2.0 * (y * y + z * z)), // yaw; psi
    )
}

/// Convert a set of Euler angles to a Quaternion. [^1]
///
/// The input Euler angles should be in the order of roll, pitch, and yaw (rad).
///
/// # Example
///
/// ```
/// use nalgebra::Vector3;
/// use navitools::eul2quat;
///
/// let tol = 1e-15;
/// let eul = Vector3::<f64>::new(20.0, -30.0, 45.0).map(|x| x.to_radians());
/// let q = eul2quat(&eul);
/// let ans = [
///     0.861642437457362,
///     0.252504510495226,
///     -0.171296910377507,
///     0.405550429228256,
/// ];
///
/// assert!((q.w - ans[0]).abs() < tol);
/// assert!((q.i - ans[1]).abs() < tol);
/// assert!((q.j - ans[2]).abs() < tol);
/// assert!((q.k - ans[3]).abs() < tol);
/// ```
///  [^1]: Groves' book Eqn. (2.38).
///
pub fn eul2quat(eul: &Vector3<f64>) -> Quaternion<f64> {
    let cr = (eul.x * 0.5).cos();
    let sr = (eul.x * 0.5).sin();
    let cp = (eul.y * 0.5).cos();
    let sp = (eul.y * 0.5).sin();
    let cy = (eul.z * 0.5).cos();
    let sy = (eul.z * 0.5).sin();

    let w = cr * cp * cy + sr * sp * sy;
    let x = sr * cp * cy - cr * sp * sy;
    let y = cr * sp * cy + sr * cp * sy;
    let z = cr * cp * sy - sr * sp * cy;
    Quaternion::new(w, x, y, z)
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn test_ctm2quat() {
        let tol = 1e-15;

        let ctm = Matrix3::new(
            0.4330, 0.2500, -0.8660, 0.1768, 0.9186, 0.3536, 0.8839, -0.3062, 0.3536,
        );
        let quat = ctm2quat(&ctm);
        let ans = Quaternion::new(
            0.822374610503024,
            0.200577690377752,
            0.531965596229204,
            0.0222526325184169,
        );
        assert!((quat.w - ans.w).abs() < tol);
        assert!((quat.i - ans.i).abs() < tol);
        assert!((quat.j - ans.j).abs() < tol);
        assert!((quat.k - ans.k).abs() < tol);
    }

    #[test]
    fn test_quat2ctm() {
        #[rustfmt::skip]
        fn check_elements(ctm: Matrix3<f64>, ctm_ans: Matrix3<f64>) {
            let tol = 1e-15;
            for i in 0..3 {
                for j in 0..3 {
                    assert!(
                        (ctm[(i, j)] - ctm_ans[(i, j)]).abs() < tol,
                        "Matrices are not equal at position ({}, {}): {} != {}",
                        i, j, ctm[(i, j)], ctm_ans[(i, j)]
                    );
                }
            }
        }

        let quat = Quaternion::new(1.0, 0.5, 0.3, 0.1);
        let ctm = quat2ctm(&quat);
        let ctm_ans = Matrix3::new(
            0.851851851851852,
            0.370370370370370,
            -0.370370370370370,
            0.0740740740740740,
            0.614814814814815,
            0.785185185185185,
            0.518518518518518,
            -0.696296296296296,
            0.496296296296296,
        );
        check_elements(ctm, ctm_ans);
    }

    #[test]
    fn test_quat2eul() {
        let tol = 1e-15;

        let q = Quaternion::new(-0.381, 0.673, 0.525, -0.354);
        let eul = quat2eul(&q);
        let ans = [-2.048644224176739, 0.076582973677212, 1.371560496488055];
        assert!((eul.x - ans[0]).abs() < tol);
        assert!((eul.y - ans[1]).abs() < tol);
        assert!((eul.z - ans[2]).abs() < tol);
    }

    #[test]
    fn test_eul2quat() {
        let tol = 1e-15;

        let eul = Vector3::<f64>::new(-20.0, 35.0, -10.0).map(|x| x.to_radians());
        let q = eul2quat(&eul);
        let ans = [
            0.940204814404249,
            -0.139170934562037,
            0.309444478562957,
            -0.029840788044221,
        ];
        assert!((q.w - ans[0]).abs() < tol);
        assert!((q.i - ans[1]).abs() < tol);
        assert!((q.j - ans[2]).abs() < tol);
        assert!((q.k - ans[3]).abs() < tol);
    }
}
