//! Geometric primitives and intersection tests.
//!
//! Provides rays, planes, axis-aligned bounding boxes, spheres, and
//! ray-intersection routines.

use glam::Vec3;
use serde::{Deserialize, Serialize};
use std::fmt;

/// Maximum iterations for GJK collision detection (2D and 3D).
pub const GJK_MAX_ITERATIONS: usize = 64;

/// Maximum iterations for EPA penetration depth (2D and 3D).
pub const EPA_MAX_ITERATIONS: usize = 64;

/// A ray defined by an origin and a direction.
///
/// # Examples
///
/// ```
/// use hisab::geo::{Ray, Sphere, ray_sphere};
/// use glam::Vec3;
///
/// let ray = Ray::new(Vec3::new(0.0, 0.0, -5.0), Vec3::Z).unwrap();
/// let sphere = Sphere::new(Vec3::ZERO, 1.0).unwrap();
/// let t = ray_sphere(&ray, &sphere).unwrap();
/// assert!((t - 4.0).abs() < 1e-5);
/// ```
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct Ray {
    pub origin: Vec3,
    /// Should be normalized for correct distance results.
    pub direction: Vec3,
}

impl Ray {
    /// Create a new ray. Direction is normalized automatically.
    ///
    /// # Errors
    /// Returns [`crate::HisabError::InvalidInput`] if `direction` is zero-length.
    #[inline]
    pub fn new(origin: Vec3, direction: Vec3) -> Result<Self, crate::HisabError> {
        let len_sq = direction.length_squared();
        if len_sq < crate::EPSILON_F32 {
            return Err(crate::HisabError::InvalidInput(
                "ray direction must be non-zero".into(),
            ));
        }
        Ok(Self {
            origin,
            direction: direction.normalize(),
        })
    }

    /// Point along the ray at parameter `t`.
    #[must_use]
    #[inline]
    pub fn at(&self, t: f32) -> Vec3 {
        self.origin + self.direction * t
    }
}

impl fmt::Display for Ray {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        let p = f.precision();
        let o = self.origin;
        let d = self.direction;
        match p {
            Some(p) => write!(
                f,
                "Ray({:.p$}, {:.p$}, {:.p$} -> {:.p$}, {:.p$}, {:.p$})",
                o.x, o.y, o.z, d.x, d.y, d.z
            ),
            None => write!(
                f,
                "Ray({}, {}, {} -> {}, {}, {})",
                o.x, o.y, o.z, d.x, d.y, d.z
            ),
        }
    }
}

/// An infinite plane defined by a normal and a signed distance from the origin.
///
/// Points **on** the plane satisfy `dot(normal, point) - distance == 0`.
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct Plane {
    pub normal: Vec3,
    pub distance: f32,
}

impl Plane {
    /// Create a plane from a point on the plane and a normal.
    ///
    /// # Errors
    /// Returns [`crate::HisabError::InvalidInput`] if `normal` is zero-length.
    #[inline]
    pub fn from_point_normal(point: Vec3, normal: Vec3) -> Result<Self, crate::HisabError> {
        let len_sq = normal.length_squared();
        if len_sq < crate::EPSILON_F32 {
            return Err(crate::HisabError::InvalidInput(
                "plane normal must be non-zero".into(),
            ));
        }
        let n = normal * len_sq.sqrt().recip();
        Ok(Self {
            normal: n,
            distance: n.dot(point),
        })
    }

    /// Signed distance from a point to the plane.
    /// Positive = same side as normal, negative = opposite side.
    #[must_use]
    #[inline]
    pub fn signed_distance(&self, point: Vec3) -> f32 {
        self.normal.dot(point) - self.distance
    }
}

impl fmt::Display for Plane {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        let p = f.precision();
        let n = self.normal;
        match p {
            Some(p) => write!(
                f,
                "Plane(n=({:.p$}, {:.p$}, {:.p$}), d={:.p$})",
                n.x, n.y, n.z, self.distance
            ),
            None => write!(
                f,
                "Plane(n=({}, {}, {}), d={})",
                n.x, n.y, n.z, self.distance
            ),
        }
    }
}

/// An axis-aligned bounding box.
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct Aabb {
    pub min: Vec3,
    pub max: Vec3,
}

impl Aabb {
    /// Create a new AABB. Min/max are corrected if swapped.
    #[must_use]
    #[inline]
    pub fn new(a: Vec3, b: Vec3) -> Self {
        Self {
            min: a.min(b),
            max: a.max(b),
        }
    }

    /// Check whether a point is inside (or on the boundary of) this AABB.
    #[must_use]
    #[inline]
    pub fn contains(&self, point: Vec3) -> bool {
        point.cmpge(self.min).all() && point.cmple(self.max).all()
    }

    /// Center point of the AABB.
    #[must_use]
    #[inline]
    pub fn center(&self) -> Vec3 {
        (self.min + self.max) * 0.5
    }

    /// Size (extents) of the AABB.
    #[must_use]
    #[inline]
    pub fn size(&self) -> Vec3 {
        self.max - self.min
    }

    /// Merge two AABBs into one that encloses both.
    #[must_use]
    #[inline]
    pub fn merge(&self, other: &Aabb) -> Aabb {
        Aabb {
            min: self.min.min(other.min),
            max: self.max.max(other.max),
        }
    }
}

impl fmt::Display for Aabb {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        let p = f.precision();
        match p {
            Some(p) => write!(
                f,
                "Aabb(({:.p$}, {:.p$}, {:.p$})..({:.p$}, {:.p$}, {:.p$}))",
                self.min.x, self.min.y, self.min.z, self.max.x, self.max.y, self.max.z
            ),
            None => write!(
                f,
                "Aabb(({}, {}, {})..({}, {}, {}))",
                self.min.x, self.min.y, self.min.z, self.max.x, self.max.y, self.max.z
            ),
        }
    }
}

/// A sphere defined by a center and radius.
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct Sphere {
    pub center: Vec3,
    pub radius: f32,
}

impl Sphere {
    /// Create a new sphere.
    ///
    /// # Errors
    /// Returns [`crate::HisabError::InvalidInput`] if `radius` is negative.
    #[inline]
    pub fn new(center: Vec3, radius: f32) -> Result<Self, crate::HisabError> {
        if radius < 0.0 {
            return Err(crate::HisabError::InvalidInput(
                "sphere radius must be non-negative".into(),
            ));
        }
        Ok(Self { center, radius })
    }

    /// Check whether a point is inside (or on the surface of) this sphere.
    #[must_use]
    #[inline]
    pub fn contains_point(&self, point: Vec3) -> bool {
        (point - self.center).length_squared() <= self.radius * self.radius
    }
}

impl fmt::Display for Sphere {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        let p = f.precision();
        let c = self.center;
        match p {
            Some(p) => write!(
                f,
                "Sphere(({:.p$}, {:.p$}, {:.p$}), r={:.p$})",
                c.x, c.y, c.z, self.radius
            ),
            None => write!(f, "Sphere(({}, {}, {}), r={})", c.x, c.y, c.z, self.radius),
        }
    }
}

/// Ray-plane intersection. Returns the `t` parameter if the ray hits the plane
/// (only `t >= 0`, i.e. forward hits).
#[must_use]
#[inline]
pub fn ray_plane(ray: &Ray, plane: &Plane) -> Option<f32> {
    let denom = plane.normal.dot(ray.direction);
    if denom.abs() < crate::EPSILON_F32 {
        return None; // Ray parallel to plane
    }
    let t = (plane.distance - plane.normal.dot(ray.origin)) / denom;
    if t >= 0.0 { Some(t) } else { None }
}

/// Ray-sphere intersection using the quadratic formula.
/// Returns the nearest `t >= 0` if the ray hits the sphere.
///
/// Assumes `ray.direction` is normalized (guaranteed by `Ray::new`),
/// so the quadratic coefficient `a = 1` and is eliminated.
#[must_use]
#[inline]
pub fn ray_sphere(ray: &Ray, sphere: &Sphere) -> Option<f32> {
    let oc = ray.origin - sphere.center;
    // With normalized direction: a=1, so b=2*dot(oc,d), c=dot(oc,oc)-r²
    // Use half-b form: half_b = dot(oc, d), discriminant = half_b² - c
    let half_b = oc.dot(ray.direction);
    let c = oc.dot(oc) - sphere.radius * sphere.radius;
    let discriminant = half_b * half_b - c;

    if discriminant < 0.0 {
        return None;
    }

    let sqrt_d = discriminant.sqrt();
    let t1 = -half_b - sqrt_d;
    let t2 = -half_b + sqrt_d;

    if t1 >= 0.0 {
        Some(t1)
    } else if t2 >= 0.0 {
        Some(t2)
    } else {
        None
    }
}

/// Ray-AABB intersection using the slab method.
/// Returns the nearest `t >= 0` if the ray hits the AABB.
#[must_use]
#[inline]
pub fn ray_aabb(ray: &Ray, aabb: &Aabb) -> Option<f32> {
    let origin = ray.origin.to_array();
    let dir = ray.direction.to_array();
    let bb_min = aabb.min.to_array();
    let bb_max = aabb.max.to_array();

    let mut t_min = f32::NEG_INFINITY;
    let mut t_max = f32::INFINITY;

    for i in 0..3 {
        if dir[i].abs() < crate::EPSILON_F32 {
            if origin[i] < bb_min[i] || origin[i] > bb_max[i] {
                return None;
            }
        } else {
            let inv_d = 1.0 / dir[i];
            let mut t1 = (bb_min[i] - origin[i]) * inv_d;
            let mut t2 = (bb_max[i] - origin[i]) * inv_d;
            if t1 > t2 {
                std::mem::swap(&mut t1, &mut t2);
            }
            t_min = t_min.max(t1);
            t_max = t_max.min(t2);
            if t_min > t_max {
                return None;
            }
        }
    }

    if t_min >= 0.0 {
        Some(t_min)
    } else if t_max >= 0.0 {
        Some(t_max)
    } else {
        None
    }
}

// ---------------------------------------------------------------------------
// Primitives
// ---------------------------------------------------------------------------

/// A triangle defined by three vertices.
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct Triangle {
    pub vertices: [Vec3; 3],
}

impl Triangle {
    #[must_use]
    #[inline]
    pub fn new(a: Vec3, b: Vec3, c: Vec3) -> Self {
        Self {
            vertices: [a, b, c],
        }
    }

    /// Face normal (not normalized). Returns the cross product of two edges.
    ///
    /// The magnitude equals twice the triangle's area. Use [`unit_normal`](Self::unit_normal)
    /// for a normalized version.
    #[must_use]
    #[inline]
    pub fn normal(&self) -> Vec3 {
        let edge1 = self.vertices[1] - self.vertices[0];
        let edge2 = self.vertices[2] - self.vertices[0];
        edge1.cross(edge2)
    }

    /// Normalized face normal.
    ///
    /// Returns [`Vec3::Y`] for degenerate triangles (collinear/coincident vertices).
    #[must_use]
    #[inline]
    pub fn unit_normal(&self) -> Vec3 {
        let n = self.normal();
        let len_sq = n.length_squared();
        if len_sq < crate::EPSILON_F32 {
            return Vec3::Y; // Degenerate — arbitrary fallback
        }
        n * len_sq.sqrt().recip()
    }

    /// Area of the triangle.
    #[must_use]
    #[inline]
    pub fn area(&self) -> f32 {
        self.normal().length() * 0.5
    }

    /// Centroid (average of the three vertices).
    #[must_use]
    #[inline]
    pub fn centroid(&self) -> Vec3 {
        (self.vertices[0] + self.vertices[1] + self.vertices[2]) / 3.0
    }
}

impl fmt::Display for Triangle {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        let p = f.precision();
        let [a, b, c] = self.vertices;
        match p {
            Some(p) => write!(
                f,
                "Triangle(({:.p$}, {:.p$}, {:.p$}), ({:.p$}, {:.p$}, {:.p$}), ({:.p$}, {:.p$}, {:.p$}))",
                a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z
            ),
            None => write!(
                f,
                "Triangle(({}, {}, {}), ({}, {}, {}), ({}, {}, {}))",
                a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z
            ),
        }
    }
}

/// An infinite line defined by a point and a direction.
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct Line {
    pub origin: Vec3,
    /// Normalized direction.
    pub direction: Vec3,
}

impl Line {
    /// Create a new line. Direction is normalized automatically.
    ///
    /// # Errors
    /// Returns [`crate::HisabError::InvalidInput`] if `direction` is zero-length.
    #[inline]
    pub fn new(origin: Vec3, direction: Vec3) -> Result<Self, crate::HisabError> {
        let len_sq = direction.length_squared();
        if len_sq < crate::EPSILON_F32 {
            return Err(crate::HisabError::InvalidInput(
                "line direction must be non-zero".into(),
            ));
        }
        Ok(Self {
            origin,
            direction: direction.normalize(),
        })
    }

    /// Closest point on this infinite line to the given point.
    #[must_use]
    #[inline]
    pub fn closest_point(&self, point: Vec3) -> Vec3 {
        let v = point - self.origin;
        let t = v.dot(self.direction);
        self.origin + self.direction * t
    }

    /// Distance from a point to this line.
    #[must_use]
    #[inline]
    pub fn distance_to_point(&self, point: Vec3) -> f32 {
        (point - self.closest_point(point)).length()
    }
}

/// A line segment defined by start and end points.
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct Segment {
    pub start: Vec3,
    pub end: Vec3,
}

impl Segment {
    #[must_use]
    #[inline]
    pub fn new(start: Vec3, end: Vec3) -> Self {
        Self { start, end }
    }

    /// Length of the segment.
    #[must_use]
    #[inline]
    pub fn length(&self) -> f32 {
        (self.end - self.start).length()
    }

    /// Midpoint of the segment.
    #[must_use]
    #[inline]
    pub fn midpoint(&self) -> Vec3 {
        (self.start + self.end) * 0.5
    }

    /// Normalized direction from start to end.
    ///
    /// Returns [`Vec3::X`] for degenerate (zero-length) segments.
    #[must_use]
    #[inline]
    pub fn direction(&self) -> Vec3 {
        let d = self.end - self.start;
        let len_sq = d.length_squared();
        if len_sq < crate::EPSILON_F32 {
            return Vec3::X; // Degenerate — arbitrary fallback
        }
        d * len_sq.sqrt().recip()
    }

    /// Closest point on this segment to the given point.
    #[must_use]
    #[inline]
    pub fn closest_point(&self, point: Vec3) -> Vec3 {
        let ab = self.end - self.start;
        let len_sq = ab.dot(ab);
        if len_sq < crate::EPSILON_F32 {
            return self.start; // Degenerate segment
        }
        let t = ((point - self.start).dot(ab) / len_sq).clamp(0.0, 1.0);
        self.start + ab * t
    }

    /// Distance from a point to this segment.
    #[must_use]
    #[inline]
    pub fn distance_to_point(&self, point: Vec3) -> f32 {
        (point - self.closest_point(point)).length()
    }
}

/// A view frustum defined by six planes (near, far, left, right, top, bottom).
///
/// The planes' normals point **inward** — a point is inside the frustum if it is
/// on the positive side of all six planes.
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct Frustum {
    pub planes: [Plane; 6],
}

impl Frustum {
    /// Extract frustum planes from a view-projection matrix.
    ///
    /// Uses the Gribb/Hartmann method. Planes are normalized.
    #[must_use]
    pub fn from_view_projection(vp: glam::Mat4) -> Self {
        let r = vp.to_cols_array_2d();
        // Row-based extraction (transposed column-major)
        let row = |i: usize| -> [f32; 4] { [r[0][i], r[1][i], r[2][i], r[3][i]] };
        let r0 = row(0);
        let r1 = row(1);
        let r2 = row(2);
        let r3 = row(3);

        let make_plane = |a: f32, b: f32, c: f32, d: f32| -> Plane {
            let len = (a * a + b * b + c * c).sqrt();
            Plane {
                normal: Vec3::new(a / len, b / len, c / len),
                distance: -d / len,
            }
        };

        let planes = [
            // Near:   row3 + row2
            make_plane(r3[0] + r2[0], r3[1] + r2[1], r3[2] + r2[2], r3[3] + r2[3]),
            // Far:    row3 - row2
            make_plane(r3[0] - r2[0], r3[1] - r2[1], r3[2] - r2[2], r3[3] - r2[3]),
            // Left:   row3 + row0
            make_plane(r3[0] + r0[0], r3[1] + r0[1], r3[2] + r0[2], r3[3] + r0[3]),
            // Right:  row3 - row0
            make_plane(r3[0] - r0[0], r3[1] - r0[1], r3[2] - r0[2], r3[3] - r0[3]),
            // Top:    row3 - row1
            make_plane(r3[0] - r1[0], r3[1] - r1[1], r3[2] - r1[2], r3[3] - r1[3]),
            // Bottom: row3 + row1
            make_plane(r3[0] + r1[0], r3[1] + r1[1], r3[2] + r1[2], r3[3] + r1[3]),
        ];

        Self { planes }
    }

    /// Check whether a point is inside the frustum.
    #[must_use]
    #[inline]
    pub fn contains_point(&self, point: Vec3) -> bool {
        self.planes.iter().all(|p| p.signed_distance(point) >= 0.0)
    }

    /// Conservative check whether an AABB intersects the frustum.
    ///
    /// Returns `false` only if the AABB is fully outside at least one plane.
    #[must_use]
    #[inline]
    pub fn contains_aabb(&self, aabb: &Aabb) -> bool {
        for plane in &self.planes {
            // Find the corner most aligned with the plane normal (P-vertex)
            let p = Vec3::new(
                if plane.normal.x >= 0.0 {
                    aabb.max.x
                } else {
                    aabb.min.x
                },
                if plane.normal.y >= 0.0 {
                    aabb.max.y
                } else {
                    aabb.min.y
                },
                if plane.normal.z >= 0.0 {
                    aabb.max.z
                } else {
                    aabb.min.z
                },
            );
            if plane.signed_distance(p) < 0.0 {
                return false;
            }
        }
        true
    }
}

// ---------------------------------------------------------------------------
// Intersection / overlap functions
// ---------------------------------------------------------------------------

/// Ray-triangle intersection using the Möller–Trumbore algorithm.
///
/// Returns the `t` parameter if the ray hits the triangle (only `t >= 0`).
#[must_use]
#[inline]
pub fn ray_triangle(ray: &Ray, tri: &Triangle) -> Option<f32> {
    let edge1 = tri.vertices[1] - tri.vertices[0];
    let edge2 = tri.vertices[2] - tri.vertices[0];
    let h = ray.direction.cross(edge2);
    let a = edge1.dot(h);

    if a.abs() < crate::EPSILON_F32 {
        return None; // Ray parallel to triangle
    }

    let f = 1.0 / a;
    let s = ray.origin - tri.vertices[0];
    let u = f * s.dot(h);

    if !(0.0..=1.0).contains(&u) {
        return None;
    }

    let q = s.cross(edge1);
    let v = f * ray.direction.dot(q);

    if v < 0.0 || u + v > 1.0 {
        return None;
    }

    let t = f * edge2.dot(q);
    if t >= 0.0 { Some(t) } else { None }
}

/// Check whether two AABBs overlap.
#[must_use]
#[inline]
pub fn aabb_aabb(a: &Aabb, b: &Aabb) -> bool {
    a.min.cmple(b.max).all() && b.min.cmple(a.max).all()
}

