vecmat 0.7.9

use super::*;

type Mat4 = [[f64; 4]; 4];

fn close<const N: usize>(actual: [f64; N], expected: [f64; N], tolerance: f64) {
    for (a, b) in actual.into_iter().zip(expected) {
        assert!((a - b).abs() <= tolerance * (1.0 + b.abs()), "{a} != {b}");
    }
}

fn fixtures<const K: i8>() -> [QuaternionPair<f64, K>; 3] {
    [
        QuaternionPair::from_array([1.1, 0.2, -0.3, 0.4, 0.2, -0.1, 0.05, 0.15]),
        QuaternionPair::from_array([0.8, -0.4, 0.2, 0.3, -0.1, 0.3, 0.2, -0.2]),
        QuaternionPair::from_array([1.2, 0.1, 0.2, -0.1, 0.3, 0.2, -0.2, 0.1]),
    ]
}

fn algebra<const K: i8>() {
    let [a, b, c] = fixtures::<K>();
    close(
        ((a * b) * c).into_array(),
        (a * (b * c)).into_array(),
        1e-14,
    );
    close(
        (a * (b + c)).into_array(),
        (a * b + a * c).into_array(),
        1e-14,
    );
    close(
        (a * b).quaternion_conj().into_array(),
        (b.quaternion_conj() * a.quaternion_conj()).into_array(),
        1e-14,
    );
    close(
        (a * b).scalar_conj().into_array(),
        (a.scalar_conj() * b.scalar_conj()).into_array(),
        1e-14,
    );
    close(
        (a * b).combined_conj().into_array(),
        (b.combined_conj() * a.combined_conj()).into_array(),
        1e-14,
    );
    assert_eq!(a.quaternion_conj().quaternion_conj(), a);
    assert_eq!(a.scalar_conj().scalar_conj(), a);
    assert_eq!(a.combined_conj().combined_conj(), a);
    let (n, m) = a.norm_sqr();
    close(
        (a * a.quaternion_conj()).into_array(),
        [n, 0.0, 0.0, 0.0, m, 0.0, 0.0, 0.0],
        1e-14,
    );
    for value in [a, b, c] {
        let inverse = value.try_inv().expect("nonsingular fixture");
        close(
            (value * inverse).into_array(),
            QuaternionPair::<f64, K>::one().into_array(),
            1e-14,
        );
        close(
            (inverse * value).into_array(),
            QuaternionPair::<f64, K>::one().into_array(),
            1e-14,
        );
        let unit = value.try_normalize().expect("normalizable fixture");
        let (n, m) = unit.norm_sqr();
        close([n, m], [1.0, 0.0], 1e-14);
        close(
            unit.try_inv().unwrap().into_array(),
            unit.quaternion_conj().into_array(),
            1e-14,
        );
    }
}

#[test]
fn algebra_laws_and_two_sided_inverses() {
    algebra::<-1>();
    algebra::<0>();
    algebra::<1>();
}

fn invalid<const K: i8>() {
    let zero = QuaternionPair::<f64, K>::zero();
    assert!(zero.try_inv().is_none());
    assert!(zero.try_normalize().is_none());
    for scalar in [f64::INFINITY, f64::NEG_INFINITY, f64::NAN] {
        let value =
            QuaternionPair::<f64, K>::from_array([scalar, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]);
        assert!(!value.is_finite());
        assert!(value.try_inv().is_none());
        assert!(value.try_normalize().is_none());
    }
}

#[test]
fn rejects_nonfinite_and_singular_elements() {
    invalid::<-1>();
    invalid::<0>();
    invalid::<1>();
    let complex = QuaternionPair::<f64, -1>::new(Quaternion::one(), Quaternion::i());
    let dual = QuaternionPair::<f64, 0>::new(Quaternion::zero(), Quaternion::one());
    let split = QuaternionPair::<f64, 1>::new(Quaternion::one(), Quaternion::one());
    assert!(complex.try_inv().is_none() && complex.try_normalize().is_none());
    assert!(dual.try_inv().is_none() && dual.try_normalize().is_none());
    assert!(split.try_inv().is_none() && split.try_normalize().is_none());
    assert_eq!(dual * dual, QuaternionPair::zero());
}

