plasma-prp 0.1.0

//! hsMatrix44 — Plasma's 4x4 row-major matrix.
//!
//! Translated from hsMatrix44.cpp / hsMatrix44.h in Plasma/CoreLib/
//!
//! Layout: fMap[row][col], row-major. Stored flat as [f32; 16]:
//!   fMap[0][0..3] = row 0 → flat indices 0, 1, 2, 3
//!   fMap[1][0..3] = row 1 → flat indices 4, 5, 6, 7
//!   fMap[2][0..3] = row 2 → flat indices 8, 9, 10, 11
//!   fMap[3][0..3] = row 3 → flat indices 12, 13, 14, 15
//!
//! Translation is in column 3:
//!   fMap[0][3] = X → flat index 3
//!   fMap[1][3] = Y → flat index 7
//!   fMap[2][3] = Z → flat index 11
//!
//! Axis extraction (GetAxis):
//!   kRight (0): (-fMap[0][0], -fMap[1][0], -fMap[2][0]) — negated column 0
//!   kUp    (1): ( fMap[0][2],  fMap[1][2],  fMap[2][2]) — column 2
//!   kView  (2): (-fMap[0][1], -fMap[1][1], -fMap[2][1]) — negated column 1

use anyhow::Result;
use std::io::Read;
use crate::resource::prp::PlasmaRead;

/// Axis indices for GetAxis().
/// Translated from hsMatrix44.h enum
pub const K_RIGHT: usize = 0;
pub const K_UP: usize = 1;
pub const K_VIEW: usize = 2;

/// Identity flag.
/// Translated from hsMatrix44.h
const K_IS_IDENT: u32 = 0x1;

/// Epsilon for identity snapping.
/// Translated from hsMatrix44.cpp:24
const K_EPS: f32 = 1.0e-5;

/// Flat index into [f32; 16] for fMap[row][col].
#[inline]
pub const fn idx(row: usize, col: usize) -> usize {
    row * 4 + col
}

/// The identity matrix as [f32; 16].
pub const IDENTITY: [f32; 16] = [
    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,
];

/// Read an hsMatrix44 from a Plasma stream.
/// Translated from hsMatrix44::Read() in hsMatrix44.cpp
///
/// Binary format:
///   u8 hasData (1 = matrix follows, 0 = identity)
///   if hasData: 16 × f32 LE in row-major order
pub fn read_matrix44(reader: &mut impl Read) -> Result<[f32; 16]> {
    let has_data = reader.read_u8()?;
    if has_data == 0 {
        return Ok(IDENTITY);
    }
    let mut m = [0f32; 16];
    for val in &mut m {
        *val = reader.read_f32()?;
    }
    Ok(m)
}

/// Get translation from an hsMatrix44.
/// Translated from hsMatrix44::GetTranslate() in hsMatrix44.cpp
#[inline]
pub fn get_translate(m: &[f32; 16]) -> [f32; 3] {
    [m[idx(0, 3)], m[idx(1, 3)], m[idx(2, 3)]]
}

/// Get axis vector from an hsMatrix44.
/// Translated from hsMatrix44::GetAxis(int) in hsMatrix44.cpp
///
/// kRight (0): negated column 0
/// kUp    (1): column 2
/// kView  (2): negated column 1
pub fn get_axis(m: &[f32; 16], axis: usize) -> [f32; 3] {
    match axis {
        K_RIGHT => [-m[idx(0, 0)], -m[idx(1, 0)], -m[idx(2, 0)]],
        K_UP    => [ m[idx(0, 2)],  m[idx(1, 2)],  m[idx(2, 2)]],
        K_VIEW  => [-m[idx(0, 1)], -m[idx(1, 1)], -m[idx(2, 1)]],
        _ => [0.0, 0.0, 0.0],
    }
}

/// Multiply two hsMatrix44 matrices: result = a * b (row-major).
/// Translated from hsMatrix44::operator* in hsMatrix44.cpp
///
/// c[i][j] = Σ_k a[i][k] * b[k][j]
pub fn multiply(a: &[f32; 16], b: &[f32; 16]) -> [f32; 16] {
    let mut c = [0f32; 16];
    for i in 0..4 {
        for j in 0..4 {
            c[idx(i, j)] = a[idx(i, 0)] * b[idx(0, j)]
                         + a[idx(i, 1)] * b[idx(1, j)]
                         + a[idx(i, 2)] * b[idx(2, j)]
                         + a[idx(i, 3)] * b[idx(3, j)];
        }
    }
    c
}

