gemath 0.1.0

#[cfg(feature = "mat3")]
use crate::mat3::Mat3;
#[cfg(feature = "mat4")]
use crate::mat4::Mat4;
use crate::{vec3::Vec3, vec4::Vec4}; // For from_axis_angle later
use core::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Sub, SubAssign};
use core::marker::PhantomData;
use crate::math;
#[cfg(feature = "unit_vec")]
use crate::unit_vec::UnitVec3;

// Re-export shared marker types for backwards-compatible paths like `gemath::quat::Meters`.
pub use crate::markers::{Local, Meters, Pixels, Screen, World};

#[derive(Debug, Clone, Copy, PartialEq, Default)]
pub struct Radians;
#[derive(Debug, Clone, Copy, PartialEq, Default)]
pub struct Degrees;

/// Quaternion with type-level unit and coordinate space
#[cfg_attr(feature = "serde", derive(serde::Serialize, serde::Deserialize))]
#[derive(Debug, Clone, Copy, PartialEq, Default)]
pub struct Quat<Unit: Copy = (), Space: Copy = ()> {
    pub x: f32,
    pub y: f32,
    pub z: f32,
    pub w: f32,
    #[cfg_attr(feature = "serde", serde(skip))]
    _unit: PhantomData<Unit>,
    #[cfg_attr(feature = "serde", serde(skip))]
    _space: PhantomData<Space>,
}

/// Type aliases for common units and spaces
pub type Quatf32 = Quat<(),()>;
pub type QuatRadians = Quat<Radians,()>;
pub type QuatDegrees = Quat<Degrees,()>;
pub type QuatWorld = Quat<(),World>;
pub type QuatLocal = Quat<(),Local>;
pub type QuatScreen = Quat<(),Screen>;
pub type QuatRadiansWorld = Quat<Radians,World>;
pub type QuatDegreesScreen = Quat<Degrees,Screen>;

impl<Unit: Copy, Space: Copy> Quat<Unit, Space> {
    pub const IDENTITY: Self = {
        Self {
            x: 0.0,
            y: 0.0,
            z: 0.0,
            w: 1.0,
            _unit: PhantomData,
            _space: PhantomData,
        }
    };
    pub const ZERO: Self = {
        Self {
            x: 0.0,
            y: 0.0,
            z: 0.0,
            w: 0.0,
            _unit: PhantomData,
            _space: PhantomData,
        }
    };

    #[inline]
    pub const fn new(x: f32, y: f32, z: f32, w: f32) -> Self {
        Self { x, y, z, w, _unit: PhantomData, _space: PhantomData }
    }

