//! # Quat
//!
//! Quaternion type for representing 3D rotations.
//!
//! Quaternions avoid gimbal lock and interpolate smoothly,
//! making them the preferred rotation for game-engine use cases.
//!
//! # Conventions
//!
//! - Assumes right-handed coordinate system
//! - Euler angles use ZYX order (yaw, pitch, roll)
//! - Angles are in radians, unless otherwise stated
//!
//! # Example
//!
//! ```rust
//! # use roofy::math::{Quat, Vec3};
//!
//! let rotation: Quat = Quat::from_axis_angle_degrees(Vec3::Y, 90.0);
//!
//! let forward: Vec3 = Vec3::Z;
//!
//! let rotated: Vec3 = rotation * forward;
//!
//! assert!((rotated.x - 1.0).abs() < 1e-5);
//! assert!(rotated.y.abs() < 1e-5);
//! assert!(rotated.z.abs() < 1e-5);
//! ```
use std::ops::{Mul, Neg};

use crate::math::{Vec3, Vec4};

/// Quaternion representing a 3D rotation, with `f32` components.
/// Same memory layout as `[f32; 4]`, making it safe for GPU uploads.
///
/// Stored as `(x, y, z, w)`, where `(x, y, z)` is the vector part
/// and `w` is the scalar part.
#[derive(Clone, Copy, Debug, PartialEq)]
#[repr(C)]
pub struct Quat {
    /// X component of the quaternion.
    pub x: f32,
    /// Y component of the quaternion.
    pub y: f32,
    /// Z component of the quaternion.
    pub z: f32,
    /// W component of the quaternion.
    pub w: f32,
}

impl Quat {
    /// Threshold for treating a value as zero.
    pub const EPSILON: f32 = 1e-6;
    /// [`EPSILON`](Self::EPSILON) squared.
    pub const EPSILON_SQ: f32 = Self::EPSILON * Self::EPSILON;

    /// Shorthand for writing `Quat::new(f32::NAN, f32::NAN, f32::NAN, f32::NAN)`.
    pub const NAN: Self = Self::new(f32::NAN, f32::NAN, f32::NAN, f32::NAN);

    /// Shorthand for writing `Quat::new(0.0, 0.0, 0.0, 1.0)`.
    pub const IDENTITY: Self = Self::new(0.0, 0.0, 0.0, 1.0);

    /// Creates a `Quat` with `x`, `y`, `z` and `w` components.
    #[inline]
    #[must_use]
    pub const fn new(x: f32, y: f32, z: f32, w: f32) -> Self {
        Self { x, y, z, w }
    }

    /// Creates a `Quat` representing a rotation of `angle` (in radians) around `axis`.
    ///
    /// # Precondition
    /// Expects `axis` to be normalized.
    #[inline]
    #[must_use]
    pub fn from_axis_angle(axis: Vec3, angle: f32) -> Self {
        let (sin, cos): (f32, f32) = (angle * 0.5).sin_cos();

        Self::new(axis.x * sin, axis.y * sin, axis.z * sin, cos)
    }

    /// Creates a `Quat` representing a rotation of `angle` (in degrees) around `axis`.
    ///
    /// # Precondition
    /// Expects `axis` to be normalized.
    #[inline]
    #[must_use]
    pub fn from_axis_angle_degrees(axis: Vec3, angle: f32) -> Self {
        Self::from_axis_angle(axis, angle.to_radians())
    }

    /// Returns the axis and angle (in radians) of `self`.
    ///
    /// # Precondition
    /// Expects `self` to be normalized.
    ///
    /// # Edge cases
    /// If `self` is near-identity, returns `(Vec3::X, 0.0)`.
    #[inline]
    #[must_use]
    pub fn to_axis_angle(self) -> (Vec3, f32) {
        let angle: f32 = self.angle();
        let sin_half: f32 = (angle * 0.5).sin();

        if sin_half.abs() < Self::EPSILON {
            return (Vec3::X, 0.0);
        }

        let inv: f32 = 1.0 / sin_half;
        (Vec3::new(self.x * inv, self.y * inv, self.z * inv), angle)
    }