/// Transform a point by an hsMatrix44 (implicit w=1, translation applied).
/// Translated from hsMatrix44::operator*(hsPoint3) in hsMatrix44.cpp
pub fn transform_point(m: &[f32; 16], p: [f32; 3]) -> [f32; 3] {
    [
        p[0] * m[idx(0, 0)] + p[1] * m[idx(0, 1)] + p[2] * m[idx(0, 2)] + m[idx(0, 3)],
        p[0] * m[idx(1, 0)] + p[1] * m[idx(1, 1)] + p[2] * m[idx(1, 2)] + m[idx(1, 3)],
        p[0] * m[idx(2, 0)] + p[1] * m[idx(2, 1)] + p[2] * m[idx(2, 2)] + m[idx(2, 3)],
    ]
}

/// Transform a vector by an hsMatrix44 (implicit w=0, no translation).
/// Translated from hsMatrix44::operator*(hsVector3) in hsMatrix44.cpp
pub fn transform_vector(m: &[f32; 16], v: [f32; 3]) -> [f32; 3] {
    [
        v[0] * m[idx(0, 0)] + v[1] * m[idx(0, 1)] + v[2] * m[idx(0, 2)],
        v[0] * m[idx(1, 0)] + v[1] * m[idx(1, 1)] + v[2] * m[idx(1, 2)],
        v[0] * m[idx(2, 0)] + v[1] * m[idx(2, 1)] + v[2] * m[idx(2, 2)],
    ]
}

/// Make a translation matrix.
/// Translated from hsMatrix44::MakeTranslateMat() in hsMatrix44.cpp
pub fn make_translate_mat(x: f32, y: f32, z: f32) -> [f32; 16] {
    let mut m = IDENTITY;
    m[idx(0, 3)] = x;
    m[idx(1, 3)] = y;
    m[idx(2, 3)] = z;
    m
}

/// Make a scale matrix.
/// Translated from hsMatrix44::MakeScaleMat() in hsMatrix44.cpp
pub fn make_scale_mat(sx: f32, sy: f32, sz: f32) -> [f32; 16] {
    let mut m = IDENTITY;
    m[idx(0, 0)] = sx;
    m[idx(1, 1)] = sy;
    m[idx(2, 2)] = sz;
    m
}

/// Make a rotation matrix around one of the 3 axes (0=X, 1=Y, 2=Z).
/// Translated from hsMatrix44::MakeRotateMat() / SetRotate() in hsMatrix44.cpp
///
/// WARNING: SetRotate axis=1 (Y) has OPPOSITE sign convention from
/// MakeYRotation. This function uses the SetRotate convention.
pub fn make_rotate_mat(axis: usize, radians: f32) -> [f32; 16] {
    let s = radians.sin();
    let c = radians.cos();
    let mut m = IDENTITY;
    let (c1, c2) = match axis {
        0 => (1usize, 2usize), // X rotation
        1 => (0usize, 2usize), // Y rotation
        2 => (0usize, 1usize), // Z rotation
        _ => return m,
    };
    m[idx(c1, c1)] = c;
    m[idx(c2, c2)] = c;
    m[idx(c1, c2)] = s;
    m[idx(c2, c1)] = -s;
    m
}

/// 3x3 determinant helper.
/// Translated from Det3() in hsMatrix44.cpp
fn det3(
    a1: f32, a2: f32, a3: f32,
    b1: f32, b2: f32, b3: f32,
    c1: f32, c2: f32, c3: f32,
) -> f32 {
    a1 * (b2 * c3 - b3 * c2)
  - b1 * (a2 * c3 - a3 * c2)
  + c1 * (a2 * b3 - a3 * b2)
}