/// Check whether two spheres overlap.
#[must_use]
#[inline]
pub fn sphere_sphere(a: &Sphere, b: &Sphere) -> bool {
    let r = a.radius + b.radius;
    (a.center - b.center).length_squared() <= r * r
}

/// Intersection of two planes. Returns the line of intersection, or `None` if parallel.
#[must_use]
pub fn plane_plane(a: &Plane, b: &Plane) -> Option<Line> {
    let dir = a.normal.cross(b.normal);
    let len_sq = dir.dot(dir);
    if len_sq < crate::EPSILON_F32 {
        return None; // Planes are parallel
    }
    // Find a point on the intersection line
    let point = (dir.cross(b.normal) * a.distance + a.normal.cross(dir) * b.distance) / len_sq;
    Some(Line {
        origin: point,
        direction: dir.normalize(),
    })
}

// ---------------------------------------------------------------------------
// Closest-point functions
// ---------------------------------------------------------------------------

/// Closest point on a ray to a given point (clamped to `t >= 0`).
#[must_use]
#[inline]
pub fn closest_point_on_ray(ray: &Ray, point: Vec3) -> Vec3 {
    let t = (point - ray.origin).dot(ray.direction).max(0.0);
    ray.origin + ray.direction * t
}

/// Closest point on a plane to a given point.
#[must_use]
#[inline]
pub fn closest_point_on_plane(plane: &Plane, point: Vec3) -> Vec3 {
    point - plane.normal * plane.signed_distance(point)
}

/// Closest point on a sphere's surface to a given point.
///
/// If the point is at the sphere's center, returns the point offset by the radius along +X.
#[must_use]
#[inline]
pub fn closest_point_on_sphere(sphere: &Sphere, point: Vec3) -> Vec3 {
    let dir = point - sphere.center;
    let len = dir.length();
    if len < crate::EPSILON_F32 {
        return sphere.center + Vec3::new(sphere.radius, 0.0, 0.0);
    }
    sphere.center + dir * (sphere.radius / len)
}

/// Closest point on an AABB's surface or interior to a given point.
#[must_use]
#[inline]
pub fn closest_point_on_aabb(aabb: &Aabb, point: Vec3) -> Vec3 {
    point.clamp(aabb.min, aabb.max)
}

// ---------------------------------------------------------------------------
// BVH (Bounding Volume Hierarchy)
// ---------------------------------------------------------------------------

/// A node in a BVH tree.
#[derive(Debug, Clone)]
enum BvhNode {
    Leaf {
        bounds: Aabb,
        index: usize,
    },
    Internal {
        bounds: Aabb,
        left: Box<BvhNode>,
        right: Box<BvhNode>,
    },
}

/// A Bounding Volume Hierarchy for fast ray-AABB broadphase queries.
///
/// Stores a tree of AABBs built from a list of primitives. Each primitive
/// is represented by its AABB and an index into the caller's primitive array.
#[derive(Debug, Clone)]
#[must_use]
pub struct Bvh {
    root: Option<BvhNode>,
    len: usize,
}

impl Bvh {
    /// Build a BVH from a list of (aabb, index) pairs using midpoint splitting.
    pub fn build(items: &mut [(Aabb, usize)]) -> Self {
        let len = items.len();
        if items.is_empty() {
            return Self { root: None, len: 0 };
        }
        let root = Self::build_recursive(items);
        Self {
            root: Some(root),
            len,
        }
    }

    fn build_recursive(items: &mut [(Aabb, usize)]) -> BvhNode {
        if items.len() == 1 {
            return BvhNode::Leaf {
                bounds: items[0].0,
                index: items[0].1,
            };
        }

        // Compute bounding box of all items
        let mut bounds = items[0].0;
        for item in items.iter().skip(1) {
            bounds = bounds.merge(&item.0);
        }

        // Split along the longest axis at the midpoint
        let size = bounds.size();
        let axis = if size.x >= size.y && size.x >= size.z {
            0
        } else if size.y >= size.z {
            1
        } else {
            2
        };

        let mid = match axis {
            0 => (bounds.min.x + bounds.max.x) * 0.5,
            1 => (bounds.min.y + bounds.max.y) * 0.5,
            _ => (bounds.min.z + bounds.max.z) * 0.5,
        };

        // Partition items around the midpoint (O(n) instead of O(n log n) sort)
        let center_val = |aabb: &Aabb| -> f32 {
            let arr = aabb.center().to_array();
            arr[axis]
        };

        // In-place partition: items with center < mid go left
        let mut split = 0;
        for i in 0..items.len() {
            if center_val(&items[i].0) < mid {
                items.swap(i, split);
                split += 1;
            }
        }
        // Ensure at least one item on each side
        if split == 0 {
            split = 1;
        } else if split == items.len() {
            split = items.len() - 1;
        }

        let (left_items, right_items) = items.split_at_mut(split);
        let left = Self::build_recursive(left_items);
        let right = Self::build_recursive(right_items);

        BvhNode::Internal {
            bounds,
            left: Box::new(left),
            right: Box::new(right),
        }
    }

    /// Number of primitives in the BVH.
    #[must_use]
    pub fn len(&self) -> usize {
        self.len
    }

    /// Whether the BVH is empty.
    #[must_use]
    pub fn is_empty(&self) -> bool {
        self.len == 0
    }

    /// Query all primitive indices whose AABBs are hit by the ray.
    ///
    /// Returns indices in no particular order. Use the indices to test
    /// against the actual primitives for exact intersection.
    #[must_use]
    pub fn query_ray(&self, ray: &Ray) -> Vec<usize> {
        let mut results = Vec::new();
        if let Some(ref root) = self.root {
            Self::query_ray_recursive(root, ray, &mut results);
        }
        results
    }

    fn query_ray_recursive(node: &BvhNode, ray: &Ray, results: &mut Vec<usize>) {
        match node {
            BvhNode::Leaf { bounds, index } => {
                if ray_aabb(ray, bounds).is_some() {
                    results.push(*index);
                }
            }
            BvhNode::Internal {
                bounds,
                left,
                right,
            } => {
                if ray_aabb(ray, bounds).is_some() {
                    Self::query_ray_recursive(left, ray, results);
                    Self::query_ray_recursive(right, ray, results);
                }
            }
        }
    }

    /// Query all primitive indices whose AABBs overlap the given AABB.
    #[must_use]
    pub fn query_aabb(&self, query: &Aabb) -> Vec<usize> {
        let mut results = Vec::new();
        if let Some(ref root) = self.root {
            Self::query_aabb_recursive(root, query, &mut results);
        }
        results
    }

    fn query_aabb_recursive(node: &BvhNode, query: &Aabb, results: &mut Vec<usize>) {
        match node {
            BvhNode::Leaf { bounds, index } => {
                if aabb_aabb(bounds, query) {
                    results.push(*index);
                }
            }
            BvhNode::Internal {
                bounds,
                left,
                right,
            } => {
                if aabb_aabb(bounds, query) {
                    Self::query_aabb_recursive(left, query, results);
                    Self::query_aabb_recursive(right, query, results);
                }
            }
        }
    }
}

// ---------------------------------------------------------------------------
// K-d tree
// ---------------------------------------------------------------------------

/// A node in a k-d tree.
#[derive(Debug, Clone)]
enum KdNode {
    Leaf {
        point: Vec3,
        index: usize,
    },
    Split {
        axis: usize,
        split_val: f32,
        left: Box<KdNode>,
        right: Box<KdNode>,
    },
}

/// A 3D k-d tree for nearest-neighbor point queries.
#[derive(Debug, Clone)]
#[must_use]
pub struct KdTree {
    root: Option<KdNode>,
    len: usize,
}

impl KdTree {
    /// Build a k-d tree from a list of (point, index) pairs.
    pub fn build(items: &mut [(Vec3, usize)]) -> Self {
        let len = items.len();
        if items.is_empty() {
            return Self { root: None, len: 0 };
        }
        let root = Self::build_recursive(items, 0);
        Self {
            root: Some(root),
            len,
        }
    }

    fn build_recursive(items: &mut [(Vec3, usize)], depth: usize) -> KdNode {
        if items.len() == 1 {
            return KdNode::Leaf {
                point: items[0].0,
                index: items[0].1,
            };
        }

        let axis = depth % 3;
        let mid = items.len() / 2;
        // O(n) partial sort: only ensures the median element is in position
        items.select_nth_unstable_by(mid, |a, b| {
            a.0.to_array()[axis]
                .partial_cmp(&b.0.to_array()[axis])
                .unwrap_or(std::cmp::Ordering::Equal)
        });
        let split_val = items[mid].0.to_array()[axis];

        let (left_items, right_items) = items.split_at_mut(mid);
        let left = Self::build_recursive(left_items, depth + 1);
        let right = Self::build_recursive(right_items, depth + 1);

        KdNode::Split {
            axis,
            split_val,
            left: Box::new(left),
            right: Box::new(right),
        }
    }

    /// Number of points in the tree.
    #[must_use]
    pub fn len(&self) -> usize {
        self.len
    }

    /// Whether the tree is empty.
    #[must_use]
    pub fn is_empty(&self) -> bool {
        self.len == 0
    }

    /// Find the nearest neighbor to `query`.
    ///
    /// Returns `(index, distance_squared)` of the closest point,
    /// or `None` if the tree is empty.
    #[must_use]
    pub fn nearest(&self, query: Vec3) -> Option<(usize, f32)> {
        let mut best_idx = 0;
        let mut best_dist_sq = f32::INFINITY;
        if let Some(ref root) = self.root {
            Self::nearest_recursive(root, query, &mut best_idx, &mut best_dist_sq);
        }
        if best_dist_sq.is_finite() {
            Some((best_idx, best_dist_sq))
        } else {
            None
        }
    }

    fn nearest_recursive(node: &KdNode, query: Vec3, best_idx: &mut usize, best_dist_sq: &mut f32) {
        match node {
            KdNode::Leaf { point, index } => {
                let dist_sq = (*point - query).length_squared();
                if dist_sq < *best_dist_sq {
                    *best_dist_sq = dist_sq;
                    *best_idx = *index;
                }
            }
            KdNode::Split {
                axis,
                split_val,
                left,
                right,
            } => {
                let query_val = query.to_array()[*axis];
                let diff = query_val - split_val;

                // Visit the closer side first
                let (near, far) = if diff < 0.0 {
                    (left.as_ref(), right.as_ref())
                } else {
                    (right.as_ref(), left.as_ref())
                };

                Self::nearest_recursive(near, query, best_idx, best_dist_sq);

                // Only visit the far side if the splitting plane is closer than current best
                if diff * diff < *best_dist_sq {
                    Self::nearest_recursive(far, query, best_idx, best_dist_sq);
                }
            }
        }
    }

    /// Find all points within `radius` of `query`.
    ///
    /// Returns a list of `(index, distance_squared)`.
    #[must_use]
    pub fn within_radius(&self, query: Vec3, radius: f32) -> Vec<(usize, f32)> {
        let mut results = Vec::new();
        let radius_sq = radius * radius;
        if let Some(ref root) = self.root {
            Self::radius_recursive(root, query, radius_sq, &mut results);
        }
        results
    }

    fn radius_recursive(
        node: &KdNode,
        query: Vec3,
        radius_sq: f32,
        results: &mut Vec<(usize, f32)>,
    ) {
        match node {
            KdNode::Leaf { point, index } => {
                let dist_sq = (*point - query).length_squared();
                if dist_sq <= radius_sq {
                    results.push((*index, dist_sq));
                }
            }
            KdNode::Split {
                axis,
                split_val,
                left,
                right,
            } => {
                let query_val = query.to_array()[*axis];
                let diff = query_val - split_val;

                let (near, far) = if diff < 0.0 {
                    (left.as_ref(), right.as_ref())
                } else {
                    (right.as_ref(), left.as_ref())
                };

                Self::radius_recursive(near, query, radius_sq, results);

                if diff * diff <= radius_sq {
                    Self::radius_recursive(far, query, radius_sq, results);
                }
            }
        }
    }
}

// ---------------------------------------------------------------------------
// Quadtree (2D spatial index)
// ---------------------------------------------------------------------------

/// A 2D axis-aligned bounding rectangle for quadtree operations.
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct Rect {
    pub min: glam::Vec2,
    pub max: glam::Vec2,
}

impl Rect {
    /// Create a new rectangle. Min/max are corrected if swapped.
    #[must_use]
    #[inline]
    pub fn new(a: glam::Vec2, b: glam::Vec2) -> Self {
        Self {
            min: a.min(b),
            max: a.max(b),
        }
    }

    /// Check whether a 2D point is inside this rectangle.
    #[must_use]
    #[inline]
    pub fn contains_point(&self, p: glam::Vec2) -> bool {
        p.cmpge(self.min).all() && p.cmple(self.max).all()
    }

    /// Check whether two rectangles overlap.
    #[must_use]
    #[inline]
    pub fn overlaps(&self, other: &Rect) -> bool {
        self.min.cmple(other.max).all() && other.min.cmple(self.max).all()
    }

    /// Center of the rectangle.
    #[must_use]
    #[inline]
    pub fn center(&self) -> glam::Vec2 {
        (self.min + self.max) * 0.5
    }

    /// Size (extents) of the rectangle.
    #[must_use]
    #[inline]
    pub fn size(&self) -> glam::Vec2 {
        self.max - self.min
    }

    /// Merge two rectangles into one that encloses both.
    #[must_use]
    #[inline]
    pub fn merge(&self, other: &Rect) -> Rect {
        Rect {
            min: self.min.min(other.min),
            max: self.max.max(other.max),
        }
    }

    /// Area of the rectangle.
    #[must_use]
    #[inline]
    pub fn area(&self) -> f32 {
        let s = self.size();
        s.x * s.y
    }
}

/// A quadtree node.
#[derive(Debug, Clone)]
enum QuadNode {
    Empty,
    Leaf(Vec<(glam::Vec2, usize)>),
    Split {
        children: Box<[QuadNode; 4]>, // NW, NE, SW, SE
        bounds: Rect,
    },
}

/// A 2D quadtree for spatial point queries.
///
/// Points are inserted into a fixed-bounds region. The tree subdivides
/// when a leaf exceeds `max_per_leaf` items, up to `max_depth` levels.
#[derive(Debug, Clone)]
pub struct Quadtree {
    root: QuadNode,
    bounds: Rect,
    len: usize,
    max_per_leaf: usize,
    max_depth: usize,
}

impl Quadtree {
    /// Create a new empty quadtree covering the given bounds.
    ///
    /// `max_per_leaf`: max items before a leaf splits (default 8).
    /// `max_depth`: max subdivision depth (default 8).
    #[must_use]
    pub fn new(bounds: Rect, max_per_leaf: usize, max_depth: usize) -> Self {
        Self {
            root: QuadNode::Empty,
            bounds,
            len: 0,
            max_per_leaf,
            max_depth,
        }
    }

    /// Number of items in the quadtree.
    #[must_use]
    pub fn len(&self) -> usize {
        self.len
    }

    /// Whether the quadtree is empty.
    #[must_use]
    pub fn is_empty(&self) -> bool {
        self.len == 0
    }

    /// Insert a point with an associated index.
    pub fn insert(&mut self, point: glam::Vec2, index: usize) {
        if !self.bounds.contains_point(point) {
            return; // Out of bounds, ignore
        }
        Self::insert_recursive(
            &mut self.root,
            point,
            index,
            &self.bounds,
            self.max_per_leaf,
            self.max_depth,
            0,
        );
        self.len += 1;
    }

    fn insert_recursive(
        node: &mut QuadNode,
        point: glam::Vec2,
        index: usize,
        bounds: &Rect,
        max_per_leaf: usize,
        max_depth: usize,
        depth: usize,
    ) {
        match node {
            QuadNode::Empty => {
                *node = QuadNode::Leaf(vec![(point, index)]);
            }
            QuadNode::Leaf(items) => {
                items.push((point, index));
                if items.len() > max_per_leaf && depth < max_depth {
                    // Split
                    let center = bounds.center();
                    let mut children = Box::new([
                        QuadNode::Empty,
                        QuadNode::Empty,
                        QuadNode::Empty,
                        QuadNode::Empty,
                    ]);
                    for &(p, idx) in items.iter() {
                        let quadrant = Self::quadrant(p, center);
                        let child_bounds = Self::child_bounds(bounds, quadrant);
                        Self::insert_recursive(
                            &mut children[quadrant],
                            p,
                            idx,
                            &child_bounds,
                            max_per_leaf,
                            max_depth,
                            depth + 1,
                        );
                    }
                    *node = QuadNode::Split {
                        children,
                        bounds: *bounds,
                    };
                }
            }
            QuadNode::Split {
                children,
                bounds: node_bounds,
            } => {
                let center = node_bounds.center();
                let quadrant = Self::quadrant(point, center);
                let child_bounds = Self::child_bounds(node_bounds, quadrant);
                Self::insert_recursive(
                    &mut children[quadrant],
                    point,
                    index,
                    &child_bounds,
                    max_per_leaf,
                    max_depth,
                    depth + 1,
                );
            }
        }
    }

    fn quadrant(point: glam::Vec2, center: glam::Vec2) -> usize {
        let x = if point.x >= center.x { 1 } else { 0 };
        let y = if point.y >= center.y { 0 } else { 2 }; // NW=0, NE=1, SW=2, SE=3
        x + y
    }

    fn child_bounds(bounds: &Rect, quadrant: usize) -> Rect {
        let center = bounds.center();
        match quadrant {
            0 => Rect::new(
                glam::Vec2::new(bounds.min.x, center.y),
                glam::Vec2::new(center.x, bounds.max.y),
            ), // NW
            1 => Rect::new(center, bounds.max), // NE
            2 => Rect::new(bounds.min, center), // SW
            _ => Rect::new(
                glam::Vec2::new(center.x, bounds.min.y),
                glam::Vec2::new(bounds.max.x, center.y),
            ), // SE
        }
    }

    /// Query all indices of points within the given rectangle.
    #[must_use]
    pub fn query_rect(&self, query: &Rect) -> Vec<usize> {
        let mut results = Vec::new();
        Self::query_recursive(&self.root, &self.bounds, query, &mut results);
        results
    }

    fn query_recursive(node: &QuadNode, bounds: &Rect, query: &Rect, results: &mut Vec<usize>) {
        if !bounds.overlaps(query) {
            return;
        }
        match node {
            QuadNode::Empty => {}
            QuadNode::Leaf(items) => {
                for &(p, idx) in items {
                    if query.contains_point(p) {
                        results.push(idx);
                    }
                }
            }
            QuadNode::Split {
                children,
                bounds: node_bounds,
            } => {
                for q in 0..4 {
                    let child_bounds = Self::child_bounds(node_bounds, q);
                    Self::query_recursive(&children[q], &child_bounds, query, results);
                }
            }
        }
    }
}

// ---------------------------------------------------------------------------
// Octree (3D spatial index)
// ---------------------------------------------------------------------------

/// An octree node.
#[derive(Debug, Clone)]
enum OctNode {
    Empty,
    Leaf(Vec<(Vec3, usize)>),
    Split {
        children: Box<[OctNode; 8]>,
        bounds: Aabb,
    },
}

/// A 3D octree for spatial point queries.
///
/// Same design as [`Quadtree`] but in 3D with 8 children per split.
#[derive(Debug, Clone)]
pub struct Octree {
    root: OctNode,
    bounds: Aabb,
    len: usize,
    max_per_leaf: usize,
    max_depth: usize,
}