    /// Returns the axis and angle (in degrees) of `self`.
    ///
    /// # Precondition
    /// Expects `self` to be normalized.
    ///
    /// # Edge cases
    /// If `self` is near-identity, returns `(Vec3::X, 0.0)`.
    #[inline]
    #[must_use]
    pub fn to_axis_angle_degrees(self) -> (Vec3, f32) {
        let (axis, angle): (Vec3, f32) = self.to_axis_angle();
        (axis, angle.to_degrees())
    }

    /// Creates a `Quat` from Euler angles in ZYX order (yaw, pitch, roll) in radians.
    ///
    /// # Convention
    /// Assumes right-handed coordinate system.
    #[inline]
    #[must_use]
    pub fn from_euler_zyx(yaw: f32, pitch: f32, roll: f32) -> Self {
        let (sy, cy): (f32, f32) = (yaw * 0.5).sin_cos();
        let (sp, cp): (f32, f32) = (pitch * 0.5).sin_cos();
        let (sr, cr): (f32, f32) = (roll * 0.5).sin_cos();

        Self::new(
            cy * cp * sr - sy * sp * cr,
            cy * sp * cr + sy * cp * sr,
            sy * cp * cr - cy * sp * sr,
            cy * cp * cr + sy * sp * sr,
        )
    }

    /// Creates a `Quat` from Euler angles in ZYX order (yaw, pitch, roll) in degrees.
    ///
    /// # Convention
    /// Assumes right-handed coordinate system.
    #[inline]
    #[must_use]
    pub fn from_euler_degrees_zyx(yaw: f32, pitch: f32, roll: f32) -> Self {
        Self::from_euler_zyx(yaw.to_radians(), pitch.to_radians(), roll.to_radians())
    }

    /// Returns a `Vec3` with Euler angles from `self` in ZYX order (yaw, pitch, roll) in radians.
    ///
    /// # Convention
    /// Assumes right-handed coordinate system.
    ///
    /// # Gimbal lock
    /// When `pitch` ≈ `PI / 2`, yaw and roll become indistinguishable,
    /// then yaw is set to `0.0` and all rotation is expressed as roll.
    #[inline]
    #[must_use]
    pub fn to_euler_zyx(self) -> Vec3 {
        let Self { x, y, z, w } = self;

        let sin_pitch: f32 = 2.0 * (w * y - z * x);
        let pitch: f32 = sin_pitch.clamp(-1.0, 1.0).asin();

        if sin_pitch.abs() > 0.9999 {
            let yaw: f32 = 0.0;
            let roll: f32 = x.atan2(w) * 2.0;
            return Vec3::new(yaw, pitch, roll);
        }

        let yaw: f32 = (2.0 * (w * z + x * y)).atan2(1.0 - 2.0 * (y * y + z * z));
        let roll: f32 = (2.0 * (w * x + y * z)).atan2(1.0 - 2.0 * (x * x + y * y));

        Vec3::new(yaw, pitch, roll)
    }

    /// Returns a `Vec3` with Euler angles from `self` in ZYX order (yaw, pitch, roll) in degrees.
    ///
    /// # Convention
    /// Assumes right-handed coordinate system.
    ///
    /// # Gimbal lock
    /// When `pitch` ≈ `90.0`, yaw and roll become indistinguishable,
    /// then yaw is set to `0.0` and all rotation is expressed as roll.
    #[inline]
    #[must_use]
    pub fn to_euler_degrees_zyx(self) -> Vec3 {
        let e: Vec3 = self.to_euler_zyx();
        Vec3::new(e.x.to_degrees(), e.y.to_degrees(), e.z.to_degrees())
    }

