use std::fmt;
use std::ops::{Index, IndexMut, Mul, Sub};

use crate::{tuples::Tuple, Float, EPSILON};

/// Short hand for creating a Matrix4x4 set to the identity matrix.
pub fn identity() -> Matrix4x4 {
    Matrix4x4::identity()
}
/// Short hand for creating a Matrix4x4 for rotating around the X-axis.
pub fn rotation_x(radians: Float) -> Matrix4x4 {
    Matrix4x4::rotation_x(radians)
}
/// Short hand for creating a Matrix4x4 for rotating around the Y-axis.
pub fn rotation_y(radians: Float) -> Matrix4x4 {
    Matrix4x4::rotation_y(radians)
}
/// Short hand for creating a Matrix4x4 for rotating around the Z-axis.
pub fn rotation_z(radians: Float) -> Matrix4x4 {
    Matrix4x4::rotation_z(radians)
}
/// Short hand for creating a Matrix4x4 that scales in the given x,y,z axis.
pub fn scaling(x: Float, y: Float, z: Float) -> Matrix4x4 {
    Matrix4x4::scaling(x, y, z)
}
/// Short hand for creating a Matrix4x4 that shears across the given axis pairs.
pub fn shearing(xy: Float, xz: Float, yx: Float, yz: Float, zx: Float, zy: Float) -> Matrix4x4 {
    Matrix4x4::shearing(xy, xz, yx, yz, zx, zy)
}
/// Short hand for creating a Matrix4x4 that translations along the given x,y,z axis.
pub fn translation(x: Float, y: Float, z: Float) -> Matrix4x4 {
    Matrix4x4::translation(x, y, z)
}

#[derive(Debug)]
pub struct Matrix2x2 {
    m: [[Float; 2]; 2],
}
impl Matrix2x2 {
    /// Create a `Matrix2x2` with each of the given rows.
    pub fn new(r0: [Float; 2], r1: [Float; 2]) -> Matrix2x2 {
        Matrix2x2 { m: [r0, r1] }
    }

    /// Calculate the determinant of a 2x2.
    pub fn determinant(&self) -> Float {
        let m = self;
        m[(0, 0)] * m[(1, 1)] - m[(0, 1)] * m[(1, 0)]
    }
}
impl Index<(usize, usize)> for Matrix2x2 {
    type Output = Float;
    fn index(&self, (row, col): (usize, usize)) -> &Self::Output {
        &self.m[row][col]
    }
}
impl PartialEq for Matrix2x2 {
    fn eq(&self, rhs: &Matrix2x2) -> bool {
        let l = self.m;
        let r = rhs.m;
        for i in 0..2 {
            for j in 0..2 {
                let d = (l[i][j] - r[i][j]).abs();
                if d > EPSILON {
                    return false;
                }
            }
        }
        true
    }
}

#[derive(Debug)]
pub struct Matrix3x3 {
    m: [[Float; 3]; 3],
}
impl Matrix3x3 {
    /// Create a `Matrix3x2` with each of the given rows.
    pub fn new(r0: [Float; 3], r1: [Float; 3], r2: [Float; 3]) -> Matrix3x3 {
        Matrix3x3 { m: [r0, r1, r2] }
    }
    /// submatrix extracts a 2x2 matrix ignoring the 0-based `row` and `col` given.
    pub fn submatrix(&self, row: usize, col: usize) -> Matrix2x2 {
        assert!(row < 3);
        assert!(col < 3);
        let mut rows = vec![];
        for r in 0..3 {
            if r != row {
                let mut v = vec![];
                for c in 0..3 {
                    if c != col {
                        v.push(self[(r, c)]);
                    }
                }
                rows.push(v);
            }
        }
        let m = [[rows[0][0], rows[0][1]], [rows[1][0], rows[1][1]]];
        Matrix2x2 { m }
    }

    /// Compute minor of a 3x3 matrix.
    pub fn minor(&self, row: usize, col: usize) -> Float {
        self.submatrix(row, col).determinant()
    }

    /// Compute cofactor of a 3x3 matrix.
    pub fn cofactor(&self, row: usize, col: usize) -> Float {
        let negate = if (row + col) % 2 == 0 { 1. } else { -1. };
        self.submatrix(row, col).determinant() * negate
    }