impl Octree {
    /// Create a new empty octree covering the given bounds.
    #[must_use]
    pub fn new(bounds: Aabb, max_per_leaf: usize, max_depth: usize) -> Self {
        Self {
            root: OctNode::Empty,
            bounds,
            len: 0,
            max_per_leaf,
            max_depth,
        }
    }

    /// Number of items in the octree.
    #[must_use]
    pub fn len(&self) -> usize {
        self.len
    }

    /// Whether the octree is empty.
    #[must_use]
    pub fn is_empty(&self) -> bool {
        self.len == 0
    }

    /// Insert a point with an associated index.
    pub fn insert(&mut self, point: Vec3, index: usize) {
        if !self.bounds.contains(point) {
            return;
        }
        Self::insert_recursive(
            &mut self.root,
            point,
            index,
            &self.bounds,
            self.max_per_leaf,
            self.max_depth,
            0,
        );
        self.len += 1;
    }

    fn insert_recursive(
        node: &mut OctNode,
        point: Vec3,
        index: usize,
        bounds: &Aabb,
        max_per_leaf: usize,
        max_depth: usize,
        depth: usize,
    ) {
        match node {
            OctNode::Empty => {
                *node = OctNode::Leaf(vec![(point, index)]);
            }
            OctNode::Leaf(items) => {
                items.push((point, index));
                if items.len() > max_per_leaf && depth < max_depth {
                    let center = bounds.center();
                    let mut children = Box::new([
                        OctNode::Empty,
                        OctNode::Empty,
                        OctNode::Empty,
                        OctNode::Empty,
                        OctNode::Empty,
                        OctNode::Empty,
                        OctNode::Empty,
                        OctNode::Empty,
                    ]);
                    for &(p, idx) in items.iter() {
                        let octant = Self::octant(p, center);
                        let child_bounds = Self::child_bounds(bounds, octant);
                        Self::insert_recursive(
                            &mut children[octant],
                            p,
                            idx,
                            &child_bounds,
                            max_per_leaf,
                            max_depth,
                            depth + 1,
                        );
                    }
                    *node = OctNode::Split {
                        children,
                        bounds: *bounds,
                    };
                }
            }
            OctNode::Split {
                children,
                bounds: node_bounds,
            } => {
                let center = node_bounds.center();
                let octant = Self::octant(point, center);
                let child_bounds = Self::child_bounds(node_bounds, octant);
                Self::insert_recursive(
                    &mut children[octant],
                    point,
                    index,
                    &child_bounds,
                    max_per_leaf,
                    max_depth,
                    depth + 1,
                );
            }
        }
    }

    fn octant(point: Vec3, center: Vec3) -> usize {
        let x = if point.x >= center.x { 1 } else { 0 };
        let y = if point.y >= center.y { 2 } else { 0 };
        let z = if point.z >= center.z { 4 } else { 0 };
        x | y | z
    }

    fn child_bounds(bounds: &Aabb, octant: usize) -> Aabb {
        let center = bounds.center();
        let min = Vec3::new(
            if octant & 1 != 0 {
                center.x
            } else {
                bounds.min.x
            },
            if octant & 2 != 0 {
                center.y
            } else {
                bounds.min.y
            },
            if octant & 4 != 0 {
                center.z
            } else {
                bounds.min.z
            },
        );
        let max = Vec3::new(
            if octant & 1 != 0 {
                bounds.max.x
            } else {
                center.x
            },
            if octant & 2 != 0 {
                bounds.max.y
            } else {
                center.y
            },
            if octant & 4 != 0 {
                bounds.max.z
            } else {
                center.z
            },
        );
        Aabb::new(min, max)
    }

    /// Query all indices of points within the given AABB.
    #[must_use]
    pub fn query_aabb(&self, query: &Aabb) -> Vec<usize> {
        let mut results = Vec::new();
        Self::query_recursive(&self.root, &self.bounds, query, &mut results);
        results
    }

    fn query_recursive(node: &OctNode, bounds: &Aabb, query: &Aabb, results: &mut Vec<usize>) {
        if !aabb_aabb(bounds, query) {
            return;
        }
        match node {
            OctNode::Empty => {}
            OctNode::Leaf(items) => {
                for &(p, idx) in items {
                    if query.contains(p) {
                        results.push(idx);
                    }
                }
            }
            OctNode::Split {
                children,
                bounds: node_bounds,
            } => {
                for oct in 0..8 {
                    let child_bounds = Self::child_bounds(node_bounds, oct);
                    Self::query_recursive(&children[oct], &child_bounds, query, results);
                }
            }
        }
    }
}

// ---------------------------------------------------------------------------
// Spatial hash grid
// ---------------------------------------------------------------------------

/// A spatial hash grid for fast broadphase queries.
///
/// Divides space into a uniform grid of cells. Each cell stores indices
/// of items whose positions fall within it. O(1) insert and query for
/// localized spatial lookups.
#[derive(Debug, Clone)]
pub struct SpatialHash {
    inv_cell_size: f32,
    cells: std::collections::HashMap<(i32, i32, i32), Vec<usize>>,
    len: usize,
}

impl SpatialHash {
    /// Create a new spatial hash grid with the given cell size.
    ///
    /// # Errors
    ///
    /// Returns [`crate::HisabError::InvalidInput`] if `cell_size` is not positive.
    pub fn new(cell_size: f32) -> Result<Self, crate::HisabError> {
        if cell_size <= 0.0 {
            return Err(crate::HisabError::InvalidInput(
                "cell_size must be positive".into(),
            ));
        }
        Ok(Self {
            inv_cell_size: 1.0 / cell_size,
            cells: std::collections::HashMap::new(),
            len: 0,
        })
    }

    /// Number of items inserted.
    #[must_use]
    pub fn len(&self) -> usize {
        self.len
    }

    /// Whether the grid is empty.
    #[must_use]
    pub fn is_empty(&self) -> bool {
        self.len == 0
    }

    /// Number of occupied cells.
    #[must_use]
    pub fn cell_count(&self) -> usize {
        self.cells.len()
    }

    fn cell_key(&self, point: Vec3) -> (i32, i32, i32) {
        (
            (point.x * self.inv_cell_size).floor() as i32,
            (point.y * self.inv_cell_size).floor() as i32,
            (point.z * self.inv_cell_size).floor() as i32,
        )
    }

    /// Insert a point with an associated index.
    pub fn insert(&mut self, point: Vec3, index: usize) {
        let key = self.cell_key(point);
        self.cells.entry(key).or_default().push(index);
        self.len += 1;
    }

    /// Clear all items.
    pub fn clear(&mut self) {
        self.cells.clear();
        self.len = 0;
    }

    /// Query all indices in the same cell as the given point.
    #[must_use]
    pub fn query_cell(&self, point: Vec3) -> &[usize] {
        let key = self.cell_key(point);
        self.cells.get(&key).map_or(&[], |v| v.as_slice())
    }

    /// Query all indices within a radius of the given point (broadphase).
    ///
    /// Checks all cells that could contain points within `radius`.
    /// Returns candidate indices — the caller should perform exact distance
    /// checks. Cost is O((2*ceil(r/cell_size)+1)^3) in cell lookups.
    #[must_use]
    pub fn query_radius(&self, point: Vec3, radius: f32) -> Vec<usize> {
        let mut results = Vec::new();
        let cells_r = (radius * self.inv_cell_size).ceil() as i32;
        let center = self.cell_key(point);

        for dx in -cells_r..=cells_r {
            for dy in -cells_r..=cells_r {
                for dz in -cells_r..=cells_r {
                    let key = (center.0 + dx, center.1 + dy, center.2 + dz);
                    if let Some(items) = self.cells.get(&key) {
                        results.extend_from_slice(items);
                    }
                }
            }
        }
        results
    }
}

// ---------------------------------------------------------------------------
// Convex hull (2D, Andrew's monotone chain)
// ---------------------------------------------------------------------------

/// Compute the 2D convex hull of a set of points.
///
/// Returns the hull vertices in counter-clockwise order using Andrew's
/// monotone chain algorithm. O(n log n).
///
/// For fewer than 2 points, returns the input as-is. For collinear points,
/// returns only the two endpoints.
///
/// **Note:** This function clones the input slice for sorting. For very large
/// point sets, consider pre-sorting the points yourself.
#[must_use]
pub fn convex_hull_2d(points: &[glam::Vec2]) -> Vec<glam::Vec2> {
    let mut pts: Vec<glam::Vec2> = points.to_vec();
    let n = pts.len();
    if n < 2 {
        return pts;
    }

    pts.sort_unstable_by(|a, b| {
        a.x.partial_cmp(&b.x)
            .unwrap_or(std::cmp::Ordering::Equal)
            .then(a.y.partial_cmp(&b.y).unwrap_or(std::cmp::Ordering::Equal))
    });

    // Cross product of OA and OB vectors
    let cross = |o: glam::Vec2, a: glam::Vec2, b: glam::Vec2| -> f32 {
        (a.x - o.x) * (b.y - o.y) - (a.y - o.y) * (b.x - o.x)
    };

    let mut hull: Vec<glam::Vec2> = Vec::with_capacity(2 * n);

    // Lower hull
    for &p in &pts {
        while hull.len() >= 2 && cross(hull[hull.len() - 2], hull[hull.len() - 1], p) <= 0.0 {
            hull.pop();
        }
        hull.push(p);
    }

    // Upper hull
    let lower_len = hull.len() + 1;
    for &p in pts.iter().rev() {
        while hull.len() >= lower_len && cross(hull[hull.len() - 2], hull[hull.len() - 1], p) <= 0.0
        {
            hull.pop();
        }
        hull.push(p);
    }

    hull.pop(); // Remove the last point (same as first)
    hull
}

// ---------------------------------------------------------------------------
// GJK (Gilbert-Johnson-Keerthi) collision detection
// ---------------------------------------------------------------------------

/// A convex shape that can compute a support point in a given direction.
///
/// The support function returns the point on the shape that is farthest
/// in the given direction.
pub trait ConvexSupport {
    /// Return the point on the shape farthest in `direction`.
    fn support(&self, direction: glam::Vec2) -> glam::Vec2;
}

/// A convex polygon for GJK/EPA (2D).
///
/// Vertices should be in counter-clockwise order. Use [`convex_hull_2d`]
/// to construct from an arbitrary point set.
#[derive(Debug, Clone)]
pub struct ConvexPolygon {
    pub vertices: Vec<glam::Vec2>,
}

impl ConvexPolygon {
    /// Create a convex polygon. Vertices should be in CCW order.
    /// Use [`convex_hull_2d`] to ensure convexity.
    ///
    /// # Errors
    ///
    /// Returns [`crate::HisabError::InvalidInput`] if `vertices` is empty.
    pub fn new(vertices: Vec<glam::Vec2>) -> Result<Self, crate::HisabError> {
        if vertices.is_empty() {
            return Err(crate::HisabError::InvalidInput(
                "convex polygon requires at least one vertex".into(),
            ));
        }
        Ok(Self { vertices })
    }
}

impl ConvexSupport for ConvexPolygon {
    #[inline]
    fn support(&self, direction: glam::Vec2) -> glam::Vec2 {
        let mut best = self.vertices[0];
        let mut best_dot = best.dot(direction);
        for &v in &self.vertices[1..] {
            let d = v.dot(direction);
            if d > best_dot {
                best_dot = d;
                best = v;
            }
        }
        best
    }
}

/// Result of a Minkowski difference support query.
#[inline]
fn minkowski_support(
    a: &dyn ConvexSupport,
    b: &dyn ConvexSupport,
    direction: glam::Vec2,
) -> glam::Vec2 {
    a.support(direction) - b.support(-direction)
}

/// Triple product: (A × B) × C — returns the vector perpendicular to C
/// in the direction away from A, used for simplex evolution.
#[inline]
fn triple_cross_2d(a: glam::Vec2, b: glam::Vec2, c: glam::Vec2) -> glam::Vec2 {
    // In 2D: (A × B) × C = B * (C·A) - A * (C·B)
    let ca = c.dot(a);
    let cb = c.dot(b);
    b * ca - a * cb
}

/// GJK simplex evolution result.
enum GjkResult {
    /// No intersection found.
    NoIntersection,
    /// Intersection confirmed — simplex contains origin.
    Intersection([glam::Vec2; 3], usize),
}

/// Core GJK loop shared by `gjk_intersect` and `gjk_epa`.
///
/// Returns the final simplex if the origin is contained, or `NoIntersection`.
/// When the line-case direction degenerates, `on_line_degenerate` controls
/// the fallback: `true` returns early as intersecting, `false` picks a
/// perpendicular direction and keeps going (needed for EPA).
fn gjk_core(
    a: &dyn ConvexSupport,
    b: &dyn ConvexSupport,
    early_intersect_on_line: bool,
) -> GjkResult {
    let mut direction = glam::Vec2::new(1.0, 0.0);
    let mut simplex = [glam::Vec2::ZERO; 3];
    let mut simplex_len: usize = 0;

    let s = minkowski_support(a, b, direction);
    simplex[simplex_len] = s;
    simplex_len += 1;
    direction = -s;

    for _ in 0..GJK_MAX_ITERATIONS {
        let new_point = minkowski_support(a, b, direction);
        if new_point.dot(direction) < 0.0 {
            return GjkResult::NoIntersection;
        }
        simplex[simplex_len] = new_point;
        simplex_len += 1;

        match simplex_len {
            2 => {
                let b_pt = simplex[1];
                let a_pt = simplex[0];
                let ab = a_pt - b_pt;
                let ao = -b_pt;
                direction = triple_cross_2d(ab, ao, ab);
                if direction.length_squared() < crate::EPSILON_F32 {
                    if early_intersect_on_line {
                        return GjkResult::Intersection(simplex, simplex_len);
                    }
                    // Need third point — pick perpendicular
                    direction = glam::Vec2::new(-ab.y, ab.x);
                }
            }
            3 => {
                let c = simplex[2];
                let b_pt = simplex[1];
                let a_pt = simplex[0];
                let cb = b_pt - c;
                let ca = a_pt - c;
                let co = -c;

                let cb_perp = triple_cross_2d(ca, cb, cb);
                let ca_perp = triple_cross_2d(cb, ca, ca);

                if cb_perp.dot(co) > 0.0 {
                    simplex[0] = simplex[1];
                    simplex[1] = simplex[2];
                    simplex_len -= 1;
                    direction = cb_perp;
                } else if ca_perp.dot(co) > 0.0 {
                    simplex[1] = simplex[2];
                    simplex_len -= 1;
                    direction = ca_perp;
                } else {
                    return GjkResult::Intersection(simplex, simplex_len);
                }
            }
            _ => unreachable!(),
        }
    }

    GjkResult::NoIntersection
}

/// GJK collision test between two convex shapes (2D).
///
/// Returns `true` if the shapes overlap. Uses the simplex-based GJK algorithm.
#[must_use]
pub fn gjk_intersect(a: &dyn ConvexSupport, b: &dyn ConvexSupport) -> bool {
    matches!(gjk_core(a, b, true), GjkResult::Intersection(..))
}

// ---------------------------------------------------------------------------
// EPA (Expanding Polytope Algorithm) — penetration depth
// ---------------------------------------------------------------------------

/// Penetration result from EPA.
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct Penetration {
    /// Penetration normal (direction to separate).
    pub normal: glam::Vec2,
    /// Penetration depth (distance to separate).
    pub depth: f32,
}

/// Compute the penetration depth and normal between two overlapping convex shapes
/// using the Expanding Polytope Algorithm (EPA).
///
/// Must only be called when GJK has confirmed an intersection. The `simplex`
/// should be the final 3-point simplex from GJK that contains the origin.
///
/// Returns `None` if the shapes are not actually overlapping or if EPA fails.
#[must_use]
pub fn epa_penetration(
    a: &dyn ConvexSupport,
    b: &dyn ConvexSupport,
    simplex: &[glam::Vec2],
) -> Option<Penetration> {
    if simplex.len() < 3 {
        return None;
    }

    // Pre-allocate for typical EPA expansion.
    let mut polytope: Vec<glam::Vec2> = Vec::with_capacity(32);
    polytope.extend_from_slice(simplex);

    for _ in 0..EPA_MAX_ITERATIONS {
        // Find the closest edge to the origin
        let mut closest_dist = f32::INFINITY;
        let mut closest_normal = glam::Vec2::ZERO;
        let mut closest_idx = 0;

        for i in 0..polytope.len() {
            let j = (i + 1) % polytope.len();
            let edge = polytope[j] - polytope[i];
            // Outward normal (2D: perpendicular)
            let normal = glam::Vec2::new(edge.y, -edge.x).normalize();
            let dist = normal.dot(polytope[i]);

            if dist < closest_dist {
                closest_dist = dist;
                closest_normal = normal;
                closest_idx = j;
            }
        }

        // Get new support point in the direction of the closest normal
        let support = minkowski_support(a, b, closest_normal);
        let d = support.dot(closest_normal);

        if (d - closest_dist).abs() < crate::EPSILON_F32 {
            // Converged — depth is always positive
            return Some(Penetration {
                normal: closest_normal,
                depth: closest_dist.abs(),
            });
        }

        // Insert the new point into the polytope
        polytope.insert(closest_idx, support);
    }

    // Return best estimate
    let mut closest_dist = f32::INFINITY;
    let mut closest_normal = glam::Vec2::ZERO;
    for i in 0..polytope.len() {
        let j = (i + 1) % polytope.len();
        let edge = polytope[j] - polytope[i];
        let normal = glam::Vec2::new(edge.y, -edge.x).normalize();
        let dist = normal.dot(polytope[i]);
        if dist < closest_dist {
            closest_dist = dist;
            closest_normal = normal;
        }
    }
    Some(Penetration {
        normal: closest_normal,
        depth: closest_dist.abs(),
    })
}

/// Combined GJK + EPA: test intersection and compute penetration if overlapping.
///
/// Returns `None` if shapes don't overlap, or `Some(Penetration)` with
/// the separation normal and depth.
#[must_use]
pub fn gjk_epa(a: &dyn ConvexSupport, b: &dyn ConvexSupport) -> Option<Penetration> {
    match gjk_core(a, b, false) {
        GjkResult::NoIntersection => None,
        GjkResult::Intersection(simplex, len) => epa_penetration(a, b, &simplex[..len]),
    }
}

// ---------------------------------------------------------------------------
// OBB (Oriented Bounding Box)
// ---------------------------------------------------------------------------

/// An oriented bounding box defined by a center, half-extents, and rotation.
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct Obb {
    /// Center of the OBB.
    pub center: Vec3,
    /// Half-extents along each local axis.
    pub half_extents: Vec3,
    /// Rotation quaternion (local → world).
    pub rotation: glam::Quat,
}

impl Obb {
    /// Create a new OBB.
    #[must_use]
    #[inline]
    pub fn new(center: Vec3, half_extents: Vec3, rotation: glam::Quat) -> Self {
        Self {
            center,
            half_extents,
            rotation,
        }
    }

    /// The three local axes (columns of the rotation matrix) in world space.
    #[must_use]
    #[inline]
    pub fn axes(&self) -> [Vec3; 3] {
        let m = glam::Mat3::from_quat(self.rotation);
        [m.x_axis, m.y_axis, m.z_axis]
    }