    /// Returns a `Quat` that rotates from `+Z` world-forward to `forward`.
    ///
    /// # Convention
    /// Assumes right-handed coordinate system.
    ///
    /// # Precondition
    /// Expects `forward` to be normalized and `up` to not be parallel to `forward`.
    ///
    /// # Edge cases
    /// If `forward` is near-zero, returns [`IDENTITY`](Self::IDENTITY).
    #[inline]
    #[must_use]
    pub fn from_look_rotation(forward: Vec3, up: Vec3) -> Self {
        if forward.length_squared() < Self::EPSILON_SQ {
            return Self::IDENTITY;
        }

        let right: Vec3 = up.cross(forward).normalized();
        let up: Vec3 = forward.cross(right);

        let trace: f32 = right.x + up.y + forward.z;

        if trace > 0.0 {
            let s: f32 = 0.5 / (trace + 1.0).sqrt();
            Self::new(
                (up.z - forward.y) * s,
                (forward.x - right.z) * s,
                (right.y - up.x) * s,
                0.25 / s,
            )
        } else if right.x > up.y && right.x > forward.z {
            let s: f32 = 2.0 * (1.0 + right.x - up.y - forward.z).sqrt();
            Self::new(
                0.25 * s,
                (up.x + right.y) / s,
                (forward.x + right.z) / s,
                (up.z - forward.y) / s,
            )
        } else if up.y > forward.z {
            let s: f32 = 2.0 * (1.0 + up.y - right.x - forward.z).sqrt();
            Self::new(
                (up.x + right.y) / s,
                0.25 * s,
                (forward.y + up.z) / s,
                (forward.x - right.z) / s,
            )
        } else {
            let s: f32 = 2.0 * (1.0 + forward.z - right.x - up.y).sqrt();
            Self::new(
                (forward.x + right.z) / s,
                (forward.y + up.z) / s,
                0.25 * s,
                (right.y - up.x) / s,
            )
        }
    }

    /// Returns a `Quat` that rotates from `-Z` world-forward to `forward`.
    ///
    /// # Convention
    /// Assumes left-handed coordinate system.
    ///
    /// # Precondition
    /// Expects `forward` to be normalized and `up` to not be parallel to `forward`.
    ///
    /// # Edge cases
    /// If `forward` is near-zero, returns [`IDENTITY`](Self::IDENTITY).
    #[inline]
    #[must_use]
    pub fn from_look_rotation_lh(forward: Vec3, up: Vec3) -> Self {
        Self::from_look_rotation(-forward, up)
    }

    /// Returns a `Quat` that rotates `from` to `to` along the shortest arc.
    ///
    /// # Precondition
    /// Expects `from` and `to` to be normalized.
    ///
    /// # Edge cases
    /// - If either vector is near-zero, returns [`IDENTITY`](Self::IDENTITY).
    #[inline]
    #[must_use]
    pub fn from_to_rotation(from: Vec3, to: Vec3) -> Self {
        let dot: f32 = from.dot(to).clamp(-1.0, 1.0);

        if from.length_squared() < Self::EPSILON_SQ || to.length_squared() < Self::EPSILON_SQ {
            return Self::IDENTITY;
        }

        if dot < -1.0 + Self::EPSILON {
            let axis: Vec3 = Vec3::X.cross(from);
            let axis: Vec3 = if axis.length_squared() < Self::EPSILON_SQ {
                Vec3::Y.cross(from)
            } else {
                axis
            };
            return Self::from_axis_angle(axis.normalized(), std::f32::consts::PI);
        }

        let axis: Vec3 = from.cross(to);
        let s: f32 = ((1.0 + dot) * 2.0).sqrt();

        Self::new(axis.x / s, axis.y / s, axis.z / s, s * 0.5).normalized()
    }

    /// Returns the length (magnitude) of `self`.
    #[inline]
    #[must_use]
    pub fn length(self) -> f32 {
        self.dot(self).sqrt()
    }

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