    /// Compute determinant of a 3x3 matrix.
    pub fn determinant(&self) -> Float {
        (0..3).map(|i| self.cofactor(0, i) * self[(0, i)]).sum()
    }
}
impl Index<(usize, usize)> for Matrix3x3 {
    type Output = Float;
    fn index(&self, (row, col): (usize, usize)) -> &Self::Output {
        &self.m[row][col]
    }
}

impl PartialEq for Matrix3x3 {
    fn eq(&self, rhs: &Matrix3x3) -> bool {
        let l = self.m;
        let r = rhs.m;
        for i in 0..3 {
            for j in 0..3 {
                let d = (l[i][j] - r[i][j]).abs();
                if d > EPSILON {
                    return false;
                }
            }
        }
        true
    }
}

/// Matrix4x4 represents a 4x4 matrix in row-major form. So, element `m[i][j]` corresponds to m<sub>i,j</sub>
/// where `i` is the row number and `j` is the column number.
///
/// # Examples
/// ```
/// use rtchallenge::{
///     float::consts::PI,
///     matrices::Matrix4x4,
///     tuples::{point, vector},
/// };
///
/// // Individual transformations are applied in sequence.
/// let p = point(1., 0., 1.);
/// let a = Matrix4x4::rotation_x(PI / 2.);
/// let b = Matrix4x4::scaling(5., 5., 5.);
/// let c = Matrix4x4::translation(10., 5., 7.);
/// // Apply rotation first.
/// let p2 = a * p;
/// assert_eq!(p2, point(1., -1., 0.));
/// // Then apply scaling.
/// let p3 = b * p2;
/// assert_eq!(p3, point(5., -5., 0.));
/// // Then apply translation.
/// let p4 = c * p3;
/// assert_eq!(p4, point(15., 0., 7.));
///
/// // Chained transformations must be applied in reverse order.
/// let p = point(1., 0., 1.);
/// let a = Matrix4x4::rotation_x(PI / 2.);
/// let b = Matrix4x4::scaling(5., 5., 5.);
/// let c = Matrix4x4::translation(10., 5., 7.);
/// let t = c * b * a;
/// assert_eq!(t * p, point(15., 0., 7.));
/// ```
#[derive(Copy, Clone, Default)]
pub struct Matrix4x4 {
    m: [[Float; 4]; 4],
}

impl From<[Float; 16]> for Matrix4x4 {
    fn from(t: [Float; 16]) -> Self {
        Matrix4x4 {
            m: [
                [t[0], t[1], t[2], t[3]],
                [t[4], t[5], t[6], t[7]],
                [t[8], t[9], t[10], t[11]],
                [t[12], t[13], t[14], t[15]],
            ],
        }
    }
}

impl Matrix4x4 {
    /// Create a `Matrix4x4` containing the identity, all zeros with ones along the diagonal.
    pub fn identity() -> Matrix4x4 {
        Matrix4x4::new(
            [1., 0., 0., 0.],
            [0., 1., 0., 0.],
            [0., 0., 1., 0.],
            [0., 0., 0., 1.],
        )
    }

    /// Create a `Matrix4x4` with each of the given rows.
    pub fn new(r0: [Float; 4], r1: [Float; 4], r2: [Float; 4], r3: [Float; 4]) -> Matrix4x4 {
        Matrix4x4 {
            m: [r0, r1, r2, r3],
        }
    }

    /// Creates a 4x4 matrix representing a translation of x,y,z.
    pub fn translation(x: Float, y: Float, z: Float) -> Matrix4x4 {
        Matrix4x4::new(
            [1., 0., 0., x],
            [0., 1., 0., y],
            [0., 0., 1., z],
            [0., 0., 0., 1.],
        )
    }

    /// Creates a 4x4 matrix representing a scaling of x,y,z.
    pub fn scaling(x: Float, y: Float, z: Float) -> Matrix4x4 {
        Matrix4x4::new(
            [x, 0., 0., 0.],
            [0., y, 0., 0.],
            [0., 0., z, 0.],
            [0., 0., 0., 1.],
        )
    }