    /// Check whether a point is inside (or on the surface of) this OBB.
    #[must_use]
    #[inline]
    pub fn contains_point(&self, point: Vec3) -> bool {
        let d = point - self.center;
        let axes = self.axes();
        let he = self.half_extents.to_array();
        for (i, axis) in axes.iter().enumerate() {
            if d.dot(*axis).abs() > he[i] + crate::EPSILON_F32 {
                return false;
            }
        }
        true
    }

    /// Closest point on this OBB to a given point.
    #[must_use]
    #[inline]
    pub fn closest_point(&self, point: Vec3) -> Vec3 {
        let d = point - self.center;
        let axes = self.axes();
        let he = self.half_extents.to_array();
        let mut result = self.center;
        for (i, axis) in axes.iter().enumerate() {
            let dist = d.dot(*axis).clamp(-he[i], he[i]);
            result += *axis * dist;
        }
        result
    }
}

/// Ray-OBB intersection. Returns the `t` parameter if the ray hits the OBB.
#[must_use]
#[inline]
pub fn ray_obb(ray: &Ray, obb: &Obb) -> Option<f32> {
    let d = obb.center - ray.origin;
    let axes = obb.axes();
    let he = obb.half_extents.to_array();

    let mut t_min = f32::NEG_INFINITY;
    let mut t_max = f32::INFINITY;

    for i in 0..3 {
        let e = axes[i].dot(d);
        let f = axes[i].dot(ray.direction);

        if f.abs() > crate::EPSILON_F32 {
            let inv_f = 1.0 / f;
            let mut t1 = (e - he[i]) * inv_f;
            let mut t2 = (e + he[i]) * inv_f;
            if t1 > t2 {
                std::mem::swap(&mut t1, &mut t2);
            }
            t_min = t_min.max(t1);
            t_max = t_max.min(t2);
            if t_min > t_max {
                return None;
            }
        } else if (-e - he[i]) > 0.0 || (-e + he[i]) < 0.0 {
            return None;
        }
    }

    if t_min >= 0.0 {
        Some(t_min)
    } else if t_max >= 0.0 {
        Some(t_max)
    } else {
        None
    }
}

// ---------------------------------------------------------------------------
// Capsule
// ---------------------------------------------------------------------------

/// A capsule defined by a line segment and a radius.
///
/// The capsule is the Minkowski sum of the segment and a sphere.
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct Capsule {
    /// Start point of the capsule's axis.
    pub start: Vec3,
    /// End point of the capsule's axis.
    pub end: Vec3,
    /// Radius of the capsule.
    pub radius: f32,
}

impl Capsule {
    /// Create a new capsule.
    ///
    /// # Errors
    ///
    /// Returns [`crate::HisabError::InvalidInput`] if `radius` is negative.
    #[inline]
    pub fn new(start: Vec3, end: Vec3, radius: f32) -> Result<Self, crate::HisabError> {
        if radius < 0.0 {
            return Err(crate::HisabError::InvalidInput(
                "capsule radius must be non-negative".into(),
            ));
        }
        Ok(Self { start, end, radius })
    }

    /// Check whether a point is inside the capsule.
    #[must_use]
    #[inline]
    pub fn contains_point(&self, point: Vec3) -> bool {
        let seg = Segment::new(self.start, self.end);
        seg.distance_to_point(point) <= self.radius + crate::EPSILON_F32
    }

    /// Length of the capsule's axis (not including the hemispherical caps).
    #[must_use]
    #[inline]
    pub fn axis_length(&self) -> f32 {
        (self.end - self.start).length()
    }
}

/// Ray-capsule intersection. Returns the nearest `t >= 0` if the ray hits.
#[must_use]
pub fn ray_capsule(ray: &Ray, capsule: &Capsule) -> Option<f32> {
    // Test against the infinite cylinder, then clamp to segment + check hemispheres
    let ab = capsule.end - capsule.start;
    let ab_len_sq = ab.dot(ab);

    if ab_len_sq < crate::EPSILON_F32 {
        // Degenerate capsule: just a sphere
        let sphere = Sphere {
            center: capsule.start,
            radius: capsule.radius,
        };
        return ray_sphere(ray, &sphere);
    }

    // Closest approach of ray to segment axis
    let ao = ray.origin - capsule.start;
    let d_par = ray.direction.dot(ab) / ab_len_sq;
    let o_par = ao.dot(ab) / ab_len_sq;

    let d_perp = ray.direction - ab * d_par;
    let o_perp = ao - ab * o_par;

    let a = d_perp.dot(d_perp);
    let b = 2.0 * d_perp.dot(o_perp);
    let c = o_perp.dot(o_perp) - capsule.radius * capsule.radius;

    let disc = b * b - 4.0 * a * c;
    if disc < 0.0 {
        // Try sphere caps
        let s1 = Sphere {
            center: capsule.start,
            radius: capsule.radius,
        };
        let s2 = Sphere {
            center: capsule.end,
            radius: capsule.radius,
        };
        let t1 = ray_sphere(ray, &s1);
        let t2 = ray_sphere(ray, &s2);
        return match (t1, t2) {
            (Some(a), Some(b)) => Some(a.min(b)),
            (Some(a), None) | (None, Some(a)) => Some(a),
            _ => None,
        };
    }

    let inv_2a = 0.5 / a;
    let sqrt_disc = disc.sqrt();
    let t1 = (-b - sqrt_disc) * inv_2a;
    let t2 = (-b + sqrt_disc) * inv_2a;

    let mut best: Option<f32> = None;
    let mut check = |t: f32| {
        if t >= 0.0 {
            let p = ray.at(t);
            let proj = (p - capsule.start).dot(ab) / ab_len_sq;
            if (0.0..=1.0).contains(&proj) {
                best = Some(best.map_or(t, |b: f32| b.min(t)));
            }
        }
    };
    check(t1);
    check(t2);

    // Also check hemisphere caps
    let s1 = Sphere {
        center: capsule.start,
        radius: capsule.radius,
    };
    let s2 = Sphere {
        center: capsule.end,
        radius: capsule.radius,
    };
    if let Some(t) = ray_sphere(ray, &s1) {
        let p = ray.at(t);
        if (p - capsule.start).dot(ab) <= 0.0 {
            best = Some(best.map_or(t, |b: f32| b.min(t)));
        }
    }
    if let Some(t) = ray_sphere(ray, &s2) {
        let p = ray.at(t);
        if (p - capsule.end).dot(ab) >= 0.0 {
            best = Some(best.map_or(t, |b: f32| b.min(t)));
        }
    }

    best
}

// ---------------------------------------------------------------------------
// 3D GJK / EPA collision detection
// ---------------------------------------------------------------------------

/// A convex 3D shape that can compute a support point in a given direction.
pub trait ConvexSupport3D {
    /// Return the point on the shape farthest in `direction`.
    fn support(&self, direction: Vec3) -> Vec3;
}

impl ConvexSupport3D for Obb {
    #[inline]
    fn support(&self, direction: Vec3) -> Vec3 {
        let axes = self.axes();
        let he = self.half_extents.to_array();
        let mut result = self.center;
        for i in 0..3 {
            let sign = if axes[i].dot(direction) >= 0.0 {
                1.0
            } else {
                -1.0
            };
            result += axes[i] * (sign * he[i]);
        }
        result
    }
}

impl ConvexSupport3D for Capsule {
    #[inline]
    fn support(&self, direction: Vec3) -> Vec3 {
        let len = direction.length();
        let dir_norm = if len > crate::EPSILON_F32 {
            direction / len
        } else {
            Vec3::X
        };
        // Farthest endpoint in direction, plus radius offset
        let dot_start = self.start.dot(direction);
        let dot_end = self.end.dot(direction);
        let base = if dot_start >= dot_end {
            self.start
        } else {
            self.end
        };
        base + dir_norm * self.radius
    }
}

/// A convex polyhedron for 3D GJK/EPA.
#[derive(Debug, Clone)]
#[must_use]
pub struct ConvexHull3D {
    /// Vertices of the convex hull.
    pub vertices: Vec<Vec3>,
}

impl ConvexHull3D {
    /// Create a convex hull from vertices.
    ///
    /// # Errors
    ///
    /// Returns [`crate::HisabError::InvalidInput`] if `vertices` is empty.
    pub fn new(vertices: Vec<Vec3>) -> Result<Self, crate::HisabError> {
        if vertices.is_empty() {
            return Err(crate::HisabError::InvalidInput(
                "convex hull requires at least one vertex".into(),
            ));
        }
        Ok(Self { vertices })
    }
}

impl ConvexSupport3D for ConvexHull3D {
    #[inline]
    fn support(&self, direction: Vec3) -> Vec3 {
        let mut best = self.vertices[0];
        let mut best_dot = best.dot(direction);
        for &v in &self.vertices[1..] {
            let d = v.dot(direction);
            if d > best_dot {
                best_dot = d;
                best = v;
            }
        }
        best
    }
}

impl ConvexSupport3D for Sphere {
    #[inline]
    fn support(&self, direction: Vec3) -> Vec3 {
        let len = direction.length();
        if len < crate::EPSILON_F32 {
            return self.center + Vec3::new(self.radius, 0.0, 0.0);
        }
        self.center + direction * (self.radius / len)
    }
}

/// Minkowski difference support for 3D shapes.
#[inline]
fn minkowski_support_3d(a: &dyn ConvexSupport3D, b: &dyn ConvexSupport3D, direction: Vec3) -> Vec3 {
    a.support(direction) - b.support(-direction)
}

/// Penetration result from 3D EPA.
#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
pub struct Penetration3D {
    /// Penetration normal (direction to separate).
    pub normal: Vec3,
    /// Penetration depth.
    pub depth: f32,
}

/// 3D GJK collision test.
///
/// Returns `true` if the two convex shapes overlap.
#[must_use]
pub fn gjk_intersect_3d(a: &dyn ConvexSupport3D, b: &dyn ConvexSupport3D) -> bool {
    gjk_core_3d(a, b).is_some()
}

/// Combined 3D GJK + EPA.
///
/// Returns `None` if no overlap, or `Some(Penetration3D)` with separation info.
#[must_use]
pub fn gjk_epa_3d(a: &dyn ConvexSupport3D, b: &dyn ConvexSupport3D) -> Option<Penetration3D> {
    let simplex = gjk_core_3d(a, b)?;
    epa_penetration_3d(a, b, &simplex)
}

/// Core 3D GJK — returns the enclosing simplex (up to 4 points) if origin is contained.
fn gjk_core_3d(a: &dyn ConvexSupport3D, b: &dyn ConvexSupport3D) -> Option<Vec<Vec3>> {
    let mut direction = Vec3::X;
    let mut simplex: Vec<Vec3> = Vec::with_capacity(4);

    let s = minkowski_support_3d(a, b, direction);
    simplex.push(s);
    direction = -s;

    for _ in 0..GJK_MAX_ITERATIONS {
        if direction.length_squared() < crate::EPSILON_F32 {
            // Direction degenerated — pick perpendicular to existing simplex
            if simplex.len() >= 2 {
                let edge = simplex[simplex.len() - 1] - simplex[0];
                direction = if edge.x.abs() < 0.9 {
                    edge.cross(Vec3::X)
                } else {
                    edge.cross(Vec3::Y)
                };
            } else {
                direction = Vec3::Y;
            }
            if direction.length_squared() < crate::EPSILON_F32 {
                direction = Vec3::Z;
            }
        }
        let new_point = minkowski_support_3d(a, b, direction);
        if new_point.dot(direction) < 0.0 {
            return None;
        }
        simplex.push(new_point);

        if do_simplex_3d(&mut simplex, &mut direction) {
            return Some(simplex);
        }
    }
    None
}

/// Evolve the 3D simplex. Returns true if origin is enclosed.
fn do_simplex_3d(simplex: &mut Vec<Vec3>, direction: &mut Vec3) -> bool {
    match simplex.len() {
        2 => {
            // Line case
            let a = simplex[1];
            let b = simplex[0];
            let ab = b - a;
            let ao = -a;
            if ab.dot(ao) > 0.0 {
                *direction = ab.cross(ao).cross(ab);
            } else {
                simplex.clear();
                simplex.push(a);
                *direction = ao;
            }
            false
        }
        3 => {
            // Triangle case
            let a = simplex[2];
            let b = simplex[1];
            let c = simplex[0];
            let ab = b - a;
            let ac = c - a;
            let ao = -a;
            let abc = ab.cross(ac);

            if abc.cross(ac).dot(ao) > 0.0 {
                if ac.dot(ao) > 0.0 {
                    simplex.clear();
                    simplex.push(c);
                    simplex.push(a);
                    *direction = ac.cross(ao).cross(ac);
                } else {
                    simplex.clear();
                    simplex.push(b);
                    simplex.push(a);
                    return do_simplex_3d(simplex, direction); // line case
                }
            } else if ab.cross(abc).dot(ao) > 0.0 {
                simplex.clear();
                simplex.push(b);
                simplex.push(a);
                return do_simplex_3d(simplex, direction); // line case
            } else if abc.dot(ao) > 0.0 {
                *direction = abc;
            } else {
                // Below triangle
                simplex.swap(0, 1);
                *direction = -abc;
            }
            false
        }
        4 => {
            // Tetrahedron case — a is the newest point (last added)
            let a = simplex[3];
            let b = simplex[2];
            let c = simplex[1];
            let d = simplex[0];
            let ab = b - a;
            let ac = c - a;
            let ad = d - a;
            let ao = -a;

            // Compute face normals, ensuring they point outward (away from the
            // opposite vertex) by flipping if needed.
            let mut abc = ab.cross(ac);
            if abc.dot(ad) > 0.0 {
                abc = -abc;
            }
            let mut acd = ac.cross(ad);
            if acd.dot(ab) > 0.0 {
                acd = -acd;
            }
            let mut adb = ad.cross(ab);
            if adb.dot(ac) > 0.0 {
                adb = -adb;
            }

            if abc.dot(ao) > 0.0 {
                simplex.clear();
                simplex.push(c);
                simplex.push(b);
                simplex.push(a);
                return do_simplex_3d(simplex, direction); // triangle
            }
            if acd.dot(ao) > 0.0 {
                simplex.clear();
                simplex.push(d);
                simplex.push(c);
                simplex.push(a);
                return do_simplex_3d(simplex, direction);
            }
            if adb.dot(ao) > 0.0 {
                simplex.clear();
                simplex.push(b);
                simplex.push(d);
                simplex.push(a);
                return do_simplex_3d(simplex, direction);
            }
            // Origin is inside the tetrahedron
            true
        }
        _ => false,
    }
}

/// 3D EPA: compute penetration depth from the GJK simplex.
fn epa_penetration_3d(
    a: &dyn ConvexSupport3D,
    b: &dyn ConvexSupport3D,
    simplex: &[Vec3],
) -> Option<Penetration3D> {
    if simplex.is_empty() {
        return None;
    }

    // Expand simplex to a tetrahedron if needed
    let mut vertices: Vec<Vec3> = simplex.to_vec();
    // Try to ensure 4 non-coplanar points by adding support in cardinal directions
    let dirs = [
        Vec3::X,
        Vec3::Y,
        Vec3::Z,
        Vec3::NEG_X,
        Vec3::NEG_Y,
        Vec3::NEG_Z,
    ];
    for dir in &dirs {
        if vertices.len() >= 4 {
            break;
        }
        let p = minkowski_support_3d(a, b, *dir);
        let is_dup = vertices
            .iter()
            .any(|v| (*v - p).length_squared() < crate::EPSILON_F32);
        if !is_dup {
            vertices.push(p);
        }
    }
    if vertices.len() < 4 {
        return None; // Truly degenerate
    }

    // Check tetrahedron volume — if flat, try perturbing
    let vol = (vertices[1] - vertices[0])
        .dot((vertices[2] - vertices[0]).cross(vertices[3] - vertices[0]))
        .abs();
    if vol < 1e-10 {
        return None;
    }

    // Build initial polytope as triangular faces of the tetrahedron.
    // Ensure each face normal points away from the centroid (outward).
    let centroid = (vertices[0] + vertices[1] + vertices[2] + vertices[3]) * 0.25;
    let mut faces: Vec<[usize; 3]> = vec![[0, 1, 2], [0, 3, 1], [0, 2, 3], [1, 3, 2]];
    // Fix winding so normals point outward from centroid
    for face in &mut faces {
        let va = vertices[face[0]];
        let vb = vertices[face[1]];
        let vc = vertices[face[2]];
        let normal = (vb - va).cross(vc - va);
        if normal.dot(va - centroid) < 0.0 {
            face.swap(1, 2); // Reverse winding
        }
    }

    for _ in 0..EPA_MAX_ITERATIONS {
        // Find closest face to origin
        let mut closest_dist = f32::INFINITY;
        let mut closest_normal = Vec3::ZERO;

        for face in &faces {
            let va = vertices[face[0]];
            let vb = vertices[face[1]];
            let vc = vertices[face[2]];
            let cross = (vb - va).cross(vc - va);
            let len = cross.length();
            if len < crate::EPSILON_F32 {
                continue; // Skip degenerate faces
            }
            let normal = cross / len;
            let dist = normal.dot(va);

            // Ensure normal points away from origin
            let (normal, dist) = if dist < 0.0 {
                (-normal, -dist)
            } else {
                (normal, dist)
            };

            if dist < closest_dist {
                closest_dist = dist;
                closest_normal = normal;
            }
        }

        if closest_dist.is_infinite() {
            return None; // All faces degenerate this iteration
        }

        let support = minkowski_support_3d(a, b, closest_normal);
        let d = support.dot(closest_normal);

        if (d - closest_dist).abs() < crate::EPSILON_F32 {
            return Some(Penetration3D {
                normal: closest_normal,
                depth: closest_dist.abs(),
            });
        }

        // Add support point and rebuild faces visible from it
        let new_idx = vertices.len();
        vertices.push(support);

        // Find faces visible from the new point
        let mut edges: Vec<[usize; 2]> = Vec::new();
        let mut keep: Vec<bool> = vec![true; faces.len()];

        for (fi, face) in faces.iter().enumerate() {
            let va = vertices[face[0]];
            let vb = vertices[face[1]];
            let vc = vertices[face[2]];
            let normal = (vb - va).cross(vc - va);
            if normal.dot(support - va) > 0.0 {
                keep[fi] = false;
                // Collect edges
                for e in 0..3 {
                    let edge = [face[e], face[(e + 1) % 3]];
                    // Check if this edge is shared with another visible face
                    let reversed = [edge[1], edge[0]];
                    if let Some(pos) = edges.iter().position(|e| *e == reversed) {
                        edges.remove(pos);
                    } else {
                        edges.push(edge);
                    }
                }
            }
        }

        // Remove visible faces
        let mut new_faces: Vec<[usize; 3]> = Vec::new();
        for (fi, face) in faces.iter().enumerate() {
            if keep[fi] {
                new_faces.push(*face);
            }
        }

        // Create new faces from horizon edges to the new point
        for edge in &edges {
            new_faces.push([edge[0], edge[1], new_idx]);
        }

        faces = new_faces;
    }

    // Return best estimate
    let mut closest_dist = f32::INFINITY;
    let mut closest_normal = Vec3::ZERO;
    for face in &faces {
        let va = vertices[face[0]];
        let vb = vertices[face[1]];
        let vc = vertices[face[2]];
        let cross = (vb - va).cross(vc - va);
        let len = cross.length();
        if len < crate::EPSILON_F32 {
            continue;
        }
        let normal = cross / len;
        let dist = normal.dot(va).abs();
        if dist < closest_dist {
            closest_dist = dist;
            closest_normal = if normal.dot(va) >= 0.0 {
                normal
            } else {
                -normal
            };
        }
    }
    if closest_dist.is_infinite() {
        return None; // All faces degenerate
    }
    Some(Penetration3D {
        normal: closest_normal,
        depth: closest_dist,
    })
}