/// Compute the 4x4 determinant.
/// Translated from hsMatrix44::GetDeterminant() in hsMatrix44.cpp
/// Uses cofactor expansion along column 0.
pub fn determinant(m: &[f32; 16]) -> f32 {
    m[idx(0, 0)] * det3(m[idx(1, 1)], m[idx(2, 1)], m[idx(3, 1)],
                         m[idx(1, 2)], m[idx(2, 2)], m[idx(3, 2)],
                         m[idx(1, 3)], m[idx(2, 3)], m[idx(3, 3)])
  - m[idx(1, 0)] * det3(m[idx(0, 1)], m[idx(2, 1)], m[idx(3, 1)],
                         m[idx(0, 2)], m[idx(2, 2)], m[idx(3, 2)],
                         m[idx(0, 3)], m[idx(2, 3)], m[idx(3, 3)])
  + m[idx(2, 0)] * det3(m[idx(0, 1)], m[idx(1, 1)], m[idx(3, 1)],
                         m[idx(0, 2)], m[idx(1, 2)], m[idx(3, 2)],
                         m[idx(0, 3)], m[idx(1, 3)], m[idx(3, 3)])
  - m[idx(3, 0)] * det3(m[idx(0, 1)], m[idx(1, 1)], m[idx(2, 1)],
                         m[idx(0, 2)], m[idx(1, 2)], m[idx(2, 2)],
                         m[idx(0, 3)], m[idx(1, 3)], m[idx(2, 3)])
}

/// Compute the inverse of an hsMatrix44.
/// Translated from hsMatrix44::GetInverse() in hsMatrix44.cpp
/// Uses classical adjoint method: inverse = adjoint / determinant.
pub fn inverse(m: &[f32; 16]) -> [f32; 16] {
    let det = determinant(m);
    if det == 0.0 {
        return IDENTITY;
    }
    let inv_det = 1.0 / det;

    // Variable names matching C++ source for traceability
    let (a1, b1, c1, d1) = (m[0], m[1], m[2], m[3]);
    let (a2, b2, c2, d2) = (m[4], m[5], m[6], m[7]);
    let (a3, b3, c3, d3) = (m[8], m[9], m[10], m[11]);
    let (a4, b4, c4, d4) = (m[12], m[13], m[14], m[15]);

    // Translated from hsMatrix44::GetAdjoint() in hsMatrix44.cpp
    let mut adj = [0f32; 16];
    adj[idx(0, 0)] =  det3(b2, b3, b4, c2, c3, c4, d2, d3, d4);
    adj[idx(1, 0)] = -det3(a2, a3, a4, c2, c3, c4, d2, d3, d4);
    adj[idx(2, 0)] =  det3(a2, a3, a4, b2, b3, b4, d2, d3, d4);
    adj[idx(3, 0)] = -det3(a2, a3, a4, b2, b3, b4, c2, c3, c4);

    adj[idx(0, 1)] = -det3(b1, b3, b4, c1, c3, c4, d1, d3, d4);
    adj[idx(1, 1)] =  det3(a1, a3, a4, c1, c3, c4, d1, d3, d4);
    adj[idx(2, 1)] = -det3(a1, a3, a4, b1, b3, b4, d1, d3, d4);
    adj[idx(3, 1)] =  det3(a1, a3, a4, b1, b3, b4, c1, c3, c4);

    adj[idx(0, 2)] =  det3(b1, b2, b4, c1, c2, c4, d1, d2, d4);
    adj[idx(1, 2)] = -det3(a1, a2, a4, c1, c2, c4, d1, d2, d4);
    adj[idx(2, 2)] =  det3(a1, a2, a4, b1, b2, b4, d1, d2, d4);
    adj[idx(3, 2)] = -det3(a1, a2, a4, b1, b2, b4, c1, c2, c4);

    adj[idx(0, 3)] = -det3(b1, b2, b3, c1, c2, c3, d1, d2, d3);
    adj[idx(1, 3)] =  det3(a1, a2, a3, c1, c2, c3, d1, d2, d3);
    adj[idx(2, 3)] = -det3(a1, a2, a3, b1, b2, b3, d1, d2, d3);
    adj[idx(3, 3)] =  det3(a1, a2, a3, b1, b2, b3, c1, c2, c3);

    // Multiply adjoint by 1/det
    for val in &mut adj {
        *val *= inv_det;
    }
    adj
}

