sidereon-core 0.15.0

//! Navigation state and attitude conversion helpers.

use crate::astro::math::mat3::{inline_rxr, mul_vec3, Mat3};
use crate::astro::math::vec3::{cross3, dot3, norm3, scale3, sub3};

use super::{validate_finite, validate_mat3, validate_vec3, InertialError};

/// Navigation state used by the ECEF strapdown mechanizer.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct NavState {
    /// State time in seconds since J2000 on the caller's GNSS time scale.
    pub t_j2000_s: f64,
    /// IMU position in ECEF meters.
    pub position_ecef_m: [f64; 3],
    /// IMU velocity in ECEF meters per second.
    pub velocity_ecef_mps: [f64; 3],
    /// Direction cosine matrix from body axes to ECEF axes.
    pub attitude_body_to_ecef: Mat3,
    /// Closed-loop accelerometer bias estimate in m/s^2.
    pub accel_bias_mps2: [f64; 3],
    /// Closed-loop gyroscope bias estimate in rad/s.
    pub gyro_bias_rps: [f64; 3],
}

impl NavState {
    /// Build a navigation state with zero IMU biases.
    pub fn new(
        t_j2000_s: f64,
        position_ecef_m: [f64; 3],
        velocity_ecef_mps: [f64; 3],
        attitude_body_to_ecef: Mat3,
    ) -> Result<Self, InertialError> {
        let state = Self {
            t_j2000_s,
            position_ecef_m,
            velocity_ecef_mps,
            attitude_body_to_ecef,
            accel_bias_mps2: [0.0; 3],
            gyro_bias_rps: [0.0; 3],
        };
        state.validate()?;
        Ok(state)
    }

    /// Return this state with closed-loop IMU bias estimates.
    pub fn with_biases(
        mut self,
        accel_bias_mps2: [f64; 3],
        gyro_bias_rps: [f64; 3],
    ) -> Result<Self, InertialError> {
        self.accel_bias_mps2 = accel_bias_mps2;
        self.gyro_bias_rps = gyro_bias_rps;
        self.validate()?;
        Ok(self)
    }

    /// Validate finite state fields and a nondegenerate attitude matrix.
    pub fn validate(&self) -> Result<(), InertialError> {
        validate_finite(self.t_j2000_s, "t_j2000_s")?;
        validate_vec3(self.position_ecef_m, "position_ecef_m")?;
        validate_vec3(self.velocity_ecef_mps, "velocity_ecef_mps")?;
        validate_mat3(&self.attitude_body_to_ecef, "attitude_body_to_ecef")?;
        validate_vec3(self.accel_bias_mps2, "accel_bias_mps2")?;
        validate_vec3(self.gyro_bias_rps, "gyro_bias_rps")?;
        reorthonormalize_dcm(&self.attitude_body_to_ecef)?;
        Ok(())
    }

    /// Body-to-ECEF attitude as a unit quaternion.
    pub fn attitude_quaternion_body_to_ecef(&self) -> Result<AttitudeQuaternion, InertialError> {
        dcm_to_quaternion(&self.attitude_body_to_ecef)
    }

    /// Body-to-ECEF attitude as yaw, pitch, and roll angles in radians.
    pub fn attitude_yaw_pitch_roll_rad(&self) -> [f64; 3] {
        attitude_yaw_pitch_roll_rad(&self.attitude_body_to_ecef)
    }
}

/// Unit quaternion using scalar-first `(w, x, y, z)` storage.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct AttitudeQuaternion {
    /// Scalar component.
    pub w: f64,
    /// X vector component.
    pub x: f64,
    /// Y vector component.
    pub y: f64,
    /// Z vector component.
    pub z: f64,
}

impl AttitudeQuaternion {
    /// Build and normalize a quaternion.
    pub fn new(w: f64, x: f64, y: f64, z: f64) -> Result<Self, InertialError> {
        validate_finite(w, "quaternion.w")?;
        validate_finite(x, "quaternion.x")?;
        validate_finite(y, "quaternion.y")?;
        validate_finite(z, "quaternion.z")?;
        let norm = (w * w + x * x + y * y + z * z).sqrt();
        if norm <= 0.0 {
            return Err(InertialError::DegenerateAttitude);
        }
        Ok(Self {
            w: w / norm,
            x: x / norm,
            y: y / norm,
            z: z / norm,
        })
    }
}