#[cfg(test)]
mod tests {
    use super::*;

    const EPSILON: f32 = 1e-4;

    fn approx_eq(a: f32, b: f32) -> bool {
        (a - b).abs() < EPSILON
    }

    fn vec3_approx_eq(a: Vec3, b: Vec3) -> bool {
        approx_eq(a.x, b.x) && approx_eq(a.y, b.y) && approx_eq(a.z, b.z)
    }

    #[test]
    fn ray_at_parameter() {
        let r = Ray::new(Vec3::ZERO, Vec3::X).unwrap();
        assert_eq!(r.at(0.0), Vec3::ZERO);
        assert!(approx_eq(r.at(5.0).x, 5.0));
    }

    #[test]
    fn plane_from_point_normal() {
        let p = Plane::from_point_normal(Vec3::new(0.0, 1.0, 0.0), Vec3::Y).unwrap();
        assert!(approx_eq(p.distance, 1.0));
        assert!(approx_eq(p.signed_distance(Vec3::new(0.0, 2.0, 0.0)), 1.0));
        assert!(approx_eq(p.signed_distance(Vec3::new(0.0, 0.0, 0.0)), -1.0));
    }

    #[test]
    fn aabb_contains() {
        let bb = Aabb::new(Vec3::ZERO, Vec3::ONE);
        assert!(bb.contains(Vec3::splat(0.5)));
        assert!(!bb.contains(Vec3::splat(2.0)));
        assert!(bb.contains(Vec3::ZERO));
    }

    #[test]
    fn aabb_center_and_size() {
        let bb = Aabb::new(Vec3::new(-1.0, -2.0, -3.0), Vec3::new(1.0, 2.0, 3.0));
        assert_eq!(bb.center(), Vec3::ZERO);
        assert_eq!(bb.size(), Vec3::new(2.0, 4.0, 6.0));
    }

    #[test]
    fn aabb_merge() {
        let a = Aabb::new(Vec3::ZERO, Vec3::ONE);
        let b = Aabb::new(Vec3::new(-1.0, -1.0, -1.0), Vec3::new(0.5, 0.5, 0.5));
        let merged = a.merge(&b);
        assert_eq!(merged.min, Vec3::new(-1.0, -1.0, -1.0));
        assert_eq!(merged.max, Vec3::ONE);
    }

    #[test]
    fn sphere_contains() {
        let s = Sphere::new(Vec3::ZERO, 1.0).unwrap();
        assert!(s.contains_point(Vec3::ZERO));
        assert!(s.contains_point(Vec3::new(1.0, 0.0, 0.0)));
        assert!(!s.contains_point(Vec3::new(1.1, 0.0, 0.0)));
    }

    #[test]
    fn ray_plane_intersection() {
        let r = Ray::new(Vec3::ZERO, Vec3::Y).unwrap();
        let p = Plane::from_point_normal(Vec3::new(0.0, 5.0, 0.0), Vec3::Y).unwrap();
        let t = ray_plane(&r, &p).unwrap();
        assert!(approx_eq(t, 5.0));
    }

    #[test]
    fn ray_plane_parallel_no_hit() {
        let r = Ray::new(Vec3::ZERO, Vec3::X).unwrap();
        let p = Plane::from_point_normal(Vec3::new(0.0, 5.0, 0.0), Vec3::Y).unwrap();
        assert!(ray_plane(&r, &p).is_none());
    }

    #[test]
    fn ray_sphere_hit() {
        let r = Ray::new(Vec3::new(0.0, 0.0, -5.0), Vec3::Z).unwrap();
        let s = Sphere::new(Vec3::ZERO, 1.0).unwrap();
        let t = ray_sphere(&r, &s).unwrap();
        assert!(approx_eq(t, 4.0));
    }

    #[test]
    fn ray_sphere_miss() {
        let r = Ray::new(Vec3::new(0.0, 5.0, -5.0), Vec3::Z).unwrap();
        let s = Sphere::new(Vec3::ZERO, 1.0).unwrap();
        assert!(ray_sphere(&r, &s).is_none());
    }

    #[test]
    fn ray_aabb_hit() {
        let r = Ray::new(Vec3::new(0.5, 0.5, -5.0), Vec3::Z).unwrap();
        let bb = Aabb::new(Vec3::ZERO, Vec3::ONE);
        let t = ray_aabb(&r, &bb).unwrap();
        assert!(approx_eq(t, 5.0));
    }

    #[test]
    fn ray_aabb_miss() {
        let r = Ray::new(Vec3::new(5.0, 5.0, -5.0), Vec3::Z).unwrap();
        let bb = Aabb::new(Vec3::ZERO, Vec3::ONE);
        assert!(ray_aabb(&r, &bb).is_none());
    }

    #[test]
    fn ray_inside_sphere() {
        let r = Ray::new(Vec3::ZERO, Vec3::X).unwrap();
        let s = Sphere::new(Vec3::ZERO, 10.0).unwrap();
        let t = ray_sphere(&r, &s).unwrap();
        assert!(t > 0.0);
        assert!(approx_eq(t, 10.0));
    }

    #[test]
    fn ray_inside_aabb() {
        let r = Ray::new(Vec3::splat(0.5), Vec3::X).unwrap();
        let bb = Aabb::new(Vec3::ZERO, Vec3::ONE);
        let t = ray_aabb(&r, &bb).unwrap();
        assert!(t >= 0.0);
    }

    #[test]
    fn aabb_auto_corrects_min_max() {
        let bb = Aabb::new(Vec3::ONE, Vec3::ZERO);
        assert_eq!(bb.min, Vec3::ZERO);
        assert_eq!(bb.max, Vec3::ONE);
    }

    #[test]
    fn ray_normalizes_direction() {
        let r = Ray::new(Vec3::ZERO, Vec3::new(0.0, 0.0, 10.0)).unwrap();
        let len = r.direction.length();
        assert!(approx_eq(len, 1.0));
        assert!(approx_eq(r.direction.z, 1.0));
    }

    #[test]
    fn ray_at_negative_parameter() {
        let r = Ray::new(Vec3::new(1.0, 0.0, 0.0), Vec3::X).unwrap();
        let p = r.at(-2.0);
        assert!(approx_eq(p.x, -1.0));
    }

    #[test]
    fn plane_signed_distance_on_plane() {
        let p = Plane::from_point_normal(Vec3::ZERO, Vec3::Y).unwrap();
        assert!(approx_eq(p.signed_distance(Vec3::new(5.0, 0.0, -3.0)), 0.0));
    }

    #[test]
    fn plane_non_axis_normal() {
        let normal = Vec3::new(1.0, 1.0, 0.0);
        let p = Plane::from_point_normal(Vec3::ZERO, normal).unwrap();
        assert!(approx_eq(p.normal.length(), 1.0));
        assert!(approx_eq(p.distance, 0.0));
    }

    #[test]
    fn aabb_contains_boundary_max() {
        let bb = Aabb::new(Vec3::ZERO, Vec3::ONE);
        assert!(bb.contains(Vec3::ONE));
    }

    #[test]
    fn aabb_merge_identical() {
        let bb = Aabb::new(Vec3::ZERO, Vec3::ONE);
        let merged = bb.merge(&bb);
        assert_eq!(merged.min, Vec3::ZERO);
        assert_eq!(merged.max, Vec3::ONE);
    }

    #[test]
    fn aabb_merge_disjoint() {
        let a = Aabb::new(Vec3::ZERO, Vec3::ONE);
        let b = Aabb::new(Vec3::splat(5.0), Vec3::splat(6.0));
        let merged = a.merge(&b);
        assert_eq!(merged.min, Vec3::ZERO);
        assert_eq!(merged.max, Vec3::splat(6.0));
    }

    #[test]
    fn sphere_surface_point() {
        let s = Sphere::new(Vec3::ZERO, 5.0).unwrap();
        assert!(s.contains_point(Vec3::new(5.0, 0.0, 0.0)));
        assert!(s.contains_point(Vec3::new(0.0, -5.0, 0.0)));
    }

    #[test]
    fn sphere_offset_center() {
        let s = Sphere::new(Vec3::new(10.0, 0.0, 0.0), 1.0).unwrap();
        assert!(s.contains_point(Vec3::new(10.5, 0.0, 0.0)));
        assert!(!s.contains_point(Vec3::ZERO));
    }

    #[test]
    fn ray_plane_behind_origin() {
        let r = Ray::new(Vec3::new(0.0, 0.0, 5.0), Vec3::Z).unwrap();
        let p = Plane::from_point_normal(Vec3::ZERO, Vec3::Z).unwrap();
        assert!(ray_plane(&r, &p).is_none());
    }

    #[test]
    fn ray_sphere_tangent() {
        let s = Sphere::new(Vec3::ZERO, 1.0).unwrap();
        let r = Ray::new(Vec3::new(0.0, 1.0, -5.0), Vec3::Z).unwrap();
        let t = ray_sphere(&r, &s);
        assert!(t.is_some());
        assert!(approx_eq(t.unwrap(), 5.0));
    }

    #[test]
    fn ray_sphere_behind_ray() {
        let r = Ray::new(Vec3::new(0.0, 0.0, 5.0), Vec3::Z).unwrap();
        let s = Sphere::new(Vec3::ZERO, 1.0).unwrap();
        assert!(ray_sphere(&r, &s).is_none());
    }

    #[test]
    fn ray_aabb_axis_aligned_hit() {
        let r = Ray::new(Vec3::new(-5.0, 0.5, 0.5), Vec3::X).unwrap();
        let bb = Aabb::new(Vec3::ZERO, Vec3::ONE);
        let t = ray_aabb(&r, &bb).unwrap();
        assert!(approx_eq(t, 5.0));
    }

    #[test]
    fn ray_aabb_parallel_to_slab_inside() {
        let r = Ray::new(Vec3::new(-5.0, 0.5, 0.5), Vec3::X).unwrap();
        let bb = Aabb::new(Vec3::ZERO, Vec3::ONE);
        assert!(ray_aabb(&r, &bb).is_some());
    }

    #[test]
    fn ray_aabb_parallel_to_slab_outside() {
        let r = Ray::new(Vec3::new(-5.0, 5.0, 0.5), Vec3::X).unwrap();
        let bb = Aabb::new(Vec3::ZERO, Vec3::ONE);
        assert!(ray_aabb(&r, &bb).is_none());
    }

    #[test]
    fn geo_error_display() {
        use crate::HisabError;
        let e = HisabError::Degenerate("zero-length edge".to_string());
        assert_eq!(e.to_string(), "degenerate geometry: zero-length edge");
    }

    #[test]
    fn ray_serde_roundtrip() {
        let r = Ray::new(Vec3::new(1.0, 2.0, 3.0), Vec3::Y).unwrap();
        let json = serde_json::to_string(&r).unwrap();
        let r2: Ray = serde_json::from_str(&json).unwrap();
        assert_eq!(r, r2);
    }

    #[test]
    fn aabb_serde_roundtrip() {
        let bb = Aabb::new(Vec3::new(-1.0, -2.0, -3.0), Vec3::new(4.0, 5.0, 6.0));
        let json = serde_json::to_string(&bb).unwrap();
        let bb2: Aabb = serde_json::from_str(&json).unwrap();
        assert_eq!(bb, bb2);
    }

    #[test]
    fn sphere_serde_roundtrip() {
        let s = Sphere::new(Vec3::new(1.0, 2.0, 3.0), 5.0).unwrap();
        let json = serde_json::to_string(&s).unwrap();
        let s2: Sphere = serde_json::from_str(&json).unwrap();
        assert_eq!(s, s2);
    }

    #[test]
    fn aabb_zero_size() {
        let bb = Aabb::new(Vec3::splat(3.0), Vec3::splat(3.0));
        assert_eq!(bb.size(), Vec3::ZERO);
        assert_eq!(bb.center(), Vec3::splat(3.0));
        assert!(bb.contains(Vec3::splat(3.0)));
    }

    #[test]
    fn ray_plane_intersection_at_angle() {
        let r = Ray::new(Vec3::new(0.0, 5.0, 0.0), Vec3::new(0.0, -1.0, 1.0)).unwrap();
        let p = Plane::from_point_normal(Vec3::ZERO, Vec3::Y).unwrap();
        let t = ray_plane(&r, &p).unwrap();
        let hit = r.at(t);
        assert!(approx_eq(hit.y, 0.0));
    }

    #[test]
    fn ray_sphere_optimized_matches_distance() {
        // Verify half-b optimization gives correct hit distance
        let r = Ray::new(Vec3::new(0.0, 0.0, -10.0), Vec3::Z).unwrap();
        let s = Sphere::new(Vec3::ZERO, 2.0).unwrap();
        let t = ray_sphere(&r, &s).unwrap();
        assert!(approx_eq(t, 8.0)); // 10 - 2
        let hit = r.at(t);
        assert!(approx_eq(hit.z, -2.0));
    }

    #[test]
    fn ray_sphere_large_radius() {
        let r = Ray::new(Vec3::new(0.0, 0.0, -1000.0), Vec3::Z).unwrap();
        let s = Sphere::new(Vec3::ZERO, 500.0).unwrap();
        let t = ray_sphere(&r, &s).unwrap();
        assert!(approx_eq(t, 500.0));
    }

    #[test]
    fn aabb_contains_just_outside() {
        let bb = Aabb::new(Vec3::ZERO, Vec3::ONE);
        assert!(!bb.contains(Vec3::new(1.001, 0.5, 0.5)));
        assert!(!bb.contains(Vec3::new(0.5, -0.001, 0.5)));
        assert!(!bb.contains(Vec3::new(0.5, 0.5, 1.001)));
    }

    #[test]
    fn aabb_contains_all_corners() {
        let bb = Aabb::new(Vec3::new(-1.0, -1.0, -1.0), Vec3::new(1.0, 1.0, 1.0));
        // All 8 corners should be contained
        for x in [-1.0, 1.0] {
            for y in [-1.0, 1.0] {
                for z in [-1.0, 1.0] {
                    assert!(bb.contains(Vec3::new(x, y, z)));
                }
            }
        }
    }

    #[test]
    fn ray_aabb_diagonal_ray() {
        // Diagonal ray through AABB
        let r = Ray::new(Vec3::new(-5.0, -5.0, -5.0), Vec3::new(1.0, 1.0, 1.0)).unwrap();
        let bb = Aabb::new(Vec3::ZERO, Vec3::ONE);
        assert!(ray_aabb(&r, &bb).is_some());
    }

    #[test]
    fn plane_signed_distance_both_sides() {
        let p = Plane::from_point_normal(Vec3::new(0.0, 5.0, 0.0), Vec3::Y).unwrap();
        assert!(p.signed_distance(Vec3::new(0.0, 10.0, 0.0)) > 0.0);
        assert!(p.signed_distance(Vec3::new(0.0, 0.0, 0.0)) < 0.0);
        assert!(approx_eq(p.signed_distance(Vec3::new(0.0, 5.0, 0.0)), 0.0));
    }

    // -----------------------------------------------------------------------
    // V0.2 tests
    // -----------------------------------------------------------------------

    // Triangle tests
    #[test]
    fn triangle_normal() {
        let tri = Triangle::new(Vec3::ZERO, Vec3::X, Vec3::Y);
        let n = tri.normal();
        assert!(approx_eq(n.z, 1.0)); // CCW in XY plane -> +Z
    }

    #[test]
    fn triangle_area() {
        let tri = Triangle::new(
            Vec3::ZERO,
            Vec3::new(2.0, 0.0, 0.0),
            Vec3::new(0.0, 2.0, 0.0),
        );
        assert!(approx_eq(tri.area(), 2.0));
    }

    #[test]
    fn triangle_centroid() {
        let tri = Triangle::new(
            Vec3::ZERO,
            Vec3::new(3.0, 0.0, 0.0),
            Vec3::new(0.0, 3.0, 0.0),
        );
        assert!(vec3_approx_eq(tri.centroid(), Vec3::new(1.0, 1.0, 0.0)));
    }

    #[test]
    fn triangle_degenerate_area() {
        // Collinear points -> zero area
        let tri = Triangle::new(Vec3::ZERO, Vec3::X, Vec3::new(2.0, 0.0, 0.0));
        assert!(approx_eq(tri.area(), 0.0));
    }

    // Line tests
    #[test]
    fn line_closest_point() {
        let l = Line::new(Vec3::ZERO, Vec3::X).unwrap();
        let p = Vec3::new(5.0, 3.0, 0.0);
        let cp = l.closest_point(p);
        assert!(vec3_approx_eq(cp, Vec3::new(5.0, 0.0, 0.0)));
    }

    #[test]
    fn line_distance_to_point() {
        let l = Line::new(Vec3::ZERO, Vec3::X).unwrap();
        let d = l.distance_to_point(Vec3::new(5.0, 3.0, 4.0));
        assert!(approx_eq(d, 5.0)); // sqrt(9+16) = 5
    }

    #[test]
    fn line_closest_point_behind_origin() {
        // Line is infinite — should work for negative t
        let l = Line::new(Vec3::ZERO, Vec3::X).unwrap();
        let cp = l.closest_point(Vec3::new(-10.0, 1.0, 0.0));
        assert!(vec3_approx_eq(cp, Vec3::new(-10.0, 0.0, 0.0)));
    }

    // Segment tests
    #[test]
    fn segment_length_and_midpoint() {
        let s = Segment::new(Vec3::ZERO, Vec3::new(4.0, 0.0, 0.0));
        assert!(approx_eq(s.length(), 4.0));
        assert!(vec3_approx_eq(s.midpoint(), Vec3::new(2.0, 0.0, 0.0)));
    }

    #[test]
    fn segment_closest_point_clamped() {
        let s = Segment::new(Vec3::ZERO, Vec3::new(10.0, 0.0, 0.0));
        // Point past the end
        assert!(vec3_approx_eq(
            s.closest_point(Vec3::new(20.0, 0.0, 0.0)),
            Vec3::new(10.0, 0.0, 0.0)
        ));
        // Point before the start
        assert!(vec3_approx_eq(
            s.closest_point(Vec3::new(-5.0, 0.0, 0.0)),
            Vec3::ZERO
        ));
        // Point alongside
        assert!(vec3_approx_eq(
            s.closest_point(Vec3::new(5.0, 3.0, 0.0)),
            Vec3::new(5.0, 0.0, 0.0)
        ));
    }

    #[test]
    fn segment_distance() {
        let s = Segment::new(Vec3::ZERO, Vec3::new(10.0, 0.0, 0.0));
        assert!(approx_eq(
            s.distance_to_point(Vec3::new(5.0, 3.0, 0.0)),
            3.0
        ));
    }

    #[test]
    fn segment_direction_normalized() {
        let s = Segment::new(Vec3::ZERO, Vec3::new(0.0, 0.0, 10.0));
        assert!(vec3_approx_eq(s.direction(), Vec3::Z));
    }

    // Ray-triangle tests
    #[test]
    fn ray_triangle_hit() {
        let tri = Triangle::new(
            Vec3::new(-1.0, -1.0, 5.0),
            Vec3::new(1.0, -1.0, 5.0),
            Vec3::new(0.0, 1.0, 5.0),
        );
        let r = Ray::new(Vec3::ZERO, Vec3::Z).unwrap();
        let t = ray_triangle(&r, &tri).unwrap();
        assert!(approx_eq(t, 5.0));
    }