    /// Creates a 4x4 matrix representing a rotation around the x-axis.
    pub fn rotation_x(radians: Float) -> Matrix4x4 {
        let r = radians;
        Matrix4x4::new(
            [1., 0., 0., 0.],
            [0., r.cos(), -r.sin(), 0.],
            [0., r.sin(), r.cos(), 0.],
            [0., 0., 0., 1.],
        )
    }

    /// Creates a 4x4 matrix representing a rotation around the y-axis.
    pub fn rotation_y(radians: Float) -> Matrix4x4 {
        let r = radians;
        Matrix4x4::new(
            [r.cos(), 0., r.sin(), 0.],
            [0., 1., 0., 0.],
            [-r.sin(), 0., r.cos(), 0.],
            [0., 0., 0., 1.],
        )
    }

    /// Creates a 4x4 matrix representing a rotation around the z-axis.
    pub fn rotation_z(radians: Float) -> Matrix4x4 {
        let r = radians;
        Matrix4x4::new(
            [r.cos(), -r.sin(), 0., 0.],
            [r.sin(), r.cos(), 0., 0.],
            [0., 0., 1., 0.],
            [0., 0., 0., 1.],
        )
    }

    /// Transpose self, returning a new matrix that has been reflected across the diagonal.
    pub fn transpose(&self) -> Matrix4x4 {
        let m = self.m;
        Matrix4x4 {
            m: [
                [m[0][0], m[1][0], m[2][0], m[3][0]],
                [m[0][1], m[1][1], m[2][1], m[3][1]],
                [m[0][2], m[1][2], m[2][2], m[3][2]],
                [m[0][3], m[1][3], m[2][3], m[3][3]],
            ],
        }
    }
    /// Create a transform matrix that will shear (skew) points.
    /// # Examples
    pub fn shearing(xy: Float, xz: Float, yx: Float, yz: Float, zx: Float, zy: Float) -> Matrix4x4 {
        Matrix4x4::new(
            [1., xy, xz, 0.],
            [yx, 1., yz, 0.],
            [zx, zy, 1., 0.],
            [0., 0., 0., 1.],
        )
    }

    /// Returns a new matrix that is the inverse of self. If self is A, inverse returns A<sup>-1</sup>, where
    /// AA<sup>-1</sup> = I.
    /// This implementation uses a numerically stable Gauss–Jordan elimination routine to compute the inverse.
    pub fn inverse_rtiow(&self) -> Matrix4x4 {
        // TODO(wathiede): how come the C++ version doesn't need to deal with non-invertable
        // matrix.
        let mut indxc: [usize; 4] = Default::default();
        let mut indxr: [usize; 4] = Default::default();
        let mut ipiv: [usize; 4] = Default::default();
        let mut minv = self.m;

        for i in 0..4 {
            let mut irow: usize = 0;
            let mut icol: usize = 0;
            let mut big: Float = 0.;
            // Choose pivot
            for j in 0..4 {
                if ipiv[j] != 1 {
                    for (k, ipivk) in ipiv.iter().enumerate() {
                        if *ipivk == 0 {
                            if minv[j][k].abs() >= big {
                                big = minv[j][k].abs();
                                irow = j;
                                icol = k;
                            }
                        } else if *ipivk > 1 {
                            eprintln!("Singular matrix in MatrixInvert");
                        }
                    }
                }
            }
            ipiv[icol] += 1;
            // Swap rows _irow_ and _icol_ for pivot
            if irow != icol {
                // Can't figure out how to make swap work here.
                #[allow(clippy::manual_swap)]
                for k in 0..4 {
                    let tmp = minv[irow][k];
                    minv[irow][k] = minv[icol][k];
                    minv[icol][k] = tmp;
                }
            }
            indxr[i] = irow;
            indxc[i] = icol;
            if minv[icol][icol] == 0. {
                eprintln!("Singular matrix in MatrixInvert");
            }

            // Set $m[icol][icol]$ to one by scaling row _icol_ appropriately
            let pivinv: Float = minv[icol][icol].recip();
            minv[icol][icol] = 1.;
            for j in 0..4 {
                minv[icol][j] *= pivinv;
            }

            // Subtract this row from others to zero out their columns
            for j in 0..4 {
                if j != icol {
                    let save = minv[j][icol];
                    minv[j][icol] = 0.;
                    for k in 0..4 {
                        minv[j][k] -= minv[icol][k] * save;
                    }
                }
            }
        }
        // Swap columns to reflect permutation
        for j in (0..4).rev() {
            if indxr[j] != indxc[j] {
                for mi in &mut minv {
                    mi.swap(indxr[j], indxc[j])
                }
            }
        }
        Matrix4x4 { m: minv }
    }
    /// submatrix extracts a 3x3 matrix ignoring the 0-based `row` and `col` given.
    pub fn submatrix(&self, row: usize, col: usize) -> Matrix3x3 {
        assert!(row < 4);
        assert!(col < 4);
        let mut rows = vec![];
        for r in 0..4 {
            if r != row {
                let mut v = vec![];
                for c in 0..4 {
                    if c != col {
                        v.push(self[(r, c)]);
                    }
                }
                rows.push(v);
            }
        }
        let m = [
            [rows[0][0], rows[0][1], rows[0][2]],
            [rows[1][0], rows[1][1], rows[1][2]],
            [rows[2][0], rows[2][1], rows[2][2]],
        ];
        Matrix3x3 { m }
    }

