//! 4x4 matrix inverse
// Code ported from the `packed_simd` crate
// Run this code with `cargo test --example matrix_inversion`
#![feature(array_chunks, portable_simd)]
use core_simd::simd::*;
use Which::*;

// Gotta define our own 4x4 matrix since Rust doesn't ship multidim arrays yet :^)
#[derive(Copy, Clone, Debug, PartialEq, PartialOrd)]
pub struct Matrix4x4([[f32; 4]; 4]);

#[allow(clippy::too_many_lines)]
pub fn scalar_inv4x4(m: Matrix4x4) -> Option<Matrix4x4> {
    let m = m.0;

    #[rustfmt::skip]
    let mut inv = [
        // row 0:
        [
            // 0,0:
            m[1][1] * m[2][2] * m[3][3] -
            m[1][1] * m[2][3] * m[3][2] -
            m[2][1] * m[1][2] * m[3][3] +
            m[2][1] * m[1][3] * m[3][2] +
            m[3][1] * m[1][2] * m[2][3] -
            m[3][1] * m[1][3] * m[2][2],
            // 0,1:
           -m[0][1] * m[2][2] * m[3][3] +
            m[0][1] * m[2][3] * m[3][2] +
            m[2][1] * m[0][2] * m[3][3] -
            m[2][1] * m[0][3] * m[3][2] -
            m[3][1] * m[0][2] * m[2][3] +
            m[3][1] * m[0][3] * m[2][2],
            // 0,2:
            m[0][1] * m[1][2] * m[3][3] -
            m[0][1] * m[1][3] * m[3][2] -
            m[1][1] * m[0][2] * m[3][3] +
            m[1][1] * m[0][3] * m[3][2] +
            m[3][1] * m[0][2] * m[1][3] -
            m[3][1] * m[0][3] * m[1][2],
            // 0,3:
           -m[0][1] * m[1][2] * m[2][3] +
            m[0][1] * m[1][3] * m[2][2] +
            m[1][1] * m[0][2] * m[2][3] -
            m[1][1] * m[0][3] * m[2][2] -
            m[2][1] * m[0][2] * m[1][3] +
            m[2][1] * m[0][3] * m[1][2],
        ],
        // row 1
        [
            // 1,0:
           -m[1][0] * m[2][2] * m[3][3] +
            m[1][0] * m[2][3] * m[3][2] +
            m[2][0] * m[1][2] * m[3][3] -
            m[2][0] * m[1][3] * m[3][2] -
            m[3][0] * m[1][2] * m[2][3] +
            m[3][0] * m[1][3] * m[2][2],
            // 1,1:
            m[0][0] * m[2][2] * m[3][3] -
            m[0][0] * m[2][3] * m[3][2] -
            m[2][0] * m[0][2] * m[3][3] +
            m[2][0] * m[0][3] * m[3][2] +
            m[3][0] * m[0][2] * m[2][3] -
            m[3][0] * m[0][3] * m[2][2],
            // 1,2:
           -m[0][0] * m[1][2] * m[3][3] +
            m[0][0] * m[1][3] * m[3][2] +
            m[1][0] * m[0][2] * m[3][3] -
            m[1][0] * m[0][3] * m[3][2] -
            m[3][0] * m[0][2] * m[1][3] +
            m[3][0] * m[0][3] * m[1][2],
            // 1,3:
            m[0][0] * m[1][2] * m[2][3] -
            m[0][0] * m[1][3] * m[2][2] -
            m[1][0] * m[0][2] * m[2][3] +
            m[1][0] * m[0][3] * m[2][2] +
            m[2][0] * m[0][2] * m[1][3] -
            m[2][0] * m[0][3] * m[1][2],
        ],
        // row 2
        [
            // 2,0:
            m[1][0] * m[2][1] * m[3][3] -
            m[1][0] * m[2][3] * m[3][1] -
            m[2][0] * m[1][1] * m[3][3] +
            m[2][0] * m[1][3] * m[3][1] +
            m[3][0] * m[1][1] * m[2][3] -
            m[3][0] * m[1][3] * m[2][1],
            // 2,1:
           -m[0][0] * m[2][1] * m[3][3] +
            m[0][0] * m[2][3] * m[3][1] +
            m[2][0] * m[0][1] * m[3][3] -
            m[2][0] * m[0][3] * m[3][1] -
            m[3][0] * m[0][1] * m[2][3] +
            m[3][0] * m[0][3] * m[2][1],
            // 2,2:
            m[0][0] * m[1][1] * m[3][3] -
            m[0][0] * m[1][3] * m[3][1] -
            m[1][0] * m[0][1] * m[3][3] +
            m[1][0] * m[0][3] * m[3][1] +
            m[3][0] * m[0][1] * m[1][3] -
            m[3][0] * m[0][3] * m[1][1],
            // 2,3:
           -m[0][0] * m[1][1] * m[2][3] +
            m[0][0] * m[1][3] * m[2][1] +
            m[1][0] * m[0][1] * m[2][3] -
            m[1][0] * m[0][3] * m[2][1] -
            m[2][0] * m[0][1] * m[1][3] +
            m[2][0] * m[0][3] * m[1][1],
        ],
        // row 3
        [
            // 3,0:
           -m[1][0] * m[2][1] * m[3][2] +
            m[1][0] * m[2][2] * m[3][1] +
            m[2][0] * m[1][1] * m[3][2] -
            m[2][0] * m[1][2] * m[3][1] -
            m[3][0] * m[1][1] * m[2][2] +
            m[3][0] * m[1][2] * m[2][1],
            // 3,1:
            m[0][0] * m[2][1] * m[3][2] -
            m[0][0] * m[2][2] * m[3][1] -
            m[2][0] * m[0][1] * m[3][2] +
            m[2][0] * m[0][2] * m[3][1] +
            m[3][0] * m[0][1] * m[2][2] -
            m[3][0] * m[0][2] * m[2][1],
            // 3,2:
           -m[0][0] * m[1][1] * m[3][2] +
            m[0][0] * m[1][2] * m[3][1] +
            m[1][0] * m[0][1] * m[3][2] -
            m[1][0] * m[0][2] * m[3][1] -
            m[3][0] * m[0][1] * m[1][2] +
            m[3][0] * m[0][2] * m[1][1],
            // 3,3:
            m[0][0] * m[1][1] * m[2][2] -
            m[0][0] * m[1][2] * m[2][1] -
            m[1][0] * m[0][1] * m[2][2] +
            m[1][0] * m[0][2] * m[2][1] +
            m[2][0] * m[0][1] * m[1][2] -
            m[2][0] * m[0][2] * m[1][1],
        ],
    ];

    let det = m[0][0] * inv[0][0] + m[0][1] * inv[1][0] + m[0][2] * inv[2][0] + m[0][3] * inv[3][0];
    if det == 0. {
        return None;
    }

    let det_inv = 1. / det;

    for row in &mut inv {
        for elem in row.iter_mut() {
            *elem *= det_inv;
        }
    }

    Some(Matrix4x4(inv))
}

