gemath 0.1.0

use crate::vec3::Vec3;
use crate::vec4::{Vec4, Meters, Pixels, World, Local, Screen};
use core::ops::{Add, Mul, Sub};
use core::marker::PhantomData;
use crate::math;
#[cfg(feature = "unit_vec")]
use crate::unit_vec::UnitVec3;

#[derive(Debug, Clone, Copy, PartialEq)]
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
pub struct Mat4<Unit: Copy = (), Space: Copy = ()> {
    pub x_col: Vec4<Unit, Space>,
    pub y_col: Vec4<Unit, Space>,
    pub z_col: Vec4<Unit, Space>,
    pub w_col: Vec4<Unit, Space>,
    #[cfg_attr(feature = "serde", serde(skip))]
    pub _unit: PhantomData<Unit>,
    #[cfg_attr(feature = "serde", serde(skip))]
    pub _space: PhantomData<Space>,
}

// Type aliases for common units and spaces
pub type Mat4f32 = Mat4<(),()>;
pub type Mat4Meters = Mat4<Meters,()>;
pub type Mat4Pixels = Mat4<Pixels,()>;
pub type Mat4World = Mat4<(),World>;
pub type Mat4Local = Mat4<(),Local>;
pub type Mat4Screen = Mat4<(),Screen>;
pub type Mat4MetersWorld = Mat4<Meters,World>;
pub type Mat4PixelsScreen = Mat4<Pixels,Screen>;

impl<Unit: Copy, Space: Copy> Mat4<Unit, Space> {
    pub const ZERO: Self = Self {
        x_col: Vec4::ZERO,
        y_col: Vec4::ZERO,
        z_col: Vec4::ZERO,
        w_col: Vec4::ZERO,
        _unit: PhantomData,
        _space: PhantomData,
    };

    pub const IDENTITY: Self = Self {
        x_col: Vec4::new(1.0, 0.0, 0.0, 0.0),
        y_col: Vec4::new(0.0, 1.0, 0.0, 0.0),
        z_col: Vec4::new(0.0, 0.0, 1.0, 0.0),
        w_col: Vec4::new(0.0, 0.0, 0.0, 1.0),
        _unit: PhantomData,
        _space: PhantomData,
    };

    #[inline]
    pub const fn new(x_col: Vec4<Unit, Space>, y_col: Vec4<Unit, Space>, z_col: Vec4<Unit, Space>, w_col: Vec4<Unit, Space>) -> Self {
        Self { x_col, y_col, z_col, w_col, _unit: PhantomData, _space: PhantomData }
    }