    /// Compute minor of a 4x4 matrix.
    pub fn minor(&self, row: usize, col: usize) -> Float {
        self.submatrix(row, col).determinant()
    }
    /// Compute cofactor of a 4x4 matrix.
    pub fn cofactor(&self, row: usize, col: usize) -> Float {
        let negate = if (row + col) % 2 == 0 { 1. } else { -1. };
        self.submatrix(row, col).determinant() * negate
    }
    /// Compute determinant of a 4x4 matrix.
    pub fn determinant(&self) -> Float {
        (0..4).map(|i| self.cofactor(0, i) * self[(0, i)]).sum()
    }

    /// Compute invertibility of matrix (i.e. non-zero determinant.
    pub fn invertable(&self) -> bool {
        self.determinant() != 0.
    }

    /// Compute the inverse of a 4x4 matrix.
    pub fn inverse(&self) -> Matrix4x4 {
        self.inverse_rtc()
    }
    pub fn inverse_rtc(&self) -> Matrix4x4 {
        let m = self;
        if !m.invertable() {
            panic!("Matrix4x4::inverse called on matrix with determinant() == 0");
        }
        let mut m2 = Matrix4x4::identity();
        let det = m.determinant();
        for row in 0..4 {
            for col in 0..4 {
                let c = m.cofactor(row, col);
                m2[(col, row)] = c / det;
            }
        }
        m2
    }
}

impl fmt::Debug for Matrix4x4 {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        if f.alternate() {
            write!(
                f,
                "\n  {:8.5?}\n  {:8.5?}\n  {:8.5?}\n  {:8.5?}",
                self.m[0], self.m[1], self.m[2], self.m[3]
            )
        } else {
            write!(
                f,
                "[{:?} {:?} {:?} {:?}]",
                self.m[0], self.m[1], self.m[2], self.m[3]
            )
        }
    }
}

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

    /// Implement matrix multiplication for `Matrix4x4`.
    ///
    /// # Examples
    /// ```
    /// use rtchallenge::matrices::Matrix4x4;
    ///
    /// let i = Matrix4x4::identity();
    /// let m1 = Matrix4x4::identity();
    /// let m2 = Matrix4x4::identity();
    ///
    /// assert_eq!(m1 * m2, i);
    /// ```
    fn mul(self, m2: Matrix4x4) -> Matrix4x4 {
        let m1 = self;
        let mut r: Matrix4x4 = Default::default();
        for i in 0..4 {
            for j in 0..4 {
                r.m[i][j] = m1.m[i][0] * m2.m[0][j]
                    + m1.m[i][1] * m2.m[1][j]
                    + m1.m[i][2] * m2.m[2][j]
                    + m1.m[i][3] * m2.m[3][j];
            }
        }
        r
    }
}

impl Mul<Tuple> for Matrix4x4 {
    type Output = Tuple;

