astrodynamics 0.12.0

//! Deterministic small linear-algebra kernels.
//!
//! These routines keep scalar operation order explicit for parity-sensitive
//! GNSS callers. When pivot tie-breaking or accumulation order matters, the
//! variant name states the policy instead of hiding it in a local copy.

use crate::tolerances::PIVOT_EPSILON;

#[derive(Debug, Default, Clone)]
pub struct FlatLinearScratch {
    rows: Vec<f64>,
    x: Vec<f64>,
}

#[derive(Debug, Default, Clone)]
pub struct FlatNormalSolveScratch {
    a: Vec<f64>,
    b: Vec<f64>,
    x: Vec<f64>,
}

#[allow(clippy::needless_range_loop)]
pub fn solve_linear_first_tie(a: &[Vec<f64>], b: &[f64]) -> Option<Vec<f64>> {
    let n = b.len();
    let mut rows: Vec<Vec<f64>> = a
        .iter()
        .zip(b)
        .map(|(row, &bi)| {
            let mut r = row.clone();
            r.push(bi);
            r
        })
        .collect();

    for col in 0..n {
        let mut pivot_row = col;
        let mut pivot_abs = rows[col][col].abs();
        for idx in (col + 1)..n {
            let v = rows[idx][col].abs();
            if v > pivot_abs {
                pivot_abs = v;
                pivot_row = idx;
            }
        }
        if pivot_abs <= PIVOT_EPSILON {
            return None;
        }
        rows.swap(col, pivot_row);

        let pivot = rows[col].clone();
        let pivot_value = pivot[col];
        for idx in (col + 1)..n {
            let factor = rows[idx][col] / pivot_value;
            for j in 0..=n {
                rows[idx][j] -= factor * pivot[j];
            }
        }
    }

    let mut x = vec![0.0; n];
    for i in (0..n).rev() {
        let mut known = 0.0;
        for j in (i + 1)..n {
            known += rows[i][j] * x[j];
        }
        x[i] = (rows[i][n] - known) / rows[i][i];
    }
    Some(x)
}

#[allow(clippy::needless_range_loop)]
pub fn solve_linear_last_tie(mut a: Vec<Vec<f64>>, b: Vec<f64>) -> Option<Vec<f64>> {
    let n = b.len();
    for (row, bi) in a.iter_mut().zip(b) {
        row.push(bi);
    }
    for col in 0..n {
        let (pivot_row, pivot_abs) = (col..n)
            .map(|idx| (idx, a[idx][col].abs()))
            .max_by(|lhs, rhs| lhs.1.total_cmp(&rhs.1))
            .unwrap();
        if pivot_abs <= PIVOT_EPSILON {
            return None;
        }
        a.swap(col, pivot_row);
        let pivot = a[col].clone();
        let pivot_value = pivot[col];
        for row in a.iter_mut().take(n).skip(col + 1) {
            let factor = row[col] / pivot_value;
            for j in col..=n {
                row[j] -= factor * pivot[j];
            }
        }
    }
    let mut x = vec![0.0; n];
    for i in (0..n).rev() {
        let tail_sum: f64 = ((i + 1)..n).map(|j| a[i][j] * x[j]).sum();
        x[i] = (a[i][n] - tail_sum) / a[i][i];
    }
    Some(x)
}

pub fn invert_matrix_first_tie(a: &[Vec<f64>]) -> Option<Vec<Vec<f64>>> {
    let n = a.len();
    let mut columns: Vec<Vec<f64>> = Vec::with_capacity(n);
    for col in 0..n {
        let mut e = vec![0.0; n];
        e[col] = 1.0;
        columns.push(solve_linear_first_tie(a, &e)?);
    }
    Some(
        (0..n)
            .map(|i| (0..n).map(|j| columns[j][i]).collect())
            .collect(),
    )
}

pub fn invert_matrix_last_tie(a: &[Vec<f64>]) -> Option<Vec<Vec<f64>>> {
    let n = a.len();
    let mut columns = Vec::with_capacity(n);
    for col in 0..n {
        let unit = (0..n)
            .map(|idx| if idx == col { 1.0 } else { 0.0 })
            .collect();
        columns.push(solve_linear_last_tie(a.to_vec(), unit)?);
    }
    Some(transpose(&columns))
}

pub fn solve_matrix_last_tie(a: &[Vec<f64>], b: &[Vec<f64>]) -> Option<Vec<Vec<f64>>> {
    let columns = transpose(b);
    let mut solved_columns = Vec::with_capacity(columns.len());
    for col in columns {
        solved_columns.push(solve_linear_last_tie(a.to_vec(), col)?);
    }
    Some(transpose(&solved_columns))
}