    /// Creates a quaternion from a Vec4 (x, y, z, w components).
    #[inline]
    pub const fn from_vec4(v: Vec4<Unit, Space>) -> Self {
        Self {
            x: v.x,
            y: v.y,
            z: v.z,
            w: v.w,
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Returns the quaternion as a Vec4.
    #[inline]
    pub const fn to_vec4(self) -> Vec4<Unit, Space> {
        Vec4::new(self.x, self.y, self.z, self.w)
    }

    /// Returns the vector part (x, y, z) of the quaternion.
    #[inline]
    pub const fn xyz(self) -> Vec3<Unit, Space> {
        Vec3::new(self.x, self.y, self.z)
    }

    /// Creates a quaternion from an axis and an angle (typed radians).
    #[inline]
    pub fn from_axis_angle_radians(axis: Vec3<Unit, Space>, angle: crate::angle::Radians) -> Self {
        let (sin_half, cos_half) = math::sin_cos(0.5 * angle.0);
        let mut a = axis.normalize(); // Matches gb_vec3_norm behavior if axis is zero
        a = a * sin_half;
        Self {
            x: a.x,
            y: a.y,
            z: a.z,
            w: cos_half,
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Creates a quaternion from a **unit** axis and a typed radians angle.
    ///
    /// This is the “invariant-preserving” variant: the type system guarantees the axis is normalized.
    #[inline]
    #[cfg(feature = "unit_vec")]
    pub fn from_unit_axis_angle_radians(axis: UnitVec3<Unit, Space>, angle: crate::angle::Radians) -> Self {
        let (sin_half, cos_half) = math::sin_cos(0.5 * angle.0);
        let a = axis.as_vec() * sin_half;
        Self {
            x: a.x,
            y: a.y,
            z: a.z,
            w: cos_half,
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Creates a quaternion 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 quaternion 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 quaternion from Euler angles (pitch, yaw, roll) in radians.
    /// Matches the yaw -> pitch -> roll order.
    #[inline]
    pub fn from_euler_angles_radians(
        pitch: crate::angle::Radians,
        yaw: crate::angle::Radians,
        roll: crate::angle::Radians,
    ) -> Self {
        // Based on gb_quat_euler_angles which constructs axis-angle quaternions and multiplies them
        let p = Quat::from_axis_angle_radians(Vec3::new(1.0, 0.0, 0.0), pitch);
        let y = Quat::from_axis_angle_radians(Vec3::new(0.0, 1.0, 0.0), yaw);
        let r = Quat::from_axis_angle_radians(Vec3::new(0.0, 0.0, 1.0), roll);

        // Original gb_math.cpp order: (y * p) * r
        (y * p) * r
    }

    /// Creates a quaternion from Euler angles (pitch, yaw, roll) in typed degrees.
    #[inline]
    pub fn from_euler_angles_deg(
        pitch: crate::angle::Degrees,
        yaw: crate::angle::Degrees,
        roll: crate::angle::Degrees,
    ) -> Self {
        Self::from_euler_angles_radians(pitch.to_radians(), yaw.to_radians(), roll.to_radians())
    }

    /// Calculates the dot product of two quaternions.
    #[inline]
    pub const fn dot(self, rhs: Self) -> f32 {
        self.x * rhs.x + self.y * rhs.y + self.z * rhs.z + self.w * rhs.w
    }

    /// Calculates the squared length (magnitude) of the quaternion.
    #[inline]
    pub const fn length_squared(self) -> f32 {
        self.dot(self)
    }

    /// Calculates the length (magnitude) of the quaternion.
    #[inline]
    pub fn length(self) -> f32 {
        math::sqrt(self.length_squared())
    }

    /// Normalizes the quaternion.
    /// If the length is zero, returns `Quat::zero()`.
    #[inline]
    pub fn normalize(self) -> Self {
        let mag = self.length();
        if mag > 0.0 {
            self / mag
        } else {
            Self::ZERO
        }
    }

    /// Normalizes the quaternion, returning `None` if the length is zero.
    #[inline]
    pub fn try_normalize(self) -> Option<Self> {
        let mag = self.length();
        if mag > f32::EPSILON {
            Some(self / mag)
        } else {
            None
        }
    }

    /// Calculates the conjugate of the quaternion.
    #[inline]
    pub const fn conjugate(self) -> Self {
        Self {
            x: -self.x,
            y: -self.y,
            z: -self.z,
            w: self.w,
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Calculates the inverse of the quaternion.
    /// Returns `None` if the quaternion's length is zero (cannot be inverted).
    #[inline]
    pub fn inverse(self) -> Option<Self> {
        let len_sq = self.length_squared();
        if len_sq != 0.0 {
            // Matches gb_quat_inverse behavior
            Some(self.conjugate() / len_sq)
        } else {
            None
        }
    }

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

    /// Divides this quaternion by a scalar, returning `None` for invalid divisors.
    ///
    /// Returns `None` if `rhs` is zero (including `-0.0`) or non-finite (`NaN`, `±inf`).
    #[inline]
    pub fn checked_div_scalar(self, rhs: f32) -> Option<Self> {
        if rhs == 0.0 || !rhs.is_finite() {
            None
        } else {
            Some(self / rhs)
        }
    }

    /// Converts the quaternion to an axis and angle (in radians).
    /// The returned axis is normalized. If the quaternion is identity,
    /// returns (Vec3::X, 0.0) or similar, as angle is 0.
    #[inline]
    pub fn to_axis_angle(self) -> (Vec3<Unit, Space>, f32) {
        let norm_q = self.normalize(); // Matches gb_quat_axis which uses a normalized quat
        let angle = 2.0 * math::acos(norm_q.w);
        let s = math::sqrt(1.0 - norm_q.w * norm_q.w);

        if s < f32::EPSILON {
            // If s is close to zero, angle is close to 0 or PI
            // If angle is 0, axis doesn't matter. Can default to (1,0,0) or (0,0,1)
            // If angle is PI, w is 0. Axis is (x,y,z).
            // gb_quat_axis divides by sin(acos(q.w)), which is s.
            // If s is zero, means q.w is 1 or -1.
            // If q.w is 1 (identity), angle is 0. axis can be anything (e.g. x-axis).
            // If q.w is -1 (180 deg rotation), angle is PI. axis is (x,y,z) part.
            if norm_q.w.abs() > 1.0 - f32::EPSILON {
                // q.w is 1 or -1
                (Vec3::new(1.0, 0.0, 0.0), 0.0) // Angle is 0 or 2PI, axis is arbitrary
            } else {
                // Should not happen if normalized, but as a fallback
                (self.xyz(), angle)
            }
        } else {
            let axis = self.xyz() / s;
            (axis, angle)
        }
    }

    /// Converts the quaternion to an axis and typed angle (radians).
    #[inline]
    pub fn to_axis_angle_radians(self) -> (Vec3<Unit, Space>, crate::angle::Radians) {
        let (axis, angle) = self.to_axis_angle();
        (axis, crate::angle::Radians(angle))
    }

    /// Converts the quaternion to an axis and typed angle (degrees).
    #[inline]
    pub fn to_axis_angle_deg(self) -> (Vec3<Unit, Space>, crate::angle::Degrees) {
        let (axis, angle) = self.to_axis_angle();
        (axis, crate::angle::Degrees(crate::scalar::to_degrees(angle)))
    }

    /// Calculates the pitch (rotation around the x-axis) in radians.
    #[inline]
    pub fn pitch(self) -> f32 {
        // Matches gb_quat_pitch
        math::atan2(
            2.0 * (self.y * self.z + self.w * self.x),
            self.w * self.w - self.x * self.x - self.y * self.y + self.z * self.z,
        )
    }

    /// Pitch as a typed radians angle.
    #[inline]
    pub fn pitch_radians(self) -> crate::angle::Radians {
        crate::angle::Radians(self.pitch())
    }

    /// Calculates the yaw (rotation around the y-axis) in radians.
    #[inline]
    pub fn yaw(self) -> f32 {
        // Matches gb_quat_yaw
        math::asin(-2.0 * (self.x * self.z - self.w * self.y))
    }

    /// Yaw as a typed radians angle.
    #[inline]
    pub fn yaw_radians(self) -> crate::angle::Radians {
        crate::angle::Radians(self.yaw())
    }

    /// Calculates the roll (rotation around the z-axis) in radians.
    #[inline]
    pub fn roll(self) -> f32 {
        // Matches gb_quat_roll
        math::atan2(
            2.0 * (self.x * self.y + self.w * self.z),
            self.w * self.w + self.x * self.x - self.y * self.y - self.z * self.z,
        )
    }

    /// Roll as a typed radians angle.
    #[inline]
    pub fn roll_radians(self) -> crate::angle::Radians {
        crate::angle::Radians(self.roll())
    }

    /// Creates a quaternion from a 4x4 rotation matrix.
    #[inline]
    #[cfg(feature = "mat4")]
    pub fn from_mat4(m: &Mat4) -> Self {
        // Matches gb_quat_from_mat4
        // C++ gbMat4 is column-major like Rust Mat4.
        // m[col_idx][row_idx] in C++ comments refers to mat->col[c].e[r]
        // Rust m.x_col.x is m[0][0], m.x_col.y is m[0][1], m.y_col.x is m[1][0] etc.
        let mut q = Quat::ZERO;

        // These are direct mappings from C++ m[col][row] to Rust m.col_name.component_name
        let m00 = m.x_col.x; // C++ m[0][0]
        let m01 = m.x_col.y; // C++ m[0][1]
        let m02 = m.x_col.z; // C++ m[0][2]
        // m03 = m.x_col.w; // C++ m[0][3]

        let m10 = m.y_col.x; // C++ m[1][0]
        let m11 = m.y_col.y; // C++ m[1][1]
        let m12 = m.y_col.z; // C++ m[1][2]
        // m13 = m.y_col.w; // C++ m[1][3]

        let m20 = m.z_col.x; // C++ m[2][0]
        let m21 = m.z_col.y; // C++ m[2][1]
        let m22 = m.z_col.z; // C++ m[2][2]
        // m23 = m.z_col.w; // C++ m[2][3]

        // let m30 = m.w_col.x; // C++ m[3][0]
        // let m31 = m.w_col.y; // C++ m[3][1]
        // let m32 = m.w_col.z; // C++ m[3][2]
        // let m33 = m.w_col.w; // C++ m[3][3]

        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; // This is trace of 3x3 part

        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;

        match biggest_index {
            0 => {
                // W is biggest
                q.w = biggest_value;
                q.x = (m12 - m21) * mult; // C++: (m[1][2] - m[2][1]) -> (m.y_col.z - m.z_col.y)
                q.y = (m20 - m02) * mult; // C++: (m[2][0] - m[0][2]) -> (m.z_col.x - m.x_col.z)
                q.z = (m01 - m10) * mult; // C++: (m[0][1] - m[1][0]) -> (m.x_col.y - m.y_col.x)
            }
            1 => {
                // X is biggest
                // C++ original:
                // out->w = (m[1][2] - m[2][1]) * mult; -> (m.y_col.z - m.z_col.y) * mult
                // out->x = biggest_value;
                // out->y = (m[0][1] + m[1][0]) * mult; -> (m.x_col.y + m.y_col.x) * mult
                // out->z = (m[2][0] + m[0][2]) * mult; -> (m.z_col.x + m.x_col.z) * mult
                q.w = (m12 - m21) * mult;
                q.x = biggest_value;
                q.y = (m01 + m10) * mult;
                q.z = (m20 + m02) * mult;
            }
            2 => {
                // Y is biggest
                // C++ original:
                // out->w = (m[2][0] - m[0][2]) * mult; -> (m.z_col.x - m.x_col.z) * mult
                // out->x = (m[0][1] + m[1][0]) * mult; -> (m.x_col.y + m.y_col.x) * mult
                // out->y = biggest_value;
                // out->z = (m[1][2] + m[2][1]) * mult; -> (m.y_col.z + m.z_col.y) * mult
                q.w = (m20 - m02) * mult;
                q.x = (m01 + m10) * mult;
                q.y = biggest_value;
                q.z = (m12 + m21) * mult;
            }
            3 => {
                // Z is biggest
                // C++ original:
                // out->w = (m[0][1] - m[1][0]) * mult; -> (m.x_col.y - m.y_col.x) * mult
                // out->x = (m[2][0] + m[0][2]) * mult; -> (m.z_col.x + m.x_col.z) * mult
                // out->y = (m[1][2] + m[2][1]) * mult; -> (m.y_col.z + m.z_col.y) * mult
                // out->z = biggest_value;
                q.w = (m01 - m10) * mult;
                q.x = (m20 + m02) * mult;
                q.y = (m12 + m21) * mult;
                q.z = biggest_value;
            }
            _ => unreachable!(), // Should not happen
        }
        q
    }

    /// Linear interpolation between two quaternions.
    #[inline]
    pub fn lerp(self, b: Self, t: f32) -> Self {
        Quat {
            x: self.x + (b.x - self.x) * t,
            y: self.y + (b.y - self.y) * t,
            z: self.z + (b.z - self.z) * t,
            w: self.w + (b.w - self.w) * t,
            _unit: PhantomData,
            _space: PhantomData,
        }
    }

    /// Normalized linear interpolation between two quaternions.
    #[inline]
    pub fn nlerp(self, b: Self, t: f32) -> Self {
        self.lerp(b, t).normalize()
    }

    /// Spherical linear interpolation between two quaternions.
    /// Handles inputs that are not normalized and ensures the shortest path.
    pub fn slerp(self, b: Self, t: f32) -> Self {
        let mut b_shortest_path = b;
        let mut cos_theta = self.dot(b);

        // Ensure shortest path
        if cos_theta < 0.0 {
            b_shortest_path = -b_shortest_path; // Negate all components
            cos_theta = -cos_theta;
        }

        // If quaternions are (nearly) collinear, use nlerp to avoid division by zero / precision issues.
        // The threshold of 0.9999 is common.
        if cos_theta > 1.0 - 1e-4 {
            // Adjusted from > 1.0 for practical robustness
            // In the C++ code, if cos_theta > 1.0, it directly calls gb_quat_lerp.
            // However, for normalized quaternions, dot product shouldn't exceed 1.0.
            // This path handles nearly collinear quaternions by falling back to nlerp.
            return self.nlerp(b_shortest_path, t);
        }

        // Standard slerp
        let angle = math::acos(cos_theta); // acos safe due to previous checks / clamping implicit in logic
        let sin_angle = math::sin(angle);

        // Catch division by zero if angle is very small (already handled by cos_theta check)
        // or if angle is PI (handled by shortest path logic).
        // However, a direct check for sin_angle being close to zero is robust.
        if sin_angle.abs() < 1e-6 {
            return self.nlerp(b_shortest_path, t);
        }

        let s0 = math::sin((1.0 - t) * angle) / sin_angle;
        let s1 = math::sin(t * angle) / sin_angle;

        (self * s0) + (b_shortest_path * s1)
    }

    /// Approximate spherical linear interpolation.
    #[inline]
    pub fn slerp_approx(self, b: Self, t: f32) -> Self {
        let cos_omega = self.dot(b);
        // The C++ code does not handle the shortest path for slerp_approx's dot product.
        // To match C++ exactly, we use cos_omega directly.
        // let b_prime = if cos_omega < 0.0 { -b } else { b };
        // let cos_omega_prime = if cos_omega < 0.0 { -cos_omega } else { cos_omega };

        let tp = t + (1.0 - cos_omega) / 3.0 * t * (-2.0 * t * t + 3.0 * t - 1.0);
        self.nlerp(b, tp) // Using b, not b_prime, to match C++ gb_quat_nlerp(d, a, b, tp)
    }

    /// Normalized quadratic interpolation.
    #[inline]
    pub fn nquad(p: Self, a: Self, b: Self, q: Self, t: f32) -> Self {
        let x = p.nlerp(q, t);
        let y = a.nlerp(b, t);
        x.nlerp(y, 2.0 * t * (1.0 - t))
    }

    /// Spherical quadratic interpolation.
    #[inline]
    pub fn squad(p: Self, a: Self, b: Self, q: Self, t: f32) -> Self {
        let x = p.slerp(q, t);
        let y = a.slerp(b, t);
        x.slerp(y, 2.0 * t * (1.0 - t))
    }

    /// Approximate spherical quadratic interpolation.
    #[inline]
    pub fn squad_approx(p: Self, a: Self, b: Self, q: Self, t: f32) -> Self {
        let x = p.slerp_approx(q, t);
        let y = a.slerp_approx(b, t);
        x.slerp_approx(y, 2.0 * t * (1.0 - t))
    }

    /// Creates a quaternion from a 3x3 rotation matrix.
    #[cfg(feature = "mat3")]
    pub fn from_mat3(m: &Mat3) -> Self {
        // Same algorithm as from_mat4, but for 3x3
        let m00 = m.x_col.x;
        let m01 = m.x_col.y;
        let m02 = m.x_col.z;
        let m10 = m.y_col.x;
        let m11 = m.y_col.y;
        let m12 = m.y_col.z;
        let m20 = m.z_col.x;
        let m21 = m.z_col.y;
        let m22 = m.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 = Self::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
    }

    /// Converts the quaternion to a 3x3 rotation matrix.
    #[cfg(feature = "mat3")]
    pub fn to_mat3(&self) -> Mat3<Unit, Space> {
        Mat3::<Unit, Space>::from_quat(*self)
    }

    /// Returns true if the quaternion is normalized (length is approximately 1).
    pub fn is_normalized(&self) -> bool {
        (self.length() - 1.0).abs() < 1e-5
    }

    /// Returns the angle (in radians) between self and another quaternion.
    pub fn angle_between(self, other: Self) -> f32 {
        let dot = self.dot(other).clamp(-1.0, 1.0);
        2.0 * math::acos(dot.abs())
    }

    /// Angle between two quaternions as typed radians.
    #[inline]
    pub fn angle_between_radians(self, other: Self) -> crate::angle::Radians {
        crate::angle::Radians(self.angle_between(other))
    }

    /// Rotates a Vec3 in-place by this quaternion.
    pub fn rotate_vec3(&self, v: &mut Vec3<Unit, Space>) {
        *v = *self * *v;
    }

    /// Returns the logarithm of the quaternion (for advanced interpolation).
    pub fn ln(self) -> Self {
        // ln(q) = [v/|v| * acos(w/|q|), 0] if |v| > 0, else [0,0,0,0]
        let norm = self.length();
        let v = self.xyz();
        let v_len = v.length();
        if v_len > 1e-6 {
            let theta = math::acos((self.w / norm).clamp(-1.0, 1.0));
            let coeff = theta / v_len;
            Self::new(v.x * coeff, v.y * coeff, v.z * coeff, 0.0)
        } else {
            Self::ZERO
        }
    }

    /// Returns the exponential of the quaternion (for advanced interpolation).
    pub fn exp(self) -> Self {
        // exp([v, 0]) = [sin(|v|) * v/|v|, cos(|v|)]
        let v = self.xyz();
        let v_len = v.length();
        let exp_w = math::exp(self.w);
        if v_len > 1e-6 {
            let s = math::sin(v_len) / v_len;
            Self::new(v.x * s, v.y * s, v.z * s, math::cos(v_len)) * exp_w
        } else {
            Self::new(0.0, 0.0, 0.0, exp_w)
        }
    }
}

// Operator traits
impl<Unit: Copy, Space: Copy> Neg for Quat<Unit, Space> {
    type Output = Self;
    #[inline]
    fn neg(self) -> Self::Output {
        Self {
            x: -self.x,
            y: -self.y,
            z: -self.z,
            w: -self.w,
            _unit: PhantomData,
            _space: PhantomData,
        }
    }
}

impl<Unit: Copy, Space: Copy> Add<Quat<Unit, Space>> for Quat<Unit, Space> {
    type Output = Self;
    #[inline]
    fn add(self, rhs: Self) -> Self::Output {
        Self {
            x: self.x + rhs.x,
            y: self.y + rhs.y,
            z: self.z + rhs.z,
            w: self.w + rhs.w,
            _unit: PhantomData,
            _space: PhantomData,
        }
    }
}

impl<Unit: Copy, Space: Copy> AddAssign<Quat<Unit, Space>> for Quat<Unit, Space> {
    #[inline]
    fn add_assign(&mut self, rhs: Self) {
        self.x += rhs.x;
        self.y += rhs.y;
        self.z += rhs.z;
        self.w += rhs.w;
    }
}

impl<Unit: Copy, Space: Copy> Sub<Quat<Unit, Space>> for Quat<Unit, Space> {
    type Output = Self;
    #[inline]
    fn sub(self, rhs: Self) -> Self::Output {
        Self {
            x: self.x - rhs.x,
            y: self.y - rhs.y,
            z: self.z - rhs.z,
            w: self.w - rhs.w,
            _unit: PhantomData,
            _space: PhantomData,
        }
    }
}

impl<Unit: Copy, Space: Copy> SubAssign<Quat<Unit, Space>> for Quat<Unit, Space> {
    #[inline]
    fn sub_assign(&mut self, rhs: Self) {
        self.x -= rhs.x;
        self.y -= rhs.y;
        self.z -= rhs.z;
        self.w -= rhs.w;
    }
}

impl<Unit: Copy, Space: Copy> Mul<Quat<Unit, Space>> for Quat<Unit, Space> {
    type Output = Self;
    #[inline]
    fn mul(self, rhs: Self) -> Self::Output {
        Self {
            x: self.w * rhs.x + self.x * rhs.w + self.y * rhs.z - self.z * rhs.y,
            y: self.w * rhs.y - self.x * rhs.z + self.y * rhs.w + self.z * rhs.x,
            z: self.w * rhs.z + self.x * rhs.y - self.y * rhs.x + self.z * rhs.w,
            w: self.w * rhs.w - self.x * rhs.x - self.y * rhs.y - self.z * rhs.z,
            _unit: PhantomData,
            _space: PhantomData,
        }
    }
}

impl<Unit: Copy, Space: Copy> MulAssign<Quat<Unit, Space>> for Quat<Unit, Space> {
    #[inline]
    fn mul_assign(&mut self, rhs: Self) {
        *self = *self * rhs;
    }
}

impl<Unit: Copy, Space: Copy> Mul<f32> for Quat<Unit, Space> {
    type Output = Self;
    #[inline]
    fn mul(self, rhs: f32) -> Self::Output {
        Self {
            x: self.x * rhs,
            y: self.y * rhs,
            z: self.z * rhs,
            w: self.w * rhs,
            _unit: PhantomData,
            _space: PhantomData,
        }
    }
}

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

impl<Unit: Copy, Space: Copy> MulAssign<f32> for Quat<Unit, Space> {
    #[inline]
    fn mul_assign(&mut self, rhs: f32) {
        self.x *= rhs;
        self.y *= rhs;
        self.z *= rhs;
        self.w *= rhs;
    }
}

impl<Unit: Copy, Space: Copy> Div<f32> for Quat<Unit, Space> {
    type Output = Self;
    #[inline]
    fn div(self, rhs: f32) -> Self::Output {
        // Consider adding a check for rhs == 0.0 if error handling is desired
        let inv_rhs = 1.0 / rhs;
        Self {
            x: self.x * inv_rhs,
            y: self.y * inv_rhs,
            z: self.z * inv_rhs,
            w: self.w * inv_rhs,
            _unit: PhantomData,
            _space: PhantomData,
        }
    }
}

impl<Unit: Copy, Space: Copy> DivAssign<f32> for Quat<Unit, Space> {
    #[inline]
    fn div_assign(&mut self, rhs: f32) {
        // Consider adding a check for rhs == 0.0
        let inv_rhs = 1.0 / rhs;
        self.x *= inv_rhs;
        self.y *= inv_rhs;
        self.z *= inv_rhs;
        self.w *= inv_rhs;
    }
}

impl<Unit: Copy, Space: Copy> Mul<Vec3<Unit, Space>> for Quat<Unit, Space> {
    type Output = Vec3<Unit, Space>;
    #[inline]
    fn mul(self, v: Vec3<Unit, Space>) -> Self::Output {
        let qv = self.xyz(); // Vector part of quaternion
        let mut t = qv.cross(v);
        t = t * 2.0;
        let p = qv.cross(t);
        // Formula: v_rotated = v + self.w * t + p
        // (v + (t * self.w)) + p
        (v + (t * self.w)).add(p) // Explicit add for clarity if + isn't overloaded for Vec3 yet.
        // Assuming Vec3 has Add, Mul<f32>, and cross implemented.
    }
}