    #[test]
    fn ray_triangle_miss() {
        let tri = Triangle::new(
            Vec3::new(-1.0, -1.0, 5.0),
            Vec3::new(1.0, -1.0, 5.0),
            Vec3::new(0.0, 1.0, 5.0),
        );
        let r = Ray::new(Vec3::new(10.0, 10.0, 0.0), Vec3::Z).unwrap();
        assert!(ray_triangle(&r, &tri).is_none());
    }

    #[test]
    fn ray_triangle_parallel() {
        // Ray parallel to triangle plane
        let tri = Triangle::new(Vec3::ZERO, Vec3::X, Vec3::Y);
        let r = Ray::new(Vec3::new(0.0, 0.0, 1.0), Vec3::X).unwrap();
        assert!(ray_triangle(&r, &tri).is_none());
    }

    #[test]
    fn ray_triangle_behind() {
        let tri = Triangle::new(
            Vec3::new(-1.0, -1.0, -5.0),
            Vec3::new(1.0, -1.0, -5.0),
            Vec3::new(0.0, 1.0, -5.0),
        );
        let r = Ray::new(Vec3::ZERO, Vec3::Z).unwrap(); // Points away from triangle
        assert!(ray_triangle(&r, &tri).is_none());
    }

    // AABB-AABB overlap tests
    #[test]
    fn aabb_aabb_overlap() {
        let a = Aabb::new(Vec3::ZERO, Vec3::ONE);
        let b = Aabb::new(Vec3::splat(0.5), Vec3::splat(1.5));
        assert!(aabb_aabb(&a, &b));
    }

    #[test]
    fn aabb_aabb_no_overlap() {
        let a = Aabb::new(Vec3::ZERO, Vec3::ONE);
        let b = Aabb::new(Vec3::splat(2.0), Vec3::splat(3.0));
        assert!(!aabb_aabb(&a, &b));
    }

    #[test]
    fn aabb_aabb_touching() {
        let a = Aabb::new(Vec3::ZERO, Vec3::ONE);
        let b = Aabb::new(Vec3::new(1.0, 0.0, 0.0), Vec3::new(2.0, 1.0, 1.0));
        assert!(aabb_aabb(&a, &b)); // Touching edge = overlap
    }

    #[test]
    fn aabb_aabb_contained() {
        let a = Aabb::new(Vec3::ZERO, Vec3::splat(10.0));
        let b = Aabb::new(Vec3::ONE, Vec3::splat(2.0));
        assert!(aabb_aabb(&a, &b));
    }

    // Sphere-sphere overlap tests
    #[test]
    fn sphere_sphere_overlap() {
        let a = Sphere::new(Vec3::ZERO, 1.0).unwrap();
        let b = Sphere::new(Vec3::new(1.5, 0.0, 0.0), 1.0).unwrap();
        assert!(sphere_sphere(&a, &b));
    }

    #[test]
    fn sphere_sphere_no_overlap() {
        let a = Sphere::new(Vec3::ZERO, 1.0).unwrap();
        let b = Sphere::new(Vec3::new(3.0, 0.0, 0.0), 1.0).unwrap();
        assert!(!sphere_sphere(&a, &b));
    }

    #[test]
    fn sphere_sphere_touching() {
        let a = Sphere::new(Vec3::ZERO, 1.0).unwrap();
        let b = Sphere::new(Vec3::new(2.0, 0.0, 0.0), 1.0).unwrap();
        assert!(sphere_sphere(&a, &b)); // Touching = overlap
    }

    // Plane-plane intersection tests
    #[test]
    fn plane_plane_intersection() {
        let a = Plane::from_point_normal(Vec3::ZERO, Vec3::Y).unwrap();
        let b = Plane::from_point_normal(Vec3::ZERO, Vec3::X).unwrap();
        let line = plane_plane(&a, &b).unwrap();
        // Intersection should be along Z axis
        assert!(approx_eq(line.direction.z.abs(), 1.0));
    }

    #[test]
    fn plane_plane_parallel() {
        let a = Plane::from_point_normal(Vec3::ZERO, Vec3::Y).unwrap();
        let b = Plane::from_point_normal(Vec3::new(0.0, 5.0, 0.0), Vec3::Y).unwrap();
        assert!(plane_plane(&a, &b).is_none());
    }

    // Closest-point tests
    #[test]
    fn closest_on_ray_forward() {
        let r = Ray::new(Vec3::ZERO, Vec3::X).unwrap();
        let cp = closest_point_on_ray(&r, Vec3::new(5.0, 3.0, 0.0));
        assert!(vec3_approx_eq(cp, Vec3::new(5.0, 0.0, 0.0)));
    }

    #[test]
    fn closest_on_ray_clamped() {
        // Point behind the ray — should clamp to origin
        let r = Ray::new(Vec3::ZERO, Vec3::X).unwrap();
        let cp = closest_point_on_ray(&r, Vec3::new(-5.0, 3.0, 0.0));
        assert!(vec3_approx_eq(cp, Vec3::ZERO));
    }

    #[test]
    fn closest_on_plane() {
        let p = Plane::from_point_normal(Vec3::ZERO, Vec3::Y).unwrap();
        let cp = closest_point_on_plane(&p, Vec3::new(3.0, 7.0, -2.0));
        assert!(vec3_approx_eq(cp, Vec3::new(3.0, 0.0, -2.0)));
    }

    #[test]
    fn closest_on_sphere_outside() {
        let s = Sphere::new(Vec3::ZERO, 1.0).unwrap();
        let cp = closest_point_on_sphere(&s, Vec3::new(10.0, 0.0, 0.0));
        assert!(vec3_approx_eq(cp, Vec3::new(1.0, 0.0, 0.0)));
    }

    #[test]
    fn closest_on_sphere_inside() {
        let s = Sphere::new(Vec3::ZERO, 10.0).unwrap();
        let cp = closest_point_on_sphere(&s, Vec3::new(1.0, 0.0, 0.0));
        assert!(vec3_approx_eq(cp, Vec3::new(10.0, 0.0, 0.0)));
    }

    #[test]
    fn closest_on_sphere_at_center() {
        let s = Sphere::new(Vec3::ZERO, 5.0).unwrap();
        let cp = closest_point_on_sphere(&s, Vec3::ZERO);
        assert!(vec3_approx_eq(cp, Vec3::new(5.0, 0.0, 0.0)));
    }

    #[test]
    fn closest_on_aabb_inside() {
        let bb = Aabb::new(Vec3::ZERO, Vec3::splat(10.0));
        let cp = closest_point_on_aabb(&bb, Vec3::new(5.0, 5.0, 5.0));
        assert!(vec3_approx_eq(cp, Vec3::new(5.0, 5.0, 5.0)));
    }

    #[test]
    fn closest_on_aabb_outside() {
        let bb = Aabb::new(Vec3::ZERO, Vec3::ONE);
        let cp = closest_point_on_aabb(&bb, Vec3::new(5.0, 0.5, -3.0));
        assert!(vec3_approx_eq(cp, Vec3::new(1.0, 0.5, 0.0)));
    }

    // Frustum tests
    #[test]
    fn frustum_contains_origin() {
        use crate::transforms::projection_perspective;
        let proj = projection_perspective(std::f32::consts::FRAC_PI_4, 1.0, 0.1, 100.0);
        let frustum = Frustum::from_view_projection(proj);
        // Point at center of near plane should be inside
        assert!(frustum.contains_point(Vec3::new(0.0, 0.0, -1.0)));
    }

    #[test]
    fn frustum_rejects_behind_camera() {
        use crate::transforms::projection_perspective;
        let proj = projection_perspective(std::f32::consts::FRAC_PI_4, 1.0, 0.1, 100.0);
        let frustum = Frustum::from_view_projection(proj);
        // Point behind camera should be outside
        assert!(!frustum.contains_point(Vec3::new(0.0, 0.0, 1.0)));
    }

    #[test]
    fn frustum_rejects_far_point() {
        use crate::transforms::projection_perspective;
        let proj = projection_perspective(std::f32::consts::FRAC_PI_4, 1.0, 0.1, 100.0);
        let frustum = Frustum::from_view_projection(proj);
        assert!(!frustum.contains_point(Vec3::new(0.0, 0.0, -200.0)));
    }

    #[test]
    fn frustum_contains_aabb_inside() {
        use crate::transforms::projection_perspective;
        let proj = projection_perspective(std::f32::consts::FRAC_PI_4, 1.0, 0.1, 100.0);
        let frustum = Frustum::from_view_projection(proj);
        let bb = Aabb::new(Vec3::new(-0.1, -0.1, -2.0), Vec3::new(0.1, 0.1, -1.0));
        assert!(frustum.contains_aabb(&bb));
    }

    #[test]
    fn frustum_rejects_aabb_outside() {
        use crate::transforms::projection_perspective;
        let proj = projection_perspective(std::f32::consts::FRAC_PI_4, 1.0, 0.1, 100.0);
        let frustum = Frustum::from_view_projection(proj);
        let bb = Aabb::new(Vec3::splat(500.0), Vec3::splat(600.0));
        assert!(!frustum.contains_aabb(&bb));
    }

    // Serde roundtrip for new types
    #[test]
    fn triangle_serde_roundtrip() {
        let tri = Triangle::new(Vec3::ZERO, Vec3::X, Vec3::Y);
        let json = serde_json::to_string(&tri).unwrap();
        let tri2: Triangle = serde_json::from_str(&json).unwrap();
        assert_eq!(tri, tri2);
    }

    #[test]
    fn line_serde_roundtrip() {
        let l = Line::new(Vec3::ZERO, Vec3::X).unwrap();
        let json = serde_json::to_string(&l).unwrap();
        let l2: Line = serde_json::from_str(&json).unwrap();
        assert_eq!(l, l2);
    }

    #[test]
    fn segment_serde_roundtrip() {
        let s = Segment::new(Vec3::ZERO, Vec3::ONE);
        let json = serde_json::to_string(&s).unwrap();
        let s2: Segment = serde_json::from_str(&json).unwrap();
        assert_eq!(s, s2);
    }

    // --- Audit tests ---

    #[test]
    fn triangle_unit_normal() {
        let tri = Triangle::new(Vec3::ZERO, Vec3::X, Vec3::Y);
        let n = tri.unit_normal();
        assert!(approx_eq(n.length(), 1.0));
        assert!(approx_eq(n.z, 1.0));
    }

    #[test]
    fn triangle_unit_normal_3d() {
        // Tilted triangle in 3D space
        let tri = Triangle::new(
            Vec3::ZERO,
            Vec3::new(1.0, 1.0, 0.0),
            Vec3::new(0.0, 1.0, 1.0),
        );
        let n = tri.unit_normal();
        assert!(approx_eq(n.length(), 1.0));
    }

    #[test]
    fn segment_degenerate_zero_length() {
        let s = Segment::new(Vec3::ONE, Vec3::ONE);
        assert!(approx_eq(s.length(), 0.0));
        // Closest point should return the segment point itself
        assert!(vec3_approx_eq(
            s.closest_point(Vec3::new(5.0, 5.0, 5.0)),
            Vec3::ONE
        ));
    }

    #[test]
    fn plane_plane_intersection_point_on_both() {
        let a = Plane::from_point_normal(Vec3::ZERO, Vec3::Y).unwrap();
        let b = Plane::from_point_normal(Vec3::ZERO, Vec3::X).unwrap();
        let line = plane_plane(&a, &b).unwrap();
        // The origin should be close to the intersection line
        let cp = line.closest_point(Vec3::ZERO);
        assert!(approx_eq(cp.length(), 0.0));
    }

    #[test]
    fn closest_on_sphere_direction_consistent() {
        let s = Sphere::new(Vec3::ZERO, 5.0).unwrap();
        // Point along +Y axis -> closest should be along +Y
        let cp = closest_point_on_sphere(&s, Vec3::new(0.0, 100.0, 0.0));
        assert!(vec3_approx_eq(cp, Vec3::new(0.0, 5.0, 0.0)));
        // Point along -Z axis -> closest should be along -Z
        let cp2 = closest_point_on_sphere(&s, Vec3::new(0.0, 0.0, -100.0));
        assert!(vec3_approx_eq(cp2, Vec3::new(0.0, 0.0, -5.0)));
    }

    #[test]
    fn frustum_serde_roundtrip() {
        use crate::transforms::projection_perspective;
        let proj = projection_perspective(std::f32::consts::FRAC_PI_4, 1.0, 0.1, 100.0);
        let f = Frustum::from_view_projection(proj);
        let json = serde_json::to_string(&f).unwrap();
        let f2: Frustum = serde_json::from_str(&json).unwrap();
        assert_eq!(f, f2);
    }

    #[test]
    fn frustum_left_right_rejection() {
        use crate::transforms::projection_perspective;
        let proj = projection_perspective(std::f32::consts::FRAC_PI_4, 1.0, 0.1, 100.0);
        let frustum = Frustum::from_view_projection(proj);
        // Points far to the left/right should be outside
        assert!(!frustum.contains_point(Vec3::new(1000.0, 0.0, -10.0)));
        assert!(!frustum.contains_point(Vec3::new(-1000.0, 0.0, -10.0)));
    }

    #[test]
    fn ray_triangle_edge_hit() {
        // Ray hitting exactly on the edge of the triangle
        let tri = Triangle::new(
            Vec3::new(-1.0, 0.0, 5.0),
            Vec3::new(1.0, 0.0, 5.0),
            Vec3::new(0.0, 2.0, 5.0),
        );
        let r = Ray::new(Vec3::new(0.0, 0.0, 0.0), Vec3::Z).unwrap();
        // Should hit at the bottom edge (y=0)
        let t = ray_triangle(&r, &tri).unwrap();
        assert!(approx_eq(t, 5.0));
    }

    #[test]
    fn aabb_aabb_single_axis_separation() {
        // Separated only on Z axis
        let a = Aabb::new(Vec3::ZERO, Vec3::ONE);
        let b = Aabb::new(Vec3::new(0.0, 0.0, 2.0), Vec3::new(1.0, 1.0, 3.0));
        assert!(!aabb_aabb(&a, &b));
    }

    #[test]
    fn line_distance_at_origin() {
        let l = Line::new(Vec3::new(0.0, 5.0, 0.0), Vec3::X).unwrap();
        assert!(approx_eq(l.distance_to_point(Vec3::ZERO), 5.0));
    }

    #[test]
    fn closest_on_plane_already_on_plane() {
        let p = Plane::from_point_normal(Vec3::ZERO, Vec3::Y).unwrap();
        let point = Vec3::new(3.0, 0.0, -7.0);
        let cp = closest_point_on_plane(&p, point);
        assert!(vec3_approx_eq(cp, point));
    }

    #[test]
    fn closest_on_ray_along_direction() {
        let r = Ray::new(Vec3::ZERO, Vec3::X).unwrap();
        let cp = closest_point_on_ray(&r, Vec3::new(5.0, 0.0, 0.0));
        assert!(vec3_approx_eq(cp, Vec3::new(5.0, 0.0, 0.0)));
    }

    #[test]
    fn sphere_sphere_concentric() {
        let a = Sphere::new(Vec3::ZERO, 1.0).unwrap();
        let b = Sphere::new(Vec3::ZERO, 0.5).unwrap();
        assert!(sphere_sphere(&a, &b));
    }

    // --- V0.5a: BVH ---

    #[test]
    fn bvh_empty() {
        let bvh = Bvh::build(&mut []);
        assert!(bvh.is_empty());
        assert_eq!(bvh.len(), 0);
        let r = Ray::new(Vec3::ZERO, Vec3::X).unwrap();
        assert!(bvh.query_ray(&r).is_empty());
    }

    #[test]
    fn bvh_single() {
        let bb = Aabb::new(Vec3::new(-1.0, -1.0, -1.0), Vec3::ONE);
        let mut items = [(bb, 42)];
        let bvh = Bvh::build(&mut items);
        assert_eq!(bvh.len(), 1);

        let r = Ray::new(Vec3::new(0.0, 0.0, -5.0), Vec3::Z).unwrap();
        let hits = bvh.query_ray(&r);
        assert_eq!(hits, vec![42]);
    }

    #[test]
    fn bvh_ray_query_hits_and_misses() {
        let mut items: Vec<(Aabb, usize)> = (0..10)
            .map(|i| {
                let x = i as f32 * 3.0;
                (
                    Aabb::new(Vec3::new(x, 0.0, 0.0), Vec3::new(x + 1.0, 1.0, 1.0)),
                    i,
                )
            })
            .collect();
        let bvh = Bvh::build(&mut items);
        assert_eq!(bvh.len(), 10);

        // Ray hitting the first box
        let r = Ray::new(Vec3::new(0.5, 0.5, -5.0), Vec3::Z).unwrap();
        let hits = bvh.query_ray(&r);
        assert!(hits.contains(&0));
        assert!(!hits.contains(&5));

        // Ray missing everything (way above)
        let r_miss = Ray::new(Vec3::new(0.5, 100.0, -5.0), Vec3::Z).unwrap();
        assert!(bvh.query_ray(&r_miss).is_empty());
    }

    #[test]
    fn bvh_aabb_query() {
        let mut items: Vec<(Aabb, usize)> = (0..5)
            .map(|i| {
                let x = i as f32 * 2.0;
                (
                    Aabb::new(Vec3::new(x, 0.0, 0.0), Vec3::new(x + 1.0, 1.0, 1.0)),
                    i,
                )
            })
            .collect();
        let bvh = Bvh::build(&mut items);

        // Query box overlapping first two items
        let query = Aabb::new(Vec3::new(-0.5, 0.0, 0.0), Vec3::new(2.5, 1.0, 1.0));
        let hits = bvh.query_aabb(&query);
        assert!(hits.contains(&0));
        assert!(hits.contains(&1));
    }

    #[test]
    fn bvh_many_items() {
        let mut items: Vec<(Aabb, usize)> = (0..100)
            .map(|i| {
                let x = (i % 10) as f32;
                let y = (i / 10) as f32;
                (
                    Aabb::new(Vec3::new(x, y, 0.0), Vec3::new(x + 0.5, y + 0.5, 0.5)),
                    i,
                )
            })
            .collect();
        let bvh = Bvh::build(&mut items);
        assert_eq!(bvh.len(), 100);

        let r = Ray::new(Vec3::new(0.25, 0.25, -10.0), Vec3::Z).unwrap();
        let hits = bvh.query_ray(&r);
        assert!(!hits.is_empty());
    }

    // --- V0.5a: K-d tree ---

    #[test]
    fn kdtree_empty() {
        let tree = KdTree::build(&mut []);
        assert!(tree.is_empty());
        assert!(tree.nearest(Vec3::ZERO).is_none());
    }

    #[test]
    fn kdtree_single() {
        let mut items = [(Vec3::new(5.0, 0.0, 0.0), 0)];
        let tree = KdTree::build(&mut items);
        let (idx, dist_sq) = tree.nearest(Vec3::ZERO).unwrap();
        assert_eq!(idx, 0);
        assert!(approx_eq(dist_sq, 25.0));
    }