pub fn normal_equations_weighted<'a, I>(rows: I, n: usize) -> (Vec<Vec<f64>>, Vec<f64>)
where
    I: IntoIterator<Item = (&'a [f64], f64, f64)>,
{
    let mut ata = vec![vec![0.0; n]; n];
    let mut aty = vec![0.0; n];
    for (row_h, row_y, row_weight) in rows {
        let h: Vec<f64> = row_h.iter().map(|v| v * row_weight).collect();
        let y = row_y * row_weight;
        for i in 0..n {
            aty[i] += h[i] * y;
            for j in 0..n {
                ata[i][j] += h[i] * h[j];
            }
        }
    }
    (ata, aty)
}

pub fn matrix_sub(a: &[Vec<f64>], b: &[Vec<f64>]) -> Vec<Vec<f64>> {
    a.iter()
        .zip(b)
        .map(|(row_a, row_b)| row_a.iter().zip(row_b).map(|(x, y)| x - y).collect())
        .collect()
}

pub fn matmul(a: &[Vec<f64>], b: &[Vec<f64>]) -> Vec<Vec<f64>> {
    let b_t = transpose(b);
    a.iter()
        .map(|row| {
            b_t.iter()
                .map(|col| row.iter().zip(col).fold(0.0, |acc, (x, y)| acc + x * y))
                .collect()
        })
        .collect()
}

pub fn transpose(matrix: &[Vec<f64>]) -> Vec<Vec<f64>> {
    if matrix.is_empty() {
        return Vec::new();
    }
    (0..matrix[0].len())
        .map(|col| matrix.iter().map(|row| row[col]).collect())
        .collect()
}

pub fn invert_flat_first_tie_into(
    a: &[f64],
    n: usize,
    out: &mut Vec<f64>,
    scratch: &mut FlatLinearScratch,
) -> Option<()> {
    out.resize(n * n, 0.0);
    scratch.rows.resize(n * (n + 1), 0.0);
    scratch.x.resize(n, 0.0);

    for col in 0..n {
        for i in 0..n {
            let src = i * n;
            let dst = i * (n + 1);
            scratch.rows[dst..(dst + n)].copy_from_slice(&a[src..(src + n)]);
            scratch.rows[dst + n] = if i == col { 1.0 } else { 0.0 };
        }
        solve_augmented_flat_first_tie_in_place(&mut scratch.rows, n, &mut scratch.x)?;
        for i in 0..n {
            out[i * n + col] = scratch.x[i];
        }
    }

    Some(())
}

pub fn solve_matrix_flat_first_tie_into(
    a: &[f64],
    n: usize,
    b: &[f64],
    cols: usize,
    out: &mut Vec<f64>,
    scratch: &mut FlatLinearScratch,
) -> Option<()> {
    out.resize(n * cols, 0.0);
    scratch.rows.resize(n * (n + 1), 0.0);
    scratch.x.resize(n, 0.0);

    for col in 0..cols {
        for i in 0..n {
            let src = i * n;
            let dst = i * (n + 1);
            scratch.rows[dst..(dst + n)].copy_from_slice(&a[src..(src + n)]);
            scratch.rows[dst + n] = b[i * cols + col];
        }
        solve_augmented_flat_first_tie_in_place(&mut scratch.rows, n, &mut scratch.x)?;
        for i in 0..n {
            out[i * cols + col] = scratch.x[i];
        }
    }
    Some(())
}

#[allow(clippy::needless_range_loop)]
pub fn solve_augmented_flat_first_tie_in_place(
    rows: &mut [f64],
    n: usize,
    x: &mut [f64],
) -> Option<()> {
    let stride = n + 1;

    for col in 0..n {
        let mut pivot_row = col;
        let mut pivot_abs = rows[col * stride + col].abs();
        for idx in (col + 1)..n {
            let v = rows[idx * stride + col].abs();
            if v > pivot_abs {
                pivot_abs = v;
                pivot_row = idx;
            }
        }
        if pivot_abs <= PIVOT_EPSILON {
            return None;
        }
        if pivot_row != col {
            for j in 0..=n {
                rows.swap(col * stride + j, pivot_row * stride + j);
            }
        }

        let pivot_value = rows[col * stride + col];
        for idx in (col + 1)..n {
            let factor = rows[idx * stride + col] / pivot_value;
            for j in 0..=n {
                rows[idx * stride + j] -= factor * rows[col * stride + j];
            }
        }
    }

    for i in (0..n).rev() {
        let mut known = 0.0;
        for j in (i + 1)..n {
            known += rows[i * stride + j] * x[j];
        }
        x[i] = (rows[i * stride + n] - known) / rows[i * stride + i];
    }

    Some(())
}

pub fn solve_flat_normal_first_tie(lambda: &[f64], eta: &[f64]) -> Option<Vec<f64>> {
    let mut scratch = FlatNormalSolveScratch::default();
    solve_flat_normal_first_tie_into(lambda, eta, &mut scratch).map(|x| x.to_vec())
}

#[allow(clippy::needless_range_loop)]
pub fn solve_flat_normal_first_tie_into<'a>(
    lambda: &[f64],
    eta: &[f64],
    scratch: &'a mut FlatNormalSolveScratch,
) -> Option<&'a [f64]> {
    let n = eta.len();
    if lambda.len() != n * n {
        return None;
    }

    scratch.a.resize(n * n, 0.0);
    scratch.a.copy_from_slice(lambda);
    scratch.b.resize(n, 0.0);
    scratch.b.copy_from_slice(eta);

    for k in 0..n {
        let mut pivot = k;
        let mut pivot_abs = scratch.a[k * n + k].abs();
        for i in (k + 1)..n {
            let candidate = scratch.a[i * n + k].abs();
            if candidate > pivot_abs {
                pivot = i;
                pivot_abs = candidate;
            }
        }
        if pivot_abs <= PIVOT_EPSILON {
            return None;
        }
        if pivot != k {
            for j in 0..n {
                scratch.a.swap(k * n + j, pivot * n + j);
            }
            scratch.b.swap(k, pivot);
        }

        let diag = scratch.a[k * n + k];
        for i in (k + 1)..n {
            let factor = scratch.a[i * n + k] / diag;
            scratch.a[i * n + k] = 0.0;
            for j in (k + 1)..n {
                scratch.a[i * n + j] -= factor * scratch.a[k * n + j];
            }
            scratch.b[i] -= factor * scratch.b[k];
        }
    }

    scratch.x.resize(n, 0.0);
    for i in (0..n).rev() {
        let mut known = 0.0;
        for j in (i + 1)..n {
            known += scratch.a[i * n + j] * scratch.x[j];
        }
        scratch.x[i] = (scratch.b[i] - known) / scratch.a[i * n + i];
    }
    Some(&scratch.x)
}

