crystal_ball 0.3.0

use std::ops::{Add, AddAssign, Div, DivAssign, Index, IndexMut, Mul, MulAssign, Sub, SubAssign};

use crate::math::{Point3, Vec3};

/// A 4x4 row-major matrix.
///
/// For constructing transformations see [`Transform`](crate::math::Transform).
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct Mat4 {
    pub matrix: [[f64; 4]; 4],
}

impl Mat4 {
    #[rustfmt::skip]
    pub const IDENTITY: Mat4 = Mat4 {
        matrix: [
            [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],
        ],
    };

    /// Create a new [`Mat4`] from individual elements.
    #[rustfmt::skip]
    #[allow(clippy::too_many_arguments)]
    pub fn new(
        a00: f64, a01: f64, a02: f64, a03: f64,
        a10: f64, a11: f64, a12: f64, a13: f64,
        a20: f64, a21: f64, a22: f64, a23: f64,
        a30: f64, a31: f64, a32: f64, a33: f64,
    ) -> Self {
        Mat4 {
            matrix: [
                [a00, a01, a02, a03],
                [a10, a11, a12, a13],
                [a20, a21, a22, a23],
                [a30, a31, a32, a33],
            ]
        }
    }

    /// Calculate the determinant of a 2x2 matrix.
    fn determinant_2x2(matrix: [[f64; 2]; 2]) -> f64 {
        matrix[0][0] * matrix[1][1] - matrix[1][0] * matrix[0][1]
    }

    /// Calculate the determinant of a 3x3 matrix using Laplace expansion.
    fn determinant_3x3(matrix: [[f64; 3]; 3]) -> f64 {
        matrix[0][0]
            * Self::determinant_2x2([[matrix[1][1], matrix[1][2]], [matrix[2][1], matrix[2][2]]])
            - matrix[0][1]
                * Self::determinant_2x2([
                    [matrix[1][0], matrix[1][2]],
                    [matrix[2][0], matrix[2][2]],
                ])
            + matrix[0][2]
                * Self::determinant_2x2([
                    [matrix[1][0], matrix[1][1]],
                    [matrix[2][0], matrix[2][1]],
                ])
    }

    /// Calculate the determinant of a 3x3 submatrix, excluding the specified row and column.
    fn minor(self, row: usize, column: usize) -> f64 {
        let mut matrix = [[0.0; 3]; 3];

        for (j, y) in (0..4).filter(|y| *y != row).enumerate() {
            for (i, x) in (0..4).filter(|x| *x != column).enumerate() {
                matrix[j][i] = self[y][x];
            }
        }

        Self::determinant_3x3(matrix)
    }

    /// Calculate the cofactor (signed minor of the 3x3 submatrix, excluding the specified row and column).
    fn cofactor(&self, row: usize, column: usize) -> f64 {
        (-1i32).pow((row + column) as u32) as f64 * self.minor(row, column)
    }

    /// Calculate the matrix's determinant using Laplace expansion.
    pub fn determinant(&self) -> f64 {
        (0..4).map(|y| self.cofactor(y, 0) * self[y][0]).sum()
    }

    /// Calculate the matrix's inverse using [Cramer's rule](https://en.wikipedia.org/wiki/Cramer%27s_rule#Finding_inverse_matrix).
    ///
    /// # Panics
    /// Panics if the matrix can not be inverted (determinant = 0).
    pub fn inverse(&self) -> Self {
        let mut matrix = [[0.0; 4]; 4];

        let determinant = self.determinant();
        assert_ne!(determinant, 0.0);

        let determinant_inverse = 1.0 / determinant;

        for (y, row) in matrix.iter_mut().enumerate() {
            for (x, value) in row.iter_mut().enumerate() {
                *value = self.cofactor(x, y) * determinant_inverse;
            }
        }

        Self { matrix }
    }

    /// Transpose the matrix.
    pub fn transpose(&self) -> Self {
        Self {
            #[rustfmt::skip]
            matrix: [
                [self[0][0], self[1][0], self[2][0], self[3][0]],
                [self[0][1], self[1][1], self[2][1], self[3][1]],
                [self[0][2], self[1][2], self[2][2], self[3][2]],
                [self[0][3], self[1][3], self[2][3], self[3][3]],
            ],
        }
    }