    #[test]
    fn kdtree_nearest_basic() {
        let mut items: Vec<(Vec3, usize)> = vec![
            (Vec3::new(0.0, 0.0, 0.0), 0),
            (Vec3::new(10.0, 0.0, 0.0), 1),
            (Vec3::new(5.0, 5.0, 0.0), 2),
        ];
        let tree = KdTree::build(&mut items);

        let (idx, _) = tree.nearest(Vec3::new(0.1, 0.0, 0.0)).unwrap();
        assert_eq!(idx, 0);

        let (idx, _) = tree.nearest(Vec3::new(9.9, 0.0, 0.0)).unwrap();
        assert_eq!(idx, 1);

        let (idx, _) = tree.nearest(Vec3::new(5.0, 4.9, 0.0)).unwrap();
        assert_eq!(idx, 2);
    }

    #[test]
    fn kdtree_within_radius() {
        let mut items: Vec<(Vec3, usize)> = (0..10)
            .map(|i| (Vec3::new(i as f32, 0.0, 0.0), i))
            .collect();
        let tree = KdTree::build(&mut items);

        let results = tree.within_radius(Vec3::new(5.0, 0.0, 0.0), 1.5);
        let indices: Vec<usize> = results.iter().map(|&(idx, _)| idx).collect();
        assert!(indices.contains(&4));
        assert!(indices.contains(&5));
        assert!(indices.contains(&6));
        assert!(!indices.contains(&3));
        assert!(!indices.contains(&7));
    }

    #[test]
    fn kdtree_within_radius_empty() {
        let mut items = [(Vec3::new(100.0, 100.0, 100.0), 0)];
        let tree = KdTree::build(&mut items);
        let results = tree.within_radius(Vec3::ZERO, 1.0);
        assert!(results.is_empty());
    }

    #[test]
    fn kdtree_many_points() {
        let mut items: Vec<(Vec3, usize)> = (0..1000)
            .map(|i| {
                let x = (i % 10) as f32;
                let y = ((i / 10) % 10) as f32;
                let z = (i / 100) as f32;
                (Vec3::new(x, y, z), i)
            })
            .collect();
        let tree = KdTree::build(&mut items);
        assert_eq!(tree.len(), 1000);

        // The point (0,0,0) should have index 0 as nearest
        let (idx, dist_sq) = tree.nearest(Vec3::new(0.01, 0.01, 0.01)).unwrap();
        assert_eq!(idx, 0);
        assert!(dist_sq < 0.01);
    }

    #[test]
    fn kdtree_nearest_exact_match() {
        let mut items: Vec<(Vec3, usize)> =
            vec![(Vec3::new(1.0, 2.0, 3.0), 7), (Vec3::new(4.0, 5.0, 6.0), 8)];
        let tree = KdTree::build(&mut items);
        let (idx, dist_sq) = tree.nearest(Vec3::new(1.0, 2.0, 3.0)).unwrap();
        assert_eq!(idx, 7);
        assert!(approx_eq(dist_sq, 0.0));
    }

    // --- V0.5a audit tests ---

    #[test]
    fn bvh_degenerate_same_position() {
        // All items at the same position — should still build and query
        let mut items: Vec<(Aabb, usize)> = (0..5)
            .map(|i| (Aabb::new(Vec3::ZERO, Vec3::ONE), i))
            .collect();
        let bvh = Bvh::build(&mut items);
        assert_eq!(bvh.len(), 5);
        let r = Ray::new(Vec3::new(0.5, 0.5, -5.0), Vec3::Z).unwrap();
        let hits = bvh.query_ray(&r);
        assert_eq!(hits.len(), 5);
    }

    #[test]
    fn bvh_aabb_query_no_overlap() {
        let mut items: Vec<(Aabb, usize)> = (0..5)
            .map(|i| {
                let x = i as f32 * 10.0;
                (
                    Aabb::new(Vec3::new(x, 0.0, 0.0), Vec3::new(x + 1.0, 1.0, 1.0)),
                    i,
                )
            })
            .collect();
        let bvh = Bvh::build(&mut items);
        let query = Aabb::new(Vec3::splat(1000.0), Vec3::splat(2000.0));
        assert!(bvh.query_aabb(&query).is_empty());
    }

    #[test]
    fn kdtree_duplicate_points() {
        // Multiple points at the same location
        let mut items: Vec<(Vec3, usize)> = (0..5).map(|i| (Vec3::ZERO, i)).collect();
        let tree = KdTree::build(&mut items);
        let (_, dist_sq) = tree.nearest(Vec3::ZERO).unwrap();
        assert!(approx_eq(dist_sq, 0.0));
    }

    #[test]
    fn kdtree_two_points() {
        let mut items = [
            (Vec3::new(0.0, 0.0, 0.0), 0),
            (Vec3::new(10.0, 0.0, 0.0), 1),
        ];
        let tree = KdTree::build(&mut items);
        let (idx, _) = tree.nearest(Vec3::new(3.0, 0.0, 0.0)).unwrap();
        assert_eq!(idx, 0); // closer to origin
        let (idx, _) = tree.nearest(Vec3::new(7.0, 0.0, 0.0)).unwrap();
        assert_eq!(idx, 1); // closer to 10
    }

    #[test]
    fn bvh_two_items() {
        let mut items = [
            (Aabb::new(Vec3::ZERO, Vec3::ONE), 0),
            (
                Aabb::new(Vec3::new(5.0, 0.0, 0.0), Vec3::new(6.0, 1.0, 1.0)),
                1,
            ),
        ];
        let bvh = Bvh::build(&mut items);
        let r = Ray::new(Vec3::new(5.5, 0.5, -5.0), Vec3::Z).unwrap();
        let hits = bvh.query_ray(&r);
        assert!(hits.contains(&1));
        assert!(!hits.contains(&0));
    }

    // --- V0.5b: Quadtree ---

    #[test]
    fn quadtree_empty() {
        let bounds = Rect::new(glam::Vec2::ZERO, glam::Vec2::splat(100.0));
        let qt = Quadtree::new(bounds, 4, 8);
        assert!(qt.is_empty());
        assert_eq!(qt.len(), 0);
    }

    #[test]
    fn quadtree_insert_and_query() {
        let bounds = Rect::new(glam::Vec2::ZERO, glam::Vec2::splat(100.0));
        let mut qt = Quadtree::new(bounds, 4, 8);
        for i in 0..10 {
            qt.insert(glam::Vec2::new(i as f32 * 10.0 + 1.0, 50.0), i);
        }
        assert_eq!(qt.len(), 10);

        let query = Rect::new(glam::Vec2::new(0.0, 40.0), glam::Vec2::new(25.0, 60.0));
        let results = qt.query_rect(&query);
        assert!(results.contains(&0)); // x=1
        assert!(results.contains(&1)); // x=11
        assert!(results.contains(&2)); // x=21
        assert!(!results.contains(&5)); // x=51
    }

    #[test]
    fn quadtree_out_of_bounds_ignored() {
        let bounds = Rect::new(glam::Vec2::ZERO, glam::Vec2::splat(10.0));
        let mut qt = Quadtree::new(bounds, 4, 8);
        qt.insert(glam::Vec2::new(100.0, 100.0), 0); // Out of bounds
        assert_eq!(qt.len(), 0);
    }

    #[test]
    fn quadtree_subdivision() {
        let bounds = Rect::new(glam::Vec2::ZERO, glam::Vec2::splat(100.0));
        let mut qt = Quadtree::new(bounds, 2, 8); // Split after 2 items
        for i in 0..10 {
            qt.insert(
                glam::Vec2::new(i as f32 * 10.0 + 1.0, i as f32 * 10.0 + 1.0),
                i,
            );
        }
        assert_eq!(qt.len(), 10);
        // Query everything
        let all = qt.query_rect(&bounds);
        assert_eq!(all.len(), 10);
    }

    #[test]
    fn rect_contains_and_overlaps() {
        let r = Rect::new(glam::Vec2::ZERO, glam::Vec2::splat(10.0));
        assert!(r.contains_point(glam::Vec2::splat(5.0)));
        assert!(!r.contains_point(glam::Vec2::splat(11.0)));

        let r2 = Rect::new(glam::Vec2::splat(5.0), glam::Vec2::splat(15.0));
        assert!(r.overlaps(&r2));

        let r3 = Rect::new(glam::Vec2::splat(20.0), glam::Vec2::splat(30.0));
        assert!(!r.overlaps(&r3));
    }

    // --- V0.5b: Octree ---

    #[test]
    fn octree_empty() {
        let bounds = Aabb::new(Vec3::ZERO, Vec3::splat(100.0));
        let ot = Octree::new(bounds, 4, 8);
        assert!(ot.is_empty());
    }

    #[test]
    fn octree_insert_and_query() {
        let bounds = Aabb::new(Vec3::ZERO, Vec3::splat(100.0));
        let mut ot = Octree::new(bounds, 4, 8);
        for i in 0..20 {
            ot.insert(Vec3::new(i as f32 * 5.0 + 1.0, 50.0, 50.0), i);
        }
        assert_eq!(ot.len(), 20);

        let query = Aabb::new(Vec3::new(0.0, 40.0, 40.0), Vec3::new(20.0, 60.0, 60.0));
        let results = ot.query_aabb(&query);
        assert!(results.contains(&0)); // x=1
        assert!(results.contains(&1)); // x=6
        assert!(results.contains(&2)); // x=11
        assert!(results.contains(&3)); // x=16
        assert!(!results.contains(&10)); // x=51
    }

    #[test]
    fn octree_out_of_bounds() {
        let bounds = Aabb::new(Vec3::ZERO, Vec3::splat(10.0));
        let mut ot = Octree::new(bounds, 4, 8);
        ot.insert(Vec3::splat(100.0), 0);
        assert_eq!(ot.len(), 0);
    }

    #[test]
    fn octree_all_octants() {
        let bounds = Aabb::new(Vec3::splat(-10.0), Vec3::splat(10.0));
        let mut ot = Octree::new(bounds, 2, 4);
        // Insert one point per octant
        for octant in 0..8u32 {
            let x = if octant & 1 != 0 { 5.0 } else { -5.0 };
            let y = if octant & 2 != 0 { 5.0 } else { -5.0 };
            let z = if octant & 4 != 0 { 5.0 } else { -5.0 };
            ot.insert(Vec3::new(x, y, z), octant as usize);
        }
        assert_eq!(ot.len(), 8);
        // Query the entire space
        let all = ot.query_aabb(&bounds);
        assert_eq!(all.len(), 8);
    }

    // --- V0.5b: Spatial hash ---

    #[test]
    fn spatial_hash_empty() {
        let sh = SpatialHash::new(1.0).unwrap();
        assert!(sh.is_empty());
        assert_eq!(sh.cell_count(), 0);
    }

    #[test]
    fn spatial_hash_insert_and_query_cell() {
        let mut sh = SpatialHash::new(10.0).unwrap();
        sh.insert(Vec3::new(5.0, 5.0, 5.0), 0);
        sh.insert(Vec3::new(7.0, 3.0, 1.0), 1);
        sh.insert(Vec3::new(15.0, 5.0, 5.0), 2); // Different cell
        assert_eq!(sh.len(), 3);

        let cell = sh.query_cell(Vec3::new(5.0, 5.0, 5.0));
        assert!(cell.contains(&0));
        assert!(cell.contains(&1));
        assert!(!cell.contains(&2));
    }

    #[test]
    fn spatial_hash_query_radius() {
        let mut sh = SpatialHash::new(5.0).unwrap();
        for i in 0..20 {
            sh.insert(Vec3::new(i as f32, 0.0, 0.0), i);
        }
        // Query around x=10 with radius 3 => cells [1] and [2] (covering 5..15)
        let results = sh.query_radius(Vec3::new(10.0, 0.0, 0.0), 3.0);
        assert!(results.contains(&10));
        // Items in the same cell range should be candidates
        assert!(results.contains(&11));
    }

    #[test]
    fn spatial_hash_clear() {
        let mut sh = SpatialHash::new(1.0).unwrap();
        sh.insert(Vec3::ZERO, 0);
        sh.insert(Vec3::ONE, 1);
        assert_eq!(sh.len(), 2);
        sh.clear();
        assert!(sh.is_empty());
        assert_eq!(sh.cell_count(), 0);
    }

    #[test]
    fn spatial_hash_negative_coords() {
        let mut sh = SpatialHash::new(1.0).unwrap();
        sh.insert(Vec3::new(-5.0, -5.0, -5.0), 0);
        let cell = sh.query_cell(Vec3::new(-5.0, -5.0, -5.0));
        assert!(cell.contains(&0));
    }

    #[test]
    fn rect_serde_roundtrip() {
        let r = Rect::new(glam::Vec2::new(1.0, 2.0), glam::Vec2::new(3.0, 4.0));
        let json = serde_json::to_string(&r).unwrap();
        let r2: Rect = serde_json::from_str(&json).unwrap();
        assert_eq!(r, r2);
    }

    // --- V0.5b audit tests ---

    #[test]
    fn rect_size() {
        let r = Rect::new(glam::Vec2::new(1.0, 2.0), glam::Vec2::new(4.0, 6.0));
        let s = r.size();
        assert!(approx_eq(s.x, 3.0));
        assert!(approx_eq(s.y, 4.0));
    }

    #[test]
    fn quadtree_query_no_results() {
        let bounds = Rect::new(glam::Vec2::ZERO, glam::Vec2::splat(100.0));
        let mut qt = Quadtree::new(bounds, 4, 8);
        for i in 0..10 {
            qt.insert(glam::Vec2::new(i as f32, i as f32), i);
        }
        let query = Rect::new(glam::Vec2::splat(80.0), glam::Vec2::splat(90.0));
        assert!(qt.query_rect(&query).is_empty());
    }

    #[test]
    fn octree_query_all() {
        let bounds = Aabb::new(Vec3::ZERO, Vec3::splat(10.0));
        let mut ot = Octree::new(bounds, 4, 8);
        for i in 0..5 {
            ot.insert(Vec3::splat(i as f32 + 1.0), i);
        }
        // Query the full bounds should return everything
        let all = ot.query_aabb(&bounds);
        assert_eq!(all.len(), 5);
    }

    #[test]
    fn spatial_hash_many_in_one_cell() {
        let mut sh = SpatialHash::new(100.0).unwrap(); // Huge cell
        for i in 0..50 {
            sh.insert(Vec3::new(i as f32, 0.0, 0.0), i); // All in one cell
        }
        let cell = sh.query_cell(Vec3::ZERO);
        assert_eq!(cell.len(), 50);
    }

    #[test]
    fn quadtree_max_depth_prevents_infinite_split() {
        let bounds = Rect::new(glam::Vec2::ZERO, glam::Vec2::splat(10.0));
        let mut qt = Quadtree::new(bounds, 1, 3); // Split after 1 item, max 3 levels
        // Insert many items at the same spot — should stop splitting at depth 3
        for i in 0..20 {
            qt.insert(glam::Vec2::splat(5.0), i);
        }
        assert_eq!(qt.len(), 20);
        let all = qt.query_rect(&bounds);
        assert_eq!(all.len(), 20);
    }

    #[test]
    fn octree_max_depth_prevents_infinite_split() {
        let bounds = Aabb::new(Vec3::ZERO, Vec3::splat(10.0));
        let mut ot = Octree::new(bounds, 1, 3);
        for i in 0..20 {
            ot.insert(Vec3::splat(5.0), i);
        }
        assert_eq!(ot.len(), 20);
        let all = ot.query_aabb(&bounds);
        assert_eq!(all.len(), 20);
    }

    // --- V0.5c: Convex hull ---

    #[test]
    fn convex_hull_square() {
        let pts = vec![
            glam::Vec2::new(0.0, 0.0),
            glam::Vec2::new(1.0, 0.0),
            glam::Vec2::new(1.0, 1.0),
            glam::Vec2::new(0.0, 1.0),
            glam::Vec2::new(0.5, 0.5), // Interior point
        ];
        let hull = convex_hull_2d(&pts);
        assert_eq!(hull.len(), 4); // Only the 4 corners
    }

    #[test]
    fn convex_hull_triangle() {
        let pts = vec![
            glam::Vec2::new(0.0, 0.0),
            glam::Vec2::new(2.0, 0.0),
            glam::Vec2::new(1.0, 2.0),
        ];
        let hull = convex_hull_2d(&pts);
        assert_eq!(hull.len(), 3);
    }

    #[test]
    fn convex_hull_collinear() {
        // All points on a line — hull should be just the endpoints
        let pts = vec![
            glam::Vec2::new(0.0, 0.0),
            glam::Vec2::new(1.0, 0.0),
            glam::Vec2::new(2.0, 0.0),
            glam::Vec2::new(3.0, 0.0),
        ];
        let hull = convex_hull_2d(&pts);
        assert_eq!(hull.len(), 2);
    }

    #[test]
    fn convex_hull_single_point() {
        let pts = vec![glam::Vec2::new(5.0, 5.0)];
        let hull = convex_hull_2d(&pts);
        assert_eq!(hull.len(), 1);
    }

    #[test]
    fn convex_hull_many_interior() {
        // Circle of points + many interior
        let mut pts = Vec::new();
        for i in 0..8 {
            let angle = i as f32 * std::f32::consts::TAU / 8.0;
            pts.push(glam::Vec2::new(angle.cos() * 10.0, angle.sin() * 10.0));
        }
        // Add 50 interior points
        for i in 0..50 {
            let x = (i % 10) as f32 - 5.0;
            let y = (i / 10) as f32 - 2.5;
            pts.push(glam::Vec2::new(x, y));
        }
        let hull = convex_hull_2d(&pts);
        assert_eq!(hull.len(), 8); // Only the circle points
    }

    // --- V0.5c: GJK ---

    fn make_square(cx: f32, cy: f32, half: f32) -> ConvexPolygon {
        ConvexPolygon::new(vec![
            glam::Vec2::new(cx - half, cy - half),
            glam::Vec2::new(cx + half, cy - half),
            glam::Vec2::new(cx + half, cy + half),
            glam::Vec2::new(cx - half, cy + half),
        ])
        .unwrap()
    }

    #[test]
    fn gjk_overlapping_squares() {
        let a = make_square(0.0, 0.0, 1.0);
        let b = make_square(0.5, 0.5, 1.0);
        assert!(gjk_intersect(&a, &b));
    }

    #[test]
    fn gjk_separated_squares() {
        let a = make_square(0.0, 0.0, 1.0);
        let b = make_square(5.0, 0.0, 1.0);
        assert!(!gjk_intersect(&a, &b));
    }

    #[test]
    fn gjk_touching_squares() {
        let a = make_square(0.0, 0.0, 1.0);
        let b = make_square(2.0, 0.0, 1.0); // Touching at x=1
        // Touching = on boundary, may or may not register depending on epsilon
        // GJK typically returns true for touching
        let _ = gjk_intersect(&a, &b); // Just verify no panic
    }

    #[test]
    fn gjk_contained() {
        let a = make_square(0.0, 0.0, 5.0); // Big
        let b = make_square(0.0, 0.0, 1.0); // Small, inside A
        assert!(gjk_intersect(&a, &b));
    }

    #[test]
    fn gjk_same_shape() {
        let a = make_square(0.0, 0.0, 1.0);
        let b = make_square(0.0, 0.0, 1.0);
        assert!(gjk_intersect(&a, &b));
    }

    // --- V0.5c: EPA ---

    #[test]
    fn epa_overlapping_squares_depth() {
        let a = make_square(0.0, 0.0, 1.0);
        let b = make_square(1.0, 0.0, 1.0); // Overlap of 1.0 in X
        let pen = gjk_epa(&a, &b);
        assert!(pen.is_some());
        let p = pen.unwrap();
        assert!(p.depth > 0.0);
        assert!(p.depth < 2.1); // At most full overlap
    }