    /// Implement matrix multiplication for `Matrix4x4` * `Tuple`.
    ///
    /// # Examples
    /// ```
    /// use rtchallenge::matrices::Matrix4x4;
    /// use rtchallenge::tuples::Tuple;
    ///
    /// let a = Matrix4x4::new(
    ///     [1., 2., 3., 4.],
    ///     [2., 4., 4., 2.],
    ///     [8., 6., 4., 1.],
    ///     [0., 0., 0., 1.],
    /// );
    /// let b = Tuple::new(1., 2., 3., 1.);
    ///
    /// assert_eq!(a * b, Tuple::new(18., 24., 33., 1.));
    /// ```
    fn mul(self, t: Tuple) -> Tuple {
        let m = self;
        Tuple {
            x: m.m[0][0] * t.x + m.m[0][1] * t.y + m.m[0][2] * t.z + m.m[0][3] * t.w,
            y: m.m[1][0] * t.x + m.m[1][1] * t.y + m.m[1][2] * t.z + m.m[1][3] * t.w,
            z: m.m[2][0] * t.x + m.m[2][1] * t.y + m.m[2][2] * t.z + m.m[2][3] * t.w,
            w: m.m[3][0] * t.x + m.m[3][1] * t.y + m.m[3][2] * t.z + m.m[3][3] * t.w,
        }
    }
}

impl Sub for Matrix4x4 {
    type Output = Matrix4x4;

    fn sub(self, m2: Matrix4x4) -> Matrix4x4 {
        let m1 = self;
        let mut r: Matrix4x4 = Default::default();
        for i in 0..4 {
            for j in 0..4 {
                r.m[i][j] = m1.m[i][j] - m2.m[i][j];
            }
        }
        r
    }
}

impl PartialEq for Matrix4x4 {
    fn eq(&self, rhs: &Matrix4x4) -> bool {
        let l = self.m;
        let r = rhs.m;
        for i in 0..4 {
            for j in 0..4 {
                let d = (l[i][j] - r[i][j]).abs();
                if d > EPSILON {
                    return false;
                }
            }
        }
        true
    }
}

impl Index<(usize, usize)> for Matrix4x4 {
    type Output = Float;
    fn index(&self, (row, col): (usize, usize)) -> &Self::Output {
        &self.m[row][col]
    }
}

