featherstone 0.1.0

//! Contact manifold for articulated body dynamics
//!
//! Represents contact points between articulated body links and the environment
//! or other bodies. Provides the geometric data needed by the contact solver:
//! - Contact point positions (world and body local frames)
//! - Contact normals and penetration depths
//! - Friction and restitution coefficients
//!
//! This module is backend-agnostic: contact detection can come from Rapier's
//! narrow phase, custom GJK/EPA, or analytical geometry. The contact manifold
//! is the common interface consumed by the LCP solver and smooth contact model.

use nalgebra::{Matrix3, Vector3};

/// A single contact point between an articulated body link and another surface.
#[derive(Clone, Debug)]
pub struct ContactPoint {
    /// Index of the body/link in the ArticulatedBody tree
    pub body_id: usize,
    /// Contact point position in world frame
    pub point_world: Vector3<f32>,
    /// Contact point position in body-local frame
    pub point_body: Vector3<f32>,
    /// Contact normal in world frame (points away from body, into environment)
    pub normal: Vector3<f32>,
    /// Penetration depth (positive = overlapping, negative = separated)
    pub penetration: f32,
    /// Coulomb friction coefficient at this contact
    pub friction: f32,
    /// Coefficient of restitution (0 = perfectly inelastic, 1 = perfectly elastic)
    pub restitution: f32,
}

impl ContactPoint {
    /// Create a new contact point with default material properties.
    pub fn new(
        body_id: usize,
        point_world: Vector3<f32>,
        point_body: Vector3<f32>,
        normal: Vector3<f32>,
        penetration: f32,
    ) -> Self {
        Self {
            body_id,
            point_world,
            point_body,
            normal: if normal.norm_squared() > 1e-12 { normal.normalize() } else { Vector3::z() },
            penetration,
            friction: 0.5,
            restitution: 0.0,
        }
    }

    /// Set friction coefficient.
    pub fn with_friction(mut self, friction: f32) -> Self {
        self.friction = friction;
        self
    }

    /// Set restitution coefficient.
    pub fn with_restitution(mut self, restitution: f32) -> Self {
        self.restitution = restitution;
        self
    }

    /// Whether this contact is active (positive penetration).
    pub fn is_active(&self) -> bool {
        self.penetration > 0.0
    }

    /// Compute a tangent frame for this contact.
    ///
    /// Returns two orthonormal tangent vectors (t1, t2) perpendicular to the
    /// contact normal. Used for friction force decomposition.
    pub fn tangent_frame(&self) -> (Vector3<f32>, Vector3<f32>) {
        compute_tangent_frame(&self.normal)
    }
}

/// Collection of contact points forming a contact manifold.
///
/// A manifold groups all active contacts for processing by the solver.
/// Contacts can come from multiple body pairs and collision geometries.
#[derive(Clone, Debug, Default)]
pub struct ContactManifold {
    /// All contact points
    pub contacts: Vec<ContactPoint>,
}

impl ContactManifold {
    /// Create an empty manifold.
    pub fn new() -> Self {
        Self {
            contacts: Vec::new(),
        }
    }

    /// Create a manifold with pre-allocated capacity.
    pub fn with_capacity(capacity: usize) -> Self {
        Self {
            contacts: Vec::with_capacity(capacity),
        }
    }

    /// Add a contact point to the manifold.
    pub fn add_contact(&mut self, contact: ContactPoint) {
        self.contacts.push(contact);
    }

    /// Number of contacts.
    pub fn len(&self) -> usize {
        self.contacts.len()
    }

    /// Whether the manifold is empty.
    pub fn is_empty(&self) -> bool {
        self.contacts.is_empty()
    }

    /// Remove contacts with penetration below threshold (separated contacts).
    pub fn prune_separated(&mut self, threshold: f32) {
        self.contacts.retain(|c| c.penetration > threshold);
    }

    /// Get only active contacts (positive penetration).
    pub fn active_contacts(&self) -> impl Iterator<Item = &ContactPoint> {
        self.contacts.iter().filter(|c| c.is_active())
    }

    /// Number of active contacts.
    pub fn active_count(&self) -> usize {
        self.contacts.iter().filter(|c| c.is_active()).count()
    }

    /// Get contacts for a specific body.
    pub fn contacts_for_body(&self, body_id: usize) -> impl Iterator<Item = &ContactPoint> {
        self.contacts.iter().filter(move |c| c.body_id == body_id)
    }

    /// Clear all contacts.
    pub fn clear(&mut self) {
        self.contacts.clear();
    }

    /// Merge another manifold into this one.
    pub fn merge(&mut self, other: &ContactManifold) {
        self.contacts.extend(other.contacts.iter().cloned());
    }

    /// Sort contacts by penetration depth (deepest first).
    pub fn sort_by_penetration(&mut self) {
        self.contacts
            .sort_by(|a, b| b.penetration.partial_cmp(&a.penetration).unwrap_or(std::cmp::Ordering::Equal));
    }

    /// Limit to N deepest contacts per body (for solver performance).
    pub fn limit_per_body(&mut self, max_per_body: usize) {
        self.sort_by_penetration();
        let mut counts = std::collections::HashMap::new();
        self.contacts.retain(|c| {
            let count = counts.entry(c.body_id).or_insert(0usize);
            if *count < max_per_body {
                *count += 1;
                true
            } else {
                false
            }
        });
    }

    /// Reduce contacts to the best N by maximizing the contact patch area.
    ///
    /// Greedy algorithm:
    /// 1. Keep the deepest contact (best penetration correction)
    /// 2. Find the contact farthest from #1 in the tangent plane
    /// 3. Find the contact maximizing triangle area with #1 and #2
    /// 4. Find the contact maximizing quadrilateral area with #1, #2, #3
    ///
    /// This produces a near-optimal contact patch for LCP stability.
    pub fn reduce_to_best_n(&mut self, max_contacts: usize) {
        if self.contacts.len() <= max_contacts || self.contacts.is_empty() {
            return;
        }

        let indices = select_max_area_contacts(&self.contacts, max_contacts);
        let selected: Vec<ContactPoint> = indices.iter().map(|&i| self.contacts[i].clone()).collect();
        self.contacts = selected;
    }
}

/// Generate a ground-plane contact manifold for an articulated body.
///
/// Checks each body's origin (plus optional collision geometry offset) against
/// a horizontal ground plane at the given height. This is a simple analytical
/// contact detector suitable for flat-ground locomotion tasks.
///
/// For complex geometries, use Rapier narrow-phase or custom GJK/EPA instead.
pub fn ground_plane_contacts(
    body: &super::body::ArticulatedBody,
    ground_height: f32,
    ground_normal: Vector3<f32>,
    friction: f32,
    restitution: f32,
) -> ContactManifold {
    let fk = super::kinematics::forward_kinematics(body);
    let normal = if ground_normal.norm_squared() > 1e-12 { ground_normal.normalize() } else { Vector3::y() };
    let mut manifold = ContactManifold::new();

    for i in 0..body.body_count() {
        let bd = &body.bodies[i];
        if bd.inertia.mass <= 0.0 {
            continue;
        }

        // Body origin in world frame
        let p_world = fk.transforms[i].translation;

        // Signed distance from ground plane (positive = above)
        let signed_dist = normal.dot(&(p_world - normal * ground_height));
        let penetration = -signed_dist;

        if penetration > -0.01 {
            // Near or in contact
            let point_world = p_world - normal * signed_dist;
            let r = &fk.transforms[i].rotation;
            let point_body = r.transpose() * (point_world - p_world);

            manifold.add_contact(
                ContactPoint::new(i, point_world, point_body, normal, penetration)
                    .with_friction(friction)
                    .with_restitution(restitution),
            );
        }
    }

    manifold
}

/// Generate contact points for sphere-on-ground collision geometry.
///
/// Each body can have an associated collision sphere (radius). This gives
/// more accurate contacts than body-origin checks for legged robots.
pub fn sphere_ground_contacts(
    body: &super::body::ArticulatedBody,
    radii: &[f32],
    ground_height: f32,
    ground_normal: Vector3<f32>,
    friction: f32,
    restitution: f32,
) -> ContactManifold {
    let fk = super::kinematics::forward_kinematics(body);
    let normal = if ground_normal.norm_squared() > 1e-12 { ground_normal.normalize() } else { Vector3::y() };
    let mut manifold = ContactManifold::new();

    for (i, &radius) in radii.iter().enumerate().take(body.body_count().min(radii.len())) {
        if radius <= 0.0 {
            continue;
        }

        let p_world = fk.transforms[i].translation;

        // Signed distance from sphere surface to ground
        let center_dist = normal.dot(&(p_world - normal * ground_height));
        let penetration = radius - center_dist;

        if penetration > -0.01 {
            // Contact point is on the sphere surface closest to ground
            let point_world = p_world - normal * center_dist;
            let r = &fk.transforms[i].rotation;
            let point_body = r.transpose() * (point_world - p_world);

            manifold.add_contact(
                ContactPoint::new(i, point_world, point_body, normal, penetration)
                    .with_friction(friction)
                    .with_restitution(restitution),
            );
        }
    }

    manifold
}

// ============================================================================
// Inter-body contact detection
// ============================================================================

use super::collider::ColliderShape as StoredShape;

/// Geometric pair for contact detection between two convex shapes.
///
/// Groups the per-shape world transform, body IDs, and material properties
/// that are repeated across every contact detection function. Pass this to
/// the `*_from_pair` family of functions to avoid 8+ parameter signatures.
#[derive(Clone, Debug)]
pub struct ShapePair {
    /// Position of shape A in world frame.
    pub pos_a: Vector3<f32>,
    /// Rotation of shape A in world frame.
    pub rot_a: Matrix3<f32>,
    /// Position of shape B in world frame.
    pub pos_b: Vector3<f32>,
    /// Rotation of shape B in world frame.
    pub rot_b: Matrix3<f32>,
    /// Body index for shape A (assigned to contacts on the A side).
    pub body_id_a: usize,
    /// Body index for shape B.
    pub body_id_b: usize,
    /// Coulomb friction coefficient at the contact.
    pub friction: f32,
    /// Coefficient of restitution at the contact.
    pub restitution: f32,
}