/// Check parity (mirror/reflection) of the 3x3 rotation submatrix.
/// Translated from hsMatrix44::GetParity() in hsMatrix44.cpp
/// Returns true if the rotation has negative parity (reflection).
pub fn get_parity(m: &[f32; 16]) -> bool {
    // row0 cross row1, then dot with row2
    let r0 = [m[idx(0, 0)], m[idx(0, 1)], m[idx(0, 2)]];
    let r1 = [m[idx(1, 0)], m[idx(1, 1)], m[idx(1, 2)]];
    let r2 = [m[idx(2, 0)], m[idx(2, 1)], m[idx(2, 2)]];
    let cross = [
        r0[1] * r1[2] - r0[2] * r1[1],
        r0[2] * r1[0] - r0[0] * r1[2],
        r0[0] * r1[1] - r0[1] * r1[0],
    ];
    let dot = cross[0] * r2[0] + cross[1] * r2[1] + cross[2] * r2[2];
    dot < 0.0
}

/// Check if a matrix is identity (within epsilon) and snap to exact values.
/// Translated from hsMatrix44::IsIdentity() in hsMatrix44.cpp
/// Note: This is a MUTATING operation — snaps near-identity values.
pub fn is_identity(m: &mut [f32; 16]) -> bool {
    let mut is_ident = true;
    for i in 0..4 {
        for j in 0..4 {
            let val = m[idx(i, j)];
            if i == j {
                if (val - 1.0).abs() <= K_EPS {
                    m[idx(i, j)] = 1.0;
                } else {
                    is_ident = false;
                }
            } else {
                if val.abs() <= K_EPS {
                    m[idx(i, j)] = 0.0;
                } else {
                    is_ident = false;
                }
            }
        }
    }
    is_ident
}

/// Transpose a matrix.
/// Translated from hsMatrix44::GetTranspose() in hsMatrix44.cpp
pub fn transpose(m: &[f32; 16]) -> [f32; 16] {
    let mut t = [0f32; 16];
    for i in 0..4 {
        for j in 0..4 {
            t[idx(i, j)] = m[idx(j, i)];
        }
    }
    t
}

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

    #[test]
    fn test_identity() {
        let mut m = IDENTITY;
        assert!(is_identity(&mut m));
    }

    #[test]
    fn test_translate() {
        let m = make_translate_mat(1.0, 2.0, 3.0);
        assert_eq!(get_translate(&m), [1.0, 2.0, 3.0]);
    }

    #[test]
    fn test_transform_point() {
        let m = make_translate_mat(10.0, 20.0, 30.0);
        let p = transform_point(&m, [1.0, 2.0, 3.0]);
        assert_eq!(p, [11.0, 22.0, 33.0]);
    }

    #[test]
    fn test_multiply_identity() {
        let a = make_translate_mat(5.0, 6.0, 7.0);
        let result = multiply(&a, &IDENTITY);
        assert_eq!(a, result);
        let result2 = multiply(&IDENTITY, &a);
        assert_eq!(a, result2);
    }

    #[test]
    fn test_inverse_identity() {
        let inv = inverse(&IDENTITY);
        assert_eq!(inv, IDENTITY);
    }

    #[test]
    fn test_inverse_translate() {
        let m = make_translate_mat(5.0, -3.0, 7.0);
        let inv = inverse(&m);
        let result = multiply(&m, &inv);
        let mut result_copy = result;
        assert!(is_identity(&mut result_copy));
    }

    #[test]
    fn test_determinant_identity() {
        assert!((determinant(&IDENTITY) - 1.0).abs() < 1e-6);
    }

    #[test]
    fn test_parity_identity() {
        assert!(!get_parity(&IDENTITY));
    }

    #[test]
    fn test_get_axis_identity() {
        let m = IDENTITY;
        assert_eq!(get_axis(&m, K_RIGHT), [-1.0, 0.0, 0.0]);
        assert_eq!(get_axis(&m, K_UP), [0.0, 0.0, 1.0]);
        assert_eq!(get_axis(&m, K_VIEW), [0.0, -1.0, 0.0]);
    }
}