#[allow(clippy::needless_range_loop)]
pub fn normal_matrix_4_weighted_column_outer(rows: &[[f64; 4]], weights: &[f64]) -> [[f64; 4]; 4] {
    let mut a = [[0.0_f64; 4]; 4];
    for i in 0..4 {
        for j in 0..4 {
            let mut s = 0.0_f64;
            for k in 0..rows.len() {
                s += rows[k][i] * weights[k] * rows[k][j];
            }
            a[i][j] = s;
        }
    }
    a
}

#[allow(clippy::needless_range_loop)]
pub fn normal_matrix_4_unweighted_row_outer(rows: &[[f64; 4]]) -> [[f64; 4]; 4] {
    let mut a = [[0.0_f64; 4]; 4];
    for row in rows {
        for i in 0..4 {
            for j in 0..4 {
                a[i][j] += row[i] * row[j];
            }
        }
    }
    a
}

pub fn mat4_vec4(m: &[[f64; 4]; 4], v: &[f64; 4]) -> [f64; 4] {
    [
        dot4(&m[0], v),
        dot4(&m[1], v),
        dot4(&m[2], v),
        dot4(&m[3], v),
    ]
}

pub fn dot4(row: &[f64; 4], v: &[f64; 4]) -> f64 {
    row[0] * v[0] + row[1] * v[1] + row[2] * v[2] + row[3] * v[3]
}

pub fn det4_cofactor(a: &[[f64; 4]; 4]) -> f64 {
    let m01 = a[2][0] * a[3][1] - a[2][1] * a[3][0];
    let m02 = a[2][0] * a[3][2] - a[2][2] * a[3][0];
    let m03 = a[2][0] * a[3][3] - a[2][3] * a[3][0];
    let m12 = a[2][1] * a[3][2] - a[2][2] * a[3][1];
    let m13 = a[2][1] * a[3][3] - a[2][3] * a[3][1];
    let m23 = a[2][2] * a[3][3] - a[2][3] * a[3][2];

    let c0 = a[1][1] * m23 - a[1][2] * m13 + a[1][3] * m12;
    let c1 = a[1][0] * m23 - a[1][2] * m03 + a[1][3] * m02;
    let c2 = a[1][0] * m13 - a[1][1] * m03 + a[1][3] * m01;
    let c3 = a[1][0] * m12 - a[1][1] * m02 + a[1][2] * m01;

    a[0][0] * c0 - a[0][1] * c1 + a[0][2] * c2 - a[0][3] * c3
}