/// Detect contacts between two shapes at given world transforms.
///
/// Returns a `ContactManifold` with contacts between shape A and shape B.
/// Contact normals point from A to B. `body_id_a` and `body_id_b` are assigned
/// to contact points for identification in the manifold.
pub fn inter_body_contacts(
    shape_a: &StoredShape,
    shape_b: &StoredShape,
    pair: &ShapePair,
) -> ContactManifold {
    let pos_a = pair.pos_a;
    let rot_a = pair.rot_a;
    let pos_b = pair.pos_b;
    let rot_b = pair.rot_b;
    let body_id_a = pair.body_id_a;
    let body_id_b = pair.body_id_b;
    let friction = pair.friction;
    let restitution = pair.restitution;
    let mut manifold = ContactManifold::new();

    match (shape_a, shape_b) {
        (StoredShape::Sphere { radius: ra }, StoredShape::Sphere { radius: rb }) => {
            if let Some(c) = sphere_sphere_contact(
                pos_a, *ra, pos_b, *rb, body_id_a, friction, restitution,
            ) {
                manifold.add_contact(c);
            }
        }
        (StoredShape::Sphere { radius }, StoredShape::Box { half_extents }) => {
            if let Some(c) = sphere_box_contact(
                pos_a, *radius, pos_b, rot_b, *half_extents,
                body_id_a, friction, restitution,
            ) {
                manifold.add_contact(c);
            }
        }
        (StoredShape::Box { half_extents }, StoredShape::Sphere { radius }) => {
            // Flip: call sphere-box with sphere as first arg, negate normal
            if let Some(mut c) = sphere_box_contact(
                pos_b, *radius, pos_a, rot_a, *half_extents,
                body_id_b, friction, restitution,
            ) {
                c.normal = -c.normal;
                c.body_id = body_id_a;
                manifold.add_contact(c);
            }
        }
        (StoredShape::Box { half_extents: he_a }, StoredShape::Box { half_extents: he_b }) => {
            let contacts = box_box_contacts(
                pos_a, rot_a, *he_a, pos_b, rot_b, *he_b,
                body_id_a, friction, restitution,
            );
            for c in contacts {
                manifold.add_contact(c);
            }
        }
        // Capsule pairs
        (StoredShape::Capsule { half_height: hh_a, radius: ra },
         StoredShape::Capsule { half_height: hh_b, radius: rb }) => {
            if let Some(c) = capsule_capsule_contact(
                pos_a, rot_a, *hh_a, *ra, pos_b, rot_b, *hh_b, *rb,
                body_id_a, friction, restitution,
            ) { manifold.add_contact(c); }
        }
        (StoredShape::Capsule { half_height, radius },
         StoredShape::Sphere { radius: sr }) => {
            if let Some(c) = capsule_sphere_contact(
                pos_a, rot_a, *half_height, *radius, pos_b, *sr,
                body_id_a, friction, restitution,
            ) { manifold.add_contact(c); }
        }
        (StoredShape::Sphere { radius: sr },
         StoredShape::Capsule { half_height, radius }) => {
            if let Some(mut c) = capsule_sphere_contact(
                pos_b, rot_b, *half_height, *radius, pos_a, *sr,
                body_id_b, friction, restitution,
            ) { c.normal = -c.normal; c.body_id = body_id_a; manifold.add_contact(c); }
        }
        (StoredShape::Capsule { half_height, radius },
         StoredShape::Box { half_extents }) => {
            for c in capsule_box_contact(
                pos_a, rot_a, *half_height, *radius, pos_b, rot_b, *half_extents,
                body_id_a, friction, restitution,
            ) { manifold.add_contact(c); }
        }
        (StoredShape::Box { half_extents },
         StoredShape::Capsule { half_height, radius }) => {
            for mut c in capsule_box_contact(
                pos_b, rot_b, *half_height, *radius, pos_a, rot_a, *half_extents,
                body_id_b, friction, restitution,
            ) { c.normal = -c.normal; c.body_id = body_id_a; manifold.add_contact(c); }
        }
        // Cylinder treated as capsule with same half_height/radius
        (StoredShape::Cylinder { half_height, radius },
         StoredShape::Sphere { radius: sr }) => {
            if let Some(c) = capsule_sphere_contact(
                pos_a, rot_a, *half_height, *radius, pos_b, *sr,
                body_id_a, friction, restitution,
            ) { manifold.add_contact(c); }
        }
        (StoredShape::Sphere { radius: sr },
         StoredShape::Cylinder { half_height, radius }) => {
            if let Some(mut c) = capsule_sphere_contact(
                pos_b, rot_b, *half_height, *radius, pos_a, *sr,
                body_id_b, friction, restitution,
            ) { c.normal = -c.normal; c.body_id = body_id_a; manifold.add_contact(c); }
        }
        // Any pair involving ConvexHull or DecomposedMesh: use GJK/EPA
        _ if matches!(shape_a, StoredShape::ConvexHull { .. } | StoredShape::DecomposedMesh { .. })
          || matches!(shape_b, StoredShape::ConvexHull { .. } | StoredShape::DecomposedMesh { .. }) => {
            let contacts = gjk_contact_pair(
                shape_a, pos_a, rot_a, shape_b, pos_b, rot_b,
                body_id_a, friction, restitution,
            );
            for c in contacts { manifold.add_contact(c); }
        }
        // Cylinder-Cylinder: treat both as capsules
        (StoredShape::Cylinder { half_height: hh_a, radius: ra },
         StoredShape::Cylinder { half_height: hh_b, radius: rb }) => {
            if let Some(c) = capsule_capsule_contact(
                pos_a, rot_a, *hh_a, *ra, pos_b, rot_b, *hh_b, *rb,
                body_id_a, friction, restitution,
            ) { manifold.add_contact(c); }
        }
        // Cylinder-Box: treat cylinder as capsule
        (StoredShape::Cylinder { half_height, radius },
         StoredShape::Box { half_extents }) => {
            for c in capsule_box_contact(
                pos_a, rot_a, *half_height, *radius, pos_b, rot_b, *half_extents,
                body_id_a, friction, restitution,
            ) { manifold.add_contact(c); }
        }
        (StoredShape::Box { half_extents },
         StoredShape::Cylinder { half_height, radius }) => {
            for mut c in capsule_box_contact(
                pos_b, rot_b, *half_height, *radius, pos_a, rot_a, *half_extents,
                body_id_b, friction, restitution,
            ) { c.normal = -c.normal; c.body_id = body_id_a; manifold.add_contact(c); }
        }
        // Cylinder-Capsule: treat cylinder as capsule
        (StoredShape::Cylinder { half_height: hh_a, radius: ra },
         StoredShape::Capsule { half_height: hh_b, radius: rb }) => {
            if let Some(c) = capsule_capsule_contact(
                pos_a, rot_a, *hh_a, *ra, pos_b, rot_b, *hh_b, *rb,
                body_id_a, friction, restitution,
            ) { manifold.add_contact(c); }
        }
        (StoredShape::Capsule { half_height: hh_a, radius: ra },
         StoredShape::Cylinder { half_height: hh_b, radius: rb }) => {
            if let Some(c) = capsule_capsule_contact(
                pos_a, rot_a, *hh_a, *ra, pos_b, rot_b, *hh_b, *rb,
                body_id_a, friction, restitution,
            ) { manifold.add_contact(c); }
        }
        _ => {}
    }

    manifold
}

/// Sphere-sphere contact detection.
///
/// Returns a contact if the two spheres overlap or are within tolerance.
/// Normal points from A to B.
pub fn sphere_sphere_contact(
    pos_a: Vector3<f32>,
    radius_a: f32,
    pos_b: Vector3<f32>,
    radius_b: f32,
    body_id: usize,
    friction: f32,
    restitution: f32,
) -> Option<ContactPoint> {
    let diff = pos_b - pos_a;
    let dist = diff.norm();
    let penetration = radius_a + radius_b - dist;

    if penetration < -0.01 {
        return None;
    }

    let normal = if dist > 1e-8 {
        diff / dist
    } else {
        Vector3::y() // Degenerate: overlapping centers, push up
    };

    // Contact point at midpoint of overlap
    let point_world = pos_a + normal * (radius_a - penetration * 0.5);
    let point_body = -normal * (radius_a - penetration * 0.5);

    Some(
        ContactPoint::new(body_id, point_world, point_body, normal, penetration)
            .with_friction(friction)
            .with_restitution(restitution),
    )
}

/// Sphere-box contact detection.
///
/// Finds the closest point on the box to the sphere center, then checks distance.
/// Normal points from sphere (body A) toward box (body B).
#[allow(clippy::too_many_arguments)]
pub fn sphere_box_contact(
    sphere_pos: Vector3<f32>,
    radius: f32,
    box_pos: Vector3<f32>,
    box_rot: Matrix3<f32>,
    half_extents: Vector3<f32>,
    body_id: usize,
    friction: f32,
    restitution: f32,
) -> Option<ContactPoint> {
    // Transform sphere center to box local frame
    let local_center = box_rot.transpose() * (sphere_pos - box_pos);

    // Clamp to box surface (closest point on AABB)
    let closest_local = Vector3::new(
        local_center.x.clamp(-half_extents.x, half_extents.x),
        local_center.y.clamp(-half_extents.y, half_extents.y),
        local_center.z.clamp(-half_extents.z, half_extents.z),
    );

    let diff_local = local_center - closest_local;
    let dist = diff_local.norm();

    // Check if sphere center is inside the box
    let inside = local_center.x.abs() <= half_extents.x
        && local_center.y.abs() <= half_extents.y
        && local_center.z.abs() <= half_extents.z;

    let penetration = if inside {
        // Sphere center inside box: find closest face for pushout
        let face_dists = [
            half_extents.x - local_center.x.abs(),
            half_extents.y - local_center.y.abs(),
            half_extents.z - local_center.z.abs(),
        ];
        let min_dist = face_dists.iter().copied().fold(f32::MAX, f32::min);
        radius + min_dist
    } else {
        radius - dist
    };

    if penetration < -0.01 {
        return None;
    }

    let normal_local = if inside {
        // Push out along closest face
        let face_dists = [
            half_extents.x - local_center.x.abs(),
            half_extents.y - local_center.y.abs(),
            half_extents.z - local_center.z.abs(),
        ];
        let min_idx = face_dists
            .iter()
            .enumerate()
            .min_by(|a, b| a.1.partial_cmp(b.1).unwrap_or(std::cmp::Ordering::Equal))
            .map(|(i, _)| i)
            .unwrap_or(1);
        let mut n = Vector3::zeros();
        n[min_idx] = if local_center[min_idx] >= 0.0 { 1.0 } else { -1.0 };
        n
    } else if dist > 1e-8 {
        diff_local / dist
    } else {
        Vector3::y()
    };

    // Transform normal back to world frame
    let normal_world = box_rot * normal_local;
    let closest_world = box_pos + box_rot * closest_local;
    let point_body = -normal_world * (radius - penetration * 0.5);

    Some(
        ContactPoint::new(body_id, closest_world, point_body, normal_world, penetration)
            .with_friction(friction)
            .with_restitution(restitution),
    )
}