/// Convert a body-to-ECEF DCM to a unit quaternion.
pub fn dcm_to_quaternion(dcm: &Mat3) -> Result<AttitudeQuaternion, InertialError> {
    validate_mat3(dcm, "dcm")?;
    let dcm = reorthonormalize_dcm(dcm)?;
    let trace = dcm[0][0] + dcm[1][1] + dcm[2][2];
    if trace > 0.0 {
        let s = (trace + 1.0).sqrt() * 2.0;
        AttitudeQuaternion::new(
            0.25 * s,
            (dcm[2][1] - dcm[1][2]) / s,
            (dcm[0][2] - dcm[2][0]) / s,
            (dcm[1][0] - dcm[0][1]) / s,
        )
    } else if dcm[0][0] > dcm[1][1] && dcm[0][0] > dcm[2][2] {
        let s = (1.0 + dcm[0][0] - dcm[1][1] - dcm[2][2]).sqrt() * 2.0;
        AttitudeQuaternion::new(
            (dcm[2][1] - dcm[1][2]) / s,
            0.25 * s,
            (dcm[0][1] + dcm[1][0]) / s,
            (dcm[0][2] + dcm[2][0]) / s,
        )
    } else if dcm[1][1] > dcm[2][2] {
        let s = (1.0 + dcm[1][1] - dcm[0][0] - dcm[2][2]).sqrt() * 2.0;
        AttitudeQuaternion::new(
            (dcm[0][2] - dcm[2][0]) / s,
            (dcm[0][1] + dcm[1][0]) / s,
            0.25 * s,
            (dcm[1][2] + dcm[2][1]) / s,
        )
    } else {
        let s = (1.0 + dcm[2][2] - dcm[0][0] - dcm[1][1]).sqrt() * 2.0;
        AttitudeQuaternion::new(
            (dcm[1][0] - dcm[0][1]) / s,
            (dcm[0][2] + dcm[2][0]) / s,
            (dcm[1][2] + dcm[2][1]) / s,
            0.25 * s,
        )
    }
}

/// Convert a unit quaternion to a body-to-ECEF DCM.
pub fn quaternion_to_dcm(quaternion: AttitudeQuaternion) -> Mat3 {
    let w = quaternion.w;
    let x = quaternion.x;
    let y = quaternion.y;
    let z = quaternion.z;
    [
        [
            1.0 - 2.0 * (y * y + z * z),
            2.0 * (x * y - z * w),
            2.0 * (x * z + y * w),
        ],
        [
            2.0 * (x * y + z * w),
            1.0 - 2.0 * (x * x + z * z),
            2.0 * (y * z - x * w),
        ],
        [
            2.0 * (x * z - y * w),
            2.0 * (y * z + x * w),
            1.0 - 2.0 * (x * x + y * y),
        ],
    ]
}

/// Convert a body-to-ECEF DCM to yaw, pitch, roll radians using a ZYX sequence.
pub fn attitude_yaw_pitch_roll_rad(dcm: &Mat3) -> [f64; 3] {
    let pitch = (-dcm[2][0]).asin();
    let roll = dcm[2][1].atan2(dcm[2][2]);
    let yaw = dcm[1][0].atan2(dcm[0][0]);
    [yaw, pitch, roll]
}

/// Re-orthonormalize a direction-cosine matrix into a right-handed frame.
pub fn reorthonormalize_dcm(dcm: &Mat3) -> Result<Mat3, InertialError> {
    validate_mat3(dcm, "dcm")?;
    let c0 = [dcm[0][0], dcm[1][0], dcm[2][0]];
    let c1_raw = [dcm[0][1], dcm[1][1], dcm[2][1]];
    let e0 = unit(c0)?;
    let c1_orth = sub3(c1_raw, scale3(e0, dot3(e0, c1_raw)));
    let e1 = unit(c1_orth)?;
    let e2 = unit(cross3(e0, e1))?;
    let e1 = unit(cross3(e2, e0))?;
    Ok([
        [e0[0], e1[0], e2[0]],
        [e0[1], e1[1], e2[1]],
        [e0[2], e1[2], e2[2]],
    ])
}

pub(crate) fn mat3_identity() -> Mat3 {
    [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
}

pub(crate) fn mat3_add(a: &Mat3, b: &Mat3) -> Mat3 {
    let mut out = [[0.0; 3]; 3];
    for i in 0..3 {
        for j in 0..3 {
            out[i][j] = a[i][j] + b[i][j];
        }
    }
    out
}

pub(crate) fn mat3_scale(a: &Mat3, scale: f64) -> Mat3 {
    let mut out = [[0.0; 3]; 3];
    for i in 0..3 {
        for j in 0..3 {
            out[i][j] = a[i][j] * scale;
        }
    }
    out
}

pub(crate) fn mat3_mul_vec(a: &Mat3, v: [f64; 3]) -> [f64; 3] {
    mul_vec3(a, v)
}

pub(crate) fn mat3_mul(a: &Mat3, b: &Mat3) -> Mat3 {
    inline_rxr(a, b)
}

pub(crate) fn skew(v: [f64; 3]) -> Mat3 {
    [[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]]
}

fn unit(v: [f64; 3]) -> Result<[f64; 3], InertialError> {
    let norm = norm3(v);
    if norm > 0.0 {
        Ok(scale3(v, 1.0 / norm))
    } else {
        Err(InertialError::DegenerateAttitude)
    }
}

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

    #[test]
    fn quaternion_roundtrip_preserves_right_angle_yaw() {
        let half = core::f64::consts::FRAC_1_SQRT_2;
        let q = AttitudeQuaternion::new(half, 0.0, 0.0, half).expect("quaternion");
        let dcm = quaternion_to_dcm(q);
        let ypr = attitude_yaw_pitch_roll_rad(&dcm);
        assert!((ypr[0] - core::f64::consts::FRAC_PI_2).abs() <= 4.0e-16);
        assert!(ypr[1].abs() <= 1.0e-16);
        assert!(ypr[2].abs() <= 1.0e-16);

        let back = dcm_to_quaternion(&dcm).expect("back");
        assert!((back.w - half).abs() <= 2.0e-16);
        assert!((back.z - half).abs() <= 2.0e-16);
    }
}