gemath 0.1.0

use crate::vec3::{Vec3, 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;

/// 3x3 matrix with type-level unit and coordinate space
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct Mat3<Unit: Copy = (), Space: Copy = ()> {
    pub x_col: Vec3<Unit, Space>, // First column
    pub y_col: Vec3<Unit, Space>, // Second column
    pub z_col: Vec3<Unit, Space>, // Third column
    #[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 Mat3f32 = Mat3<(),()>;
pub type Mat3Meters = Mat3<Meters,()>;
pub type Mat3Pixels = Mat3<Pixels,()>;
pub type Mat3World = Mat3<(),World>;
pub type Mat3Local = Mat3<(),Local>;
pub type Mat3Screen = Mat3<(),Screen>;
pub type Mat3MetersWorld = Mat3<Meters,World>;
pub type Mat3PixelsScreen = Mat3<Pixels,Screen>;

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

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

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

    /// Creates a matrix from individual elements, assuming column-major order.
    /// m00, m10, m20 are first column, etc.
    #[inline]
    pub const fn from_cols_array(data: &[f32; 9]) -> Self {
        Self {
            x_col: Vec3::new(data[0], data[1], data[2]),
            y_col: Vec3::new(data[3], data[4], data[5]),
            z_col: Vec3::new(data[6], data[7], data[8]),
            _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,
        r1c0: f32,
        r1c1: f32,
        r1c2: f32,
        r2c0: f32,
        r2c1: f32,
        r2c2: f32,
    ) -> Self {
        Self {
            x_col: Vec3::new(r0c0, r1c0, r2c0),
            y_col: Vec3::new(r0c1, r1c1, r2c1),
            z_col: Vec3::new(r0c2, r1c2, r2c2),
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Calculates the determinant of the matrix.
    #[inline]
    pub const fn determinant(&self) -> f32 {
        let x = &self.x_col;
        let y = &self.y_col;
        let z = &self.z_col;
        x.x * (y.y * z.z - y.z * z.y) - y.x * (x.y * z.z - x.z * z.y)
            + z.x * (x.y * y.z - x.z * y.y)
    }

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

    /// Calculates the inverse of the matrix.
    /// Returns `None` if the determinant is zero (or very close to it).
    #[inline]
    pub fn inverse(&self) -> Option<Self> {
        let det = self.determinant();
        if det.abs() < 1e-7 {
            // EPSILON
            return None;
        }

        let inv_det = 1.0 / det;
        // Treat x_col/y_col/z_col as columns (column-major storage).
        let m00 = self.x_col.x;
        let m01 = self.y_col.x;
        let m02 = self.z_col.x;
        let m10 = self.x_col.y;
        let m11 = self.y_col.y;
        let m12 = self.z_col.y;
        let m20 = self.x_col.z;
        let m21 = self.y_col.z;
        let m22 = self.z_col.z;

        let inv00 = (m11 * m22 - m12 * m21) * inv_det;
        let inv01 = (m02 * m21 - m01 * m22) * inv_det;
        let inv02 = (m01 * m12 - m02 * m11) * inv_det;
        let inv10 = (m12 * m20 - m10 * m22) * inv_det;
        let inv11 = (m00 * m22 - m02 * m20) * inv_det;
        let inv12 = (m02 * m10 - m00 * m12) * inv_det;
        let inv20 = (m10 * m21 - m11 * m20) * inv_det;
        let inv21 = (m01 * m20 - m00 * m21) * inv_det;
        let inv22 = (m00 * m11 - m01 * m10) * inv_det;

        Some(Self {
            x_col: Vec3::new(inv00, inv10, inv20),
            y_col: Vec3::new(inv01, inv11, inv21),
            z_col: Vec3::new(inv02, inv12, inv22),
            _unit: PhantomData,
            _space: PhantomData,
        })
    }

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

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

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

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

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

    /// Returns true if the matrix is orthonormal (columns are unit and perpendicular).
    pub fn is_orthonormal(&self) -> bool {
        let c0 = self.x_col;
        let c1 = self.y_col;
        let c2 = self.z_col;
        (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 shear factors shxy, shxz, shyx, shyz, shzx, shzy.
    pub const fn from_shear(shxy: f32, shxz: f32, shyx: f32, shyz: f32, shzx: f32, shzy: f32) -> Self {
        Self {
            x_col: Vec3::new(1.0, shyx, shzx),
            y_col: Vec3::new(shxy, 1.0, shzy),
            z_col: Vec3::new(shxz, shyz, 1.0),
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Creates a scale matrix.
    ///
    /// This is a pure scale (no rotation / translation).
    ///
    /// # Example
    ///
    /// ```
    /// use gemath::{Mat3, Vec3};
    /// let m: Mat3<(), ()> = Mat3::from_scale(Vec3::new(2.0, 3.0, 4.0));
    /// let v = Vec3::new(1.0, 1.0, 1.0);
    /// assert_eq!(m * v, Vec3::new(2.0, 3.0, 4.0));
    /// ```
    #[inline]
    pub const fn from_scale(v: Vec3) -> Self {
        Self {
            x_col: Vec3::new(v.x, 0.0, 0.0),
            y_col: Vec3::new(0.0, v.y, 0.0),
            z_col: Vec3::new(0.0, 0.0, v.z),
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Creates a rotation matrix from an axis and an angle (typed radians).
    ///
    /// This matches the crate's quaternion rotation convention.
    ///
    /// # Example
    ///
    /// ```
    /// use gemath::{Mat3, Vec3};
    /// let m: Mat3<(), ()> = Mat3::from_axis_angle_radians(
    ///     Vec3::new(0.0, 0.0, 1.0),
    ///     gemath::angle::Radians(core::f32::consts::FRAC_PI_2),
    /// );
    /// let v = Vec3::new(1.0, 0.0, 0.0);
    /// let r = m * v;
    /// assert!((r - Vec3::new(0.0, 1.0, 0.0)).length() < 1e-5);
    /// ```
    #[inline]
    pub fn from_axis_angle_radians(axis: Vec3<Unit, Space>, angle: crate::angle::Radians) -> Self {
        Self::from_quat(crate::quat::Quat::from_axis_angle_radians(axis, angle))
    }

    /// Creates a rotation matrix from an axis and an angle (typed degrees).
    #[inline]
    pub fn from_axis_angle_deg(axis: Vec3<Unit, Space>, 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 {
        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 Mat3 from a quaternion (rotation part only).
    pub fn from_quat(q: crate::quat::Quat<Unit, Space>) -> Self {
        // Standard formula for 3x3 rotation matrix from quaternion
        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: Vec3::new(1.0 - (yy + zz), xy + wz, xz - wy),
            y_col: Vec3::new(xy - wz, 1.0 - (xx + zz), yz + wx),
            z_col: Vec3::new(xz + wy, yz - wx, 1.0 - (xx + yy)),
            _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
        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
    }
}

impl<Unit: Copy, Space: Copy> Add for Mat3<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,
            _unit: PhantomData,
            _space: PhantomData,
        }
    }
}

impl<Unit: Copy, Space: Copy> Sub for Mat3<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,
            _unit: PhantomData,
            _space: PhantomData,
        }
    }
}

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

// Matrix-Vector multiplication
impl<Unit: Copy, Space: Copy> Mul<Vec3<Unit, Space>> for Mat3<Unit, Space> {
    type Output = Vec3<Unit, Space>;
    #[inline]
    fn mul(self, v: Vec3<Unit, Space>) -> Vec3<Unit, Space> {
        Vec3::new(
            self.x_col.x * v.x + self.y_col.x * v.y + self.z_col.x * v.z,
            self.x_col.y * v.x + self.y_col.y * v.y + self.z_col.y * v.z,
            self.x_col.z * v.x + self.y_col.z * v.y + self.z_col.z * v.z,
        )
    }
}

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

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