/// Box-box contact detection using Separating Axis Theorem (SAT).
///
/// Tests 15 potential separating axes (3 face normals × 2 + 9 edge cross products).
/// Returns contact points on the overlapping region.
#[allow(clippy::too_many_arguments)]
pub fn box_box_contacts(
    pos_a: Vector3<f32>,
    rot_a: Matrix3<f32>,
    he_a: Vector3<f32>,
    pos_b: Vector3<f32>,
    rot_b: Matrix3<f32>,
    he_b: Vector3<f32>,
    body_id: usize,
    friction: f32,
    restitution: f32,
) -> Vec<ContactPoint> {
    let d = pos_b - pos_a;

    // Axes of each box
    let ax_a = [rot_a.column(0).into_owned(), rot_a.column(1).into_owned(), rot_a.column(2).into_owned()];
    let ax_b = [rot_b.column(0).into_owned(), rot_b.column(1).into_owned(), rot_b.column(2).into_owned()];

    let he_a_arr = [he_a.x, he_a.y, he_a.z];
    let he_b_arr = [he_b.x, he_b.y, he_b.z];

    let mut min_penetration = f32::MAX;
    let mut best_axis = Vector3::y();
    let mut best_contact_type = BoxBoxContactType::FaceA(1);

    // Test a single SAT axis, return false if separating
    let test_axis = |axis: Vector3<f32>, contact_type: BoxBoxContactType,
                         min_pen: &mut f32, best_ax: &mut Vector3<f32>,
                         best_ct: &mut BoxBoxContactType| -> bool {
        let len = axis.norm();
        if len < 1e-6 {
            return true; // Degenerate axis, skip
        }
        let axis = axis / len;

        let proj_a: f32 = he_a_arr.iter().zip(ax_a.iter())
            .map(|(&h, a)| h * axis.dot(a).abs())
            .sum();
        let proj_b: f32 = he_b_arr.iter().zip(ax_b.iter())
            .map(|(&h, b)| h * axis.dot(b).abs())
            .sum();

        let dist = d.dot(&axis).abs();
        let penetration = proj_a + proj_b - dist;

        if penetration < -0.01 {
            return false;
        }

        if penetration < *min_pen {
            *min_pen = penetration;
            *best_ax = if d.dot(&axis) >= 0.0 { axis } else { -axis };
            *best_ct = contact_type;
        }
        true
    };

    // 3 face normals of A (indices 0-2)
    for (i, ax) in ax_a.iter().enumerate() {
        if !test_axis(*ax, BoxBoxContactType::FaceA(i),
                      &mut min_penetration, &mut best_axis, &mut best_contact_type) {
            return Vec::new();
        }
    }
    // 3 face normals of B (indices 3-5)
    for (i, ax) in ax_b.iter().enumerate() {
        if !test_axis(*ax, BoxBoxContactType::FaceB(i),
                      &mut min_penetration, &mut best_axis, &mut best_contact_type) {
            return Vec::new();
        }
    }
    // 9 edge cross products (indices 6-14)
    for (i, a) in ax_a.iter().enumerate() {
        for (j, b) in ax_b.iter().enumerate() {
            if !test_axis(a.cross(b), BoxBoxContactType::EdgeEdge(i, j),
                          &mut min_penetration, &mut best_axis, &mut best_contact_type) {
                return Vec::new();
            }
        }
    }

    if min_penetration < -0.01 {
        return Vec::new();
    }

    // Generate contact points based on contact type
    match best_contact_type {
        BoxBoxContactType::FaceA(ref_idx) => {
            // A's face is reference, B provides incident face
            let ref_sign = if best_axis.dot(&ax_a[ref_idx]) >= 0.0 { 1.0 } else { -1.0 };
            let _ref_face = get_box_face_polygon(pos_a, rot_a, he_a, ref_idx, ref_sign);
            let (inc_idx, inc_sign) = select_incident_face(rot_b, best_axis);
            let inc_face = get_box_face_polygon(pos_b, rot_b, he_b, inc_idx, inc_sign);

            // Build clip planes from reference face's 4 side edges
            let ref_normal = ax_a[ref_idx] * ref_sign;
            let ref_center = pos_a + ref_normal * he_a[ref_idx];
            let t1_idx = (ref_idx + 1) % 3;
            let t2_idx = (ref_idx + 2) % 3;
            let clip_planes = [
                (ax_a[t1_idx], ref_center.dot(&ax_a[t1_idx]) - he_a[t1_idx]),
                (-ax_a[t1_idx], -(ref_center.dot(&ax_a[t1_idx]) + he_a[t1_idx])),
                (ax_a[t2_idx], ref_center.dot(&ax_a[t2_idx]) - he_a[t2_idx]),
                (-ax_a[t2_idx], -(ref_center.dot(&ax_a[t2_idx]) + he_a[t2_idx])),
            ];

            let clipped = sutherland_hodgman_clip(&inc_face, &clip_planes);
            let ref_plane_offset = ref_center.dot(&ref_normal);

            clipped.iter().filter_map(|&v| {
                let pen = ref_plane_offset - v.dot(&ref_normal);
                if pen > -0.01 {
                    let point_body = rot_a.transpose() * (v - pos_a);
                    Some(ContactPoint::new(body_id, v, point_body, best_axis, pen)
                        .with_friction(friction)
                        .with_restitution(restitution))
                } else {
                    None
                }
            }).collect()
        }
        BoxBoxContactType::FaceB(ref_idx) => {
            // B's face is reference, A provides incident face
            let ref_sign = if (-best_axis).dot(&ax_b[ref_idx]) >= 0.0 { 1.0 } else { -1.0 };
            let _ref_face = get_box_face_polygon(pos_b, rot_b, he_b, ref_idx, ref_sign);
            let (inc_idx, inc_sign) = select_incident_face(rot_a, -best_axis);
            let inc_face = get_box_face_polygon(pos_a, rot_a, he_a, inc_idx, inc_sign);

            let ref_normal = ax_b[ref_idx] * ref_sign;
            let ref_center = pos_b + ref_normal * he_b[ref_idx];
            let t1_idx = (ref_idx + 1) % 3;
            let t2_idx = (ref_idx + 2) % 3;
            let clip_planes = [
                (ax_b[t1_idx], ref_center.dot(&ax_b[t1_idx]) - he_b[t1_idx]),
                (-ax_b[t1_idx], -(ref_center.dot(&ax_b[t1_idx]) + he_b[t1_idx])),
                (ax_b[t2_idx], ref_center.dot(&ax_b[t2_idx]) - he_b[t2_idx]),
                (-ax_b[t2_idx], -(ref_center.dot(&ax_b[t2_idx]) + he_b[t2_idx])),
            ];

            let clipped = sutherland_hodgman_clip(&inc_face, &clip_planes);
            let ref_plane_offset = ref_center.dot(&ref_normal);

            clipped.iter().filter_map(|&v| {
                let pen = ref_plane_offset - v.dot(&ref_normal);
                if pen > -0.01 {
                    let point_body = rot_a.transpose() * (v - pos_a);
                    Some(ContactPoint::new(body_id, v, point_body, best_axis, pen)
                        .with_friction(friction)
                        .with_restitution(restitution))
                } else {
                    None
                }
            }).collect()
        }
        BoxBoxContactType::EdgeEdge(_i, _j) => {
            // Edge-edge: single closest-point contact (to be improved in task 1.4)
            let point_on_a = pos_a + best_axis * project_box_onto_axis(&ax_a, &he_a_arr, &best_axis);
            let point_on_b = pos_b - best_axis * project_box_onto_axis(&ax_b, &he_b_arr, &(-best_axis));
            let point_world = (point_on_a + point_on_b) * 0.5;
            let point_body = rot_a.transpose() * (point_world - pos_a);

            vec![
                ContactPoint::new(body_id, point_world, point_body, best_axis, min_penetration)
                    .with_friction(friction)
                    .with_restitution(restitution),
            ]
        }
    }
}

/// Project a box onto an axis and return the signed support distance.
fn project_box_onto_axis(
    axes: &[Vector3<f32>; 3],
    half_extents: &[f32; 3],
    direction: &Vector3<f32>,
) -> f32 {
    half_extents.iter().zip(axes.iter())
        .map(|(&h, a)| h * direction.dot(a).signum() * direction.dot(a).abs().min(h))
        .sum::<f32>()
        .max(0.0)
}

/// SAT axis classification for box-box contacts.
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum BoxBoxContactType {
    /// Face normal of box A (axis index 0-2) → A is reference face
    FaceA(usize),
    /// Face normal of box B (axis index 0-2) → B is reference face
    FaceB(usize),
    /// Edge cross product (`ax_a[i] x ax_b[j]`) -- edge-edge contact.
    EdgeEdge(usize, usize),
}

/// Get the 4 world-space vertices of a box face.
///
/// `axis_idx` is 0/1/2 for X/Y/Z face, `sign` is +1.0 or -1.0 for positive/negative face.
/// Vertices are ordered counter-clockwise when viewed from outside the box.
pub fn get_box_face_polygon(
    pos: Vector3<f32>,
    rot: Matrix3<f32>,
    he: Vector3<f32>,
    axis_idx: usize,
    sign: f32,
) -> [Vector3<f32>; 4] {
    let he_arr = [he.x, he.y, he.z];
    // The two tangent axes (not the face normal axis)
    let t1_idx = (axis_idx + 1) % 3;
    let t2_idx = (axis_idx + 2) % 3;

    let face_normal_dir = rot.column(axis_idx).into_owned();
    let t1_dir = rot.column(t1_idx).into_owned();
    let t2_dir = rot.column(t2_idx).into_owned();

    let center = pos + face_normal_dir * (he_arr[axis_idx] * sign);

    // CCW winding when viewed from outside (sign > 0)
    let (s1, s2) = if sign >= 0.0 { (1.0, 1.0) } else { (-1.0, 1.0) };
    let _ = s1; let _ = s2;

    [
        center - t1_dir * he_arr[t1_idx] - t2_dir * he_arr[t2_idx],
        center + t1_dir * he_arr[t1_idx] - t2_dir * he_arr[t2_idx],
        center + t1_dir * he_arr[t1_idx] + t2_dir * he_arr[t2_idx],
        center - t1_dir * he_arr[t1_idx] + t2_dir * he_arr[t2_idx],
    ]
}

/// Select the incident face of a box — the face most anti-aligned with the reference normal.
///
/// Returns `(axis_idx, sign)` where `rot.column(axis_idx) * sign` has minimum dot product
/// with `reference_normal`.
pub fn select_incident_face(
    rot: Matrix3<f32>,
    reference_normal: Vector3<f32>,
) -> (usize, f32) {
    let mut best_idx = 0;
    let mut best_sign = 1.0_f32;
    let mut best_dot = f32::MAX;

    for i in 0..3 {
        let axis = rot.column(i).into_owned();
        let d_pos = axis.dot(&reference_normal);
        let d_neg = -d_pos;

        if d_pos < best_dot {
            best_dot = d_pos;
            best_idx = i;
            best_sign = 1.0;
        }
        if d_neg < best_dot {
            best_dot = d_neg;
            best_idx = i;
            best_sign = -1.0;
        }
    }

    (best_idx, best_sign)
}

// ============================================================================
// Helpers
// ============================================================================

/// Compute an orthonormal tangent frame for a contact normal.
///
/// Returns (t1, t2) such that (t1, t2, normal) forms a right-handed frame.
/// Uses the "most different axis" method for numerical stability.
/// Public version of tangent frame computation for use by contact_jacobian.
pub fn compute_tangent_frame_pub(normal: &Vector3<f32>) -> (Vector3<f32>, Vector3<f32>) {
    compute_tangent_frame(normal)
}

fn compute_tangent_frame(normal: &Vector3<f32>) -> (Vector3<f32>, Vector3<f32>) {
    if normal.norm_squared() < 1e-12 {
        return (Vector3::x(), Vector3::y());
    }
    let n = normal.normalize();

    // Choose the axis most different from normal to avoid near-parallel cross product
    let reference = if n.x.abs() < 0.9 {
        Vector3::x()
    } else {
        Vector3::y()
    };

    let t1 = n.cross(&reference).normalize();
    let t2 = n.cross(&t1);

    (t1, t2)
}

// ============================================================================
// GJK/EPA contact detection for ConvexHull and mixed pairs
// ============================================================================

use super::gjk::{self, Support, SphereSupport, BoxSupport, ConvexHullSupport, GjkResult};

/// Contact detection via GJK/EPA for any StoredShape pair.
#[allow(clippy::too_many_arguments)]
fn gjk_contact_pair(
    shape_a: &StoredShape, pos_a: Vector3<f32>, rot_a: Matrix3<f32>,
    shape_b: &StoredShape, pos_b: Vector3<f32>, rot_b: Matrix3<f32>,
    body_id: usize, friction: f32, restitution: f32,
) -> Vec<ContactPoint> {
    let support_a = make_support(shape_a, pos_a, rot_a);
    let support_b = make_support(shape_b, pos_b, rot_b);

    match gjk::gjk_distance(support_a.as_ref(), support_b.as_ref(), 64) {
        GjkResult::Overlap { simplex } => {
            if let Some(epa) = gjk::epa_penetration(support_a.as_ref(), support_b.as_ref(), &simplex, 32) {
                if epa.depth > -0.01 {
                    let point_body = rot_a.transpose() * (epa.point_a - pos_a);
                    return vec![ContactPoint::new(
                        body_id, epa.point_a, point_body, epa.normal, epa.depth,
                    ).with_friction(friction).with_restitution(restitution)];
                }
            }
            Vec::new()
        }
        GjkResult::Separated { distance, closest_a, closest_b } => {
            // Near-contact: if distance < margin, generate predictive contact
            if distance < 0.01 {
                let normal = if distance > 1e-8 {
                    (closest_b - closest_a) / distance
                } else {
                    Vector3::y()
                };
                let point_body = rot_a.transpose() * (closest_a - pos_a);
                vec![ContactPoint::new(
                    body_id, closest_a, point_body, normal, -distance,
                ).with_friction(friction).with_restitution(restitution)]
            } else {
                Vec::new()
            }
        }
    }
}