impl IndexMut<(usize, usize)> for Matrix4x4 {
    fn index_mut(&mut self, (row, col): (usize, usize)) -> &mut Self::Output {
        &mut self.m[row][col]
    }
}

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

    use crate::{
        float::consts::PI,
        tuples::{point, vector},
    };

    #[test]
    fn translation() {
        let transform = Matrix4x4::translation(5., -3., 2.);
        let p = point(-3., 4., 5.);
        assert_eq!(transform * p, point(2., 1., 7.));

        let inv = transform.inverse();
        assert_eq!(inv * p, point(-8., 7., 3.));

        let v = vector(-3., 4., 5.);
        assert_eq!(transform * v, v);
    }
    #[test]
    fn scaling() {
        // A scaling matrix applied to a point.
        let transform = Matrix4x4::scaling(2., 3., 4.);
        let p = point(-4., 6., 8.);
        assert_eq!(transform * p, point(-8., 18., 32.));

        // A scaling matrix applied to a vector.
        let v = vector(-4., 6., 8.);
        assert_eq!(transform * v, vector(-8., 18., 32.));

        // Multiplying by the inverse of a scaling matrix.
        let inv = transform.inverse();
        assert_eq!(inv * v, vector(-2., 2., 2.));

        // Reflection is scaling by a negative value.
        let transform = Matrix4x4::scaling(-1., 1., 1.);
        let p = point(2., 3., 4.);
        assert_eq!(transform * p, point(-2., 3., 4.));
    }
    #[test]
    fn rotation_x() {
        // A scaling matrix applied to a point.
        let p = point(0., 1., 0.);
        let half_quarter = Matrix4x4::rotation_x(PI / 4.);
        let full_quarter = Matrix4x4::rotation_x(PI / 2.);

        assert_eq!(
            half_quarter * p,
            point(0., (2.0 as Float).sqrt() / 2., (2.0 as Float).sqrt() / 2.)
        );
        assert_eq!(full_quarter * p, point(0., 0., 1.),);
    }
    #[test]
    fn rotation_y() {
        // A scaling matrix applied to a point.
        let p = point(0., 0., 1.);
        let half_quarter = Matrix4x4::rotation_y(PI / 4.);
        let full_quarter = Matrix4x4::rotation_y(PI / 2.);

        assert_eq!(
            half_quarter * p,
            point((2.0 as Float).sqrt() / 2., 0., (2.0 as Float).sqrt() / 2.)
        );
        assert_eq!(full_quarter * p, point(1., 0., 0.,),);
    }
    #[test]
    fn rotation_z() {
        // A scaling matrix applied to a point.
        let p = point(0., 1., 0.);
        let half_quarter = Matrix4x4::rotation_z(PI / 4.);
        let full_quarter = Matrix4x4::rotation_z(PI / 2.);

        assert_eq!(
            half_quarter * p,
            point(-(2.0 as Float).sqrt() / 2., (2.0 as Float).sqrt() / 2., 0.)
        );
        assert_eq!(full_quarter * p, point(-1., 0., 0.,),);
    }
    #[test]
    fn transpose() {
        let m = Matrix4x4::new(
            [2., 0., 0., 0.],
            [3., 1., 0., 0.],
            [4., 0., 1., 0.],
            [5., 6., 7., 1.],
        );
        let m_t = Matrix4x4::new(
            [2., 3., 4., 5.],
            [0., 1., 0., 6.],
            [0., 0., 1., 7.],
            [0., 0., 0., 1.],
        );
        assert_eq!(m.transpose(), m_t);

        assert_eq!(Matrix4x4::identity(), Matrix4x4::identity().transpose());
    }
    #[test]
    fn shearing() {
        // A shearing transform moves x in proportion to y.
        let transform = Matrix4x4::shearing(1., 0., 0., 0., 0., 0.);
        let p = point(2., 3., 4.);
        assert_eq!(transform * p, point(5., 3., 4.));

        // A shearing transform moves x in proportion to z.
        let transform = Matrix4x4::shearing(0., 1., 0., 0., 0., 0.);
        let p = point(2., 3., 4.);
        assert_eq!(transform * p, point(6., 3., 4.));

        // A shearing transform moves y in proportion to x.
        let transform = Matrix4x4::shearing(0., 0., 1., 0., 0., 0.);
        let p = point(2., 3., 4.);
        assert_eq!(transform * p, point(2., 5., 4.));

        // A shearing transform moves y in proportion to z.
        let transform = Matrix4x4::shearing(0., 0., 0., 1., 0., 0.);
        let p = point(2., 3., 4.);
        assert_eq!(transform * p, point(2., 7., 4.));

        // A shearing transform moves z in proportion to x.
        let transform = Matrix4x4::shearing(0., 0., 0., 0., 1., 0.);
        let p = point(2., 3., 4.);
        assert_eq!(transform * p, point(2., 3., 6.));

        // A shearing transform moves z in proportion to y.
        let transform = Matrix4x4::shearing(0., 0., 0., 0., 0., 1.);
        let p = point(2., 3., 4.);
        assert_eq!(transform * p, point(2., 3., 7.));
    }
    #[test]
    fn inverse_rtiow() {
        let i = Matrix4x4::identity();
        assert_eq!(i.inverse_rtiow() * i, i);

        let m = Matrix4x4::new(
            [2., 0., 0., 0.],
            [0., 3., 0., 0.],
            [0., 0., 4., 0.],
            [0., 0., 0., 1.],
        );
        assert_eq!(m.inverse_rtiow() * m, i);
        assert_eq!(m * m.inverse_rtiow(), i);
    }
    #[test]
    fn determinant_2x2() {
        let a = Matrix2x2::new([1., 5.], [-3., 2.]);

        assert_eq!(a.determinant(), 17.);
    }
    #[test]
    fn determinant_3x3() {
        let a = Matrix3x3::new([1., 2., 6.], [-5., 8., -4.], [2., 6., 4.]);
        assert_eq!(a.cofactor(0, 0), 56.);
        assert_eq!(a.cofactor(0, 1), 12.);
        assert_eq!(a.cofactor(0, 2), -46.);
        assert_eq!(a.determinant(), -196.);
    }
    #[test]
    fn determinant_4x4() {
        let a = Matrix4x4::new(
            [-2., -8., 3., 5.],
            [-3., 1., 7., 3.],
            [1., 2., -9., 6.],
            [-6., 7., 7., -9.],
        );
        assert_eq!(a.cofactor(0, 0), 690.);
        assert_eq!(a.cofactor(0, 1), 447.);
        assert_eq!(a.cofactor(0, 2), 210.);
        assert_eq!(a.cofactor(0, 3), 51.);
        assert_eq!(a.determinant(), -4071.);
    }
    #[test]
    fn submatrix_3x3() {
        assert_eq!(
            Matrix3x3::new([1., 5., 0.], [-3., 2., 7.], [0., 6., -3.],).submatrix(0, 2),
            Matrix2x2::new([-3., 2.], [0., 6.])
        );
    }
    #[test]
    fn submatrix_4x4() {
        assert_eq!(
            Matrix4x4::new(
                [-6., 1., 1., 6.],
                [-8., 5., 8., 6.],
                [-1., 0., 8., 2.],
                [-7., 1., -1., 1.],
            )
            .submatrix(2, 1),
            Matrix3x3::new([-6., 1., 6.], [-8., 8., 6.], [-7., -1., 1.],)
        );
    }
    #[test]
    fn minor_3x3() {
        let a = Matrix3x3::new([3., 5., 0.], [2., -1., -7.], [6., -1., 5.]);
        let b = a.submatrix(1, 0);
        assert_eq!(b.determinant(), 25.0);
        assert_eq!(b.determinant(), a.minor(1, 0));
    }
    #[test]
    fn cofactor_3x3() {
        let a = Matrix3x3::new([3., 5., 0.], [2., -1., -7.], [6., -1., 5.]);
        assert_eq!(a.minor(0, 0), -12.);
        assert_eq!(a.cofactor(0, 0), -12.);
        assert_eq!(a.minor(1, 0), 25.);
        assert_eq!(a.cofactor(1, 0), -25.);
    }
    #[test]
    fn construct2x2() {
        let m = Matrix2x2::new([-3., 5.], [1., -2.]);

        assert_eq!(m[(0, 0)], -3.);
        assert_eq!(m[(0, 1)], 5.);
        assert_eq!(m[(1, 0)], 1.);
        assert_eq!(m[(1, 1)], -2.);
    }
    #[test]
    fn construct3x3() {
        let m = Matrix3x3::new([-3., 5., 0.], [1., -2., -7.], [0., 1., 1.]);

        assert_eq!(m[(0, 0)], -3.);
        assert_eq!(m[(1, 1)], -2.);
        assert_eq!(m[(2, 2)], 1.);
    }
    #[test]
    fn invertable() {
        let a = Matrix4x4::new(
            [6., 4., 4., 4.],
            [5., 5., 7., 6.],
            [4., -9., 3., -7.],
            [9., 1., 7., -6.],
        );
        assert_eq!(a.determinant(), -2120.);
        assert_eq!(a.invertable(), true);

        let a = Matrix4x4::new(
            [-4., 2., -2., -3.],
            [9., 6., 2., 6.],
            [0., -5., 1., -5.],
            [0., 0., 0., 0.],
        );
        assert_eq!(a.determinant(), 0.);
        assert_eq!(a.invertable(), false);
    }
    #[test]
    fn inverse() {
        let a = Matrix4x4::new(
            [-5., 2., 6., -8.],
            [1., -5., 1., 8.],
            [7., 7., -6., -7.],
            [1., -3., 7., 4.],
        );
        let b = a.inverse();

        assert_eq!(a.determinant(), 532.);
        assert_eq!(a.cofactor(2, 3), -160.);
        assert_eq!(b[(3, 2)], -160. / 532.);
        assert_eq!(a.cofactor(3, 2), 105.);
        assert_eq!(b[(2, 3)], 105. / 532.);
        assert_eq!(
            b,
            Matrix4x4::new(
                [0.21804512, 0.45112783, 0.24060151, -0.04511278],
                [-0.8082707, -1.456767, -0.44360903, 0.5206767],
                [-0.078947365, -0.2236842, -0.05263158, 0.19736843],
                [-0.52255636, -0.81390977, -0.30075186, 0.30639097]
            )
        );

        // Second test case
        assert_eq!(
            Matrix4x4::new(
                [8., -5., 9., 2.],
                [7., 5., 6., 1.],
                [-6., 0., 9., 6.],
                [-3., 0., -9., -4.],
            )
            .inverse(),
            Matrix4x4::new(
                [-0.15384616, -0.15384616, -0.2820513, -0.53846157],
                [-0.07692308, 0.12307692, 0.025641026, 0.03076923],
                [0.35897437, 0.35897437, 0.43589744, 0.9230769],
                [-0.6923077, -0.6923077, -0.7692308, -1.9230769]
            ),
        );

        // Third test case
        assert_eq!(
            Matrix4x4::new(
                [9., 3., 0., 9.],
                [-5., -2., -6., -3.],
                [-4., 9., 6., 4.],
                [-7., 6., 6., 2.],
            )
            .inverse(),
            Matrix4x4::new(
                [-0.04074074, -0.07777778, 0.14444445, -0.22222222],
                [-0.07777778, 0.033333335, 0.36666667, -0.33333334],
                [-0.029012345, -0.14629629, -0.10925926, 0.12962963],
                [0.17777778, 0.06666667, -0.26666668, 0.33333334]
            ),
        );

        let a = Matrix4x4::new(
            [3., -9., 7., 3.],
            [3., -8., 2., -9.],
            [-4., 4., 4., 1.],
            [-6., 5., -1., 1.],
        );
        let b = Matrix4x4::new(
            [8., 2., 2., 2.],
            [3., -1., 7., 0.],
            [7., 0., 5., 4.],
            [6., -2., 0., 5.],
        );
        let c = a * b;
        assert_eq!(c * b.inverse(), a);
    }
    #[test]
    fn construct4x4() {
        let m = Matrix4x4::new(
            [1., 2., 3., 4.],
            [5.5, 6.5, 7.5, 8.5],
            [9., 10., 11., 12.],
            [13.5, 14.5, 15.5, 16.5],
        );

        assert_eq!(m[(0, 0)], 1.);
        assert_eq!(m[(0, 3)], 4.);
        assert_eq!(m[(1, 0)], 5.5);
        assert_eq!(m[(1, 2)], 7.5);
        assert_eq!(m[(2, 2)], 11.);
        assert_eq!(m[(3, 0)], 13.5);
        assert_eq!(m[(3, 2)], 15.5);
    }
    #[test]
    fn equality4x4() {
        let a = Matrix4x4::new(
            [1., 2., 3., 4.],
            [5., 6., 7., 8.],
            [9., 8., 7., 6.],
            [5., 4., 3., 2.],
        );
        let b = Matrix4x4::new(
            [1., 2., 3., 4.],
            [5., 6., 7., 8.],
            [9., 8., 7., 6.],
            [5., 4., 3., 2.],
        );
        assert_eq!(a, b);
    }
    #[test]
    fn inequality4x4() {
        let a = Matrix4x4::new(
            [1., 2., 3., 4.],
            [5., 6., 7., 8.],
            [9., 8., 7., 6.],
            [5., 4., 3., 2.],
        );
        let b = Matrix4x4::new(
            [2., 3., 4., 5.],
            [6., 7., 8., 9.],
            [8., 7., 6., 5.],
            [4., 3., 2., 1.],
        );
        assert_ne!(a, b);
    }
    #[test]
    fn mul4x4() {
        let a = Matrix4x4::new(
            [1., 2., 3., 4.],
            [5., 6., 7., 8.],
            [9., 8., 7., 6.],
            [5., 4., 3., 2.],
        );
        let b = Matrix4x4::new(
            [-2., 1., 2., 3.],
            [3., 2., 1., -1.],
            [4., 3., 6., 5.],
            [1., 2., 7., 8.],
        );
        assert_eq!(
            a * b,
            Matrix4x4::new(
                [20., 22., 50., 48.],
                [44., 54., 114., 108.],
                [40., 58., 110., 102.],
                [16., 26., 46., 42.],
            )
        );
    }
}