oxiproj-core 0.1.2

//! Uncertainty propagation for coordinate transforms.
//! Computes output covariance Σ_out = J · Σ_in · Jᵀ given an input covariance
//! matrix and a 2×2 Jacobian of the transformation.

/// A 2×2 symmetric covariance matrix for 2D coordinates.
/// Stored as upper triangle: `var_x`, `var_y`, `cov_xy`.
#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct CovMatrix2x2 {
    /// Variance in X (σ_x²)
    pub var_x: f64,
    /// Variance in Y (σ_y²)
    pub var_y: f64,
    /// Covariance between X and Y (σ_xy)
    pub cov_xy: f64,
}

impl CovMatrix2x2 {
    /// Create a diagonal covariance matrix (uncorrelated X and Y).
    pub fn diagonal(var_x: f64, var_y: f64) -> Self {
        Self {
            var_x,
            var_y,
            cov_xy: 0.0,
        }
    }

    /// Create a full covariance matrix.
    pub fn new(var_x: f64, var_y: f64, cov_xy: f64) -> Self {
        Self {
            var_x,
            var_y,
            cov_xy,
        }
    }

    /// Standard deviation in X.
    pub fn std_x(&self) -> f64 {
        self.var_x.max(0.0).sqrt()
    }

    /// Standard deviation in Y.
    pub fn std_y(&self) -> f64 {
        self.var_y.max(0.0).sqrt()
    }

    /// Propagate through a linear map with 2×2 Jacobian J.
    /// Σ_out = J · Σ_in · Jᵀ
    /// J = [[j00, j01], [j10, j11]]  (row-major: j00=∂x/∂u, j01=∂x/∂v, j10=∂y/∂u, j11=∂y/∂v)
    pub fn propagate(&self, j00: f64, j01: f64, j10: f64, j11: f64) -> Self {
        // A = J · Σ_in:
        // A[0][0] = j00*var_x + j01*cov_xy
        // A[0][1] = j00*cov_xy + j01*var_y
        // A[1][0] = j10*var_x + j11*cov_xy
        // A[1][1] = j10*cov_xy + j11*var_y
        let a00 = j00 * self.var_x + j01 * self.cov_xy;
        let a01 = j00 * self.cov_xy + j01 * self.var_y;
        let a10 = j10 * self.var_x + j11 * self.cov_xy;
        let a11 = j10 * self.cov_xy + j11 * self.var_y;

        // Σ_out = A · Jᵀ:
        // var_x_out  = A[0][0]*j00 + A[0][1]*j01
        // cov_xy_out = A[0][0]*j10 + A[0][1]*j11
        // var_y_out  = A[1][0]*j10 + A[1][1]*j11
        let var_x_out = a00 * j00 + a01 * j01;
        let cov_xy_out = a00 * j10 + a01 * j11;
        let var_y_out = a10 * j10 + a11 * j11;

        Self {
            var_x: var_x_out,
            var_y: var_y_out,
            cov_xy: cov_xy_out,
        }
    }

    /// Propagate through the Jacobian from `HessianFactors`.
    /// Uses the first-order partial derivatives.
    pub fn propagate_through_jacobian(
        &self,
        dxdlam: f64,
        dxdphi: f64,
        dydlam: f64,
        dydphi: f64,
    ) -> Self {
        self.propagate(dxdlam, dxdphi, dydlam, dydphi)
    }

    /// Check if the covariance matrix is positive semi-definite.
    pub fn is_psd(&self) -> bool {
        // var_x >= 0, var_y >= 0, det = var_x*var_y - cov_xy^2 >= 0
        self.var_x >= 0.0
            && self.var_y >= 0.0
            && self.var_x * self.var_y >= self.cov_xy * self.cov_xy
    }

    /// Determinant of the covariance matrix.
    pub fn det(&self) -> f64 {
        self.var_x * self.var_y - self.cov_xy * self.cov_xy
    }

    /// Propagate through a projection's exact Jacobian from HessianFactors.
    pub fn propagate_through_hessian_factors(
        &self,
        factors: &crate::hessian_factors::HessianFactors,
    ) -> Self {
        self.propagate(
            factors.dxdlam,
            factors.dxdphi,
            factors.dydlam,
            factors.dydphi,
        )
    }
}