/// Create a boxed Support for any StoredShape. No memory leaks — all data owned.
fn make_support(shape: &StoredShape, pos: Vector3<f32>, rot: Matrix3<f32>) -> Box<dyn Support> {
    match shape {
        StoredShape::Sphere { radius } => Box::new(SphereSupport { center: pos, radius: *radius }),
        StoredShape::Box { half_extents } => Box::new(BoxSupport { center: pos, rotation: rot, half_extents: *half_extents }),
        StoredShape::ConvexHull { hull } => {
            let world_verts: Vec<Vector3<f32>> = hull.vertices.iter().map(|v| pos + rot * v).collect();
            Box::new(ConvexHullSupport { vertices_world: world_verts })
        }
        StoredShape::Capsule { half_height, radius } => {
            Box::new(SphereSupport { center: pos, radius: *half_height + *radius })
        }
        StoredShape::Cylinder { half_height, radius } => {
            Box::new(SphereSupport { center: pos, radius: (*half_height * *half_height + *radius * *radius).sqrt() })
        }
        StoredShape::DecomposedMesh { hulls } => {
            if let Some(hull) = hulls.first() {
                let world_verts: Vec<Vector3<f32>> = hull.vertices.iter().map(|v| pos + rot * v).collect();
                Box::new(ConvexHullSupport { vertices_world: world_verts })
            } else {
                Box::new(SphereSupport { center: pos, radius: 0.1 })
            }
        }
    }
}

// ============================================================================
// Capsule & Cylinder contacts
// ============================================================================

/// Closest points between two line segments (for capsule-capsule).
/// Returns (t_a, t_b) parameters in [0,1] along each segment.
fn closest_points_segments(
    a0: Vector3<f32>, a1: Vector3<f32>,
    b0: Vector3<f32>, b1: Vector3<f32>,
) -> (f32, f32) {
    let d1 = a1 - a0;
    let d2 = b1 - b0;
    let r = a0 - b0;

    let a = d1.dot(&d1);
    let e = d2.dot(&d2);
    let f = d2.dot(&r);

    if a < 1e-10 && e < 1e-10 {
        return (0.0, 0.0);
    }
    if a < 1e-10 {
        return (0.0, (f / e).clamp(0.0, 1.0));
    }
    let c = d1.dot(&r);
    if e < 1e-10 {
        return ((-c / a).clamp(0.0, 1.0), 0.0);
    }

    let b = d1.dot(&d2);
    let denom = a * e - b * b;

    let mut s = if denom.abs() > 1e-10 {
        ((b * f - c * e) / denom).clamp(0.0, 1.0)
    } else {
        0.0
    };

    let mut t = (b * s + f) / e;
    if t < 0.0 {
        t = 0.0;
        s = (-c / a).clamp(0.0, 1.0);
    } else if t > 1.0 {
        t = 1.0;
        s = ((b - c) / a).clamp(0.0, 1.0);
    }

    (s, t)
}

/// Capsule-capsule contact detection.
#[allow(clippy::too_many_arguments)]
pub fn capsule_capsule_contact(
    pos_a: Vector3<f32>, rot_a: Matrix3<f32>, half_h_a: f32, radius_a: f32,
    pos_b: Vector3<f32>, rot_b: Matrix3<f32>, half_h_b: f32, radius_b: f32,
    body_id: usize, friction: f32, restitution: f32,
) -> Option<ContactPoint> {
    let axis_a = rot_a.column(1).into_owned();
    let axis_b = rot_b.column(1).into_owned();

    let a0 = pos_a - axis_a * half_h_a;
    let a1 = pos_a + axis_a * half_h_a;
    let b0 = pos_b - axis_b * half_h_b;
    let b1 = pos_b + axis_b * half_h_b;

    let (s, t) = closest_points_segments(a0, a1, b0, b1);
    let pa = a0 + (a1 - a0) * s;
    let pb = b0 + (b1 - b0) * t;

    let diff = pb - pa;
    let dist = diff.norm();
    let penetration = radius_a + radius_b - dist;

    if penetration < -0.01 { return None; }

    let normal = if dist > 1e-8 { diff / dist } else { Vector3::y() };
    let point_world = pa + normal * (radius_a - penetration * 0.5);
    let point_body = -normal * (radius_a - penetration * 0.5);

    Some(ContactPoint::new(body_id, point_world, point_body, normal, penetration)
        .with_friction(friction).with_restitution(restitution))
}

/// Capsule-sphere contact detection.
#[allow(clippy::too_many_arguments)]
pub fn capsule_sphere_contact(
    cap_pos: Vector3<f32>, cap_rot: Matrix3<f32>, half_h: f32, cap_radius: f32,
    sphere_pos: Vector3<f32>, sphere_radius: f32,
    body_id: usize, friction: f32, restitution: f32,
) -> Option<ContactPoint> {
    let axis = cap_rot.column(1).into_owned();
    let a0 = cap_pos - axis * half_h;
    let a1 = cap_pos + axis * half_h;

    // Project sphere center onto capsule segment
    let d = a1 - a0;
    let t = ((sphere_pos - a0).dot(&d) / d.dot(&d)).clamp(0.0, 1.0);
    let closest = a0 + d * t;

    let diff = sphere_pos - closest;
    let dist = diff.norm();
    let penetration = cap_radius + sphere_radius - dist;

    if penetration < -0.01 { return None; }

    let normal = if dist > 1e-8 { diff / dist } else { Vector3::y() };
    let point_world = closest + normal * (cap_radius - penetration * 0.5);
    let point_body = Vector3::zeros();

    Some(ContactPoint::new(body_id, point_world, point_body, normal, penetration)
        .with_friction(friction).with_restitution(restitution))
}

/// Capsule-box contact: closest point on capsule axis to box, then offset by radii.
#[allow(clippy::too_many_arguments)]
pub fn capsule_box_contact(
    cap_pos: Vector3<f32>, cap_rot: Matrix3<f32>, half_h: f32, cap_radius: f32,
    box_pos: Vector3<f32>, box_rot: Matrix3<f32>, box_he: Vector3<f32>,
    body_id: usize, friction: f32, restitution: f32,
) -> Vec<ContactPoint> {
    let axis = cap_rot.column(1).into_owned();
    let mut contacts = Vec::new();

    // Test both hemisphere endpoints + midpoint against box
    for &t in &[-1.0_f32, 0.0, 1.0] {
        let sphere_pos = cap_pos + axis * (half_h * t);
        if let Some(c) = sphere_box_contact(
            sphere_pos, cap_radius, box_pos, box_rot, box_he,
            body_id, friction, restitution,
        ) {
            contacts.push(c);
        }
    }
    contacts
}

// ============================================================================
// ShapePair convenience wrappers
// ============================================================================

/// Box-box contact detection using a [`ShapePair`] to group shared parameters.
///
/// Delegates to [`box_box_contacts`]; see that function for algorithm details.
pub fn box_box_contacts_from_pair(
    pair: &ShapePair,
    he_a: Vector3<f32>,
    he_b: Vector3<f32>,
) -> Vec<ContactPoint> {
    box_box_contacts(
        pair.pos_a, pair.rot_a, he_a,
        pair.pos_b, pair.rot_b, he_b,
        pair.body_id_a, pair.friction, pair.restitution,
    )
}

/// Capsule-capsule contact detection using a [`ShapePair`] to group shared parameters.
///
/// Delegates to [`capsule_capsule_contact`]; see that function for algorithm details.
pub fn capsule_capsule_contact_from_pair(
    pair: &ShapePair,
    half_h_a: f32,
    radius_a: f32,
    half_h_b: f32,
    radius_b: f32,
) -> Option<ContactPoint> {
    capsule_capsule_contact(
        pair.pos_a, pair.rot_a, half_h_a, radius_a,
        pair.pos_b, pair.rot_b, half_h_b, radius_b,
        pair.body_id_a, pair.friction, pair.restitution,
    )
}

/// Capsule-box contact detection using a [`ShapePair`] to group shared parameters.
///
/// Delegates to [`capsule_box_contact`]; see that function for algorithm details.
pub fn capsule_box_contact_from_pair(
    pair: &ShapePair,
    half_h: f32,
    cap_radius: f32,
    box_he: Vector3<f32>,
) -> Vec<ContactPoint> {
    capsule_box_contact(
        pair.pos_a, pair.rot_a, half_h, cap_radius,
        pair.pos_b, pair.rot_b, box_he,
        pair.body_id_a, pair.friction, pair.restitution,
    )
}

/// Inter-body contact detection using a [`ShapePair`] to group shared parameters.
///
/// Delegates to [`inter_body_contacts`]; see that function for algorithm details.
pub fn inter_body_contacts_from_pair(
    pair: &ShapePair,
    shape_a: &StoredShape,
    shape_b: &StoredShape,
) -> ContactManifold {
    inter_body_contacts(shape_a, shape_b, pair)
}

// ============================================================================
// Contact reduction (max-area selection)
// ============================================================================

/// Select up to `max_n` contacts that maximize the contact patch area.
///
/// Returns indices into the `contacts` slice. Greedy algorithm:
/// 1. Deepest contact (best penetration correction)
/// 2. Farthest from #1 in tangent plane
/// 3. Maximizes triangle area with #1, #2
/// 4. Maximizes quad area with #1, #2, #3
pub fn select_max_area_contacts(contacts: &[ContactPoint], max_n: usize) -> Vec<usize> {
    let n = contacts.len();
    if n <= max_n {
        return (0..n).collect();
    }
    if n == 0 || max_n == 0 {
        return Vec::new();
    }

    // Use the first contact's normal as the projection axis
    let normal = contacts[0].normal;

    // Project contact points onto tangent plane (2D coordinates)
    let (t1, t2) = compute_tangent_frame(&normal);
    let project = |p: &Vector3<f32>| -> (f32, f32) {
        (p.dot(&t1), p.dot(&t2))
    };

    let projected: Vec<(f32, f32)> = contacts.iter()
        .map(|c| project(&c.point_world))
        .collect();

    let mut selected = Vec::with_capacity(max_n);

    // Step 1: Deepest contact
    let idx0 = contacts.iter().enumerate()
        .max_by(|a, b| a.1.penetration.partial_cmp(&b.1.penetration).unwrap_or(std::cmp::Ordering::Equal))
        .map(|(i, _)| i)
        .unwrap_or(0);
    selected.push(idx0);

    if max_n == 1 { return selected; }

    // Step 2: Farthest from #1
    let (px0, py0) = projected[idx0];
    let idx1 = (0..n)
        .filter(|i| !selected.contains(i))
        .max_by(|&a, &b| {
            let da = (projected[a].0 - px0).powi(2) + (projected[a].1 - py0).powi(2);
            let db = (projected[b].0 - px0).powi(2) + (projected[b].1 - py0).powi(2);
            da.partial_cmp(&db).unwrap_or(std::cmp::Ordering::Equal)
        })
        .unwrap_or(0);
    selected.push(idx1);

    if max_n == 2 { return selected; }

    // Step 3: Maximizes triangle area with #1, #2
    let (px1, py1) = projected[idx1];
    let idx2 = (0..n)
        .filter(|i| !selected.contains(i))
        .max_by(|&a, &b| {
            let area_a = triangle_area_2d(px0, py0, px1, py1, projected[a].0, projected[a].1);
            let area_b = triangle_area_2d(px0, py0, px1, py1, projected[b].0, projected[b].1);
            area_a.partial_cmp(&area_b).unwrap_or(std::cmp::Ordering::Equal)
        })
        .unwrap_or(0);
    selected.push(idx2);

    if max_n == 3 { return selected; }

    // Step 4+: Maximize area incrementally
    for _ in 3..max_n {
        let best = (0..n)
            .filter(|i| !selected.contains(i))
            .max_by(|&a, &b| {
                let area_a = polygon_area_with_point(&projected, &selected, projected[a]);
                let area_b = polygon_area_with_point(&projected, &selected, projected[b]);
                area_a.partial_cmp(&area_b).unwrap_or(std::cmp::Ordering::Equal)
            });
        if let Some(idx) = best {
            selected.push(idx);
        } else {
            break;
        }
    }

    selected
}

/// Signed area of a 2D triangle (absolute value).
fn triangle_area_2d(x0: f32, y0: f32, x1: f32, y1: f32, x2: f32, y2: f32) -> f32 {
    ((x1 - x0) * (y2 - y0) - (x2 - x0) * (y1 - y0)).abs() * 0.5
}