#[test]
fn complex_normalization_handles_negative_real_norm() {
    let value = QuaternionPair::<f64, -1>::new(Quaternion::zero(), Quaternion::i());
    let unit = value.try_normalize().unwrap();
    close(
        unit.into_array(),
        [0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
        1e-14,
    );
}

fn matrix_mul(a: Mat4, b: Mat4) -> Mat4 {
    core::array::from_fn(|i| core::array::from_fn(|j| (0..4).map(|k| a[i][k] * b[k][j]).sum()))
}

fn matrix_apply(a: Mat4, p: [f64; 4]) -> [f64; 4] {
    core::array::from_fn(|i| (0..4).map(|j| a[i][j] * p[j]).sum())
}

fn identity() -> Mat4 {
    core::array::from_fn(|i| core::array::from_fn(|j| if i == j { 1.0 } else { 0.0 }))
}

fn unit_axis(axis: [f64; 3]) -> [f64; 3] {
    let norm = axis.into_iter().map(|x| x * x).sum::<f64>().sqrt();
    axis.map(|x| x / norm)
}

fn cs<const K: i8>(t: f64) -> (f64, f64) {
    match K {
        -1 => (t.cosh(), t.sinh()),
        0 => (1.0, t),
        1 => (t.cos(), t.sin()),
        _ => unreachable!(),
    }
}

fn translation<const K: i8>(axis: [f64; 3], distance: f64) -> (QuaternionPair<f64, K>, Mat4) {
    let n = unit_axis(axis);
    let (c, s) = cs::<K>(distance * 0.5);
    let pair = QuaternionPair::new(
        Quaternion::new(c, 0.0, 0.0, 0.0),
        Quaternion::new(0.0, n[0] * s, n[1] * s, n[2] * s),
    );
    // Independent geodesic translation / Lorentz boost matrix, using full
    // distance and a spatial rank-one update rather than quaternion products.
    let (c, s) = cs::<K>(distance);
    let mut matrix = identity();
    matrix[0][0] = c;
    for i in 0..3 {
        matrix[0][i + 1] = -f64::from(K) * s * n[i];
        matrix[i + 1][0] = s * n[i];
        for j in 0..3 {
            matrix[i + 1][j + 1] += (c - 1.0) * n[i] * n[j];
        }
    }
    (pair, matrix)
}

fn rotation<const K: i8>(axis: [f64; 3], angle: f64) -> (QuaternionPair<f64, K>, Mat4) {
    let n = unit_axis(axis);
    let s = (angle * 0.5).sin();
    let pair = QuaternionPair::new(
        Quaternion::new((angle * 0.5).cos(), n[0] * s, n[1] * s, n[2] * s),
        Quaternion::zero(),
    );
    // Independent Rodrigues matrix in the spatial block.
    let cross = [[0.0, -n[2], n[1]], [n[2], 0.0, -n[0]], [-n[1], n[0], 0.0]];
    let mut matrix = identity();
    for i in 0..3 {
        for j in 0..3 {
            matrix[i + 1][j + 1] = if i == j { angle.cos() } else { 0.0 }
                + (1.0 - angle.cos()) * n[i] * n[j]
                + angle.sin() * cross[i][j];
        }
    }
    (pair, matrix)
}

fn apply<const K: i8>(g: QuaternionPair<f64, K>, p: [f64; 4]) -> [f64; 4] {
    // Scalar plus e times a pure spatial quaternion, with the combined
    // conjugate on the right. This also covers homogeneous Euclidean points.
    let point = QuaternionPair::new(
        Quaternion::new(p[0], 0.0, 0.0, 0.0),
        Quaternion::new(0.0, p[1], p[2], p[3]),
    );
    let moved = g * point * g.combined_conj();
    let [w, rx, ry, rz, iw, x, y, z] = moved.into_array();
    close([rx, ry, rz, iw], [0.0; 4], 1e-12);
    [w, x, y, z]
}

fn matrix_agreement<const K: i8>() {
    let mut pair = QuaternionPair::one();
    let mut matrix = identity();
    for i in 0..40 {
        let f = f64::from(i);
        let axis = [1.0, (0.7 * f).cos(), (0.3 * f).sin()];
        let (next_pair, next_matrix) = if i % 2 == 0 {
            rotation::<K>(axis, 0.11 - 0.005 * f)
        } else {
            translation::<K>(axis, 0.03 - 0.002 * f)
        };
        pair = next_pair * pair;
        matrix = matrix_mul(next_matrix, matrix);
        for point in [
            [1.0, 0.0, 0.0, 0.0],
            [0.0, 1.0, 0.0, 0.0],
            [0.0, 0.0, 1.0, 0.0],
            [0.0, 0.0, 0.0, 1.0],
            [1.3, -0.5, 0.7, 0.2],
        ] {
            close(apply(pair, point), matrix_apply(matrix, point), 1e-12);
            close(apply(-pair, point), matrix_apply(matrix, point), 1e-12);
            close(
                apply(pair.quaternion_conj(), apply(pair, point)),
                point,
                1e-12,
            );
        }
    }
    let origin = apply(pair, [1.0, 0.0, 0.0, 0.0]);
    let metric =
        origin[0] * origin[0] + f64::from(K) * origin[1..].iter().map(|x| x * x).sum::<f64>();
    close([metric], [1.0], 1e-12);
    // Quaternion-pair storage and conversion keep all eight scalar components.
    let packed = pair.into_array().map(|x| x as f32);
    let f32_pair = QuaternionPair::<f32, K>::from_array(packed);
    let (n, m) = f32_pair.norm_sqr();
    close([f64::from(n), f64::from(m)], [1.0, 0.0], 5e-7);
    assert_eq!(
        core::mem::size_of::<QuaternionPair<f32, K>>(),
        8 * core::mem::size_of::<f32>()
    );
}

#[test]
fn all_curvatures_match_independent_four_dimensional_matrices() {
    matrix_agreement::<-1>();
    matrix_agreement::<0>();
    matrix_agreement::<1>();
}

#[test]
fn spherical_antipodes_and_complete_circuits() {
    let origin = [1.0, 0.0, 0.0, 0.0];
    let (half, _) = translation::<1>([1.0, 0.0, 0.0], core::f64::consts::PI);
    close(apply(half, origin), [-1.0, 0.0, 0.0, 0.0], 1e-14);
    close(apply(half * half, origin), origin, 1e-14);
    close(
        (half * half).into_array(),
        (-QuaternionPair::<f64, 1>::one()).into_array(),
        1e-14,
    );
}

fn long_composition<const K: i8>() {
    let mut pair = QuaternionPair::<f64, K>::one();
    let (step, _) = translation::<K>([0.3, -0.7, 1.2], 0.002);
    let (turn, _) = rotation::<K>([-0.5, 0.7, 0.2], 0.07);
    for i in 0..4000 {
        pair = turn * step * pair;
        if i % 16 == 0 {
            pair = pair.try_normalize().unwrap();
        }
    }
    let (n, m) = pair.norm_sqr();
    close([n, m], [1.0, 0.0], 2e-12);
    close(
        (pair * pair.quaternion_conj()).into_array(),
        QuaternionPair::<f64, K>::one().into_array(),
        2e-12,
    );
}

#[test]
fn normalization_controls_long_composition_drift() {
    long_composition::<-1>();
    long_composition::<0>();
    long_composition::<1>();
}

#[test]
#[should_panic(expected = "quaternion-pair K must be -1, 0, or 1")]
fn rejects_unsupported_central_units() {
    let _ = QuaternionPair::<f64, 2>::one();
}