pub fn simd_inv4x4(m: Matrix4x4) -> Option<Matrix4x4> {
    let m = m.0;
    let m_0 = f32x4::from_array(m[0]);
    let m_1 = f32x4::from_array(m[1]);
    let m_2 = f32x4::from_array(m[2]);
    let m_3 = f32x4::from_array(m[3]);

    const SHUFFLE01: [Which; 4] = [First(0), First(1), Second(0), Second(1)];
    const SHUFFLE02: [Which; 4] = [First(0), First(2), Second(0), Second(2)];
    const SHUFFLE13: [Which; 4] = [First(1), First(3), Second(1), Second(3)];
    const SHUFFLE23: [Which; 4] = [First(2), First(3), Second(2), Second(3)];

    let tmp = simd_swizzle!(m_0, m_1, SHUFFLE01);
    let row1 = simd_swizzle!(m_2, m_3, SHUFFLE01);

    let row0 = simd_swizzle!(tmp, row1, SHUFFLE02);
    let row1 = simd_swizzle!(row1, tmp, SHUFFLE13);

    let tmp = simd_swizzle!(m_0, m_1, SHUFFLE23);
    let row3 = simd_swizzle!(m_2, m_3, SHUFFLE23);
    let row2 = simd_swizzle!(tmp, row3, SHUFFLE02);
    let row3 = simd_swizzle!(row3, tmp, SHUFFLE13);

    let tmp = (row2 * row3).reverse().rotate_lanes_right::<2>();
    let minor0 = row1 * tmp;
    let minor1 = row0 * tmp;
    let tmp = tmp.rotate_lanes_right::<2>();
    let minor0 = (row1 * tmp) - minor0;
    let minor1 = (row0 * tmp) - minor1;
    let minor1 = minor1.rotate_lanes_right::<2>();

    let tmp = (row1 * row2).reverse().rotate_lanes_right::<2>();
    let minor0 = (row3 * tmp) + minor0;
    let minor3 = row0 * tmp;
    let tmp = tmp.rotate_lanes_right::<2>();

    let minor0 = minor0 - row3 * tmp;
    let minor3 = row0 * tmp - minor3;
    let minor3 = minor3.rotate_lanes_right::<2>();

    let tmp = (row3 * row1.rotate_lanes_right::<2>())
        .reverse()
        .rotate_lanes_right::<2>();
    let row2 = row2.rotate_lanes_right::<2>();
    let minor0 = row2 * tmp + minor0;
    let minor2 = row0 * tmp;
    let tmp = tmp.rotate_lanes_right::<2>();
    let minor0 = minor0 - row2 * tmp;
    let minor2 = row0 * tmp - minor2;
    let minor2 = minor2.rotate_lanes_right::<2>();

    let tmp = (row0 * row1).reverse().rotate_lanes_right::<2>();
    let minor2 = minor2 + row3 * tmp;
    let minor3 = row2 * tmp - minor3;
    let tmp = tmp.rotate_lanes_right::<2>();
    let minor2 = row3 * tmp - minor2;
    let minor3 = minor3 - row2 * tmp;

    let tmp = (row0 * row3).reverse().rotate_lanes_right::<2>();
    let minor1 = minor1 - row2 * tmp;
    let minor2 = row1 * tmp + minor2;
    let tmp = tmp.rotate_lanes_right::<2>();
    let minor1 = row2 * tmp + minor1;
    let minor2 = minor2 - row1 * tmp;

    let tmp = (row0 * row2).reverse().rotate_lanes_right::<2>();
    let minor1 = row3 * tmp + minor1;
    let minor3 = minor3 - row1 * tmp;
    let tmp = tmp.rotate_lanes_right::<2>();
    let minor1 = minor1 - row3 * tmp;
    let minor3 = row1 * tmp + minor3;

    let det = row0 * minor0;
    let det = det.rotate_lanes_right::<2>() + det;
    let det = det.reverse().rotate_lanes_right::<2>() + det;

    if det.reduce_sum() == 0. {
        return None;
    }
    // calculate the reciprocal
    let tmp = f32x4::splat(1.0) / det;
    let det = tmp + tmp - det * tmp * tmp;

    let res0 = minor0 * det;
    let res1 = minor1 * det;
    let res2 = minor2 * det;
    let res3 = minor3 * det;

    let mut m = m;

    m[0] = res0.to_array();
    m[1] = res1.to_array();
    m[2] = res2.to_array();
    m[3] = res3.to_array();

    Some(Matrix4x4(m))
}

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

    #[test]
    fn test() {
    let tests: &[(Matrix4x4, Option<Matrix4x4>)] = &[
        // Identity:
        (Matrix4x4([
            [1., 0., 0., 0.],
            [0., 1., 0., 0.],
            [0., 0., 1., 0.],
            [0., 0., 0., 1.],
         ]),
         Some(Matrix4x4([
             [1., 0., 0., 0.],
             [0., 1., 0., 0.],
             [0., 0., 1., 0.],
             [0., 0., 0., 1.],
         ]))
        ),
        // None:
        (Matrix4x4([
            [1., 2., 3., 4.],
            [12., 11., 10., 9.],
            [5., 6., 7., 8.],
            [16., 15., 14., 13.],
        ]),
         None
        ),
        // Other:
        (Matrix4x4([
            [1., 1., 1., 0.],
            [0., 3., 1., 2.],
            [2., 3., 1., 0.],
            [1., 0., 2., 1.],
        ]),
         Some(Matrix4x4([
             [-3., -0.5,   1.5,  1.0],
             [ 1., 0.25, -0.25, -0.5],
             [ 3., 0.25, -1.25, -0.5],
             [-3., 0.0,    1.0,  1.0],
         ]))
        ),


    ];

        for &(input, output) in tests {
            assert_eq!(scalar_inv4x4(input), output);
            assert_eq!(simd_inv4x4(input), output);
        }
    }
}

fn main() {
    // Empty main to make cargo happy
}