/// Area of a polygon formed by existing selected points plus a candidate point.
fn polygon_area_with_point(
    projected: &[(f32, f32)],
    selected: &[usize],
    candidate: (f32, f32),
) -> f32 {
    // Sum triangle areas from the first selected point to all edges
    let mut total = 0.0_f32;
    let n = selected.len();
    if n < 2 { return 0.0; }

    let (cx, cy) = candidate;
    for i in 0..n {
        let (ax, ay) = projected[selected[i]];
        let (bx, by) = projected[selected[(i + 1) % n]];
        total += triangle_area_2d(cx, cy, ax, ay, bx, by);
    }
    total
}

// ============================================================================
// Polygon clipping (Sutherland-Hodgman)
// ============================================================================

/// Epsilon for polygon clipping vertex classification.
const CLIP_EPSILON: f32 = 1e-7;

/// Clip a convex polygon against a single half-plane.
///
/// Keeps the portion of the polygon on the *positive* side of the plane
/// (where `dot(vertex, plane_normal) >= plane_offset`).
///
/// Uses the Sutherland-Hodgman edge-walking algorithm:
/// - inside→inside: keep end vertex
/// - inside→outside: emit intersection
/// - outside→inside: emit intersection + end vertex
/// - outside→outside: emit nothing
pub fn clip_polygon_by_plane(
    polygon: &[Vector3<f32>],
    plane_normal: Vector3<f32>,
    plane_offset: f32,
) -> Vec<Vector3<f32>> {
    if polygon.is_empty() {
        return Vec::new();
    }

    let n = polygon.len();
    let mut output = Vec::with_capacity(n + 1);

    for i in 0..n {
        let current = polygon[i];
        let next = polygon[(i + 1) % n];

        let d_current = plane_normal.dot(&current) - plane_offset;
        let d_next = plane_normal.dot(&next) - plane_offset;

        let current_inside = d_current >= -CLIP_EPSILON;
        let next_inside = d_next >= -CLIP_EPSILON;

        if current_inside && next_inside {
            // Both inside: keep end vertex
            output.push(next);
        } else if current_inside && !next_inside {
            // Going out: emit intersection
            let t = d_current / (d_current - d_next);
            let intersection = current + (next - current) * t.clamp(0.0, 1.0);
            output.push(intersection);
        } else if !current_inside && next_inside {
            // Coming in: emit intersection + end vertex
            let t = d_current / (d_current - d_next);
            let intersection = current + (next - current) * t.clamp(0.0, 1.0);
            output.push(intersection);
            output.push(next);
        }
        // outside→outside: emit nothing
    }

    output
}

/// Clip a convex polygon against multiple half-planes (Sutherland-Hodgman).
///
/// Each plane is `(normal, offset)` — keeps the side where `dot(v, normal) >= offset`.
/// Iteratively clips against each plane in sequence.
pub fn sutherland_hodgman_clip(
    polygon: &[Vector3<f32>],
    clip_planes: &[(Vector3<f32>, f32)],
) -> Vec<Vector3<f32>> {
    let mut current = polygon.to_vec();

    for &(normal, offset) in clip_planes {
        if current.is_empty() {
            break;
        }
        current = clip_polygon_by_plane(&current, normal, offset);
    }

    current
}

// ============================================================================
// Tests
// ============================================================================

#[cfg(test)]
mod tests {
    use super::*;
    use super::super::collider::ColliderShape as StoredShape;
    use super::super::body::{ArticulatedBody, GenJointType};
    use super::super::spatial::{SpatialInertia, SpatialTransform};
    use approx::assert_relative_eq;
    use nalgebra::Matrix3;

    fn make_inertia(mass: f32) -> SpatialInertia {
        SpatialInertia::from_mass_inertia_at_com(mass, Matrix3::identity() * 0.01 * mass)
    }

    #[test]
    fn test_contact_point_creation() {
        let cp = ContactPoint::new(
            0,
            Vector3::new(1.0, 0.0, 0.0),
            Vector3::zeros(),
            Vector3::y(),
            0.001,
        );

        assert_eq!(cp.body_id, 0);
        assert!(cp.is_active());
        assert_relative_eq!(cp.friction, 0.5);
        assert_relative_eq!(cp.restitution, 0.0);
    }

    #[test]
    fn test_contact_point_inactive() {
        let cp = ContactPoint::new(
            0,
            Vector3::zeros(),
            Vector3::zeros(),
            Vector3::y(),
            -0.01,
        );
        assert!(!cp.is_active());
    }

    #[test]
    fn test_contact_point_with_material() {
        let cp = ContactPoint::new(
            0,
            Vector3::zeros(),
            Vector3::zeros(),
            Vector3::y(),
            0.001,
        )
        .with_friction(0.8)
        .with_restitution(0.3);

        assert_relative_eq!(cp.friction, 0.8);
        assert_relative_eq!(cp.restitution, 0.3);
    }

    #[test]
    fn test_tangent_frame_orthonormal() {
        let normals = [
            Vector3::y(),
            Vector3::x(),
            Vector3::z(),
            Vector3::new(1.0, 1.0, 1.0).normalize(),
            Vector3::new(-0.3, 0.9, 0.2).normalize(),
        ];

        for normal in &normals {
            let (t1, t2) = compute_tangent_frame(normal);

            // Orthogonality
            assert_relative_eq!(t1.dot(normal), 0.0, epsilon = 1e-5);
            assert_relative_eq!(t2.dot(normal), 0.0, epsilon = 1e-5);
            assert_relative_eq!(t1.dot(&t2), 0.0, epsilon = 1e-5);

            // Unit length
            assert_relative_eq!(t1.norm(), 1.0, epsilon = 1e-5);
            assert_relative_eq!(t2.norm(), 1.0, epsilon = 1e-5);
        }
    }

    #[test]
    fn test_manifold_operations() {
        let mut manifold = ContactManifold::new();
        assert!(manifold.is_empty());
        assert_eq!(manifold.len(), 0);

        manifold.add_contact(ContactPoint::new(
            0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.01,
        ));
        manifold.add_contact(ContactPoint::new(
            1, Vector3::zeros(), Vector3::zeros(), Vector3::y(), -0.01,
        ));
        manifold.add_contact(ContactPoint::new(
            0, Vector3::x(), Vector3::zeros(), Vector3::y(), 0.005,
        ));

        assert_eq!(manifold.len(), 3);
        assert_eq!(manifold.active_count(), 2);
        assert_eq!(manifold.contacts_for_body(0).count(), 2);
        assert_eq!(manifold.contacts_for_body(1).count(), 1);
    }

    #[test]
    fn test_manifold_prune() {
        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(
            0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.01,
        ));
        manifold.add_contact(ContactPoint::new(
            1, Vector3::zeros(), Vector3::zeros(), Vector3::y(), -0.01,
        ));