pub fn minor3_of_4(a: &[[f64; 4]; 4], skip_r: usize, skip_c: usize) -> f64 {
    let mut rows = [0_usize; 3];
    let mut cols = [0_usize; 3];
    let mut row_idx = 0;
    let mut col_idx = 0;
    for row in 0..4 {
        if row != skip_r {
            rows[row_idx] = row;
            row_idx += 1;
        }
    }
    for col in 0..4 {
        if col != skip_c {
            cols[col_idx] = col;
            col_idx += 1;
        }
    }

    let b00 = a[rows[0]][cols[0]];
    let b01 = a[rows[0]][cols[1]];
    let b02 = a[rows[0]][cols[2]];
    let b10 = a[rows[1]][cols[0]];
    let b11 = a[rows[1]][cols[1]];
    let b12 = a[rows[1]][cols[2]];
    let b20 = a[rows[2]][cols[0]];
    let b21 = a[rows[2]][cols[1]];
    let b22 = a[rows[2]][cols[2]];

    b00 * (b11 * b22 - b12 * b21) - b01 * (b10 * b22 - b12 * b20) + b02 * (b10 * b21 - b11 * b20)
}

#[allow(clippy::needless_range_loop)]
pub fn invert_4x4_cofactor(a: &[[f64; 4]; 4]) -> Option<[[f64; 4]; 4]> {
    let det = det4_cofactor(a);
    if det == 0.0 {
        return None;
    }

    let mut inv = [[0.0_f64; 4]; 4];
    for j in 0..4 {
        for i in 0..4 {
            let sign = if (i + j) % 2 == 0 { 1.0 } else { -1.0 };
            inv[j][i] = sign * minor3_of_4(a, i, j) / det;
        }
    }
    Some(inv)
}

pub fn invert_3x3_adjugate(m: &[[f64; 3]; 3]) -> Option<[[f64; 3]; 3]> {
    let [[a, b, c], [d, e, f], [g, h, i]] = *m;
    let det = a * (e * i - f * h) - b * (d * i - f * g) + c * (d * h - e * g);
    if det.abs() <= PIVOT_EPSILON {
        return None;
    }
    let inv = 1.0 / det;
    Some([
        [
            (e * i - f * h) * inv,
            (c * h - b * i) * inv,
            (b * f - c * e) * inv,
        ],
        [
            (f * g - d * i) * inv,
            (a * i - c * g) * inv,
            (c * d - a * f) * inv,
        ],
        [
            (d * h - e * g) * inv,
            (b * g - a * h) * inv,
            (a * e - b * d) * inv,
        ],
    ])
}

#[allow(clippy::needless_range_loop)]
pub fn invert_symmetric_pd(n: &[Vec<f64>]) -> Option<Vec<Vec<f64>>> {
    let p = n.len();
    let mut l = vec![vec![0.0_f64; p]; p];
    for i in 0..p {
        for j in 0..=i {
            let mut s = n[i][j];
            for k in 0..j {
                s -= l[i][k] * l[j][k];
            }
            if i == j {
                #[allow(clippy::neg_cmp_op_on_partial_ord)]
                let nonpositive_or_nan = !(s > 0.0);
                if nonpositive_or_nan || !s.is_finite() {
                    return None;
                }
                l[i][j] = s.sqrt();
            } else {
                l[i][j] = s / l[j][j];
            }
        }
    }

    let mut li = vec![vec![0.0_f64; p]; p];
    for i in 0..p {
        li[i][i] = 1.0 / l[i][i];
        for j in 0..i {
            let mut s = 0.0_f64;
            for k in j..i {
                s -= l[i][k] * li[k][j];
            }
            li[i][j] = s / l[i][i];
        }
    }

    let mut inv = vec![vec![0.0_f64; p]; p];
    for i in 0..p {
        for j in 0..p {
            let mut s = 0.0_f64;
            for k in 0..p {
                s += li[k][i] * li[k][j];
            }
            inv[i][j] = s;
        }
    }
    Some(inv)
}

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

    #[test]
    fn first_tie_solver_inverts_known_matrix() {
        let a = vec![vec![4.0, 7.0], vec![2.0, 6.0]];
        let inv = invert_matrix_first_tie(&a).unwrap();
        assert_eq!(inv[0][0].to_bits(), 0.6000000000000001f64.to_bits());
        assert_eq!(inv[0][1].to_bits(), (-0.7000000000000001f64).to_bits());
        assert_eq!(inv[1][0].to_bits(), (-0.2f64).to_bits());
        assert_eq!(inv[1][1].to_bits(), 0.4f64.to_bits());
    }

    #[test]
    fn flat_normal_solver_reports_singular() {
        assert!(solve_flat_normal_first_tie(&[1.0, 2.0, 2.0, 4.0], &[1.0, 2.0]).is_none());
    }

    #[test]
    fn cofactor_inverse_rejects_singular_4x4() {
        let a = [[0.0; 4]; 4];
        assert!(invert_4x4_cofactor(&a).is_none());
    }
}