    /// Normalizes `self` to have a length of 1, while preserving direction.
    ///
    /// # Edge cases
    /// If `self` is near-zero, sets `self` to [`IDENTITY`](Self::IDENTITY).
    #[inline]
    pub fn normalize(&mut self) {
        let len_sq: f32 = self.length_squared();

        if len_sq < Self::EPSILON_SQ {
            *self = Self::IDENTITY;
            return;
        }

        let inv_len: f32 = 1.0 / len_sq.sqrt();
        self.x *= inv_len;
        self.y *= inv_len;
        self.z *= inv_len;
        self.w *= inv_len;
    }

    /// Returns a unit-length `Quat` with direction of `self`.
    ///
    /// # Edge cases
    /// If `self` is near-zero, returns [`IDENTITY`](Self::IDENTITY).
    #[inline]
    #[must_use]
    pub fn normalized(self) -> Self {
        let len_sq: f32 = self.length_squared();

        if len_sq < Self::EPSILON_SQ {
            return Self::IDENTITY;
        }

        let inv_len: f32 = 1.0 / len_sq.sqrt();
        Self::new(
            self.x * inv_len,
            self.y * inv_len,
            self.z * inv_len,
            self.w * inv_len,
        )
    }

    /// Returns `true` if `self` approximately has a length of `1.0`.
    ///
    /// Uses [`EPSILON`](Self::EPSILON) as threshold.
    #[inline]
    #[must_use]
    pub const fn is_normalized(self) -> bool {
        (self.length_squared() - 1.0).abs() < Self::EPSILON
    }

    /// Returns the dot product of `self` and `other`.
    #[inline]
    #[must_use]
    pub const fn dot(self, other: Self) -> f32 {
        self.x * other.x + self.y * other.y + self.z * other.z + self.w * other.w
    }

    /// Conjugates `self`.
    #[inline]
    pub const fn conjugate(&mut self) {
        self.x = -self.x;
        self.y = -self.y;
        self.z = -self.z;
    }

    /// Returns the conjugate (inverse for unit quaternions) of `self`.
    #[inline]
    #[must_use]
    pub const fn conjugated(self) -> Self {
        Self::new(-self.x, -self.y, -self.z, self.w)
    }

    /// Inverts `self`.
    ///
    /// # Edge cases
    /// If `self` is near-zero, sets `self` to [`IDENTITY`](Self::IDENTITY).
    #[inline]
    pub const fn inverse(&mut self) {
        let len_sq: f32 = self.length_squared();

        if len_sq < Self::EPSILON_SQ {
            *self = Self::IDENTITY;
            return;
        }

        let inv_len: f32 = 1.0 / len_sq;
        self.x = -self.x * inv_len;
        self.y = -self.y * inv_len;
        self.z = -self.z * inv_len;
        self.w *= inv_len;
    }

    /// Returns the inverse of `self`.
    ///
    /// # Edge cases
    /// If `self` is near-zero, returns [`IDENTITY`](Self::IDENTITY).
    #[inline]
    #[must_use]
    pub const fn inversed(self) -> Self {
        let len_sq: f32 = self.length_squared();

        if len_sq < Self::EPSILON_SQ {
            return Self::IDENTITY;
        }

        let inv_len: f32 = 1.0 / len_sq;
        Self::new(
            -self.x * inv_len,
            -self.y * inv_len,
            -self.z * inv_len,
            self.w * inv_len,
        )
    }

    /// Returns the rotation angle of `self` in radians.
    ///
    /// # Precondition
    /// Expects `self` to be normalized.
    #[inline]
    #[must_use]
    pub fn angle(self) -> f32 {
        self.w.clamp(-1.0, 1.0).acos() * 2.0
    }

    /// Returns the rotation angle of `self` in degrees.
    ///
    /// # Precondition
    /// Expects `self` to be normalized.
    #[inline]
    #[must_use]
    pub fn angle_degrees(self) -> f32 {
        self.angle().to_degrees()
    }