    #[test]
    fn epa_no_overlap() {
        let a = make_square(0.0, 0.0, 1.0);
        let b = make_square(5.0, 0.0, 1.0);
        assert!(gjk_epa(&a, &b).is_none());
    }

    #[test]
    fn epa_deep_overlap() {
        let a = make_square(0.0, 0.0, 2.0);
        let b = make_square(0.5, 0.0, 2.0); // 3.5 overlap in X
        let pen = gjk_epa(&a, &b).unwrap();
        assert!(pen.depth > 0.0);
    }

    #[test]
    fn convex_support_polygon() {
        let poly = make_square(0.0, 0.0, 1.0);
        let s = poly.support(glam::Vec2::X);
        assert!(approx_eq(s.x, 1.0));
        let s = poly.support(-glam::Vec2::Y);
        assert!(approx_eq(s.y, -1.0));
    }

    // --- V0.5c audit tests ---

    #[test]
    fn epa_depth_always_positive() {
        let a = make_square(0.0, 0.0, 1.0);
        let b = make_square(0.5, 0.0, 1.0);
        let pen = gjk_epa(&a, &b).unwrap();
        assert!(pen.depth > 0.0);
        assert!(pen.depth <= 2.0); // Can't exceed full overlap
    }

    #[test]
    fn convex_hull_empty() {
        let hull = convex_hull_2d(&[]);
        assert!(hull.is_empty());
    }

    #[test]
    fn convex_hull_two_points() {
        let pts = vec![glam::Vec2::ZERO, glam::Vec2::new(5.0, 0.0)];
        let hull = convex_hull_2d(&pts);
        assert_eq!(hull.len(), 2);
    }

    #[test]
    fn gjk_triangles() {
        let a = ConvexPolygon::new(vec![
            glam::Vec2::new(0.0, 0.0),
            glam::Vec2::new(2.0, 0.0),
            glam::Vec2::new(1.0, 2.0),
        ])
        .unwrap();
        let b = ConvexPolygon::new(vec![
            glam::Vec2::new(1.0, 0.0),
            glam::Vec2::new(3.0, 0.0),
            glam::Vec2::new(2.0, 2.0),
        ])
        .unwrap();
        assert!(gjk_intersect(&a, &b)); // Overlapping triangles
    }

    #[test]
    fn penetration_serde_roundtrip() {
        let p = Penetration {
            normal: glam::Vec2::new(1.0, 0.0),
            depth: 0.5,
        };
        let json = serde_json::to_string(&p).unwrap();
        let p2: Penetration = serde_json::from_str(&json).unwrap();
        assert_eq!(p, p2);
    }

    #[test]
    fn gjk_epa_symmetric() {
        // gjk_epa(a, b) and gjk_epa(b, a) should both detect collision
        let a = make_square(0.0, 0.0, 1.0);
        let b = make_square(0.5, 0.5, 1.0);
        assert!(gjk_epa(&a, &b).is_some());
        assert!(gjk_epa(&b, &a).is_some());
    }

    // --- V1.0b: Display impls ---

    #[test]
    fn ray_display() {
        let r = Ray::new(Vec3::new(1.0, 0.0, 0.0), Vec3::X).unwrap();
        assert_eq!(r.to_string(), "Ray(1, 0, 0 -> 1, 0, 0)");
    }

    #[test]
    fn ray_display_precision() {
        let r = Ray::new(Vec3::new(1.0, 2.0, 3.0), Vec3::X).unwrap();
        assert_eq!(format!("{r:.1}"), "Ray(1.0, 2.0, 3.0 -> 1.0, 0.0, 0.0)");
    }

    #[test]
    fn plane_display() {
        let p = Plane::from_point_normal(Vec3::ZERO, Vec3::Y).unwrap();
        assert_eq!(p.to_string(), "Plane(n=(0, 1, 0), d=0)");
    }

    #[test]
    fn plane_display_precision() {
        let p = Plane::from_point_normal(Vec3::ZERO, Vec3::Y).unwrap();
        assert_eq!(format!("{p:.2}"), "Plane(n=(0.00, 1.00, 0.00), d=0.00)");
    }

    #[test]
    fn aabb_display() {
        let a = Aabb::new(Vec3::ZERO, Vec3::ONE);
        assert_eq!(a.to_string(), "Aabb((0, 0, 0)..(1, 1, 1))");
    }

    #[test]
    fn sphere_display() {
        let sp = Sphere::new(Vec3::new(1.0, 2.0, 3.0), 5.0).unwrap();
        assert_eq!(sp.to_string(), "Sphere((1, 2, 3), r=5)");
    }

    #[test]
    fn sphere_display_precision() {
        let sp = Sphere::new(Vec3::ZERO, 2.5).unwrap();
        assert_eq!(format!("{sp:.1}"), "Sphere((0.0, 0.0, 0.0), r=2.5)");
    }

    #[test]
    fn triangle_display() {
        let t = Triangle::new(Vec3::ZERO, Vec3::X, Vec3::Y);
        assert_eq!(t.to_string(), "Triangle((0, 0, 0), (1, 0, 0), (0, 1, 0))");
    }

    // --- V1.0b: Rect::merge / Rect::area ---

    #[test]
    fn rect_merge() {
        let a = Rect::new(glam::Vec2::ZERO, glam::Vec2::ONE);
        let b = Rect::new(glam::Vec2::new(2.0, 2.0), glam::Vec2::new(3.0, 3.0));
        let m = a.merge(&b);
        assert_eq!(m.min, glam::Vec2::ZERO);
        assert_eq!(m.max, glam::Vec2::new(3.0, 3.0));
    }

    #[test]
    fn rect_merge_overlapping() {
        let a = Rect::new(glam::Vec2::ZERO, glam::Vec2::new(2.0, 2.0));
        let b = Rect::new(glam::Vec2::ONE, glam::Vec2::new(3.0, 3.0));
        let m = a.merge(&b);
        assert_eq!(m.min, glam::Vec2::ZERO);
        assert_eq!(m.max, glam::Vec2::new(3.0, 3.0));
    }

    #[test]
    fn rect_area() {
        let r = Rect::new(glam::Vec2::ZERO, glam::Vec2::new(3.0, 4.0));
        assert!((r.area() - 12.0).abs() < 1e-6);
    }

    #[test]
    fn rect_area_zero() {
        let r = Rect::new(glam::Vec2::ZERO, glam::Vec2::new(5.0, 0.0));
        assert!((r.area()).abs() < 1e-6);
    }

    // --- Audit: edge-case hardening ---

    #[test]
    fn plane_from_point_normal_zero_normal_errors() {
        let result = Plane::from_point_normal(Vec3::ZERO, Vec3::ZERO);
        assert!(result.is_err());
    }

    #[test]
    fn segment_direction_degenerate_no_nan() {
        let s = Segment::new(Vec3::splat(5.0), Vec3::splat(5.0));
        let d = s.direction();
        assert!(!d.x.is_nan() && !d.y.is_nan() && !d.z.is_nan());
        assert!(approx_eq(d.length(), 1.0));
    }

    #[test]
    fn triangle_unit_normal_degenerate_no_nan() {
        let t = Triangle::new(Vec3::ZERO, Vec3::ZERO, Vec3::ZERO);
        let n = t.unit_normal();
        assert!(!n.x.is_nan() && !n.y.is_nan() && !n.z.is_nan());
        assert!(approx_eq(n.length(), 1.0));
    }

    #[test]
    fn triangle_unit_normal_collinear_no_nan() {
        let t = Triangle::new(Vec3::ZERO, Vec3::X, Vec3::new(2.0, 0.0, 0.0));
        let n = t.unit_normal();
        assert!(!n.x.is_nan() && !n.y.is_nan() && !n.z.is_nan());
        assert!(approx_eq(n.length(), 1.0));
    }

    // --- 3D GJK / EPA tests ---

    fn make_box_3d(center: Vec3, half: f32) -> ConvexHull3D {
        let h = Vec3::splat(half);
        ConvexHull3D::new(vec![
            center + Vec3::new(-h.x, -h.y, -h.z),
            center + Vec3::new(h.x, -h.y, -h.z),
            center + Vec3::new(-h.x, h.y, -h.z),
            center + Vec3::new(h.x, h.y, -h.z),
            center + Vec3::new(-h.x, -h.y, h.z),
            center + Vec3::new(h.x, -h.y, h.z),
            center + Vec3::new(-h.x, h.y, h.z),
            center + Vec3::new(h.x, h.y, h.z),
        ])
        .unwrap()
    }

    #[test]
    fn gjk3d_overlapping_boxes() {
        let a = make_box_3d(Vec3::ZERO, 1.0);
        let b = make_box_3d(Vec3::new(0.5, 0.0, 0.0), 1.0);
        assert!(gjk_intersect_3d(&a, &b));
    }

    #[test]
    fn gjk3d_separated_boxes() {
        let a = make_box_3d(Vec3::ZERO, 1.0);
        let b = make_box_3d(Vec3::new(5.0, 0.0, 0.0), 1.0);
        assert!(!gjk_intersect_3d(&a, &b));
    }

    #[test]
    fn gjk3d_same_shape() {
        let a = make_box_3d(Vec3::ZERO, 1.0);
        assert!(gjk_intersect_3d(&a, &a));
    }

    #[test]
    fn gjk3d_sphere_sphere() {
        let a = Sphere::new(Vec3::ZERO, 1.0).unwrap();
        let b = Sphere::new(Vec3::new(1.5, 0.0, 0.0), 1.0).unwrap();
        assert!(gjk_intersect_3d(&a, &b));

        let c = Sphere::new(Vec3::new(5.0, 0.0, 0.0), 1.0).unwrap();
        assert!(!gjk_intersect_3d(&a, &c));
    }

    #[test]
    fn gjk3d_sphere_box() {
        let a = Sphere::new(Vec3::ZERO, 1.0).unwrap();
        let b = make_box_3d(Vec3::new(0.5, 0.0, 0.0), 0.5);
        assert!(gjk_intersect_3d(&a, &b));
    }

    #[test]
    fn gjk_epa_3d_penetration() {
        // Use spheres for cleaner EPA (non-degenerate simplex)
        let a = Sphere::new(Vec3::ZERO, 1.0).unwrap();
        let b = Sphere::new(Vec3::new(1.0, 0.0, 0.0), 1.0).unwrap();
        assert!(gjk_intersect_3d(&a, &b));
        let pen = gjk_epa_3d(&a, &b);
        assert!(pen.is_some());
        let p = pen.unwrap();
        assert!(p.depth > 0.0);
        assert!(p.depth < 2.0); // Can't penetrate more than sum of radii
    }

    #[test]
    fn gjk_epa_3d_no_overlap() {
        let a = make_box_3d(Vec3::ZERO, 1.0);
        let b = make_box_3d(Vec3::new(5.0, 0.0, 0.0), 1.0);
        assert!(gjk_epa_3d(&a, &b).is_none());
    }

    #[test]
    fn gjk3d_convex_hull_empty_errors() {
        assert!(ConvexHull3D::new(vec![]).is_err());
    }

    // --- OBB tests ---

    #[test]
    fn obb_contains_center() {
        let obb = Obb::new(Vec3::new(1.0, 2.0, 3.0), Vec3::ONE, glam::Quat::IDENTITY);
        assert!(obb.contains_point(Vec3::new(1.0, 2.0, 3.0)));
    }

    #[test]
    fn obb_contains_corner() {
        let obb = Obb::new(Vec3::ZERO, Vec3::ONE, glam::Quat::IDENTITY);
        assert!(obb.contains_point(Vec3::ONE));
        assert!(!obb.contains_point(Vec3::splat(1.5)));
    }

    #[test]
    fn obb_rotated_containment() {
        let rot = glam::Quat::from_rotation_z(std::f32::consts::FRAC_PI_4);
        let obb = Obb::new(Vec3::ZERO, Vec3::new(2.0, 0.5, 1.0), rot);
        // Center should always be inside
        assert!(obb.contains_point(Vec3::ZERO));
    }

    #[test]
    fn obb_closest_point() {
        let obb = Obb::new(Vec3::ZERO, Vec3::ONE, glam::Quat::IDENTITY);
        let cp = obb.closest_point(Vec3::new(5.0, 0.0, 0.0));
        assert!(approx_eq(cp.x, 1.0));
        assert!(approx_eq(cp.y, 0.0));
    }

    #[test]
    fn ray_obb_hit() {
        let obb = Obb::new(Vec3::new(5.0, 0.0, 0.0), Vec3::ONE, glam::Quat::IDENTITY);
        let r = Ray::new(Vec3::ZERO, Vec3::X).unwrap();
        let t = ray_obb(&r, &obb);
        assert!(t.is_some());
        assert!(approx_eq(t.unwrap(), 4.0));
    }

    #[test]
    fn ray_obb_miss() {
        let obb = Obb::new(Vec3::new(5.0, 0.0, 0.0), Vec3::ONE, glam::Quat::IDENTITY);
        let r = Ray::new(Vec3::ZERO, Vec3::Y).unwrap();
        assert!(ray_obb(&r, &obb).is_none());
    }

    #[test]
    fn gjk3d_obb_vs_obb() {
        let a = Obb::new(Vec3::ZERO, Vec3::ONE, glam::Quat::IDENTITY);
        let b = Obb::new(Vec3::new(1.5, 0.0, 0.0), Vec3::ONE, glam::Quat::IDENTITY);
        assert!(gjk_intersect_3d(&a, &b));

        let c = Obb::new(Vec3::new(5.0, 0.0, 0.0), Vec3::ONE, glam::Quat::IDENTITY);
        assert!(!gjk_intersect_3d(&a, &c));
    }

    // --- Capsule tests ---

    #[test]
    fn capsule_contains_center() {
        let cap = Capsule::new(Vec3::ZERO, Vec3::new(0.0, 2.0, 0.0), 0.5).unwrap();
        assert!(cap.contains_point(Vec3::new(0.0, 1.0, 0.0)));
    }

    #[test]
    fn capsule_contains_near_surface() {
        let cap = Capsule::new(Vec3::ZERO, Vec3::new(0.0, 2.0, 0.0), 1.0).unwrap();
        assert!(cap.contains_point(Vec3::new(0.9, 1.0, 0.0)));
        assert!(!cap.contains_point(Vec3::new(2.0, 1.0, 0.0)));
    }

    #[test]
    fn capsule_negative_radius_errors() {
        assert!(Capsule::new(Vec3::ZERO, Vec3::Y, -1.0).is_err());
    }

    #[test]
    fn capsule_axis_length() {
        let cap = Capsule::new(Vec3::ZERO, Vec3::new(0.0, 3.0, 0.0), 0.5).unwrap();
        assert!(approx_eq(cap.axis_length(), 3.0));
    }

    #[test]
    fn ray_capsule_hit() {
        let cap = Capsule::new(Vec3::new(5.0, -1.0, 0.0), Vec3::new(5.0, 1.0, 0.0), 0.5).unwrap();
        let r = Ray::new(Vec3::ZERO, Vec3::X).unwrap();
        let t = ray_capsule(&r, &cap);
        assert!(t.is_some());
        assert!(t.unwrap() > 4.0 && t.unwrap() < 5.0);
    }

    #[test]
    fn ray_capsule_miss() {
        let cap = Capsule::new(Vec3::new(5.0, -1.0, 0.0), Vec3::new(5.0, 1.0, 0.0), 0.5).unwrap();
        let r = Ray::new(Vec3::ZERO, Vec3::Y).unwrap();
        assert!(ray_capsule(&r, &cap).is_none());
    }

    #[test]
    fn gjk3d_capsule_vs_sphere() {
        let cap = Capsule::new(Vec3::ZERO, Vec3::new(0.0, 2.0, 0.0), 0.5).unwrap();
        let sphere = Sphere::new(Vec3::new(1.0, 1.0, 0.0), 1.0).unwrap();
        assert!(gjk_intersect_3d(&cap, &sphere));

        let far = Sphere::new(Vec3::new(10.0, 0.0, 0.0), 0.5).unwrap();
        assert!(!gjk_intersect_3d(&cap, &far));
    }

    // --- 3D GJK winding stress tests ---

    #[test]
    fn gjk3d_all_axis_offsets() {
        // Test overlap detection along all 6 axis directions to stress winding
        let a = make_box_3d(Vec3::ZERO, 1.0);
        for &offset in &[
            Vec3::new(1.5, 0.0, 0.0),
            Vec3::new(-1.5, 0.0, 0.0),
            Vec3::new(0.0, 1.5, 0.0),
            Vec3::new(0.0, -1.5, 0.0),
            Vec3::new(0.0, 0.0, 1.5),
            Vec3::new(0.0, 0.0, -1.5),
        ] {
            let b = make_box_3d(offset, 1.0);
            assert!(
                gjk_intersect_3d(&a, &b),
                "should overlap at offset {offset:?}"
            );
        }
        for &offset in &[
            Vec3::new(3.0, 0.0, 0.0),
            Vec3::new(-3.0, 0.0, 0.0),
            Vec3::new(0.0, 3.0, 0.0),
            Vec3::new(0.0, -3.0, 0.0),
            Vec3::new(0.0, 0.0, 3.0),
            Vec3::new(0.0, 0.0, -3.0),
        ] {
            let b = make_box_3d(offset, 1.0);
            assert!(
                !gjk_intersect_3d(&a, &b),
                "should NOT overlap at offset {offset:?}"
            );
        }
    }

    #[test]
    fn gjk3d_diagonal_overlap() {
        let a = make_box_3d(Vec3::ZERO, 1.0);
        let b = make_box_3d(Vec3::new(1.0, 1.0, 1.0), 1.0);
        assert!(gjk_intersect_3d(&a, &b));
    }

    #[test]
    fn gjk3d_asymmetric_shapes() {
        // Tall capsule vs small sphere at different positions
        let cap = Capsule::new(Vec3::new(0.0, -5.0, 0.0), Vec3::new(0.0, 5.0, 0.0), 0.5).unwrap();
        // Near the middle of the capsule
        let s1 = Sphere::new(Vec3::new(1.0, 0.0, 0.0), 1.0).unwrap();
        assert!(gjk_intersect_3d(&cap, &s1));
        // Near the top cap
        let s2 = Sphere::new(Vec3::new(0.0, 5.5, 0.0), 0.5).unwrap();
        assert!(gjk_intersect_3d(&cap, &s2));
        // Well beyond the bottom
        let s3 = Sphere::new(Vec3::new(0.0, -7.0, 0.0), 0.5).unwrap();
        assert!(!gjk_intersect_3d(&cap, &s3));
    }

    #[test]
    fn gjk_epa_3d_symmetric_penetration() {
        // Deeper overlap for cleaner EPA convergence
        let a = make_box_3d(Vec3::ZERO, 2.0);
        let b = make_box_3d(Vec3::new(1.0, 0.0, 0.0), 2.0);
        let p1 = gjk_epa_3d(&a, &b);
        let p2 = gjk_epa_3d(&b, &a);
        // Both should detect overlap (GJK) — EPA may or may not produce depth
        assert!(gjk_intersect_3d(&a, &b));
        assert!(gjk_intersect_3d(&b, &a));
        // If both EPA succeed, depths should be in the same ballpark
        if let (Some(pen1), Some(pen2)) = (p1, p2) {
            assert!(pen1.depth > 0.0);
            assert!(pen2.depth > 0.0);
        }
    }
}
