mat4 0.2.1

use core::ops::{Add, Mul, Sub};

use num_traits::{One, Zero};

#[inline]
pub fn set<T>(
    out: &mut [T; 16],
    m00: T,
    m01: T,
    m02: T,
    m03: T,
    m10: T,
    m11: T,
    m12: T,
    m13: T,
    m20: T,
    m21: T,
    m22: T,
    m23: T,
    m30: T,
    m31: T,
    m32: T,
    m33: T,
) -> &mut [T; 16] {
    out[0] = m00;
    out[4] = m01;
    out[8] = m02;
    out[12] = m03;
    out[1] = m10;
    out[5] = m11;
    out[9] = m12;
    out[13] = m13;
    out[2] = m20;
    out[6] = m21;
    out[10] = m22;
    out[14] = m23;
    out[3] = m30;
    out[7] = m31;
    out[11] = m32;
    out[15] = m33;
    out
}

#[inline(always)]
pub fn set_identity<T>(out: &mut [T; 16]) -> &mut [T; 16]
where
    T: One + Zero,
{
    set(
        out,
        T::one(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::one(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::one(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::one(),
    )
}

#[inline(always)]
pub fn set_zero<T>(out: &mut [T; 16]) -> &mut [T; 16]
where
    T: Zero,
{
    set(
        out,
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
    )
}

#[inline(always)]
pub fn set_one<T>(out: &mut [T; 16]) -> &mut [T; 16]
where
    T: One,
{
    set(
        out,
        T::one(),
        T::one(),
        T::one(),
        T::one(),
        T::one(),
        T::one(),
        T::one(),
        T::one(),
        T::one(),
        T::one(),
        T::one(),
        T::one(),
        T::one(),
        T::one(),
        T::one(),
        T::one(),
    )
}

#[inline]
pub fn set_position<'out, T>(out: &'out mut [T; 16], v: &[T; 3]) -> &'out mut [T; 16]
where
    T: Clone,
{
    out[12] = v[0].clone();
    out[13] = v[1].clone();
    out[14] = v[2].clone();
    out
}

#[inline]
pub fn set_mat2<'out, T>(out: &'out mut [T; 16], m: &[T; 4]) -> &'out mut [T; 16]
where
    T: Zero + One + Clone,
{
    set(
        out,
        m[0].clone(),
        m[2].clone(),
        T::zero(),
        T::zero(),
        m[1].clone(),
        m[3].clone(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::one(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::one(),
    )
}
#[inline]
pub fn set_mat32<'out, T>(out: &'out mut [T; 16], m: &[T; 6]) -> &'out mut [T; 16]
where
    T: Zero + One + Clone,
{
    set(
        out,
        m[0].clone(),
        m[2].clone(),
        T::zero(),
        m[4].clone(),
        m[1].clone(),
        m[3].clone(),
        T::zero(),
        m[5].clone(),
        T::zero(),
        T::zero(),
        T::one(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::one(),
    )
}
#[inline]
pub fn set_mat3<'out, T>(out: &'out mut [T; 16], m: &[T; 9]) -> &'out mut [T; 16]
where
    T: Zero + One + Clone,
{
    set(
        out,
        m[0].clone(),
        m[3].clone(),
        m[6].clone(),
        T::zero(),
        m[1].clone(),
        m[4].clone(),
        m[7].clone(),
        T::zero(),
        m[2].clone(),
        m[5].clone(),
        m[8].clone(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::zero(),
        T::one(),
    )
}

/// # Example
/// ```
/// let mut m = mat4::new_identity::<f32>();
/// mat4::set_quat(&mut m, &[0_f32, 0_f32, 0_f32, 1_f32]);
/// assert_eq!(m, mat4::new_identity::<f32>());
/// ```
#[inline]
pub fn set_quat<'out, T>(out: &'out mut [T; 16], q: &[T; 4]) -> &'out mut [T; 16]
where
    T: One + Zero,
    for<'a, 'b> &'a T: Mul<&'b T, Output = T> + Add<&'b T, Output = T> + Sub<&'b T, Output = T>,
{
    let x = &q[0];
    let y = &q[1];
    let z = &q[2];
    let w = &q[3];
    let x2 = x + x;
    let y2 = y + y;
    let z2 = z + z;
    let xx = x * &x2;
    let xy = x * &y2;
    let xz = x * &z2;
    let yy = y * &y2;
    let yz = y * &z2;
    let zz = z * &z2;
    let wx = w * &x2;
    let wy = w * &y2;
    let wz = w * &z2;

    out[0] = &T::one() - &(&yy + &zz);
    out[4] = &xy - &wz;
    out[8] = &xz + &wy;

    out[1] = &xy + &wz;
    out[5] = &T::one() - &(&xx + &zz);
    out[9] = &yz - &wx;

    out[2] = &xz - &wy;
    out[6] = &yz + &wx;
    out[10] = &T::one() - &(&xx + &yy);

    out[3] = T::zero();
    out[7] = T::zero();
    out[11] = T::zero();

    out[12] = T::zero();
    out[13] = T::zero();
    out[14] = T::zero();
    out[15] = T::one();

    out
}