    /// Returns a `Quat` linearly interpolated between `self` and `to`, with factor `t`, normalized.
    ///
    /// # Precondition
    /// Expects `self` and `to` to be normalized.
    #[inline]
    #[must_use]
    pub fn lerp(self, mut to: Self, t: f32) -> Self {
        if self.dot(to) < 0.0 {
            to = -to;
        }

        Self::new(
            self.x + (to.x - self.x) * t,
            self.y + (to.y - self.y) * t,
            self.z + (to.z - self.z) * t,
            self.w + (to.w - self.w) * t,
        )
        .normalized()
    }

    /// Returns a `Quat` linearly interpolated between `self` and `to`, with factor `t`, normalized.
    ///
    /// Clamps `t` between `0.0` and `1.0`.
    ///
    /// # Precondition
    /// Expects `self` and `to` to be normalized.
    #[inline]
    #[must_use]
    pub fn lerp_clamped(self, to: Self, t: f32) -> Self {
        self.lerp(to, t.clamp(0.0, 1.0))
    }

    /// Returns a `Quat` spherically interpolated between `self` and `to`, with factor `t`.
    ///
    /// Falls back to [`lerp`](Self::lerp), when `self` and `to` are nearly identical.
    ///
    /// # Precondition
    /// Expects `self` and `to` to be normalized.
    #[inline]
    #[must_use]
    pub fn slerp(self, mut to: Self, t: f32) -> Self {
        let mut dot: f32 = self.dot(to).clamp(-1.0, 1.0);

        if dot < 0.0 {
            to = -to;
            dot = -dot;
        }

        if dot > 1.0 - Self::EPSILON {
            return self.lerp(to, t);
        }

        let theta: f32 = dot.acos();
        let sin_theta: f32 = theta.sin();

        let a: f32 = ((1.0 - t) * theta).sin() / sin_theta;
        let b: f32 = (t * theta).sin() / sin_theta;

        Self::new(
            a * self.x + b * to.x,
            a * self.y + b * to.y,
            a * self.z + b * to.z,
            a * self.w + b * to.w,
        )
    }

    /// Returns a `Quat` spherically interpolated between `self` and `to`, with factor `t`.
    ///
    /// Falls back to [`lerp`](Self::lerp), when `self` and `to` are nearly identical.
    ///
    /// Clamps `t` between `0.0` and `1.0`.
    ///
    /// # Precondition
    /// Expects `self` and `to` to be normalized.
    #[inline]
    #[must_use]
    pub fn slerp_clamped(self, to: Self, t: f32) -> Self {
        self.slerp(to, t.clamp(0.0, 1.0))
    }

    /// Returns a `[f32; 4]` copy of the components.
    #[inline]
    #[must_use]
    pub const fn to_array(self) -> [f32; 4] {
        [self.x, self.y, self.z, self.w]
    }

    /// Returns `true` if `self` and `other` approximately represent the same rotation.
    ///
    /// Uses [`EPSILON`](Self::EPSILON) as threshold.
    ///
    /// # Precondition
    /// Expects `self` and `other` to be normalized.
    #[inline]
    #[must_use]
    pub const fn approx_eq(self, other: Self) -> bool {
        self.dot(other).abs() > 1.0 - Self::EPSILON
    }

    /// Returns `true` if all components of `self` are finite.
    #[inline]
    #[must_use]
    pub const fn is_finite(self) -> bool {
        self.x.is_finite() && self.y.is_finite() && self.z.is_finite() && self.w.is_finite()
    }

    /// Returns `true` if any components of `self` is `NaN`.
    #[inline]
    #[must_use]
    pub const fn is_nan(self) -> bool {
        self.x.is_nan() || self.y.is_nan() || self.z.is_nan() || self.w.is_nan()
    }
}

impl Mul for Quat {
    type Output = Self;