    pub fn maximum_norm(&self) -> f64 {
        (0..4).flat_map(|y| self[y]).sum()
    }

    pub fn abs(&self) -> f64 {
        self.maximum_norm()
    }
}

impl Add<Mat4> for Mat4 {
    type Output = Mat4;

    fn add(self, rhs: Mat4) -> Self::Output {
        Self {
            matrix: [
                [
                    self[0][0] + rhs[0][0],
                    self[0][1] + rhs[0][1],
                    self[0][2] + rhs[0][2],
                    self[0][3] + rhs[0][3],
                ],
                [
                    self[1][0] + rhs[1][0],
                    self[1][1] + rhs[1][1],
                    self[1][2] + rhs[1][2],
                    self[1][3] + rhs[1][3],
                ],
                [
                    self[2][0] + rhs[2][0],
                    self[2][1] + rhs[2][1],
                    self[2][2] + rhs[2][2],
                    self[2][3] + rhs[2][3],
                ],
                [
                    self[3][0] + rhs[3][0],
                    self[3][1] + rhs[3][1],
                    self[3][2] + rhs[3][2],
                    self[3][3] + rhs[3][3],
                ],
            ],
        }
    }
}

impl AddAssign<Mat4> for Mat4 {
    fn add_assign(&mut self, rhs: Mat4) {
        *self = *self + rhs
    }
}

impl Sub<Mat4> for Mat4 {
    type Output = Mat4;

    fn sub(self, rhs: Mat4) -> Self::Output {
        Self {
            matrix: [
                [
                    self[0][0] - rhs[0][0],
                    self[0][1] - rhs[0][1],
                    self[0][2] - rhs[0][2],
                    self[0][3] - rhs[0][3],
                ],
                [
                    self[1][0] - rhs[1][0],
                    self[1][1] - rhs[1][1],
                    self[1][2] - rhs[1][2],
                    self[1][3] - rhs[1][3],
                ],
                [
                    self[2][0] - rhs[2][0],
                    self[2][1] - rhs[2][1],
                    self[2][2] - rhs[2][2],
                    self[2][3] - rhs[2][3],
                ],
                [
                    self[3][0] - rhs[3][0],
                    self[3][1] - rhs[3][1],
                    self[3][2] - rhs[3][2],
                    self[3][3] - rhs[3][3],
                ],
            ],
        }
    }
}

impl SubAssign<Mat4> for Mat4 {
    fn sub_assign(&mut self, rhs: Mat4) {
        *self = *self - rhs
    }
}

impl Mul<Mat4> for Mat4 {
    type Output = Mat4;

    fn mul(self, rhs: Mat4) -> Self::Output {
        let mut matrix = [[0.0; 4]; 4];
        for y in 0..4 {
            for x in 0..4 {
                for i in 0..4 {
                    matrix[y][x] += self[y][i] * rhs[i][x]
                }
            }
        }

        Mat4 { matrix }
    }
}

impl Mul<Vec3> for Mat4 {
    type Output = Vec3;

    fn mul(self, rhs: Vec3) -> Self::Output {
        Vec3::new(
            self[0][0] * rhs.x + self[0][1] * rhs.y + self[0][2] * rhs.z,
            self[1][0] * rhs.x + self[1][1] * rhs.y + self[1][2] * rhs.z,
            self[2][0] * rhs.x + self[2][1] * rhs.y + self[2][2] * rhs.z,
        )
    }
}

impl Mul<Point3> for Mat4 {
    type Output = Point3;

    fn mul(self, rhs: Point3) -> Self::Output {
        let x = self[0][0] * rhs.x + self[0][1] * rhs.y + self[0][2] * rhs.z + self[0][3];
        let y = self[1][0] * rhs.x + self[1][1] * rhs.y + self[1][2] * rhs.z + self[1][3];
        let z = self[2][0] * rhs.x + self[2][1] * rhs.y + self[2][2] * rhs.z + self[2][3];
        let w = self[3][0] * rhs.x + self[3][1] * rhs.y + self[3][2] * rhs.z + self[3][3];

        let point = Point3::new(x, y, z);

        if w == 1.0 {
            point
        } else {
            point / w
        }
    }
}