impl core::fmt::Display for CovMatrix2x2 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(
            f,
            "Cov([[{:.6e}, {:.6e}], [{:.6e}, {:.6e}]])",
            self.var_x, self.cov_xy, self.cov_xy, self.var_y
        )
    }
}

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

    #[test]
    fn identity_jacobian_preserves_covariance() {
        // J = I → Σ_out = Σ_in
        let sigma = CovMatrix2x2::new(4.0, 9.0, 1.5);
        let out = sigma.propagate(1.0, 0.0, 0.0, 1.0);
        assert!((out.var_x - 4.0).abs() < 1e-12);
        assert!((out.var_y - 9.0).abs() < 1e-12);
        assert!((out.cov_xy - 1.5).abs() < 1e-12);
    }

    #[test]
    fn scale_jacobian_scales_variance() {
        // J = [[s, 0], [0, s]] → var_x_out = s²*var_x
        let sigma = CovMatrix2x2::diagonal(1.0, 1.0);
        let s = 2.5;
        let out = sigma.propagate(s, 0.0, 0.0, s);
        assert!((out.var_x - s * s).abs() < 1e-12);
        assert!((out.var_y - s * s).abs() < 1e-12);
        assert!(out.cov_xy.abs() < 1e-12);
    }

    #[test]
    fn rotation_jacobian_preserves_trace() {
        // A pure rotation J = [[c,-s],[s,c]] applied to σ²·I
        // should keep trace = 2σ² (rotation preserves total variance)
        let sigma = CovMatrix2x2::diagonal(4.0, 4.0);
        let angle = 0.7_f64;
        let c = angle.cos();
        let s = angle.sin();
        let out = sigma.propagate(c, -s, s, c);
        let trace = out.var_x + out.var_y;
        assert!((trace - 8.0).abs() < 1e-10, "trace={}", trace);
    }

    #[test]
    fn psd_check() {
        assert!(CovMatrix2x2::diagonal(1.0, 1.0).is_psd());
        assert!(CovMatrix2x2::new(4.0, 1.0, 1.0).is_psd()); // det=3 > 0
                                                            // Not PSD: cov_xy too large
        assert!(!CovMatrix2x2::new(1.0, 1.0, 2.0).is_psd()); // det=1-4=-3 < 0
    }

    #[test]
    fn std_deviations() {
        let cov = CovMatrix2x2::diagonal(9.0, 16.0);
        assert!((cov.std_x() - 3.0).abs() < 1e-12);
        assert!((cov.std_y() - 4.0).abs() < 1e-12);
    }

    #[test]
    fn propagate_through_jacobian_same_as_propagate() {
        let sigma = CovMatrix2x2::new(2.0, 3.0, 0.5);
        let (j00, j01, j10, j11) = (1.5, 0.3, 0.2, 2.0);
        let a = sigma.propagate(j00, j01, j10, j11);
        let b = sigma.propagate_through_jacobian(j00, j01, j10, j11);
        assert_eq!(a, b);
    }

    #[test]
    fn mercator_covariance_propagation() {
        // Mercator at equator: J = [[1/cos(0), 0], [0, 1/cos(0)]] = [[1,0],[0,1]]
        // (simplified: in reality J depends on ellipsoid and phi)
        let sigma_input = CovMatrix2x2::diagonal(1e-6, 1e-6); // 1 microradian² each
        let out = sigma_input.propagate(1.0, 0.0, 0.0, 1.0);
        assert!(out.is_psd());
        assert!((out.var_x - 1e-6).abs() < 1e-15);
    }

    #[test]
    fn propagate_preserves_psd_and_matches_independent_product() {
        // Deterministic LCG sweep. For a random Jacobian J and a random PSD
        // input Σ (built as L·Lᵀ so det ≥ 0), Σ_out = J·Σ·Jᵀ must:
        //  (a) equal an INDEPENDENT triple product computed with the other
        //      association B = Σ·Jᵀ then J·B (the code multiplies J·Σ first);
        //  (b) be symmetric (cov reachable from either off-diagonal entry);
        //  (c) stay positive semi-definite — congruence preserves PSD, and
        //      det(Σ_out) = det(J)²·det(Σ) ≥ 0.
        let mut state: u64 = 0x00C0_FFEE_1234_5678;
        let mut next = || {
            state = state
                .wrapping_mul(6364136223846793005)
                .wrapping_add(1442695040888963407);
            (state >> 11) as f64 / (1u64 << 53) as f64
        };
        for _ in 0..1000 {
            let l00 = next() * 3.0 + 0.1;
            let l10 = next() * 4.0 - 2.0;
            let l11 = next() * 3.0 + 0.1;
            let var_x = l00 * l00;
            let cov_xy = l00 * l10;
            let var_y = l10 * l10 + l11 * l11;
            let sigma = CovMatrix2x2::new(var_x, var_y, cov_xy);
            assert!(sigma.is_psd(), "constructed input must be PSD");

            let (j00, j01, j10, j11) = (
                next() * 4.0 - 2.0,
                next() * 4.0 - 2.0,
                next() * 4.0 - 2.0,
                next() * 4.0 - 2.0,
            );
            let out = sigma.propagate(j00, j01, j10, j11);

            // Independent association: B = Σ·Jᵀ, then Σ_out = J·B.
            let b00 = var_x * j00 + cov_xy * j01;
            let b01 = var_x * j10 + cov_xy * j11;
            let b10 = cov_xy * j00 + var_y * j01;
            let b11 = cov_xy * j10 + var_y * j11;
            let o00 = j00 * b00 + j01 * b10;
            let o01 = j00 * b01 + j01 * b11;
            let o10 = j10 * b00 + j11 * b10;
            let o11 = j10 * b01 + j11 * b11;

            let tol = |ref_val: f64| 1e-9 * (1.0 + ref_val.abs());
            assert!((out.var_x - o00).abs() <= tol(o00), "var_x");
            assert!((out.var_y - o11).abs() <= tol(o11), "var_y");
            assert!((out.cov_xy - o01).abs() <= tol(o01), "cov from o01");
            assert!(
                (out.cov_xy - o10).abs() <= tol(o10),
                "cov from o10 (symmetry)"
            );
            assert!((o01 - o10).abs() <= tol(o01), "result must be symmetric");
            // PSD: det ≥ 0 up to a tiny relative rounding slack.
            let det = out.det();
            assert!(
                det >= -1e-6 * (1.0 + out.var_x.abs() * out.var_y.abs()),
                "Σ_out must stay PSD: det={det}"
            );
        }
    }

    #[cfg(feature = "serde")]
    #[test]
    fn cov_matrix_serde_round_trip() {
        let original = CovMatrix2x2::new(4.0, 9.0, 1.5);
        let json = serde_json::to_string(&original).expect("serialize");
        let restored: CovMatrix2x2 = serde_json::from_str(&json).expect("deserialize");
        assert_eq!(original, restored);
    }
}