    /// Combines two quaternions, applying `rhs` first, then `self`.
    ///
    /// # Precondition
    /// Expects `self` and `rhs` to be normalized.
    #[inline]
    fn mul(self, rhs: Self) -> Self::Output {
        Self::new(
            self.w * rhs.x + self.x * rhs.w + self.y * rhs.z - self.z * rhs.y,
            self.w * rhs.y - self.x * rhs.z + self.y * rhs.w + self.z * rhs.x,
            self.w * rhs.z + self.x * rhs.y - self.y * rhs.x + self.z * rhs.w,
            self.w * rhs.w - self.x * rhs.x - self.y * rhs.y - self.z * rhs.z,
        )
    }
}

impl Mul<Vec3> for Quat {
    type Output = Vec3;

    /// Rotates `rhs` by `self`.
    ///
    /// # Precondition
    /// Expects `self` to be a normalized.
    #[inline]
    fn mul(self, rhs: Vec3) -> Self::Output {
        let u: Vec3 = Vec3::new(self.x, self.y, self.z);
        let t: Vec3 = u.cross(rhs) * 2.0;

        rhs + t * self.w + u.cross(t)
    }
}

impl Neg for Quat {
    type Output = Self;

    /// Negates all components of `self`.
    #[inline]
    fn neg(self) -> Self::Output {
        Self::new(-self.x, -self.y, -self.z, -self.w)
    }
}

impl From<[f32; 4]> for Quat {
    /// Creates a `Quat` from a `[f32; 4]`.
    /// Elements map to `(x, y, z, w)`.
    #[inline]
    fn from(v: [f32; 4]) -> Self {
        Self::new(v[0], v[1], v[2], v[3])
    }
}

impl From<Quat> for [f32; 4] {
    /// Creates a `[f32; 4]` from a `Quat`.
    /// Elements map to `[x, y, z, w]`.
    #[inline]
    fn from(v: Quat) -> Self {
        [v.x, v.y, v.z, v.w]
    }
}

impl From<&[f32]> for Quat {
    /// Creates a `Quat` from a `&[f32]`.
    /// First 4 elements map to `(x, y, z, w)`.
    ///
    /// # Panics
    /// Panics if `v` is less than 4 elements long.
    #[inline]
    fn from(v: &[f32]) -> Self {
        Self::new(v[0], v[1], v[2], v[3])
    }
}

impl From<Vec4> for Quat {
    /// Creates a `Vec4` from a `Vec4`.
    /// Elements map to `(x, y, z, w)`.
    #[inline]
    fn from(v: Vec4) -> Self {
        Self::new(v.x, v.y, v.z, v.w)
    }
}

impl AsRef<[f32; 4]> for Quat {
    /// Returns a `&[f32; 4]` reference to the `Quat` components.
    /// Elements map to `[x, y, z, w]`.
    #[inline]
    fn as_ref(&self) -> &[f32; 4] {
        unsafe { &*(self as *const Quat as *const [f32; 4]) }
    }
}

impl AsRef<[f32]> for Quat {
    /// Returns a `&[f32]` reference to the `Quat` components.
    /// Elements map to `[x, y, z, w]`.
    #[inline]
    fn as_ref(&self) -> &[f32] {
        let arr: &[f32; 4] = self.as_ref();
        &arr[..]
    }
}

impl AsMut<[f32; 4]> for Quat {
    /// Returns a `&[f32; 4]` mutable reference to the `Quat` components.
    /// Elements map to `[x, y, z, w]`.
    #[inline]
    fn as_mut(&mut self) -> &mut [f32; 4] {
        unsafe { &mut *(self as *mut Quat as *mut [f32; 4]) }
    }
}

impl AsMut<[f32]> for Quat {
    /// Returns a `&[f32]` mutable reference to the `Quat` components.
    /// Elements map to `[x, y, z, w]`.
    #[inline]
    fn as_mut(&mut self) -> &mut [f32] {
        let arr: &mut [f32; 4] = self.as_mut();
        &mut arr[..]
    }
}

impl Default for Quat {
    /// Default is [`IDENTITY`](Self::IDENTITY).
    #[inline]
    fn default() -> Self {
        Self::IDENTITY
    }
}