impl Mul<f64> for Mat4 {
    type Output = Mat4;

    fn mul(self, rhs: f64) -> Self::Output {
        let mut matrix = [[0.0; 4]; 4];
        for y in 0..4 {
            for x in 0..4 {
                matrix[y][x] = self[y][x] * rhs;
            }
        }

        Mat4 { matrix }
    }
}

impl MulAssign<Mat4> for Mat4 {
    fn mul_assign(&mut self, rhs: Mat4) {
        *self = *self * rhs
    }
}

impl MulAssign<f64> for Mat4 {
    fn mul_assign(&mut self, rhs: f64) {
        *self = *self * rhs
    }
}

impl Div<f64> for Mat4 {
    type Output = Mat4;

    fn div(self, rhs: f64) -> Self::Output {
        let rhs_inverse = 1.0 / rhs;

        let mut matrix = [[0.0; 4]; 4];
        for y in 0..4 {
            for x in 0..4 {
                matrix[y][x] = self[y][x] * rhs_inverse;
            }
        }

        Mat4 { matrix }
    }
}

impl DivAssign<f64> for Mat4 {
    fn div_assign(&mut self, rhs: f64) {
        *self = *self / rhs
    }
}

impl Default for Mat4 {
    fn default() -> Self {
        Mat4::IDENTITY
    }
}

impl From<[[f64; 4]; 4]> for Mat4 {
    fn from(a: [[f64; 4]; 4]) -> Self {
        Mat4 { matrix: a }
    }
}

impl Index<usize> for Mat4 {
    type Output = [f64; 4];

    fn index(&self, index: usize) -> &Self::Output {
        &self.matrix[index]
    }
}

impl IndexMut<usize> for Mat4 {
    fn index_mut(&mut self, index: usize) -> &mut Self::Output {
        &mut self.matrix[index]
    }
}

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

    use crate::math::Mat4;

    #[test]
    fn inverse_test() {
        #[rustfmt::skip]
            let translation_matrix = Mat4::new(
            1.0, 0.0, 0.0, 4.0,
            0.0, 1.0, 0.0, 2.0,
            0.0, 0.0, 1.0, 0.0,
            0.0, 0.0, 0.0, 1.0,
        );
        #[rustfmt::skip]
            let translation_matrix_inverse = Mat4::new(
            1.0, 0.0, 0.0, -4.0,
            0.0, 1.0, 0.0, -2.0,
            0.0, 0.0, 1.0, 0.0,
            0.0, 0.0, 0.0, 1.0,
        );

        #[rustfmt::skip]
            let scale_matrix = Mat4::new(
            5.0, 0.0, 0.0, 0.0,
            0.0, 4.0, 0.0, 0.0,
            0.0, 0.0, 3.0, 0.0,
            0.0, 0.0, 0.0, 2.0,
        );
        #[rustfmt::skip]
            let scale_matrix_inverse = Mat4::new(
            1.0 / 5.0, 0.0, 0.0, 0.0,
            0.0, 1.0 / 4.0, 0.0, 0.0,
            0.0, 0.0, 1.0 / 3.0, 0.0,
            0.0, 0.0, 0.0, 1.0 / 2.0,
        );

        // TODO: assert_approx_eq only uses the norm and does not compare the actual values
        assert_approx_eq!(Mat4::IDENTITY, Mat4::IDENTITY.inverse());
        assert_approx_eq!(translation_matrix.inverse(), translation_matrix_inverse);
        assert_approx_eq!(
            translation_matrix * translation_matrix_inverse,
            Mat4::IDENTITY
        );
        assert_approx_eq!(scale_matrix.inverse(), scale_matrix_inverse);
        assert_approx_eq!(scale_matrix * scale_matrix_inverse, Mat4::IDENTITY);
    }
}