    /// Creates a matrix from individual elements, assuming column-major order.
    #[inline]
    pub const fn from_cols_array(data: &[f32; 16]) -> Self {
        Self {
            x_col: Vec4::new(data[0], data[1], data[2], data[3]),
            y_col: Vec4::new(data[4], data[5], data[6], data[7]),
            z_col: Vec4::new(data[8], data[9], data[10], data[11]),
            w_col: Vec4::new(data[12], data[13], data[14], data[15]),
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Creates a matrix from individual elements, assuming row-major order for input.
    #[inline]
    pub const fn from_rows(
        r0c0: f32,
        r0c1: f32,
        r0c2: f32,
        r0c3: f32,
        r1c0: f32,
        r1c1: f32,
        r1c2: f32,
        r1c3: f32,
        r2c0: f32,
        r2c1: f32,
        r2c2: f32,
        r2c3: f32,
        r3c0: f32,
        r3c1: f32,
        r3c2: f32,
        r3c3: f32,
    ) -> Self {
        Self {
            x_col: Vec4::new(r0c0, r1c0, r2c0, r3c0),
            y_col: Vec4::new(r0c1, r1c1, r2c1, r3c1),
            z_col: Vec4::new(r0c2, r1c2, r2c2, r3c2),
            w_col: Vec4::new(r0c3, r1c3, r2c3, r3c3),
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Creates a translation matrix.
    #[inline]
    pub const fn from_translation(v: Vec3) -> Self {
        Self {
            x_col: Vec4::new(1.0, 0.0, 0.0, 0.0),
            y_col: Vec4::new(0.0, 1.0, 0.0, 0.0),
            z_col: Vec4::new(0.0, 0.0, 1.0, 0.0),
            w_col: Vec4::new(v.x, v.y, v.z, 1.0),
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Creates a rotation matrix from an axis and an angle (typed radians).
    #[inline]
    pub fn from_axis_angle_radians(axis: Vec3, angle: crate::angle::Radians) -> Self {
        let c = math::cos(angle.0);
        let s = math::sin(angle.0);

        let axis_norm = axis.normalize(); // Corresponds to `axis` after gb_vec3_norm in C++
        let t = axis_norm * (1.0 - c); // Corresponds to `t` in C++

        // Reminder: Self is column-major: x_col, y_col, z_col, w_col
        // C++ `rot[col_idx][row_idx]`
        // rot[0][0] -> x_col.x
        // rot[0][1] -> x_col.y
        // rot[1][0] -> y_col.x

        let r0c0 = c + t.x * axis_norm.x;
        let r1c0 = t.y * axis_norm.x - s * axis_norm.z;
        let r2c0 = t.z * axis_norm.x + s * axis_norm.y;

        let r0c1 = t.x * axis_norm.y + s * axis_norm.z;
        let r1c1 = c + t.y * axis_norm.y;
        let r2c1 = t.z * axis_norm.y - s * axis_norm.x;

        let r0c2 = t.x * axis_norm.z - s * axis_norm.y;
        let r1c2 = t.y * axis_norm.z + s * axis_norm.x;
        let r2c2 = c + t.z * axis_norm.z;

        Self {
            x_col: Vec4::new(r0c0, r1c0, r2c0, 0.0),
            y_col: Vec4::new(r0c1, r1c1, r2c1, 0.0),
            z_col: Vec4::new(r0c2, r1c2, r2c2, 0.0),
            w_col: Vec4::new(0.0, 0.0, 0.0, 1.0),
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Creates a rotation matrix from an axis and an angle (typed degrees).
    #[inline]
    pub fn from_axis_angle_deg(axis: Vec3, angle: crate::angle::Degrees) -> Self {
        Self::from_axis_angle_radians(axis, angle.to_radians())
    }

    /// Creates a rotation matrix from a **unit** axis and a typed radians angle.
    #[inline]
    #[cfg(feature = "unit_vec")]
    pub fn from_unit_axis_angle_radians(axis: UnitVec3<Unit, Space>, angle: crate::angle::Radians) -> Self {
        // Build through quaternion to keep rotation convention consistent.
        Self::from_quat(crate::quat::Quat::from_unit_axis_angle_radians(axis, angle))
    }

    /// Creates a rotation matrix from a **unit** axis and a typed degrees angle.
    #[inline]
    #[cfg(feature = "unit_vec")]
    pub fn from_unit_axis_angle_deg(axis: UnitVec3<Unit, Space>, angle: crate::angle::Degrees) -> Self {
        Self::from_unit_axis_angle_radians(axis, angle.to_radians())
    }

    /// Creates a rotation matrix around the X axis (typed radians).
    ///
    /// This matches the crate's quaternion rotation convention:
    /// `Mat4::from_rotation_x_radians(a) == Mat4::from_quat(Quat::from_axis_angle_radians(X, a))`.
    #[inline]
    pub fn from_rotation_x_radians(angle: crate::angle::Radians) -> Self {
        Self::from_quat(crate::quat::Quat::from_axis_angle_radians(Vec3::new(1.0, 0.0, 0.0), angle))
    }

    /// Creates a rotation matrix around the X axis (typed degrees).
    #[inline]
    pub fn from_rotation_x_deg(angle: crate::angle::Degrees) -> Self {
        Self::from_rotation_x_radians(angle.to_radians())
    }

    /// Creates a rotation matrix around the Y axis (typed radians).
    ///
    /// This matches the crate's quaternion rotation convention:
    /// `Mat4::from_rotation_y_radians(a) == Mat4::from_quat(Quat::from_axis_angle_radians(Y, a))`.
    #[inline]
    pub fn from_rotation_y_radians(angle: crate::angle::Radians) -> Self {
        Self::from_quat(crate::quat::Quat::from_axis_angle_radians(Vec3::new(0.0, 1.0, 0.0), angle))
    }

    /// Creates a rotation matrix around the Y axis (typed degrees).
    #[inline]
    pub fn from_rotation_y_deg(angle: crate::angle::Degrees) -> Self {
        Self::from_rotation_y_radians(angle.to_radians())
    }

    /// Creates a rotation matrix around the Z axis (typed radians).
    ///
    /// This matches the crate's quaternion rotation convention:
    /// `Mat4::from_rotation_z_radians(a) == Mat4::from_quat(Quat::from_axis_angle_radians(Z, a))`.
    #[inline]
    pub fn from_rotation_z_radians(angle: crate::angle::Radians) -> Self {
        Self::from_quat(crate::quat::Quat::from_axis_angle_radians(Vec3::new(0.0, 0.0, 1.0), angle))
    }

    /// Creates a rotation matrix around the Z axis (typed degrees).
    #[inline]
    pub fn from_rotation_z_deg(angle: crate::angle::Degrees) -> Self {
        Self::from_rotation_z_radians(angle.to_radians())
    }

    /// Creates a scale matrix.
    #[inline]
    pub const fn from_scale(v: Vec3) -> Self {
        Self {
            x_col: Vec4::new(v.x, 0.0, 0.0, 0.0),
            y_col: Vec4::new(0.0, v.y, 0.0, 0.0),
            z_col: Vec4::new(0.0, 0.0, v.z, 0.0),
            w_col: Vec4::new(0.0, 0.0, 0.0, 1.0),
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Returns the transpose of the matrix.
    #[inline]
    pub const fn transpose(&self) -> Self {
        Self {
            x_col: Vec4::new(self.x_col.x, self.y_col.x, self.z_col.x, self.w_col.x),
            y_col: Vec4::new(self.x_col.y, self.y_col.y, self.z_col.y, self.w_col.y),
            z_col: Vec4::new(self.x_col.z, self.y_col.z, self.z_col.z, self.w_col.z),
            w_col: Vec4::new(self.x_col.w, self.y_col.w, self.z_col.w, self.w_col.w),
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Calculates the inverse of the matrix.
    /// Returns `None` if the matrix is not invertible (determinant is close to zero).
    /// The implementation is a direct translation of the C++ gb_mat4_inverse function.
    #[inline]
    pub fn inverse(&self) -> Option<Self> {
        let m = self; // for brevity, matching C++ style where `m` is `in`

        let sf00 = m.z_col.z * m.w_col.w - m.w_col.z * m.z_col.w;
        let sf01 = m.z_col.y * m.w_col.w - m.w_col.y * m.z_col.w;
        let sf02 = m.z_col.y * m.w_col.z - m.w_col.y * m.z_col.z;
        let sf03 = m.z_col.x * m.w_col.w - m.w_col.x * m.z_col.w;
        let sf04 = m.z_col.x * m.w_col.z - m.w_col.x * m.z_col.z;
        let sf05 = m.z_col.x * m.w_col.y - m.w_col.x * m.z_col.y; // Used in inverse

        let sf06 = m.y_col.z * m.w_col.w - m.w_col.z * m.y_col.w;
        let sf07 = m.y_col.y * m.w_col.w - m.w_col.y * m.y_col.w;
        let sf08 = m.y_col.y * m.w_col.z - m.w_col.y * m.y_col.z;
        let sf09 = m.y_col.x * m.w_col.w - m.w_col.x * m.y_col.w;
        let sf10 = m.y_col.x * m.w_col.z - m.w_col.x * m.y_col.z;
        let sf11 = m.y_col.y * m.w_col.w - m.w_col.y * m.y_col.w; // Same as sf07, used in inverse
        let sf12 = m.y_col.x * m.w_col.y - m.w_col.x * m.y_col.y;

        let sf13 = m.y_col.z * m.z_col.w - m.z_col.z * m.y_col.w;
        let sf14 = m.y_col.y * m.z_col.w - m.z_col.y * m.y_col.w;
        let sf15 = m.y_col.y * m.z_col.z - m.z_col.y * m.y_col.z;
        let sf16 = m.y_col.x * m.z_col.w - m.z_col.x * m.y_col.w;
        let sf17 = m.y_col.x * m.z_col.z - m.z_col.x * m.y_col.z;
        let sf18 = m.y_col.x * m.z_col.y - m.z_col.x * m.y_col.y;

        let mut o = Self::ZERO; // Output matrix, initialized to zero

        o.x_col.x = m.y_col.y * sf00 - m.y_col.z * sf01 + m.y_col.w * sf02;
        o.y_col.x = -m.y_col.x * sf00 + m.y_col.z * sf03 - m.y_col.w * sf04;
        o.z_col.x = m.y_col.x * sf01 - m.y_col.y * sf03 + m.y_col.w * sf05;
        o.w_col.x = -m.y_col.x * sf02 + m.y_col.y * sf04 - m.y_col.z * sf05;

        o.x_col.y = -m.x_col.y * sf00 + m.x_col.z * sf01 - m.x_col.w * sf02;
        o.y_col.y = m.x_col.x * sf00 - m.x_col.z * sf03 + m.x_col.w * sf04;
        o.z_col.y = -m.x_col.x * sf01 + m.x_col.y * sf03 - m.x_col.w * sf05;
        o.w_col.y = m.x_col.x * sf02 - m.x_col.y * sf04 + m.x_col.z * sf05;

        o.x_col.z = m.x_col.y * sf06 - m.x_col.z * sf07 + m.x_col.w * sf08;
        o.y_col.z = -m.x_col.x * sf06 + m.x_col.z * sf09 - m.x_col.w * sf10;
        o.z_col.z = m.x_col.x * sf11 - m.x_col.y * sf09 + m.x_col.w * sf12;
        o.w_col.z = -m.x_col.x * sf08 + m.x_col.y * sf10 - m.x_col.z * sf12;

        o.x_col.w = -m.x_col.y * sf13 + m.x_col.z * sf14 - m.x_col.w * sf15;
        o.y_col.w = m.x_col.x * sf13 - m.x_col.z * sf16 + m.x_col.w * sf17;
        o.z_col.w = -m.x_col.x * sf14 + m.x_col.y * sf16 - m.x_col.w * sf18;
        o.w_col.w = m.x_col.x * sf15 - m.x_col.y * sf17 + m.x_col.z * sf18;

        let det_check = m.x_col.x * o.x_col.x
            + m.x_col.y * o.y_col.x
            + m.x_col.z * o.z_col.x
            + m.x_col.w * o.w_col.x;

        if det_check.abs() < 1e-7 {
            // EPSILON
            return None;
        }

        let ood = 1.0 / det_check;

        o.x_col = o.x_col * ood;
        o.y_col = o.y_col * ood;
        o.z_col = o.z_col * ood;
        o.w_col = o.w_col * ood;

        Some(o)
    }

    /// Alias for [`Mat4::inverse`].
    #[inline]
    pub fn try_inverse(&self) -> Option<Self> {
        self.inverse()
    }

    /// Calculates the determinant of the matrix.
    #[inline]
    pub fn determinant(&self) -> f32 {
        let m = self;
        let m00 = m.x_col.x;
        let m01 = m.y_col.x;
        let m02 = m.z_col.x;
        let m03 = m.w_col.x;
        let m10 = m.x_col.y;
        let m11 = m.y_col.y;
        let m12 = m.z_col.y;
        let m13 = m.w_col.y;
        let m20 = m.x_col.z;
        let m21 = m.y_col.z;
        let m22 = m.z_col.z;
        let m23 = m.w_col.z;
        let m30 = m.x_col.w;
        let m31 = m.y_col.w;
        let m32 = m.z_col.w;
        let m33 = m.w_col.w;

        #[inline]
        fn det3(
            a00: f32, a01: f32, a02: f32,
            a10: f32, a11: f32, a12: f32,
            a20: f32, a21: f32, a22: f32,
        ) -> f32 {
            a00 * (a11 * a22 - a12 * a21)
                - a01 * (a10 * a22 - a12 * a20)
                + a02 * (a10 * a21 - a11 * a20)
        }

        m00 * det3(m11, m12, m13, m21, m22, m23, m31, m32, m33)
            - m01 * det3(m10, m12, m13, m20, m22, m23, m30, m32, m33)
            + m02 * det3(m10, m11, m13, m20, m21, m23, m30, m31, m33)
            - m03 * det3(m10, m11, m12, m20, m21, m22, m30, m31, m32)
    }

    /// Creates a left-handed orthographic projection matrix with a depth range of [0, 1].
    /// (DirectX style: z will be mapped to 0 for near plane, 1 for far plane).
    #[inline]
    pub fn orthographic_lh_zo(
        left: f32,
        right: f32,
        bottom: f32,
        top: f32,
        near: f32,
        far: f32,
    ) -> Self {
        let r_minus_l = right - left;
        let t_minus_b = top - bottom;
        let f_minus_n = far - near;

        // Avoid division by zero if any range is zero.
        // The C++ code doesn't explicitly handle this, but it's good practice.
        // For now, we'll assume valid inputs as per typical library behavior.

        let mut m = Self::ZERO;

        m.x_col.x = 2.0 / r_minus_l;
        m.y_col.y = 2.0 / t_minus_b;
        m.z_col.z = 1.0 / f_minus_n; // For LH_ZO
        m.w_col.w = 1.0;

        m.w_col.x = -(right + left) / r_minus_l;
        m.w_col.y = -(top + bottom) / t_minus_b;
        m.w_col.z = -near / f_minus_n; // For LH_ZO

        m
    }

    /// Creates a left-handed perspective projection matrix with a depth range of [0, 1].
    /// (DirectX style: z will be mapped to 0 for near plane, 1 for far plane after perspective divide).
    #[inline]
    pub fn perspective_lh_zo(
        fovy_radians: f32,
        aspect_ratio: f32,
        near_plane: f32,
        far_plane: f32,
    ) -> Self {
        let tan_half_fovy = math::tan(0.5 * fovy_radians);

        // Avoid division by zero if aspect_ratio or tan_half_fovy is zero, or near == far.
        // For now, assume valid inputs.

        let mut m = Self::ZERO;

        m.x_col.x = 1.0 / (aspect_ratio * tan_half_fovy);
        m.y_col.y = 1.0 / tan_half_fovy;
        m.z_col.z = far_plane / (far_plane - near_plane);
        m.z_col.w = 1.0; // Puts view-space Z into W for perspective divide
        m.w_col.z = -near_plane * far_plane / (far_plane - near_plane);
        // m.w_col.w is 0.0 from ZERO initialization, which is correct.

        m
    }

    /// Creates a left-handed look-at view matrix.
    /// Translates gb_mat4_look_at directly.
    #[inline]
    pub fn look_at_lh(eye: Vec3, target: Vec3, world_up: Vec3) -> Self {
        let f = (target - eye).normalize(); // Corresponds to CamZ or `f` in C++
        let s = f.cross(world_up).normalize(); // Corresponds to -CamX or `s` in C++ (f.cross(up) is -CamX if up is Y and f is Z)
        let u = s.cross(f); // Corresponds to CamY or `u` in C++ ((-CamX).cross(CamZ) is CamY)

        // The C++ code gb_mat4_look_at is equivalent to:
        // Mat4::from_rows(
        //     s.x, u.x, -f.x, -s.dot(eye),
        //     s.y, u.y, -f.y, -u.dot(eye),
        //     s.z, u.z, -f.z,  f.dot(eye),  <-- note +f.dot(eye) from C++
        //     0.0, 0.0,  0.0,  1.0
        // );
        // And then transposing it if from_rows created a row-major matrix.
        // Our Mat4 struct is column-major. The C++ gbMat4 stores columns.
        // gbFloat4 *m = gb_float44_m(out); -> m[col_idx][row_idx]
        // m[0][0] = s.x;  // x_col.x
        // m[1][0] = s.y;  // y_col.x
        // m[2][0] = s.z;  // z_col.x
        // m[3][0] = -s.dot(eye); // w_col.x
        // ... this means the columns of the matrix are [s,u,-f] for rotation part, and translation derived from them.

        Self {
            x_col: Vec4::new(s.x, u.x, -f.x, 0.0),
            y_col: Vec4::new(s.y, u.y, -f.y, 0.0),
            z_col: Vec4::new(s.z, u.z, -f.z, 0.0),
            w_col: Vec4::new(-s.dot(eye), -u.dot(eye), f.dot(eye), 1.0),
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Returns the i-th column (0, 1, 2, or 3).
    pub const fn col(&self, i: usize) -> Option<Vec4<Unit, Space>> {
        match i {
            0 => Some(self.x_col),
            1 => Some(self.y_col),
            2 => Some(self.z_col),
            3 => Some(self.w_col),
            _ => None,
        }
    }

    /// Sets the i-th column (0, 1, 2, or 3).
    pub fn set_col(&mut self, i: usize, v: Vec4<Unit, Space>) {
        match i {
            0 => self.x_col = v,
            1 => self.y_col = v,
            2 => self.z_col = v,
            3 => self.w_col = v,
            _ => {}
        }
    }

    /// Returns the i-th row (0, 1, 2, or 3).
    pub const fn row(&self, i: usize) -> Option<Vec4<Unit, Space>> {
        match i {
            0 => Some(Vec4::new(
                self.x_col.x,
                self.y_col.x,
                self.z_col.x,
                self.w_col.x,
            )),
            1 => Some(Vec4::new(
                self.x_col.y,
                self.y_col.y,
                self.z_col.y,
                self.w_col.y,
            )),
            2 => Some(Vec4::new(
                self.x_col.z,
                self.y_col.z,
                self.z_col.z,
                self.w_col.z,
            )),
            3 => Some(Vec4::new(
                self.x_col.w,
                self.y_col.w,
                self.z_col.w,
                self.w_col.w,
            )),
            _ => None,
        }
    }

    /// Sets the i-th row (0, 1, 2, or 3).
    pub fn set_row(&mut self, i: usize, v: Vec4<Unit, Space>) {
        match i {
            0 => {
                self.x_col.x = v.x;
                self.y_col.x = v.y;
                self.z_col.x = v.z;
                self.w_col.x = v.w;
            }
            1 => {
                self.x_col.y = v.x;
                self.y_col.y = v.y;
                self.z_col.y = v.z;
                self.w_col.y = v.w;
            }
            2 => {
                self.x_col.z = v.x;
                self.y_col.z = v.y;
                self.z_col.z = v.z;
                self.w_col.z = v.w;
            }
            3 => {
                self.x_col.w = v.x;
                self.y_col.w = v.y;
                self.z_col.w = v.z;
                self.w_col.w = v.w;
            }
            _ => {}
        }
    }

    /// Returns true if the matrix is orthonormal (rotation part only).
    pub fn is_orthonormal(&self) -> bool {
        let c0 = self.x_col.xyz();
        let c1 = self.y_col.xyz();
        let c2 = self.z_col.xyz();
        (c0.length() - 1.0).abs() < 1e-5
            && (c1.length() - 1.0).abs() < 1e-5
            && (c2.length() - 1.0).abs() < 1e-5
            && (c0.dot(c1)).abs() < 1e-5
            && (c0.dot(c2)).abs() < 1e-5
            && (c1.dot(c2)).abs() < 1e-5
    }

    /// Returns a shear matrix with 6 shear factors.
    pub const fn from_shear(shxy: f32, shxz: f32, shyx: f32, shyz: f32, shzx: f32, shzy: f32) -> Self {
        // Shear matrix: https://en.wikipedia.org/wiki/Shear_matrix
        // | 1 shxy shxz 0 |
        // | shyx 1 shyz 0 |
        // | shzx shzy 1 0 |
        // | 0 0 0 1 |
        Self {
            x_col: Vec4::new(1.0, shyx, shzx, 0.0),
            y_col: Vec4::new(shxy, 1.0, shzy, 0.0),
            z_col: Vec4::new(shxz, shyz, 1.0, 0.0),
            w_col: Vec4::new(0.0, 0.0, 0.0, 1.0),
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Creates a Mat4 from a quaternion (rotation part only, no translation).
    pub fn from_quat(q: crate::quat::Quat<Unit, Space>) -> Self {
        let (x, y, z, w) = (q.x, q.y, q.z, q.w);
        let x2 = x + x;
        let y2 = y + y;
        let z2 = z + z;
        let xx = x * x2;
        let yy = y * y2;
        let zz = z * z2;
        let xy = x * y2;
        let xz = x * z2;
        let yz = y * z2;
        let wx = w * x2;
        let wy = w * y2;
        let wz = w * z2;
        Self {
            x_col: Vec4::new(1.0 - (yy + zz), xy + wz, xz - wy, 0.0),
            y_col: Vec4::new(xy - wz, 1.0 - (xx + zz), yz + wx, 0.0),
            z_col: Vec4::new(xz + wy, yz - wx, 1.0 - (xx + yy), 0.0),
            w_col: Vec4::new(0.0, 0.0, 0.0, 1.0),
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Converts the rotation part of this matrix to a quaternion.
    pub fn to_quat(&self) -> crate::quat::Quat {
        // Use the same logic as Quat::from_mat4 but for 3x3 part
        let m00 = self.x_col.x;
        let m01 = self.x_col.y;
        let m02 = self.x_col.z;
        let m10 = self.y_col.x;
        let m11 = self.y_col.y;
        let m12 = self.y_col.z;
        let m20 = self.z_col.x;
        let m21 = self.z_col.y;
        let m22 = self.z_col.z;
        let four_x_squared_minus_1 = m00 - m11 - m22;
        let four_y_squared_minus_1 = m11 - m00 - m22;
        let four_z_squared_minus_1 = m22 - m00 - m11;
        let four_w_squared_minus_1 = m00 + m11 + m22;
        let mut biggest_index = 0;
        let mut four_biggest_squared_minus_1 = four_w_squared_minus_1;
        if four_x_squared_minus_1 > four_biggest_squared_minus_1 {
            four_biggest_squared_minus_1 = four_x_squared_minus_1;
            biggest_index = 1;
        }
        if four_y_squared_minus_1 > four_biggest_squared_minus_1 {
            four_biggest_squared_minus_1 = four_y_squared_minus_1;
            biggest_index = 2;
        }
        if four_z_squared_minus_1 > four_biggest_squared_minus_1 {
            four_biggest_squared_minus_1 = four_z_squared_minus_1;
            biggest_index = 3;
        }
        let biggest_value = math::sqrt(four_biggest_squared_minus_1 + 1.0) * 0.5;
        let mult = 0.25 / biggest_value;
        let mut q = crate::quat::Quat::ZERO;
        match biggest_index {
            0 => {
                q.w = biggest_value;
                q.x = (m12 - m21) * mult;
                q.y = (m20 - m02) * mult;
                q.z = (m01 - m10) * mult;
            }
            1 => {
                q.w = (m12 - m21) * mult;
                q.x = biggest_value;
                q.y = (m01 + m10) * mult;
                q.z = (m20 + m02) * mult;
            }
            2 => {
                q.w = (m20 - m02) * mult;
                q.x = (m01 + m10) * mult;
                q.y = biggest_value;
                q.z = (m12 + m21) * mult;
            }
            3 => {
                q.w = (m01 - m10) * mult;
                q.x = (m20 + m02) * mult;
                q.y = (m12 + m21) * mult;
                q.z = biggest_value;
            }
            _ => unreachable!(),
        }
        q
    }

    /// Transforms a point (Vec3, w=1) by this matrix.
    pub fn transform_point(&self, p: Vec3) -> Vec3 {
        let v = Vec4::new(p.x, p.y, p.z, 1.0);
        let r = *self * v;
        Vec3::new(r.x, r.y, r.z)
    }

    /// Transforms a direction vector (Vec3, w=0) by this matrix.
    pub fn transform_vector(&self, v: Vec3) -> Vec3 {
        let v4 = Vec4::new(v.x, v.y, v.z, 0.0);
        let r = *self * v4;
        Vec3::new(r.x, r.y, r.z)
    }

    /// Decomposes this matrix into translation, rotation (as quaternion), and scale.
    pub fn decompose(&self) -> (Vec3, crate::quat::Quat, Vec3) {
        // Extract translation
        let translation = Vec3::new(self.w_col.x, self.w_col.y, self.w_col.z);
        // Extract scale from column lengths
        let sx = self.x_col.xyz().length();
        let sy = self.y_col.xyz().length();
        let sz = self.z_col.xyz().length();
        let scale = Vec3::new(sx, sy, sz);
        // Remove scale from rotation part
        let mut rot = *self;
        rot.x_col = rot.x_col / sx;
        rot.y_col = rot.y_col / sy;
        rot.z_col = rot.z_col / sz;
        let rotation = rot.to_quat();
        (translation, rotation, scale)
    }

    /// Composes a Mat4 from translation, rotation (as quaternion), and scale.
    pub fn compose(translation: Vec3, rotation: crate::quat::Quat<Unit, Space>, scale: Vec3) -> Self {
        let rot = Self::from_quat(rotation);
        let mut m = rot;
        m.x_col = m.x_col * scale.x;
        m.y_col = m.y_col * scale.y;
        m.z_col = m.z_col * scale.z;
        m.w_col = Vec4::new(translation.x, translation.y, translation.z, 1.0);
        m
    }

    /// Alias for [`Mat4::compose`].
    ///
    /// This name is common in other math libraries: TRS = Translation * Rotation * Scale.
    #[inline]
    pub fn from_trs(translation: Vec3, rotation: crate::quat::Quat<Unit, Space>, scale: Vec3) -> Self {
        Self::compose(translation, rotation, scale)
    }
}

impl<Unit: Copy, Space: Copy> Add for Mat4<Unit, Space> {
    type Output = Self;
    #[inline]
    fn add(self, rhs: Self) -> Self::Output {
        Self {
            x_col: self.x_col + rhs.x_col,
            y_col: self.y_col + rhs.y_col,
            z_col: self.z_col + rhs.z_col,
            w_col: self.w_col + rhs.w_col,
            _unit: PhantomData,
            _space: PhantomData,
        }
    }
}

impl<Unit: Copy, Space: Copy> Sub for Mat4<Unit, Space> {
    type Output = Self;
    #[inline]
    fn sub(self, rhs: Self) -> Self::Output {
        Self {
            x_col: self.x_col - rhs.x_col,
            y_col: self.y_col - rhs.y_col,
            z_col: self.z_col - rhs.z_col,
            w_col: self.w_col - rhs.w_col,
            _unit: PhantomData,
            _space: PhantomData,
        }
    }
}

impl<Unit: Copy, Space: Copy> Mul<Mat4<Unit, Space>> for Mat4<Unit, Space> {
    type Output = Self;
    #[inline]
    fn mul(self, rhs: Mat4<Unit, Space>) -> Self::Output {
        // Based on gb_mat4_mul which calls gb_float44_mul
        // out[j][i] = mat1[0][i]*mat2[j][0] +
        //             mat1[1][i]*mat2[j][1] +
        //             mat1[2][i]*mat2[j][2] +
        //             mat1[3][i]*mat2[j][3];
        // where mat1 is `self` and mat2 is `rhs`.
        // `i` is row index, `j` is column index.

        // x_col (j=0)
        let x_col_x = self.x_col.x * rhs.x_col.x
            + self.y_col.x * rhs.x_col.y
            + self.z_col.x * rhs.x_col.z
            + self.w_col.x * rhs.x_col.w; // i=0
        let x_col_y = self.x_col.y * rhs.x_col.x
            + self.y_col.y * rhs.x_col.y
            + self.z_col.y * rhs.x_col.z
            + self.w_col.y * rhs.x_col.w; // i=1
        let x_col_z = self.x_col.z * rhs.x_col.x
            + self.y_col.z * rhs.x_col.y
            + self.z_col.z * rhs.x_col.z
            + self.w_col.z * rhs.x_col.w; // i=2
        let x_col_w = self.x_col.w * rhs.x_col.x
            + self.y_col.w * rhs.x_col.y
            + self.z_col.w * rhs.x_col.z
            + self.w_col.w * rhs.x_col.w; // i=3

        // y_col (j=1)
        let y_col_x = self.x_col.x * rhs.y_col.x
            + self.y_col.x * rhs.y_col.y
            + self.z_col.x * rhs.y_col.z
            + self.w_col.x * rhs.y_col.w; // i=0
        let y_col_y = self.x_col.y * rhs.y_col.x
            + self.y_col.y * rhs.y_col.y
            + self.z_col.y * rhs.y_col.z
            + self.w_col.y * rhs.y_col.w; // i=1
        let y_col_z = self.x_col.z * rhs.y_col.x
            + self.y_col.z * rhs.y_col.y
            + self.z_col.z * rhs.y_col.z
            + self.w_col.z * rhs.y_col.w; // i=2
        let y_col_w = self.x_col.w * rhs.y_col.x
            + self.y_col.w * rhs.y_col.y
            + self.z_col.w * rhs.y_col.z
            + self.w_col.w * rhs.y_col.w; // i=3

        // z_col (j=2)
        let z_col_x = self.x_col.x * rhs.z_col.x
            + self.y_col.x * rhs.z_col.y
            + self.z_col.x * rhs.z_col.z
            + self.w_col.x * rhs.z_col.w; // i=0
        let z_col_y = self.x_col.y * rhs.z_col.x
            + self.y_col.y * rhs.z_col.y
            + self.z_col.y * rhs.z_col.z
            + self.w_col.y * rhs.z_col.w; // i=1
        let z_col_z = self.x_col.z * rhs.z_col.x
            + self.y_col.z * rhs.z_col.y
            + self.z_col.z * rhs.z_col.z
            + self.w_col.z * rhs.z_col.w; // i=2
        let z_col_w = self.x_col.w * rhs.z_col.x
            + self.y_col.w * rhs.z_col.y
            + self.z_col.w * rhs.z_col.z
            + self.w_col.w * rhs.z_col.w; // i=3

        // w_col (j=3)
        let w_col_x = self.x_col.x * rhs.w_col.x
            + self.y_col.x * rhs.w_col.y
            + self.z_col.x * rhs.w_col.z
            + self.w_col.x * rhs.w_col.w; // i=0
        let w_col_y = self.x_col.y * rhs.w_col.x
            + self.y_col.y * rhs.w_col.y
            + self.z_col.y * rhs.w_col.z
            + self.w_col.y * rhs.w_col.w; // i=1
        let w_col_z = self.x_col.z * rhs.w_col.x
            + self.y_col.z * rhs.w_col.y
            + self.z_col.z * rhs.w_col.z
            + self.w_col.z * rhs.w_col.w; // i=2
        let w_col_w = self.x_col.w * rhs.w_col.x
            + self.y_col.w * rhs.w_col.y
            + self.z_col.w * rhs.w_col.z
            + self.w_col.w * rhs.w_col.w; // i=3

        Self {
            x_col: Vec4::new(x_col_x, x_col_y, x_col_z, x_col_w),
            y_col: Vec4::new(y_col_x, y_col_y, y_col_z, y_col_w),
            z_col: Vec4::new(z_col_x, z_col_y, z_col_z, z_col_w),
            w_col: Vec4::new(w_col_x, w_col_y, w_col_z, w_col_w),
            _unit: PhantomData,
            _space: PhantomData,
        }
    }
}

impl<Unit: Copy, Space: Copy> Mul<Vec4<Unit, Space>> for Mat4<Unit, Space> {
    type Output = Vec4<Unit, Space>;
    #[inline]
    fn mul(self, rhs: Vec4<Unit, Space>) -> Self::Output {
        // Based on gb_mat4_mul_vec4 which calls gb_float44_mul_vec4
        // out->x = m[0][0]*v.x + m[1][0]*v.y + m[2][0]*v.z + m[3][0]*v.w;
        // out->y = m[0][1]*v.x + m[1][1]*v.y + m[2][1]*v.z + m[3][1]*v.w;
        // out->z = m[0][2]*v.x + m[1][2]*v.y + m[2][2]*v.z + m[3][2]*v.w;
        // out->w = m[0][3]*v.x + m[1][3]*v.y + m[2][3]*v.z + m[3][3]*v.w;
        // where m[col][row]
        // self.x_col.x is m[0][0], self.x_col.y is m[0][1]
        // self.y_col.x is m[1][0], self.y_col.y is m[1][1]
        let x = self.x_col.x * rhs.x
            + self.y_col.x * rhs.y
            + self.z_col.x * rhs.z
            + self.w_col.x * rhs.w;
        let y = self.x_col.y * rhs.x
            + self.y_col.y * rhs.y
            + self.z_col.y * rhs.z
            + self.w_col.y * rhs.w;
        let z = self.x_col.z * rhs.x
            + self.y_col.z * rhs.y
            + self.z_col.z * rhs.z
            + self.w_col.z * rhs.w;
        let w = self.x_col.w * rhs.x
            + self.y_col.w * rhs.y
            + self.z_col.w * rhs.z
            + self.w_col.w * rhs.w;
        Vec4::new(x, y, z, w)
    }
}

impl<Unit: Copy, Space: Copy> Mul<f32> for Mat4<Unit, Space> {
    type Output = Self;
    #[inline]
    fn mul(self, rhs: f32) -> Self::Output {
        Self {
            x_col: self.x_col * rhs,
            y_col: self.y_col * rhs,
            z_col: self.z_col * rhs,
            w_col: self.w_col * rhs,
            _unit: PhantomData,
            _space: PhantomData,
        }
    }
}

impl<Unit: Copy, Space: Copy> Mul<Mat4<Unit, Space>> for f32 {
    type Output = Mat4<Unit, Space>;
    #[inline]
    fn mul(self, rhs: Mat4<Unit, Space>) -> Self::Output {
        rhs * self // Reuse Mat4 * f32
    }
}

// TODO: Implement various transform constructors (handedness variants, typed angles, etc.)