        manifold.prune_separated(0.0);
        assert_eq!(manifold.len(), 1);
        assert_eq!(manifold.contacts[0].body_id, 0);
    }

    #[test]
    fn test_manifold_limit_per_body() {
        let mut manifold = ContactManifold::new();
        for j in 0..5 {
            manifold.add_contact(ContactPoint::new(
                0,
                Vector3::new(j as f32, 0.0, 0.0),
                Vector3::zeros(),
                Vector3::y(),
                0.001 * (j + 1) as f32,
            ));
        }
        manifold.add_contact(ContactPoint::new(
            1, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.01,
        ));

        manifold.limit_per_body(2);
        // Body 0: keep 2 deepest, body 1: keep 1
        assert_eq!(manifold.contacts_for_body(0).count(), 2);
        assert_eq!(manifold.contacts_for_body(1).count(), 1);
    }

    #[test]
    fn test_manifold_merge() {
        let mut m1 = ContactManifold::new();
        m1.add_contact(ContactPoint::new(
            0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.01,
        ));

        let mut m2 = ContactManifold::new();
        m2.add_contact(ContactPoint::new(
            1, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.02,
        ));

        m1.merge(&m2);
        assert_eq!(m1.len(), 2);
    }

    #[test]
    fn test_ground_plane_contacts() {
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));

        // Pendulum hanging down — body origin at world origin
        body.add_body(
            "link",
            -1,
            GenJointType::Revolute { axis: Vector3::z() },
            make_inertia(1.0),
            SpatialTransform::identity(),
        );

        // Ground at y = -0.1 with normal pointing up
        let manifold = ground_plane_contacts(
            &body, -0.1, Vector3::y(), 0.5, 0.0,
        );

        // Body origin at (0, 0, 0), ground at y=-0.1
        // signed_dist = y - (-0.1) = 0.1, penetration = -0.1
        // Since -0.1 > -0.01 is false, no contact
        assert_eq!(manifold.active_count(), 0);

        // Ground at y = 0.05 — body is below ground
        let manifold = ground_plane_contacts(
            &body, 0.05, Vector3::y(), 0.5, 0.0,
        );
        assert!(manifold.active_count() > 0);
        assert!(manifold.contacts[0].penetration > 0.0);
    }

    #[test]
    fn test_sphere_ground_contacts() {
        let mut body = ArticulatedBody::new();
        body.add_body(
            "link",
            -1,
            GenJointType::Fixed,
            make_inertia(1.0),
            SpatialTransform::from_translation(Vector3::new(0.0, 0.1, 0.0)),
        );

        let radii = [0.05];
        // Body center at y=0.1, radius=0.05, ground at y=0
        // center_dist = 0.1, penetration = 0.05 - 0.1 = -0.05 → no active contact
        let manifold = sphere_ground_contacts(
            &body, &radii, 0.0, Vector3::y(), 0.5, 0.0,
        );
        assert_eq!(manifold.active_count(), 0);

        // Ground at y=0.08 → center_dist = 0.02, penetration = 0.05 - 0.02 = 0.03
        let manifold = sphere_ground_contacts(
            &body, &radii, 0.08, Vector3::y(), 0.5, 0.0,
        );
        assert_eq!(manifold.active_count(), 1);
        assert_relative_eq!(manifold.contacts[0].penetration, 0.03, epsilon = 1e-4);
    }

    #[test]
    fn test_sort_by_penetration() {
        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(
            0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.001,
        ));
        manifold.add_contact(ContactPoint::new(
            1, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.01,
        ));
        manifold.add_contact(ContactPoint::new(
            2, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.005,
        ));

        manifold.sort_by_penetration();

        assert_eq!(manifold.contacts[0].body_id, 1); // deepest
        assert_eq!(manifold.contacts[1].body_id, 2);
        assert_eq!(manifold.contacts[2].body_id, 0); // shallowest
    }

    // ==========================================================================
    // Inter-body contact tests
    // ==========================================================================

    #[test]
    fn test_sphere_sphere_contact() {
        // Two spheres overlapping: radius 0.5 each, centers 0.8 apart
        let c = sphere_sphere_contact(
            Vector3::new(0.0, 0.0, 0.0), 0.5,
            Vector3::new(0.8, 0.0, 0.0), 0.5,
            0, 0.5, 0.0,
        );
        assert!(c.is_some());
        let c = c.unwrap();
        assert_relative_eq!(c.penetration, 0.2, epsilon = 1e-5);
        // Normal should point from A to B (positive x)
        assert!(c.normal.x > 0.9);
    }

    #[test]
    fn test_sphere_sphere_separated() {
        // Two spheres far apart: radius 0.5 each, centers 2.0 apart
        let c = sphere_sphere_contact(
            Vector3::new(0.0, 0.0, 0.0), 0.5,
            Vector3::new(2.0, 0.0, 0.0), 0.5,
            0, 0.5, 0.0,
        );
        assert!(c.is_none());
    }

    #[test]
    fn test_sphere_sphere_touching() {
        // Exactly touching: radius 0.5 each, centers 1.0 apart
        let c = sphere_sphere_contact(
            Vector3::zeros(), 0.5,
            Vector3::new(1.0, 0.0, 0.0), 0.5,
            0, 0.5, 0.0,
        );
        assert!(c.is_some());
        let c = c.unwrap();
        assert_relative_eq!(c.penetration, 0.0, epsilon = 1e-5);
    }

    #[test]
    fn test_sphere_box_contact_face() {
        // Sphere above a box, touching the top face
        let c = sphere_box_contact(
            Vector3::new(0.0, 0.55, 0.0), 0.1, // sphere at y=0.55, r=0.1
            Vector3::zeros(), Matrix3::identity(), // box at origin, axis-aligned
            Vector3::new(0.5, 0.5, 0.5), // half-extents 0.5
            0, 0.5, 0.0,
        );
        assert!(c.is_some());
        let c = c.unwrap();
        // Penetration: sphere surface at y=0.45, box top at y=0.5 → pen = 0.05
        assert_relative_eq!(c.penetration, 0.05, epsilon = 1e-4);
        // Normal should point from sphere toward box (negative y — pushing sphere up)
        assert!(c.normal.y > 0.9, "normal.y = {}", c.normal.y);
    }

    #[test]
    fn test_sphere_box_separated() {
        // Sphere well above the box
        let c = sphere_box_contact(
            Vector3::new(0.0, 1.0, 0.0), 0.1,
            Vector3::zeros(), Matrix3::identity(),
            Vector3::new(0.5, 0.5, 0.5),
            0, 0.5, 0.0,
        );
        assert!(c.is_none());
    }

    #[test]
    fn test_box_box_contact() {
        // Two axis-aligned boxes overlapping
        let contacts = box_box_contacts(
            Vector3::new(0.0, 0.0, 0.0), Matrix3::identity(), Vector3::new(0.5, 0.5, 0.5),
            Vector3::new(0.9, 0.0, 0.0), Matrix3::identity(), Vector3::new(0.5, 0.5, 0.5),
            0, 0.5, 0.0,
        );
        assert!(!contacts.is_empty());
        let c = &contacts[0];
        assert_relative_eq!(c.penetration, 0.1, epsilon = 1e-4);
        // Normal should point from A to B (positive x)
        assert!(c.normal.x > 0.9, "normal.x = {}", c.normal.x);
    }

    #[test]
    fn test_box_box_separated() {
        // Two boxes far apart
        let contacts = box_box_contacts(
            Vector3::zeros(), Matrix3::identity(), Vector3::new(0.5, 0.5, 0.5),
            Vector3::new(3.0, 0.0, 0.0), Matrix3::identity(), Vector3::new(0.5, 0.5, 0.5),
            0, 0.5, 0.0,
        );
        assert!(contacts.is_empty());
    }

    #[test]
    fn test_inter_body_contacts_dispatch() {
        // Verify inter_body_contacts dispatches correctly
        let shape_s = StoredShape::Sphere { radius: 0.3 };
        let shape_b = StoredShape::Box { half_extents: Vector3::new(0.5, 0.5, 0.5) };

        // Sphere-sphere
        let pair = ShapePair {
            pos_a: Vector3::zeros(), rot_a: Matrix3::identity(),
            pos_b: Vector3::new(0.4, 0.0, 0.0), rot_b: Matrix3::identity(),
            body_id_a: 0, body_id_b: 1, friction: 0.5, restitution: 0.0,
        };
        let m = inter_body_contacts(&shape_s, &shape_s, &pair);
        assert!(!m.is_empty());

        // Sphere-box
        let pair = ShapePair {
            pos_a: Vector3::new(0.0, 0.75, 0.0), rot_a: Matrix3::identity(),
            pos_b: Vector3::zeros(), rot_b: Matrix3::identity(),
            body_id_a: 0, body_id_b: 1, friction: 0.5, restitution: 0.0,
        };
        let m = inter_body_contacts(&shape_s, &shape_b, &pair);
        assert!(!m.is_empty());

        // Box-sphere (reversed)
        let pair = ShapePair {
            pos_a: Vector3::zeros(), rot_a: Matrix3::identity(),
            pos_b: Vector3::new(0.0, 0.75, 0.0), rot_b: Matrix3::identity(),
            body_id_a: 0, body_id_b: 1, friction: 0.5, restitution: 0.0,
        };
        let m = inter_body_contacts(&shape_b, &shape_s, &pair);
        assert!(!m.is_empty());
    }

    // ==========================================================================
    // Polygon clipping tests
    // ==========================================================================

    #[test]
    fn test_clip_polygon_no_clip() {
        // Square fully on positive side of plane → unchanged
        let square = vec![
            Vector3::new(0.0, 0.0, 0.0),
            Vector3::new(1.0, 0.0, 0.0),
            Vector3::new(1.0, 1.0, 0.0),
            Vector3::new(0.0, 1.0, 0.0),
        ];
        let result = clip_polygon_by_plane(&square, Vector3::x(), -1.0);
        assert_eq!(result.len(), 4);
    }

    #[test]
    fn test_clip_polygon_full_clip() {
        // Square fully on negative side of plane → empty
        let square = vec![
            Vector3::new(0.0, 0.0, 0.0),
            Vector3::new(1.0, 0.0, 0.0),
            Vector3::new(1.0, 1.0, 0.0),
            Vector3::new(0.0, 1.0, 0.0),
        ];
        let result = clip_polygon_by_plane(&square, Vector3::x(), 5.0);
        assert!(result.is_empty());
    }

    #[test]
    fn test_clip_polygon_half_clip() {
        // Square [0,1]x[0,1] clipped by x >= 0.5 → right half (4 vertices)
        let square = vec![
            Vector3::new(0.0, 0.0, 0.0),
            Vector3::new(1.0, 0.0, 0.0),
            Vector3::new(1.0, 1.0, 0.0),
            Vector3::new(0.0, 1.0, 0.0),
        ];
        let result = clip_polygon_by_plane(&square, Vector3::x(), 0.5);
        assert_eq!(result.len(), 4);
        // All resulting vertices should have x >= 0.5
        for v in &result {
            assert!(v.x >= 0.5 - 1e-5, "x={}", v.x);
        }
    }

    #[test]
    fn test_sutherland_hodgman_clip_quad_against_quad() {
        // Clip a 2x2 square centered at origin against a 1x1 square's side planes
        let big_square = vec![
            Vector3::new(-1.0, -1.0, 0.0),
            Vector3::new(1.0, -1.0, 0.0),
            Vector3::new(1.0, 1.0, 0.0),
            Vector3::new(-1.0, 1.0, 0.0),
        ];
        let clip_planes = vec![
            (Vector3::x(), -0.5),   // x >= -0.5
            (-Vector3::x(), -0.5),  // x <= 0.5 (i.e., -x >= -0.5)
            (Vector3::y(), -0.5),   // y >= -0.5
            (-Vector3::y(), -0.5),  // y <= 0.5
        ];
        let result = sutherland_hodgman_clip(&big_square, &clip_planes);
        assert_eq!(result.len(), 4);
        for v in &result {
            assert!(v.x >= -0.5 - 1e-5 && v.x <= 0.5 + 1e-5, "x={}", v.x);
            assert!(v.y >= -0.5 - 1e-5 && v.y <= 0.5 + 1e-5, "y={}", v.y);
        }
    }

    #[test]
    fn test_clip_degenerate_empty() {
        let result = clip_polygon_by_plane(&[], Vector3::x(), 0.0);
        assert!(result.is_empty());

        let result = sutherland_hodgman_clip(&[], &[(Vector3::x(), 0.0)]);
        assert!(result.is_empty());
    }

    #[test]
    fn test_clip_numerical_stability() {
        // Edge nearly parallel to clip plane (within epsilon)
        let polygon = vec![
            Vector3::new(0.0, 0.0, 0.0),
            Vector3::new(1.0, 1e-8, 0.0),
            Vector3::new(1.0, 1.0, 0.0),
            Vector3::new(0.0, 1.0, 0.0),
        ];
        // Clip at y >= 0.0 — bottom edge is nearly on the plane
        let result = clip_polygon_by_plane(&polygon, Vector3::y(), 0.0);
        // Should not panic or produce NaN
        assert!(!result.is_empty());
        for v in &result {
            assert!(v.x.is_finite() && v.y.is_finite() && v.z.is_finite());
        }
    }

    // ==========================================================================
    // Contact reduction tests
    // ==========================================================================

    #[test]
    fn test_reduce_fewer_than_max() {
        let mut manifold = ContactManifold::new();
        for i in 0..3 {
            manifold.add_contact(ContactPoint::new(
                0, Vector3::new(i as f32, 0.0, 0.0), Vector3::zeros(), Vector3::y(), 0.01,
            ));
        }
        manifold.reduce_to_best_n(4);
        assert_eq!(manifold.len(), 3); // No reduction needed
    }

    #[test]
    fn test_reduce_preserves_deepest() {
        let mut manifold = ContactManifold::new();
        // 6 contacts, one very deep
        for i in 0..6 {
            let pen = if i == 3 { 0.1 } else { 0.001 };
            manifold.add_contact(ContactPoint::new(
                0, Vector3::new(i as f32 * 0.1, 0.0, 0.0), Vector3::zeros(),
                Vector3::y(), pen,
            ));
        }
        manifold.reduce_to_best_n(4);
        assert_eq!(manifold.len(), 4);
        // Deepest contact (pen=0.1) must be in result
        assert!(manifold.contacts.iter().any(|c| (c.penetration - 0.1).abs() < 1e-5));
    }

    #[test]
    fn test_reduce_square_patch() {
        let mut manifold = ContactManifold::new();
        // 4 corners of a square + 4 midpoints
        let corners = [
            Vector3::new(-0.5, 0.0, -0.5),
            Vector3::new(0.5, 0.0, -0.5),
            Vector3::new(0.5, 0.0, 0.5),
            Vector3::new(-0.5, 0.0, 0.5),
        ];
        let mids = [
            Vector3::new(0.0, 0.0, -0.5),
            Vector3::new(0.5, 0.0, 0.0),
            Vector3::new(0.0, 0.0, 0.5),
            Vector3::new(-0.5, 0.0, 0.0),
        ];
        for &p in corners.iter().chain(mids.iter()) {
            manifold.add_contact(ContactPoint::new(
                0, p, Vector3::zeros(), Vector3::y(), 0.01,
            ));
        }
        manifold.reduce_to_best_n(4);
        assert_eq!(manifold.len(), 4);
        // Result should maximize area — corners should be selected (area=1.0)
        // vs midpoints which form a smaller diamond (area=0.5)
    }

    #[test]
    fn test_select_max_area_indices() {
        let contacts: Vec<ContactPoint> = (0..6).map(|i| {
            ContactPoint::new(
                0, Vector3::new(i as f32 * 0.2, 0.0, (i % 2) as f32 * 0.5),
                Vector3::zeros(), Vector3::y(), 0.01,
            )
        }).collect();
        let indices = select_max_area_contacts(&contacts, 4);
        assert_eq!(indices.len(), 4);
        // All indices should be valid
        for &idx in &indices {
            assert!(idx < 6);
        }
        // No duplicates
        let unique: std::collections::HashSet<_> = indices.iter().collect();
        assert_eq!(unique.len(), 4);
    }

    // ======================================================================
    // Capsule contact geometry tests
    // ======================================================================

    #[test]
    fn test_capsule_capsule_aligned_overlapping() {
        // Two upright capsules side by side, overlapping
        let c = capsule_capsule_contact(
            Vector3::new(0.0, 0.0, 0.0), Matrix3::identity(), 0.5, 0.1,
            Vector3::new(0.15, 0.0, 0.0), Matrix3::identity(), 0.5, 0.1,
            0, 0.5, 0.0,
        );
        assert!(c.is_some(), "Overlapping capsules should produce contact");
        let c = c.unwrap();
        assert!(c.penetration > 0.0, "Penetration should be positive");
        // Normal should point from A to B (positive x)
        assert!(c.normal.x > 0.5, "Normal should point along x: {:?}", c.normal);
    }

    #[test]
    fn test_capsule_capsule_separated() {
        let c = capsule_capsule_contact(
            Vector3::zeros(), Matrix3::identity(), 0.5, 0.1,
            Vector3::new(5.0, 0.0, 0.0), Matrix3::identity(), 0.5, 0.1,
            0, 0.5, 0.0,
        );
        assert!(c.is_none(), "Far-apart capsules should produce no contact");
    }

    #[test]
    fn test_capsule_sphere_contact_touching() {
        // Sphere beside capsule
        let c = capsule_sphere_contact(
            Vector3::zeros(), Matrix3::identity(), 0.5, 0.1,
            Vector3::new(0.15, 0.0, 0.0), 0.1,
            0, 0.5, 0.0,
        );
        assert!(c.is_some(), "Touching capsule-sphere should produce contact");
        let c = c.unwrap();
        assert!(c.penetration > 0.0);
    }

    #[test]
    fn test_capsule_sphere_at_endpoint() {
        // Sphere at the top endpoint of capsule
        let c = capsule_sphere_contact(
            Vector3::zeros(), Matrix3::identity(), 0.5, 0.1,
            Vector3::new(0.0, 0.55, 0.0), 0.1, // just above top hemisphere
            0, 0.5, 0.0,
        );
        assert!(c.is_some(), "Sphere at capsule endpoint should produce contact");
        let c = c.unwrap();
        assert!(c.penetration > 0.0);
        // Normal should point upward (from capsule to sphere)
        assert!(c.normal.y > 0.5, "Normal should point up: {:?}", c.normal);
    }

    #[test]
    fn test_sphere_box_inside_box() {
        // Sphere center is inside the box - should still produce valid contact
        let c = sphere_box_contact(
            Vector3::new(0.0, 0.0, 0.0), 0.1, // sphere at box center
            Vector3::zeros(), Matrix3::identity(),
            Vector3::new(0.5, 0.5, 0.5),
            0, 0.5, 0.0,
        );
        assert!(c.is_some(), "Sphere inside box should produce contact");
        let c = c.unwrap();
        assert!(c.penetration > 0.0, "Penetration should be positive");
        // Normal should push toward closest face
        let n_abs = Vector3::new(c.normal.x.abs(), c.normal.y.abs(), c.normal.z.abs());
        let max_component = n_abs.x.max(n_abs.y).max(n_abs.z);
        assert!(max_component > 0.9, "Normal should be axis-aligned for centered sphere");
    }

    #[test]
    fn test_sphere_sphere_coincident_centers() {
        // Degenerate case: sphere centers at same position
        let c = sphere_sphere_contact(
            Vector3::zeros(), 0.5,
            Vector3::zeros(), 0.5,
            0, 0.5, 0.0,
        );
        assert!(c.is_some(), "Coincident spheres should produce contact");
        let c = c.unwrap();
        assert_relative_eq!(c.penetration, 1.0, epsilon = 1e-5); // ra + rb - 0
        // Normal should default to y-up for degenerate case
        assert_relative_eq!(c.normal.y, 1.0, epsilon = 1e-5);
    }

    #[test]
    fn test_contact_normal_always_normalized() {
        // Intent: all contact functions should return unit normals
        let test_cases: Vec<Option<ContactPoint>> = vec![
            sphere_sphere_contact(
                Vector3::zeros(), 0.5,
                Vector3::new(0.8, 0.0, 0.0), 0.5,
                0, 0.5, 0.0,
            ),
            sphere_box_contact(
                Vector3::new(0.0, 0.55, 0.0), 0.1,
                Vector3::zeros(), Matrix3::identity(),
                Vector3::new(0.5, 0.5, 0.5),
                0, 0.5, 0.0,
            ),
            capsule_sphere_contact(
                Vector3::zeros(), Matrix3::identity(), 0.5, 0.1,
                Vector3::new(0.15, 0.0, 0.0), 0.1,
                0, 0.5, 0.0,
            ),
        ];

        for (i, case) in test_cases.into_iter().enumerate() {
            if let Some(c) = case {
                let norm = c.normal.norm();
                assert!(
                    (norm - 1.0).abs() < 1e-4,
                    "Contact {i} normal should be unit length, got {norm}"
                );
            }
        }
    }

    #[test]
    fn test_inter_body_capsule_dispatch() {
        // Verify inter_body_contacts dispatches capsule-capsule
        let shape_c = StoredShape::Capsule { half_height: 0.5, radius: 0.1 };
        let pair = ShapePair {
            pos_a: Vector3::zeros(), rot_a: Matrix3::identity(),
            pos_b: Vector3::new(0.15, 0.0, 0.0), rot_b: Matrix3::identity(),
            body_id_a: 0, body_id_b: 1, friction: 0.5, restitution: 0.0,
        };
        let m = inter_body_contacts(&shape_c, &shape_c, &pair);
        assert!(!m.is_empty(), "Capsule-capsule dispatch should find contact");
    }

    #[test]
    fn test_contact_zero_normal_fallback() {
        // Intent: zero-length normal should fallback to z-axis, not NaN
        let cp = ContactPoint::new(
            0,
            Vector3::zeros(),
            Vector3::zeros(),
            Vector3::zeros(), // zero normal!
            0.01,
        );
        let norm = cp.normal.norm();
        assert!(
            (norm - 1.0).abs() < 1e-5,
            "Zero normal should be replaced with fallback, got norm={norm}"
        );
    }

    // ====================================================================
    // Builder chaining tests
    // ====================================================================

    #[test]
    fn test_with_friction_and_restitution_chaining() {
        let cp = ContactPoint::new(0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.01)
            .with_friction(0.8)
            .with_restitution(0.6);
        assert_relative_eq!(cp.friction, 0.8, epsilon = 1e-10);
        assert_relative_eq!(cp.restitution, 0.6, epsilon = 1e-10);
        // Other fields unchanged
        assert_relative_eq!(cp.penetration, 0.01, epsilon = 1e-10);
    }

    // ====================================================================
    // Box face polygon tests
    // ====================================================================

    #[test]
    fn test_get_box_face_polygon_identity_z_positive() {
        let poly = get_box_face_polygon(
            Vector3::zeros(), Matrix3::identity(), Vector3::new(1.0, 1.0, 1.0),
            2, 1.0, // Z-axis, positive face
        );
        // All 4 vertices should have z = 1.0 (the +Z face)
        for (i, v) in poly.iter().enumerate() {
            assert!((v.z - 1.0).abs() < 1e-5,
                "vertex {i} should be on z=1 face, got z={}", v.z);
        }
        // Vertices should span the face: x in [-1,1], y in [-1,1]
        let xs: Vec<f32> = poly.iter().map(|v| v.x).collect();
        let ys: Vec<f32> = poly.iter().map(|v| v.y).collect();
        assert!(xs.iter().any(|&x| x < -0.5), "should have x < -0.5");
        assert!(xs.iter().any(|&x| x > 0.5), "should have x > 0.5");
        assert!(ys.iter().any(|&y| y < -0.5), "should have y < -0.5");
        assert!(ys.iter().any(|&y| y > 0.5), "should have y > 0.5");
    }

    #[test]
    fn test_get_box_face_polygon_negative_face() {
        let poly = get_box_face_polygon(
            Vector3::zeros(), Matrix3::identity(), Vector3::new(2.0, 3.0, 4.0),
            1, -1.0, // Y-axis, negative face → y = -3
        );
        for (i, v) in poly.iter().enumerate() {
            assert!((v.y - (-3.0)).abs() < 1e-5,
                "vertex {i} should be on y=-3 face, got y={}", v.y);
        }
    }

    // ====================================================================
    // Select incident face tests
    // ====================================================================

    #[test]
    fn test_select_incident_face_aligned_with_y() {
        // Reference normal pointing +Y → incident face is the -Y face
        let (idx, sign) = select_incident_face(Matrix3::identity(), Vector3::y());
        assert_eq!(idx, 1, "should select Y axis");
        assert!((sign - (-1.0)).abs() < 1e-5,
            "should pick negative Y face (most anti-aligned), got sign={sign}");
    }

    #[test]
    fn test_select_incident_face_aligned_with_neg_x() {
        let (idx, sign) = select_incident_face(Matrix3::identity(), -Vector3::x());
        assert_eq!(idx, 0, "should select X axis");
        assert!((sign - 1.0).abs() < 1e-5,
            "should pick positive X face (anti-aligned with -X normal), got sign={sign}");
    }

    // ====================================================================
    // Tangent frame tests
    // ====================================================================

    #[test]
    fn test_compute_tangent_frame_orthonormal() {
        for normal in &[Vector3::x(), Vector3::y(), Vector3::z(),
                        Vector3::new(1.0, 1.0, 1.0).normalize()] {
            let (t1, t2) = compute_tangent_frame_pub(normal);
            // Orthogonality
            assert!(t1.dot(normal).abs() < 1e-5, "t1 should be perpendicular to normal");
            assert!(t2.dot(normal).abs() < 1e-5, "t2 should be perpendicular to normal");
            assert!(t1.dot(&t2).abs() < 1e-5, "t1 should be perpendicular to t2");
            // Unit length
            assert_relative_eq!(t1.norm(), 1.0, epsilon = 1e-5);
            assert_relative_eq!(t2.norm(), 1.0, epsilon = 1e-5);
        }
    }

    // ====================================================================
    // ShapePair / from_pair dispatch tests
    // ====================================================================

    #[test]
    fn test_box_box_contacts_from_pair_overlapping() {
        let pair = ShapePair {
            pos_a: Vector3::new(0.0, 0.0, 0.0),
            rot_a: Matrix3::identity(),
            pos_b: Vector3::new(0.9, 0.0, 0.0), // overlapping by 0.1
            rot_b: Matrix3::identity(),
            body_id_a: 0,
            body_id_b: 1,
            friction: 0.4,
            restitution: 0.2,
        };
        let contacts = box_box_contacts_from_pair(&pair,
            Vector3::new(0.5, 0.5, 0.5), Vector3::new(0.5, 0.5, 0.5));
        // Should detect contact
        assert!(!contacts.is_empty(), "overlapping boxes should produce contacts");
        // Friction/restitution should come from pair
        assert_relative_eq!(contacts[0].friction, 0.4, epsilon = 1e-5);
        assert_relative_eq!(contacts[0].restitution, 0.2, epsilon = 1e-5);
    }

    #[test]
    fn test_capsule_capsule_from_pair_overlapping() {
        let pair = ShapePair {
            pos_a: Vector3::zeros(),
            rot_a: Matrix3::identity(),
            pos_b: Vector3::new(0.15, 0.0, 0.0), // close together
            rot_b: Matrix3::identity(),
            body_id_a: 0,
            body_id_b: 1,
            friction: 0.3,
            restitution: 0.1,
        };
        let result = capsule_capsule_contact_from_pair(&pair, 0.5, 0.1, 0.5, 0.1);
        assert!(result.is_some(), "close capsules should produce contact");
        let cp = result.unwrap();
        assert_relative_eq!(cp.friction, 0.3, epsilon = 1e-5);
    }

    #[test]
    fn test_inter_body_contacts_from_pair_spheres() {
        let pair = ShapePair {
            pos_a: Vector3::zeros(),
            rot_a: Matrix3::identity(),
            pos_b: Vector3::new(0.15, 0.0, 0.0),
            rot_b: Matrix3::identity(),
            body_id_a: 0,
            body_id_b: 1,
            friction: 0.5,
            restitution: 0.0,
        };
        let shape_a = StoredShape::Sphere { radius: 0.1 };
        let shape_b = StoredShape::Sphere { radius: 0.1 };
        let manifold = inter_body_contacts_from_pair(&pair, &shape_a, &shape_b);
        // Two spheres separated by 0.15, radii 0.1 each → overlap = 0.05
        assert!(manifold.active_count() > 0, "overlapping spheres should produce contacts");
    }

    // ── SLAM Cycle 1: Contact intent/property tests ───────────────────

    #[test]
    fn intent_ground_contacts_only_for_penetrating_bodies() {
        // Intent: ground_plane_contacts should not generate contacts for bodies above ground
        let mut body = ArticulatedBody::new();
        body.set_gravity(Vector3::new(0.0, -9.81, 0.0));
        body.add_body("link", -1, GenJointType::Floating,
            SpatialInertia::sphere(1.0, 0.5),
            SpatialTransform::identity());
        body.set_joint_q(0, &[0.0, 5.0, 0.0, 1.0, 0.0, 0.0, 0.0]); // high up

        let manifold = ground_plane_contacts(&body, 0.0, Vector3::y(), 0.5, 0.0);
        assert_eq!(manifold.active_count(), 0,
            "body at y=5 should have no ground contacts, got {}", manifold.active_count());
    }

    #[test]
    fn intent_sphere_sphere_separated_no_contact() {
        // Intent: Two non-touching spheres should produce no active contacts
        let pos_a = Vector3::new(0.0, 0.0, 0.0);
        let pos_b = Vector3::new(5.0, 0.0, 0.0);
        let result = sphere_sphere_contact(pos_a, 0.5, pos_b, 0.5, 0, 0.5, 0.0);
        assert!(result.is_none() || result.as_ref().map_or(true, |c| c.penetration < 0.0),
            "Spheres 5m apart with r=0.5 should not contact");
    }

    #[test]
    fn property_contact_normal_always_unit() {
        // Property: Contact normals must always be unit vectors
        let pos_a = Vector3::new(0.0, 0.0, 0.0);
        let pos_b = Vector3::new(0.8, 0.0, 0.0);
        if let Some(contact) = sphere_sphere_contact(pos_a, 0.5, pos_b, 0.5, 0, 0.5, 0.0) {
            let norm = contact.normal.norm();
            assert!((norm - 1.0).abs() < 0.01,
                "Contact normal should be unit, got norm={norm}");
        }
    }

    #[test]
    fn intent_manifold_prune_removes_separated() {
        // Intent: prune_separated should remove contacts with negative penetration
        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.01));
        manifold.add_contact(ContactPoint::new(0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), -0.5));
        manifold.add_contact(ContactPoint::new(0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.005));

        manifold.prune_separated(-0.001);
        assert_eq!(manifold.active_count(), 2, "Should keep 2 active contacts, got {}", manifold.active_count());
    }

    // ── SLAM Cycle 6: Tests for untested public functions ─────────────

    #[test]
    fn test_capsule_box_contact_overlapping() {
        // Capsule inside box should detect contacts
        let cap_pos = Vector3::new(0.0, 0.0, 0.0);
        let cap_rot = Matrix3::identity();
        let half_h = 0.3;
        let cap_radius = 0.1;
        let box_pos = Vector3::new(0.0, 0.0, 0.0);
        let box_rot = Matrix3::identity();
        let box_he = Vector3::new(0.5, 0.5, 0.5);

        let contacts = capsule_box_contact(
            cap_pos, cap_rot, half_h, cap_radius,
            box_pos, box_rot, box_he,
            0, 0.5, 0.0,
        );
        // Capsule is fully inside the box, so contacts may or may not be generated
        // depending on sphere_box_contact behavior for inside-box case.
        // At minimum, verify no panic and finite values.
        for c in &contacts {
            assert!(c.point_world.x.is_finite(), "contact point must be finite");
            assert!(c.normal.norm() > 0.5, "contact normal must be non-zero");
        }
    }

    #[test]
    fn test_capsule_box_contact_separated() {
        // Capsule far from box should produce no contacts
        let cap_pos = Vector3::new(10.0, 0.0, 0.0);
        let cap_rot = Matrix3::identity();
        let box_pos = Vector3::new(0.0, 0.0, 0.0);
        let box_rot = Matrix3::identity();
        let box_he = Vector3::new(0.5, 0.5, 0.5);

        let contacts = capsule_box_contact(
            cap_pos, cap_rot, 0.3, 0.1,
            box_pos, box_rot, box_he,
            0, 0.5, 0.0,
        );
        assert!(contacts.is_empty(), "separated capsule-box should have no contacts");
    }

    #[test]
    fn test_select_max_area_contacts_returns_correct_count() {
        // select_max_area_contacts should return at most max_n indices
        let contacts = vec![
            ContactPoint::new(0, Vector3::new(0.0, 0.0, 0.0), Vector3::zeros(), Vector3::y(), 0.01),
            ContactPoint::new(0, Vector3::new(1.0, 0.0, 0.0), Vector3::zeros(), Vector3::y(), 0.01),
            ContactPoint::new(0, Vector3::new(0.0, 0.0, 1.0), Vector3::zeros(), Vector3::y(), 0.01),
            ContactPoint::new(0, Vector3::new(1.0, 0.0, 1.0), Vector3::zeros(), Vector3::y(), 0.01),
            ContactPoint::new(0, Vector3::new(0.5, 0.0, 0.5), Vector3::zeros(), Vector3::y(), 0.01),
        ];
        let selected = select_max_area_contacts(&contacts, 3);
        assert!(selected.len() <= 3, "should select at most 3, got {}", selected.len());
        // All indices should be valid
        for &idx in &selected {
            assert!(idx < contacts.len(), "index {idx} out of range");
        }
    }

    // ── SLAM Cycle 8: Edge cases and wrapper tests ────────────────────

    #[test]
    fn test_inter_body_contacts_sphere_sphere() {
        let shape = StoredShape::Sphere { radius: 0.5 };
        let pair = ShapePair {
            pos_a: Vector3::new(0.0, 0.0, 0.0),
            rot_a: Matrix3::identity(),
            pos_b: Vector3::new(0.8, 0.0, 0.0),
            rot_b: Matrix3::identity(),
            body_id_a: 0,
            body_id_b: 1,
            friction: 0.5,
            restitution: 0.0,
        };
        let manifold = inter_body_contacts(&shape, &shape, &pair);
        // Spheres with 0.8m gap and 0.5 radii overlap by 0.2
        assert!(!manifold.is_empty(), "overlapping spheres should produce contacts");
    }

    #[test]
    fn test_inter_body_contacts_separated_spheres() {
        let shape = StoredShape::Sphere { radius: 0.5 };
        let pair = ShapePair {
            pos_a: Vector3::new(0.0, 0.0, 0.0),
            rot_a: Matrix3::identity(),
            pos_b: Vector3::new(5.0, 0.0, 0.0),
            rot_b: Matrix3::identity(),
            body_id_a: 0,
            body_id_b: 1,
            friction: 0.5,
            restitution: 0.0,
        };
        let manifold = inter_body_contacts(&shape, &shape, &pair);
        assert_eq!(manifold.active_count(), 0, "separated spheres should have no active contacts");
    }

    #[test]
    fn test_box_box_contacts_from_pair_wrapper() {
        let pair = ShapePair {
            pos_a: Vector3::new(0.0, 0.0, 0.0),
            rot_a: Matrix3::identity(),
            pos_b: Vector3::new(0.9, 0.0, 0.0),
            rot_b: Matrix3::identity(),
            body_id_a: 0,
            body_id_b: 1,
            friction: 0.5,
            restitution: 0.0,
        };
        let he = Vector3::new(0.5, 0.5, 0.5);
        let manifold = box_box_contacts_from_pair(&pair, he, he);
        // Boxes at 0 and 0.9 with half_extent 0.5 overlap by 0.1
        assert!(!manifold.is_empty(), "overlapping boxes should produce contacts");
    }

    #[test]
    fn test_manifold_merge_three_contacts() {
        let mut m1 = ContactManifold::new();
        m1.add_contact(ContactPoint::new(0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.01));
        let mut m2 = ContactManifold::new();
        m2.add_contact(ContactPoint::new(1, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.02));
        m2.add_contact(ContactPoint::new(1, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.03));

        m1.merge(&m2);
        assert_eq!(m1.len(), 3, "merged manifold should have 3 contacts");
    }

    #[test]
    fn test_manifold_clear() {
        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.01));
        manifold.add_contact(ContactPoint::new(0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.02));
        assert_eq!(manifold.len(), 2);
        manifold.clear();
        assert_eq!(manifold.len(), 0, "cleared manifold should be empty");
    }

    #[test]
    fn test_manifold_sort_by_penetration_ordering() {
        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.01));
        manifold.add_contact(ContactPoint::new(0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.05));
        manifold.add_contact(ContactPoint::new(0, Vector3::zeros(), Vector3::zeros(), Vector3::y(), 0.02));

        manifold.sort_by_penetration();
        // After sort, deepest (0.05) should be first
        assert!(manifold.contacts[0].penetration >= manifold.contacts[1].penetration,
            "sort should put deepest first");
    }

    #[test]
    fn test_reduce_to_best_n_fewer_than_max() {
        // If we have fewer contacts than max, all should be kept
        let mut manifold = ContactManifold::new();
        manifold.add_contact(ContactPoint::new(0, Vector3::new(0.0, 0.0, 0.0), Vector3::zeros(), Vector3::y(), 0.01));
        manifold.add_contact(ContactPoint::new(0, Vector3::new(1.0, 0.0, 0.0), Vector3::zeros(), Vector3::y(), 0.02));

        manifold.reduce_to_best_n(5);
        assert_eq!(manifold.len(), 2, "should keep all when fewer than max");
    }

    #[test]
    fn test_reduce_to_best_n_exactly_max() {
        let mut manifold = ContactManifold::new();
        for i in 0..4 {
            manifold.add_contact(ContactPoint::new(0,
                Vector3::new(i as f32, 0.0, 0.0), Vector3::zeros(), Vector3::y(), 0.01));
        }
        manifold.reduce_to_best_n(4);
        assert_eq!(manifold.len(), 4, "should keep all when exactly max");
    }

    // ── SLAM Cycle 16: Inter-body box, manifold capacity ──────────────

    #[test]
    fn test_inter_body_contacts_box_box() {
        let shape = StoredShape::Box { half_extents: Vector3::new(0.5, 0.5, 0.5) };
        let pair = ShapePair {
            pos_a: Vector3::new(0.0, 0.0, 0.0),
            rot_a: Matrix3::identity(),
            pos_b: Vector3::new(0.9, 0.0, 0.0),
            rot_b: Matrix3::identity(),
            body_id_a: 0,
            body_id_b: 1,
            friction: 0.5,
            restitution: 0.0,
        };
        let manifold = inter_body_contacts(&shape, &shape, &pair);
        assert!(!manifold.is_empty(), "overlapping boxes via inter_body should produce contacts");
    }

    #[test]
    fn test_capsule_sphere_contact_overlapping() {
        let result = capsule_sphere_contact(
            Vector3::new(0.0, 0.0, 0.0), Matrix3::identity(), 0.3, 0.1, // capsule
            Vector3::new(0.15, 0.0, 0.0), 0.2, // sphere overlapping capsule
            0, 0.5, 0.0,
        );
        assert!(result.is_some(), "overlapping capsule-sphere should produce a contact");
        let c = result.unwrap();
        assert!(c.penetration > 0.0, "should have positive penetration");
    }

    #[test]
    fn test_capsule_capsule_contact_from_pair_wrapper() {
        let pair = ShapePair {
            pos_a: Vector3::new(0.0, 0.0, 0.0),
            rot_a: Matrix3::identity(),
            pos_b: Vector3::new(0.3, 0.0, 0.0),
            rot_b: Matrix3::identity(),
            body_id_a: 0, body_id_b: 1, friction: 0.5, restitution: 0.0,
        };
        let result = capsule_capsule_contact_from_pair(&pair, 0.3, 0.1, 0.3, 0.1);
        // Two capsules close together
        for c in &result {
            assert!(c.normal.norm() > 0.5, "contact normal should be non-zero");
        }
    }

    #[test]
    fn test_manifold_with_capacity_empty() {
        let manifold = ContactManifold::with_capacity(10);
        assert!(manifold.is_empty());
        assert_eq!(manifold.len(), 0);
    }
}