//! Step, broadphase, manifold update, warm start, contact solver, position solver, event generation.

use std::collections::{BTreeMap, BTreeSet};

use crate::collider::ColliderHandle;
use crate::event::CollisionEvent;
use crate::spatial_hash::SpatialHashGrid;

use super::narrowphase::*;
use super::state::PhysicsState2d;
use super::types::*;
use super::{body_ah, coll_ah};

impl PhysicsState2d {
    // -----------------------------------------------------------------------
    // Simulation step
    // -----------------------------------------------------------------------

    #[allow(clippy::too_many_arguments)]
    pub fn step(
        &mut self,
        gravity: [f64; 3],
        dt: f64,
        velocity_iterations: u32,
        position_iterations: u32,
        slop: f64,
        correction: f64,
        max_velocity: f64,
        constraint_frequency: f64,
        constraint_damping_ratio: f64,
        broadphase_kind: crate::config::BroadphaseKind,
        solver_kind: crate::config::SolverKind,
    ) -> Vec<CollisionEvent> {
        match solver_kind {
            crate::config::SolverKind::Xpbd => self.step_xpbd(
                gravity,
                dt,
                velocity_iterations,
                slop,
                max_velocity,
                constraint_frequency,
                broadphase_kind,
            ),
            _ => self.step_si(
                gravity,
                dt,
                velocity_iterations,
                position_iterations,
                slop,
                correction,
                max_velocity,
                constraint_frequency,
                constraint_damping_ratio,
                broadphase_kind,
            ),
        }
    }

    // -----------------------------------------------------------------------
    // Sequential Impulse step (default)
    // -----------------------------------------------------------------------

    #[allow(clippy::too_many_arguments)]
    fn step_si(
        &mut self,
        gravity: [f64; 3],
        dt: f64,
        velocity_iterations: u32,
        position_iterations: u32,
        slop: f64,
        correction: f64,
        max_velocity: f64,
        constraint_frequency: f64,
        constraint_damping_ratio: f64,
        broadphase_kind: crate::config::BroadphaseKind,
    ) -> Vec<CollisionEvent> {
        let gravity_2d = [gravity[0], gravity[1]];
        // 1. Integrate velocities
        for rb in self.bodies.values_mut() {
            rb.integrate_velocities(gravity_2d, dt, max_velocity);
        }

        // 2. Broadphase (with speculative expansion by velocity)
        let broad_pairs = self.broadphase(broadphase_kind, dt);

        // 3. Narrowphase — raw contacts
        let contacts = self.narrowphase(&broad_pairs);

        // 4. Update manifold cache (match contacts to existing manifolds)
        self.update_manifolds(&contacts);

        // 5. Wake sleeping bodies on contact with non-sleeping moving bodies
        self.wake_on_contact();

        // 6. Warm start — apply cached impulses from previous frame
        self.warm_start();

        // 7. Solve velocity constraints (using manifolds with accumulation)
        self.solve_contacts(velocity_iterations);

        // 8. Solve joint constraints
        self.solve_joints(dt, velocity_iterations);

        // 9. Positional correction (soft constraint or Baumgarte)
        self.solve_positions(
            position_iterations,
            slop,
            correction,
            dt,
            constraint_frequency,
            constraint_damping_ratio,
        );

        // 10. Integrate positions
        for rb in self.bodies.values_mut() {
            rb.integrate_positions(dt);
        }

        // 11. Build simulation islands and sleep check
        self.build_islands_and_sleep(dt);

        // 12. Clear forces
        for rb in self.bodies.values_mut() {
            rb.clear_forces();
        }

        // 13. Generate collision events (from manifold keys)
        self.generate_events()
    }

    // -----------------------------------------------------------------------
    // XPBD step — position-based dynamics with compliance
    // -----------------------------------------------------------------------

    #[allow(clippy::too_many_arguments)]
    fn step_xpbd(
        &mut self,
        gravity: [f64; 3],
        dt: f64,
        iterations: u32,
        slop: f64,
        max_velocity: f64,
        constraint_frequency: f64,
        broadphase_kind: crate::config::BroadphaseKind,
    ) -> Vec<CollisionEvent> {
        let gravity_2d = [gravity[0], gravity[1]];

        // 1. Store previous positions and integrate velocities
        for rb in self.bodies.values_mut() {
            rb.prev_position = rb.position;
            rb.prev_rotation = rb.rotation;
            rb.integrate_velocities(gravity_2d, dt, max_velocity);
        }

        // 2. Predict positions: x += v * dt
        for rb in self.bodies.values_mut() {
            if !rb.is_dynamic() || rb.inv_mass == 0.0 || rb.is_sleeping {
                continue;
            }
            rb.position[0] += rb.linear_velocity[0] * dt;
            rb.position[1] += rb.linear_velocity[1] * dt;
            if !rb.fixed_rotation {
                rb.rotation += rb.angular_velocity * dt;
            }
        }

        // 3. Broadphase + narrowphase at predicted positions
        let broad_pairs = self.broadphase(broadphase_kind, dt);
        let contacts = self.narrowphase(&broad_pairs);
        self.update_manifolds(&contacts);

        // 4. Wake on contact
        self.wake_on_contact();

        // 5. XPBD constraint solving — position-level with compliance
        // Compliance: α̃ = 1 / (stiffness * dt²)
        let alpha_tilde = if constraint_frequency > 0.0 && dt > EPSILON {
            let omega = 2.0 * std::f64::consts::PI * constraint_frequency;
            1.0 / (omega * omega * dt * dt)
        } else {
            0.0 // infinite stiffness (or dt too small for compliance)
        };

        // Initialize Lagrange multipliers for each manifold point
        let manifold_keys: Vec<ManifoldKey> = self.manifolds.keys().copied().collect();
        let mut lambdas: BTreeMap<(ManifoldKey, usize), f64> = BTreeMap::new();
        for key in &manifold_keys {
            if let Some(m) = self.manifolds.get(key) {
                for pi in 0..m.points.len() {
                    lambdas.insert((*key, pi), 0.0);
                }
            }
        }

        for _ in 0..iterations {
            // Contact constraints
            self.xpbd_solve_contacts(&manifold_keys, &mut lambdas, alpha_tilde, slop);

            // Joint constraints
            self.xpbd_solve_joints(dt, alpha_tilde);
        }

        // 6. Derive velocities from position change
        let inv_dt = if dt > EPSILON { 1.0 / dt } else { 0.0 };
        for rb in self.bodies.values_mut() {
            if !rb.is_dynamic() || rb.inv_mass == 0.0 || rb.is_sleeping {
                continue;
            }
            rb.linear_velocity[0] = (rb.position[0] - rb.prev_position[0]) * inv_dt;
            rb.linear_velocity[1] = (rb.position[1] - rb.prev_position[1]) * inv_dt;
            if !rb.fixed_rotation {
                rb.angular_velocity = (rb.rotation - rb.prev_rotation) * inv_dt;
            }
        }

        // 7. Velocity-level friction and restitution
        self.xpbd_velocity_solve(&manifold_keys);

        // 8. Islands + sleep
        self.build_islands_and_sleep(dt);

        // 9. Clear forces
        for rb in self.bodies.values_mut() {
            rb.clear_forces();
        }

        // 10. Generate collision events
        self.generate_events()
    }

    /// Wake sleeping bodies that are in contact with moving non-sleeping bodies.
    fn wake_on_contact(&mut self) {
        for manifold in self.manifolds.values() {
            let a_sleeping = self
                .bodies
                .get(body_ah(manifold.body_a))
                .is_some_and(|b| b.is_sleeping);
            let b_sleeping = self
                .bodies
                .get(body_ah(manifold.body_b))
                .is_some_and(|b| b.is_sleeping);
            let a_moving = self.bodies.get(body_ah(manifold.body_a)).is_some_and(|b| {
                !b.is_sleeping
                    && b.is_dynamic()
                    && (b.linear_velocity[0].abs() > SLEEP_VELOCITY_THRESHOLD
                        || b.linear_velocity[1].abs() > SLEEP_VELOCITY_THRESHOLD
                        || b.angular_velocity.abs() > SLEEP_VELOCITY_THRESHOLD)
            });
            let b_moving = self.bodies.get(body_ah(manifold.body_b)).is_some_and(|b| {
                !b.is_sleeping
                    && b.is_dynamic()
                    && (b.linear_velocity[0].abs() > SLEEP_VELOCITY_THRESHOLD
                        || b.linear_velocity[1].abs() > SLEEP_VELOCITY_THRESHOLD
                        || b.angular_velocity.abs() > SLEEP_VELOCITY_THRESHOLD)
            });
            if a_sleeping
                && b_moving
                && let Some(ba) = self.bodies.get_mut(body_ah(manifold.body_a))
            {
                ba.is_sleeping = false;
                ba.sleep_timer = 0.0;
            }
            if b_sleeping
                && a_moving
                && let Some(bb) = self.bodies.get_mut(body_ah(manifold.body_b))
            {
                bb.is_sleeping = false;
                bb.sleep_timer = 0.0;
            }
        }
    }

    // -----------------------------------------------------------------------
    // Broadphase — spatial hash grid
    // -----------------------------------------------------------------------

    fn broadphase(
        &self,
        kind: crate::config::BroadphaseKind,
        dt: f64,
    ) -> Vec<(ColliderHandle, ColliderHandle)> {
        use crate::aabb_tree::{AabbTree, TreeAabb};

        // Compute AABBs for all colliders, expanded by velocity for speculative contacts
        let collider_aabbs: Vec<(ColliderHandle, Aabb2d)> = self
            .colliders
            .values()
            .filter_map(|c| {
                let rb = self.bodies.get(body_ah(c.body))?;
                let mut aabb = c.world_aabb(rb.position, rb.rotation);
                // Speculative expansion: expand AABB along velocity direction
                let dx = rb.linear_velocity[0] * dt;
                let dy = rb.linear_velocity[1] * dt;
                if dx > 0.0 {
                    aabb.max[0] += dx;
                } else {
                    aabb.min[0] += dx;
                }
                if dy > 0.0 {
                    aabb.max[1] += dy;
                } else {
                    aabb.min[1] += dy;
                }
                Some((c.handle, aabb))
            })
            .collect();

        // Get candidate pairs from broadphase algorithm
        let candidates = match kind {
            crate::config::BroadphaseKind::SpatialHash => {
                let cell_size = SpatialHashGrid::<ColliderHandle>::auto_cell_size(
                    collider_aabbs.iter().map(|(_, aabb)| {
                        let w = aabb.max[0] - aabb.min[0];
                        let h = aabb.max[1] - aabb.min[1];
                        w.max(h)
                    }),
                    collider_aabbs.len(),
                );
                let mut grid = SpatialHashGrid::new(cell_size);
                for (handle, aabb) in &collider_aabbs {
                    grid.insert_2d(*handle, aabb.min, aabb.max);
                }
                grid.query_pairs()
            }
            crate::config::BroadphaseKind::AabbTree => {
                let mut tree = AabbTree::new();
                for (handle, aabb) in &collider_aabbs {
                    tree.insert(*handle, TreeAabb::from_2d(aabb.min, aabb.max));
                }
                tree.query_pairs()
            }
        };

        // Build AABB lookup for overlap verification
        let aabb_map: BTreeMap<ColliderHandle, Aabb2d> = collider_aabbs.into_iter().collect();

        // Filter candidates
        let mut pairs = Vec::with_capacity(candidates.len());
        for (ha, hb) in candidates {
            let ca = match self.colliders.get(coll_ah(ha)) {
                Some(c) => c,
                None => continue,
            };
            let cb = match self.colliders.get(coll_ah(hb)) {
                Some(c) => c,
                None => continue,
            };
            // Skip same-body
            if ca.body == cb.body {
                continue;
            }
            // Skip static-static
            if let (Some(ba), Some(bb)) = (
                self.bodies.get(body_ah(ca.body)),
                self.bodies.get(body_ah(cb.body)),
            ) && ba.is_static()
                && bb.is_static()
            {
                continue;
            }
            // Skip sensor-sensor
            if ca.is_sensor && cb.is_sensor {
                continue;
            }
            // Skip if collision layers don't match
            if (ca.collision_layer & cb.collision_mask) == 0
                && (cb.collision_layer & ca.collision_mask) == 0
            {
                continue;
            }
            // Verify AABB overlap (spatial hash cells are conservative)
            if let (Some(aabb_a), Some(aabb_b)) = (aabb_map.get(&ha), aabb_map.get(&hb))
                && aabb_a.overlaps(aabb_b)
            {
                pairs.push((ha, hb));
            }
        }
        pairs
    }

    // -----------------------------------------------------------------------
    // Narrowphase
    // -----------------------------------------------------------------------

    fn narrowphase(&self, broad_pairs: &[(ColliderHandle, ColliderHandle)]) -> Vec<Contact> {
        let mut contacts = Vec::new();

        for (ha, hb) in broad_pairs {
            let ca = match self.colliders.get(coll_ah(*ha)) {
                Some(c) => c,
                None => continue,
            };
            let cb = match self.colliders.get(coll_ah(*hb)) {
                Some(c) => c,
                None => continue,
            };
            let ba = match self.bodies.get(body_ah(ca.body)) {
                Some(b) => b,
                None => continue,
            };
            let bb = match self.bodies.get(body_ah(cb.body)) {
                Some(b) => b,
                None => continue,
            };

            let pos_a = world_pos(ba.position, ba.rotation, ca.offset);
            let pos_b = world_pos(bb.position, bb.rotation, cb.offset);

            // One-shot manifold generation: get all contact points in a single pass
            let multi = generate_contacts_multi(
                &ca.shape,
                pos_a,
                ba.rotation,
                &cb.shape,
                pos_b,
                bb.rotation,
            );
            for (normal, depth, point) in multi {
                contacts.push(Contact {
                    collider_a: *ha,
                    collider_b: *hb,
                    body_a: ca.body,
                    body_b: cb.body,
                    normal,
                    depth,
                    point,
                });
            }
        }

        contacts
    }

    // -----------------------------------------------------------------------
    // Manifold update — match new contacts to existing manifolds
    // -----------------------------------------------------------------------

    fn update_manifolds(&mut self, contacts: &[Contact]) {
        // Track which manifold keys appear in this frame's contacts
        let mut current_keys: BTreeSet<ManifoldKey> = BTreeSet::new();

        for contact in contacts {
            let key = ordered_manifold_key(contact.collider_a, contact.collider_b);
            current_keys.insert(key);

            // Transform contact point to body-local coordinates
            let (local_a, local_b) = {
                let pos_a = self
                    .bodies
                    .get(body_ah(contact.body_a))
                    .map(|b| (b.position, b.rotation))
                    .unwrap_or(([0.0, 0.0], 0.0));
                let pos_b = self
                    .bodies
                    .get(body_ah(contact.body_b))
                    .map(|b| (b.position, b.rotation))
                    .unwrap_or(([0.0, 0.0], 0.0));
                (
                    world_to_local(contact.point, pos_a.0, pos_a.1),
                    world_to_local(contact.point, pos_b.0, pos_b.1),
                )
            };

            if let Some(manifold) = self.manifolds.get_mut(&key) {
                // Existing manifold — update normal and match points
                manifold.normal = if key.0 == contact.collider_a {
                    contact.normal
                } else {
                    [-contact.normal[0], -contact.normal[1]]
                };
                manifold.body_a = if key.0 == contact.collider_a {
                    contact.body_a
                } else {
                    contact.body_b
                };
                manifold.body_b = if key.0 == contact.collider_a {
                    contact.body_b
                } else {
                    contact.body_a
                };

                let new_local_a = if key.0 == contact.collider_a {
                    local_a
                } else {
                    local_b
                };
                let new_local_b = if key.0 == contact.collider_a {
                    local_b
                } else {
                    local_a
                };

                // Try to match the new contact point to an existing manifold point
                let mut best_idx: Option<usize> = None;
                let mut best_dist_sq = MANIFOLD_MATCH_THRESHOLD * MANIFOLD_MATCH_THRESHOLD;
                for (i, mp) in manifold.points.iter().enumerate() {
                    let dx = mp.local_a[0] - new_local_a[0];
                    let dy = mp.local_a[1] - new_local_a[1];
                    let dist_sq = dx * dx + dy * dy;
                    if dist_sq < best_dist_sq {
                        best_dist_sq = dist_sq;
                        best_idx = Some(i);
                    }
                }

                if let Some(idx) = best_idx {
                    // Update the matched point, preserving accumulated impulses
                    manifold.points[idx].local_a = new_local_a;
                    manifold.points[idx].local_b = new_local_b;
                    manifold.points[idx].depth = contact.depth;
                } else {
                    // New unmatched point — add it to the manifold (incremental building)
                    if manifold.points.len() < MAX_MANIFOLD_POINTS {
                        manifold.points.push(ManifoldPoint {
                            local_a: new_local_a,
                            local_b: new_local_b,
                            normal_impulse: 0.0,
                            tangent_impulse: 0.0,
                            depth: contact.depth,
                        });
                    } else {
                        // Already at max points — replace the one with the smallest
                        // accumulated normal impulse
                        let min_idx = manifold
                            .points
                            .iter()
                            .enumerate()
                            .min_by(|(_, a), (_, b)| {
                                a.normal_impulse
                                    .partial_cmp(&b.normal_impulse)
                                    .unwrap_or(std::cmp::Ordering::Equal)
                            })
                            .map(|(i, _)| i)
                            .unwrap_or(0);
                        manifold.points[min_idx] = ManifoldPoint {
                            local_a: new_local_a,
                            local_b: new_local_b,
                            normal_impulse: 0.0,
                            tangent_impulse: 0.0,
                            depth: contact.depth,
                        };
                    }
                }
            } else {
                // New manifold
                let (ca, cb, ba, bb, normal, la, lb) = if key.0 == contact.collider_a {
                    (
                        contact.collider_a,
                        contact.collider_b,
                        contact.body_a,
                        contact.body_b,
                        contact.normal,
                        local_a,
                        local_b,
                    )
                } else {
                    (
                        contact.collider_b,
                        contact.collider_a,
                        contact.body_b,
                        contact.body_a,
                        [-contact.normal[0], -contact.normal[1]],
                        local_b,
                        local_a,
                    )
                };
                self.manifolds.insert(
                    key,
                    ContactManifold {
                        collider_a: ca,
                        collider_b: cb,
                        body_a: ba,
                        body_b: bb,
                        normal,
                        points: vec![ManifoldPoint {
                            local_a: la,
                            local_b: lb,
                            normal_impulse: 0.0,
                            tangent_impulse: 0.0,
                            depth: contact.depth,
                        }],
                    },
                );
            }
        }

        // Re-validate old manifold points by re-projecting from body-local to world space.
        // Points that have separated beyond the tolerance are removed.
        for manifold in self.manifolds.values_mut() {
            if !current_keys.contains(&(manifold.collider_a, manifold.collider_b)) {
                continue;
            }
            let (pos_a, rot_a) = self
                .bodies
                .get(body_ah(manifold.body_a))
                .map(|b| (b.position, b.rotation))
                .unwrap_or(([0.0, 0.0], 0.0));
            let (pos_b, rot_b) = self
                .bodies
                .get(body_ah(manifold.body_b))
                .map(|b| (b.position, b.rotation))
                .unwrap_or(([0.0, 0.0], 0.0));
            let n = manifold.normal;
            manifold.points.retain(|pt| {
                let world_a = local_to_world(pt.local_a, pos_a, rot_a);
                let world_b = local_to_world(pt.local_b, pos_b, rot_b);
                let sep = (world_b[0] - world_a[0]) * n[0] + (world_b[1] - world_a[1]) * n[1];
                sep < MANIFOLD_REVALIDATION_TOLERANCE
            });
        }

        // Remove manifolds for pairs no longer in contact
        self.manifolds.retain(|k, _| current_keys.contains(k));
    }

    // -----------------------------------------------------------------------
    // Warm starting — apply cached impulses from previous frame
    // -----------------------------------------------------------------------

    fn warm_start(&mut self) {
        // Collect warm-start data to avoid borrow conflicts
        struct WarmData {
            body_a: crate::body::BodyHandle,
            body_b: crate::body::BodyHandle,
            impulse_n: [f64; 2],
            impulse_t: [f64; 2],
            ra_cross_n: f64,
            rb_cross_n: f64,
            ra_cross_t: f64,
            rb_cross_t: f64,
            j_n: f64,
            jt: f64,
        }

        let warm_data: Vec<WarmData> = self
            .manifolds
            .values()
            .flat_map(|manifold| {
                let pos_a = self
                    .bodies
                    .get(body_ah(manifold.body_a))
                    .map(|b| b.position)
                    .unwrap_or([0.0, 0.0]);
                let pos_b = self
                    .bodies
                    .get(body_ah(manifold.body_b))
                    .map(|b| b.position)
                    .unwrap_or([0.0, 0.0]);

                // Check if this is a sensor contact
                let is_sensor = match (
                    self.colliders.get(coll_ah(manifold.collider_a)),
                    self.colliders.get(coll_ah(manifold.collider_b)),
                ) {
                    (Some(a), Some(b)) => a.is_sensor || b.is_sensor,
                    _ => false,
                };
                if is_sensor {
                    return Vec::new();
                }

                let n = manifold.normal;
                let tangent = [-n[1], n[0]];

                manifold
                    .points
                    .iter()
                    .filter_map(|mp| {
                        let j_n = mp.normal_impulse * WARM_START_FACTOR;
                        let jt = mp.tangent_impulse * WARM_START_FACTOR;
                        if j_n.abs() < EPSILON && jt.abs() < EPSILON {
                            return None;
                        }

                        // Reconstruct world-space contact point from body A's local coords
                        let rot_a = self
                            .bodies
                            .get(body_ah(manifold.body_a))
                            .map(|b| b.rotation)
                            .unwrap_or(0.0);
                        let cp = local_to_world(mp.local_a, pos_a, rot_a);

                        let ra = [cp[0] - pos_a[0], cp[1] - pos_a[1]];
                        let rb = [cp[0] - pos_b[0], cp[1] - pos_b[1]];

                        Some(WarmData {
                            body_a: manifold.body_a,
                            body_b: manifold.body_b,
                            impulse_n: [j_n * n[0], j_n * n[1]],
                            impulse_t: [jt * tangent[0], jt * tangent[1]],
                            ra_cross_n: ra[0] * n[1] - ra[1] * n[0],
                            rb_cross_n: rb[0] * n[1] - rb[1] * n[0],
                            ra_cross_t: ra[0] * tangent[1] - ra[1] * tangent[0],
                            rb_cross_t: rb[0] * tangent[1] - rb[1] * tangent[0],
                            j_n,
                            jt,
                        })
                    })
                    .collect::<Vec<_>>()
            })
            .collect();

        for wd in &warm_data {
            let total_impulse = [
                wd.impulse_n[0] + wd.impulse_t[0],
                wd.impulse_n[1] + wd.impulse_t[1],
            ];
            if let Some(ba) = self.bodies.get_mut(body_ah(wd.body_a))
                && ba.is_dynamic()
            {
                ba.linear_velocity[0] -= total_impulse[0] * ba.inv_mass;
                ba.linear_velocity[1] -= total_impulse[1] * ba.inv_mass;
                ba.angular_velocity -=
                    (wd.ra_cross_n * wd.j_n + wd.ra_cross_t * wd.jt) * ba.inv_inertia;
            }
            if let Some(bb) = self.bodies.get_mut(body_ah(wd.body_b))
                && bb.is_dynamic()
            {
                bb.linear_velocity[0] += total_impulse[0] * bb.inv_mass;
                bb.linear_velocity[1] += total_impulse[1] * bb.inv_mass;
                bb.angular_velocity +=
                    (wd.rb_cross_n * wd.j_n + wd.rb_cross_t * wd.jt) * bb.inv_inertia;
            }
        }
    }

    // -----------------------------------------------------------------------
    // Contact constraint solver with accumulated impulses, friction, angular response
    // -----------------------------------------------------------------------

    fn solve_contacts(&mut self, iterations: u32) {
        use crate::material::CombineRule;

        /// Velocity threshold below which restitution is zeroed to prevent
        /// micro-bouncing of resting objects.
        const RESTITUTION_VELOCITY_THRESHOLD: f64 = 1.0;

        // Pre-extract material properties per manifold to avoid repeated lookups
        struct ManifoldMaterial {
            restitution: f64,
            friction: f64,
            static_friction: f64,
            rolling_friction: f64,
            is_sensor: bool,
        }

        /// Pick the higher-priority combine rule, then apply it.
        fn combine_property(a: f64, b: f64, rule_a: CombineRule, rule_b: CombineRule) -> f64 {
            let rule = rule_a.max(rule_b);
            rule.combine(a, b)
        }

        // Shock propagation: order manifolds bottom-up so contacts touching
        // static/kinematic bodies are solved first, improving stacking stability.
        let keys: Vec<ManifoldKey> = {
            let mut keys: Vec<ManifoldKey> = self.manifolds.keys().copied().collect();

            // Compute "level" per body: 0 = static/kinematic, then BFS from those
            let mut body_level: BTreeMap<crate::body::BodyHandle, u32> = BTreeMap::new();
            for rb in self.bodies.values() {
                if rb.is_static() || !rb.is_dynamic() {
                    body_level.insert(rb.handle, 0);
                }
            }

            // BFS propagation through contact graph
            let mut changed = true;
            while changed {
                changed = false;
                for manifold in self.manifolds.values() {
                    let level_a = body_level.get(&manifold.body_a).copied();
                    let level_b = body_level.get(&manifold.body_b).copied();

                    match (level_a, level_b) {
                        (Some(la), None) => {
                            body_level.insert(manifold.body_b, la + 1);
                            changed = true;
                        }
                        (None, Some(lb)) => {
                            body_level.insert(manifold.body_a, lb + 1);
                            changed = true;
                        }
                        (Some(la), Some(lb)) => {
                            if la + 1 < lb {
                                body_level.insert(manifold.body_b, la + 1);
                                changed = true;
                            } else if lb + 1 < la {
                                body_level.insert(manifold.body_a, lb + 1);
                                changed = true;
                            }
                        }
                        (None, None) => {}
                    }
                }
            }

            // Sort manifolds: lower min-level contacts first (closer to ground)
            keys.sort_by_key(|key| {
                let m = &self.manifolds[key];
                let la = body_level.get(&m.body_a).copied().unwrap_or(u32::MAX);
                let lb = body_level.get(&m.body_b).copied().unwrap_or(u32::MAX);
                la.min(lb)
            });

            keys
        };
        let materials: Vec<ManifoldMaterial> = keys
            .iter()
            .map(|key| {
                let manifold = &self.manifolds[key];
                let (rest, fric, sfric, roll_fric, sensor) = match (
                    self.colliders.get(coll_ah(manifold.collider_a)),
                    self.colliders.get(coll_ah(manifold.collider_b)),
                ) {
                    (Some(a), Some(b)) => (
                        combine_property(
                            a.material.restitution,
                            b.material.restitution,
                            a.material.restitution_combine,
                            b.material.restitution_combine,
                        ),
                        combine_property(
                            a.material.friction,
                            b.material.friction,
                            a.material.friction_combine,
                            b.material.friction_combine,
                        ),
                        combine_property(
                            a.material.effective_static_friction(),
                            b.material.effective_static_friction(),
                            a.material.friction_combine,
                            b.material.friction_combine,
                        ),
                        (a.material.rolling_friction + b.material.rolling_friction) * 0.5,
                        a.is_sensor || b.is_sensor,
                    ),
                    _ => (0.0, 0.0, 0.0, 0.0, false),
                };
                ManifoldMaterial {
                    restitution: rest,
                    friction: fric,
                    static_friction: sfric,
                    rolling_friction: roll_fric,
                    is_sensor: sensor,
                }
            })
            .collect();

        for _ in 0..iterations {
            for (ki, key) in keys.iter().enumerate() {
                if materials[ki].is_sensor {
                    continue;
                }

                let manifold = match self.manifolds.get(key) {
                    Some(m) => m,
                    None => continue,
                };

                let (inv_mass_a, inv_inertia_a, pos_a, rot_a) = {
                    let ba = match self.bodies.get(body_ah(manifold.body_a)) {
                        Some(b) => b,
                        None => continue,
                    };
                    (ba.inv_mass, ba.inv_inertia, ba.position, ba.rotation)
                };
                let (inv_mass_b, inv_inertia_b, pos_b) = {
                    let bb = match self.bodies.get(body_ah(manifold.body_b)) {
                        Some(b) => b,
                        None => continue,
                    };
                    (bb.inv_mass, bb.inv_inertia, bb.position)
                };

                if inv_mass_a == 0.0 && inv_mass_b == 0.0 {
                    continue;
                }

                let n = manifold.normal;
                let body_a_handle = manifold.body_a;
                let body_b_handle = manifold.body_b;
                let num_points = manifold.points.len();

                // --- Block solver: coupled 2x2 normal solve for 2-point manifolds ---
                if num_points == 2 {
                    // Pre-compute contact geometry for both points
                    let mut ra = [[0.0; 2]; 2];
                    let mut rb = [[0.0; 2]; 2];
                    let mut ra_cross_n = [0.0; 2];
                    let mut rb_cross_n = [0.0; 2];

                    for pi in 0..2 {
                        let mp = &self.manifolds[key].points[pi];
                        let cp = local_to_world(mp.local_a, pos_a, rot_a);
                        ra[pi] = [cp[0] - pos_a[0], cp[1] - pos_a[1]];
                        rb[pi] = [cp[0] - pos_b[0], cp[1] - pos_b[1]];
                        ra_cross_n[pi] = ra[pi][0] * n[1] - ra[pi][1] * n[0];
                        rb_cross_n[pi] = rb[pi][0] * n[1] - rb[pi][1] * n[0];
                    }

                    // Build 2x2 effective mass matrix K
                    // K[i][j] = inv_mass_a + inv_mass_b
                    //         + ra_cross_n[i] * ra_cross_n[j] * inv_inertia_a
                    //         + rb_cross_n[i] * rb_cross_n[j] * inv_inertia_b
                    let k00 = inv_mass_a
                        + inv_mass_b
                        + ra_cross_n[0] * ra_cross_n[0] * inv_inertia_a
                        + rb_cross_n[0] * rb_cross_n[0] * inv_inertia_b;
                    let k01 = inv_mass_a
                        + inv_mass_b
                        + ra_cross_n[0] * ra_cross_n[1] * inv_inertia_a
                        + rb_cross_n[0] * rb_cross_n[1] * inv_inertia_b;
                    let k11 = inv_mass_a
                        + inv_mass_b
                        + ra_cross_n[1] * ra_cross_n[1] * inv_inertia_a
                        + rb_cross_n[1] * rb_cross_n[1] * inv_inertia_b;

                    // Invert the 2x2 matrix K
                    let det = k00 * k11 - k01 * k01;
                    if det.abs() > EPSILON {
                        let inv_det = 1.0 / det;

                        // Read current velocities
                        let (vel_a, angvel_a) = {
                            let ba = match self.bodies.get(body_ah(body_a_handle)) {
                                Some(b) => b,
                                None => continue,
                            };
                            (ba.linear_velocity, ba.angular_velocity)
                        };
                        let (vel_b, angvel_b) = {
                            let bb = match self.bodies.get(body_ah(body_b_handle)) {
                                Some(b) => b,
                                None => continue,
                            };
                            (bb.linear_velocity, bb.angular_velocity)
                        };

                        // Compute velocity errors at both points
                        let mut vn = [0.0; 2];
                        for pi in 0..2 {
                            let va = [
                                vel_a[0] - angvel_a * ra[pi][1],
                                vel_a[1] + angvel_a * ra[pi][0],
                            ];
                            let vb = [
                                vel_b[0] - angvel_b * rb[pi][1],
                                vel_b[1] + angvel_b * rb[pi][0],
                            ];
                            vn[pi] = (vb[0] - va[0]) * n[0] + (vb[1] - va[1]) * n[1];
                        }

                        // Restitution bias
                        let rest = materials[ki].restitution;
                        let bias0 = if vn[0].abs() < RESTITUTION_VELOCITY_THRESHOLD {
                            0.0
                        } else {
                            rest * vn[0]
                        };
                        let bias1 = if vn[1].abs() < RESTITUTION_VELOCITY_THRESHOLD {
                            0.0
                        } else {
                            rest * vn[1]
                        };

                        // RHS: b = -(vn + restitution * vn)
                        let b0 = -(vn[0] + bias0);
                        let b1 = -(vn[1] + bias1);

                        // Solve K * delta_j = b  =>  delta_j = K_inv * b
                        let dj0 = (k11 * b0 - k01 * b1) * inv_det;
                        let dj1 = (-k01 * b0 + k00 * b1) * inv_det;

                        // Accumulated clamping: j_new = max(j_old + dj, 0)
                        let j_old0 = self.manifolds[key].points[0].normal_impulse;
                        let j_old1 = self.manifolds[key].points[1].normal_impulse;

                        let mut j_new0 = j_old0 + dj0;
                        let mut j_new1 = j_old1 + dj1;

                        // Four-case clamping for the non-negative constraint
                        if j_new0 >= 0.0 && j_new1 >= 0.0 {
                            // Case 1: both non-negative, accept
                        } else if j_new0 >= 0.0 {
                            // Case 2: j1 < 0, clamp j1 = 0, re-solve j0
                            j_new1 = 0.0;
                            j_new0 = if k00.abs() > EPSILON {
                                (-(vn[0] + bias0) / k00 + j_old0).max(0.0)
                            } else {
                                0.0
                            };
                        } else if j_new1 >= 0.0 {
                            // Case 3: j0 < 0, clamp j0 = 0, re-solve j1
                            j_new0 = 0.0;
                            j_new1 = if k11.abs() > EPSILON {
                                (-(vn[1] + bias1) / k11 + j_old1).max(0.0)
                            } else {
                                0.0
                            };
                        } else {
                            // Case 4: both negative, clamp both to 0
                            j_new0 = 0.0;
                            j_new1 = 0.0;
                        }

                        let j_applied0 = j_new0 - j_old0;
                        let j_applied1 = j_new1 - j_old1;

                        if let Some(m) = self.manifolds.get_mut(key) {
                            m.points[0].normal_impulse = j_new0;
                            m.points[1].normal_impulse = j_new1;
                        }

                        // Apply combined impulse from both points
                        let total_impulse_x = (j_applied0 + j_applied1) * n[0];
                        let total_impulse_y = (j_applied0 + j_applied1) * n[1];
                        let total_ang_a = ra_cross_n[0] * j_applied0 + ra_cross_n[1] * j_applied1;
                        let total_ang_b = rb_cross_n[0] * j_applied0 + rb_cross_n[1] * j_applied1;

                        if let Some(ba) = self.bodies.get_mut(body_ah(body_a_handle))
                            && ba.is_dynamic()
                        {
                            ba.linear_velocity[0] -= total_impulse_x * ba.inv_mass;
                            ba.linear_velocity[1] -= total_impulse_y * ba.inv_mass;
                            ba.angular_velocity -= total_ang_a * ba.inv_inertia;
                        }
                        if let Some(bb) = self.bodies.get_mut(body_ah(body_b_handle))
                            && bb.is_dynamic()
                        {
                            bb.linear_velocity[0] += total_impulse_x * bb.inv_mass;
                            bb.linear_velocity[1] += total_impulse_y * bb.inv_mass;
                            bb.angular_velocity += total_ang_b * bb.inv_inertia;
                        }
                    }

                    // Friction + rolling friction still per-point after block normal solve
                    for pi in 0..2 {
                        let j_accumulated = self.manifolds[key].points[pi].normal_impulse;
                        self.solve_contact_friction(
                            key,
                            pi,
                            &ra[pi],
                            &rb[pi],
                            &n,
                            inv_mass_a,
                            inv_mass_b,
                            inv_inertia_a,
                            inv_inertia_b,
                            body_a_handle,
                            body_b_handle,
                            materials[ki].friction,
                            materials[ki].static_friction,
                            materials[ki].rolling_friction,
                            j_accumulated,
                        );
                    }
                    continue;
                }

                // --- Single-point solver (num_points == 1) ---
                for pi in 0..num_points {
                    let mp = &self.manifolds[key].points[pi];

                    // Reconstruct world-space contact point from body A's local coords
                    let cp = local_to_world(mp.local_a, pos_a, rot_a);
                    let ra = [cp[0] - pos_a[0], cp[1] - pos_a[1]];
                    let rb = [cp[0] - pos_b[0], cp[1] - pos_b[1]];

                    // Re-read velocities (they change during iteration)
                    let (vel_a, angvel_a) = {
                        let ba = match self.bodies.get(body_ah(body_a_handle)) {
                            Some(b) => b,
                            None => continue,
                        };
                        (ba.linear_velocity, ba.angular_velocity)
                    };
                    let (vel_b, angvel_b) = {
                        let bb = match self.bodies.get(body_ah(body_b_handle)) {
                            Some(b) => b,
                            None => continue,
                        };
                        (bb.linear_velocity, bb.angular_velocity)
                    };

                    // Relative velocity at contact point (including angular)
                    let vel_a_at_cp = [vel_a[0] - angvel_a * ra[1], vel_a[1] + angvel_a * ra[0]];
                    let vel_b_at_cp = [vel_b[0] - angvel_b * rb[1], vel_b[1] + angvel_b * rb[0]];
                    let rel_vel = [
                        vel_b_at_cp[0] - vel_a_at_cp[0],
                        vel_b_at_cp[1] - vel_a_at_cp[1],
                    ];
                    let vel_along_normal = rel_vel[0] * n[0] + rel_vel[1] * n[1];

                    // Angular effective mass
                    let ra_cross_n = ra[0] * n[1] - ra[1] * n[0];
                    let rb_cross_n = rb[0] * n[1] - rb[1] * n[0];
                    let inv_mass_sum = inv_mass_a
                        + inv_mass_b
                        + ra_cross_n * ra_cross_n * inv_inertia_a
                        + rb_cross_n * rb_cross_n * inv_inertia_b;

                    // Normal impulse with accumulation
                    if inv_mass_sum < EPSILON {
                        continue;
                    }
                    let restitution = if vel_along_normal.abs() < RESTITUTION_VELOCITY_THRESHOLD {
                        0.0
                    } else {
                        materials[ki].restitution
                    };
                    let j_new = -(1.0 + restitution) * vel_along_normal / inv_mass_sum;
                    let j_old = self.manifolds[key].points[pi].normal_impulse;
                    let j_accumulated = (j_old + j_new).max(0.0);
                    let j_applied = j_accumulated - j_old;
                    if let Some(m) = self.manifolds.get_mut(key) {
                        m.points[pi].normal_impulse = j_accumulated;
                    }

                    let impulse_n = [j_applied * n[0], j_applied * n[1]];

                    if let Some(ba) = self.bodies.get_mut(body_ah(body_a_handle))
                        && ba.is_dynamic()
                    {
                        ba.linear_velocity[0] -= impulse_n[0] * ba.inv_mass;
                        ba.linear_velocity[1] -= impulse_n[1] * ba.inv_mass;
                        ba.angular_velocity -= ra_cross_n * j_applied * ba.inv_inertia;
                    }
                    if let Some(bb) = self.bodies.get_mut(body_ah(body_b_handle))
                        && bb.is_dynamic()
                    {
                        bb.linear_velocity[0] += impulse_n[0] * bb.inv_mass;
                        bb.linear_velocity[1] += impulse_n[1] * bb.inv_mass;
                        bb.angular_velocity += rb_cross_n * j_applied * bb.inv_inertia;
                    }

                    // Friction + rolling friction
                    self.solve_contact_friction(
                        key,
                        pi,
                        &ra,
                        &rb,
                        &n,
                        inv_mass_a,
                        inv_mass_b,
                        inv_inertia_a,
                        inv_inertia_b,
                        body_a_handle,
                        body_b_handle,
                        materials[ki].friction,
                        materials[ki].static_friction,
                        materials[ki].rolling_friction,
                        j_accumulated,
                    );
                }
            }
        }
    }

    /// Shared friction + rolling friction solver for a single contact point.
    #[allow(clippy::too_many_arguments)]
    fn solve_contact_friction(
        &mut self,
        key: &ManifoldKey,
        pi: usize,
        ra: &[f64; 2],
        rb: &[f64; 2],
        n: &[f64; 2],
        inv_mass_a: f64,
        inv_mass_b: f64,
        inv_inertia_a: f64,
        inv_inertia_b: f64,
        body_a_handle: crate::body::BodyHandle,
        body_b_handle: crate::body::BodyHandle,
        kinetic_friction: f64,
        static_friction: f64,
        rolling_friction: f64,
        j_accumulated: f64,
    ) {
        if kinetic_friction > 0.0 || static_friction > 0.0 {
            let (vel_a, angvel_a) = {
                let ba = match self.bodies.get(body_ah(body_a_handle)) {
                    Some(b) => b,
                    None => return,
                };
                (ba.linear_velocity, ba.angular_velocity)
            };
            let (vel_b, angvel_b) = {
                let bb = match self.bodies.get(body_ah(body_b_handle)) {
                    Some(b) => b,
                    None => return,
                };
                (bb.linear_velocity, bb.angular_velocity)
            };

            let vel_a_at_cp = [vel_a[0] - angvel_a * ra[1], vel_a[1] + angvel_a * ra[0]];
            let vel_b_at_cp = [vel_b[0] - angvel_b * rb[1], vel_b[1] + angvel_b * rb[0]];
            let rel_vel = [
                vel_b_at_cp[0] - vel_a_at_cp[0],
                vel_b_at_cp[1] - vel_a_at_cp[1],
            ];

            let tangent = [-n[1], n[0]];
            let vel_along_tangent = rel_vel[0] * tangent[0] + rel_vel[1] * tangent[1];

            let ra_cross_t = ra[0] * tangent[1] - ra[1] * tangent[0];
            let rb_cross_t = rb[0] * tangent[1] - rb[1] * tangent[0];
            let inv_mass_sum_t = inv_mass_a
                + inv_mass_b
                + ra_cross_t * ra_cross_t * inv_inertia_a
                + rb_cross_t * rb_cross_t * inv_inertia_b;

            if inv_mass_sum_t < EPSILON {
                return;
            }
            let jt_new = -vel_along_tangent / inv_mass_sum_t;
            let jt_old = self.manifolds[key].points[pi].tangent_impulse;

            let mu = if vel_along_tangent.abs() < 0.01 {
                static_friction
            } else {
                kinetic_friction
            };
            let max_friction = j_accumulated.abs() * mu;
            let jt_accumulated = (jt_old + jt_new).clamp(-max_friction, max_friction);
            let jt_applied = jt_accumulated - jt_old;
            if let Some(m) = self.manifolds.get_mut(key) {
                m.points[pi].tangent_impulse = jt_accumulated;
            }

            let impulse_t = [jt_applied * tangent[0], jt_applied * tangent[1]];

            if let Some(ba) = self.bodies.get_mut(body_ah(body_a_handle))
                && ba.is_dynamic()
            {
                ba.linear_velocity[0] -= impulse_t[0] * ba.inv_mass;
                ba.linear_velocity[1] -= impulse_t[1] * ba.inv_mass;
                ba.angular_velocity -= ra_cross_t * jt_applied * ba.inv_inertia;
            }
            if let Some(bb) = self.bodies.get_mut(body_ah(body_b_handle))
                && bb.is_dynamic()
            {
                bb.linear_velocity[0] += impulse_t[0] * bb.inv_mass;
                bb.linear_velocity[1] += impulse_t[1] * bb.inv_mass;
                bb.angular_velocity += rb_cross_t * jt_applied * bb.inv_inertia;
            }
        }

        // Rolling friction
        if rolling_friction > 0.0 {
            let normal_force = j_accumulated.abs();
            let roll_torque = rolling_friction * normal_force;

            if let Some(ba) = self.bodies.get_mut(body_ah(body_a_handle))
                && ba.is_dynamic()
                && ba.angular_velocity.abs() > EPSILON
            {
                let old_sign = ba.angular_velocity > 0.0;
                let sign = if old_sign { -1.0 } else { 1.0 };
                ba.angular_velocity += sign * roll_torque * ba.inv_inertia;
                if (ba.angular_velocity > 0.0) != old_sign {
                    ba.angular_velocity = 0.0;
                }
            }
            if let Some(bb) = self.bodies.get_mut(body_ah(body_b_handle))
                && bb.is_dynamic()
                && bb.angular_velocity.abs() > EPSILON
            {
                let old_sign = bb.angular_velocity > 0.0;
                let sign = if old_sign { -1.0 } else { 1.0 };
                bb.angular_velocity += sign * roll_torque * bb.inv_inertia;
                if (bb.angular_velocity > 0.0) != old_sign {
                    bb.angular_velocity = 0.0;
                }
            }
        }
    }

    // -----------------------------------------------------------------------
    // Positional correction (split impulse — pseudo-velocity based)
    // -----------------------------------------------------------------------

    #[allow(clippy::too_many_arguments)]
    fn solve_positions(
        &mut self,
        iterations: u32,
        slop: f64,
        percent: f64,
        dt: f64,
        constraint_frequency: f64,
        constraint_damping_ratio: f64,
    ) {
        // Collect positional correction data from manifolds
        struct PosCorrection {
            body_a: crate::body::BodyHandle,
            body_b: crate::body::BodyHandle,
            normal: [f64; 2],
            depth: f64,
            is_sensor: bool,
        }

        let corrections: Vec<PosCorrection> = self
            .manifolds
            .values()
            .flat_map(|manifold| {
                let is_sensor = match (
                    self.colliders.get(coll_ah(manifold.collider_a)),
                    self.colliders.get(coll_ah(manifold.collider_b)),
                ) {
                    (Some(a), Some(b)) => a.is_sensor || b.is_sensor,
                    _ => false,
                };
                manifold.points.iter().map(move |mp| PosCorrection {
                    body_a: manifold.body_a,
                    body_b: manifold.body_b,
                    normal: manifold.normal,
                    depth: mp.depth,
                    is_sensor,
                })
            })
            .collect();

        // Reset pseudo-velocities before position solve
        for rb in self.bodies.values_mut() {
            rb.pseudo_velocity = [0.0, 0.0];
            rb.pseudo_angular_velocity = 0.0;
        }

        for _ in 0..iterations {
            for corr in &corrections {
                if corr.is_sensor {
                    continue;
                }

                let inv_mass_a = self
                    .bodies
                    .get(body_ah(corr.body_a))
                    .map(|b| b.inv_mass)
                    .unwrap_or(0.0);
                let inv_mass_b = self
                    .bodies
                    .get(body_ah(corr.body_b))
                    .map(|b| b.inv_mass)
                    .unwrap_or(0.0);
                let inv_mass_sum = inv_mass_a + inv_mass_b;

                if inv_mass_sum == 0.0 {
                    continue;
                }

                let n = corr.normal;
                let penetration = (corr.depth - slop).max(0.0);

                // Soft constraint (ERP/CFM spring-damper) or Baumgarte fallback
                let bias = if constraint_frequency > 0.0 && dt > 0.0 {
                    // Derive ERP from spring frequency and damping ratio
                    let omega = 2.0 * std::f64::consts::PI * constraint_frequency;
                    let d = 2.0 * constraint_damping_ratio * omega;
                    let k = omega * omega;
                    let erp = dt * k / (d + dt * k);
                    erp * penetration / (inv_mass_sum * dt)
                } else {
                    penetration * percent / inv_mass_sum
                };

                if let Some(ba) = self.bodies.get_mut(body_ah(corr.body_a))
                    && ba.is_dynamic()
                {
                    ba.pseudo_velocity[0] -= bias * n[0] * ba.inv_mass;
                    ba.pseudo_velocity[1] -= bias * n[1] * ba.inv_mass;
                }
                if let Some(bb) = self.bodies.get_mut(body_ah(corr.body_b))
                    && bb.is_dynamic()
                {
                    bb.pseudo_velocity[0] += bias * n[0] * bb.inv_mass;
                    bb.pseudo_velocity[1] += bias * n[1] * bb.inv_mass;
                }
            }
        }
    }

    // -----------------------------------------------------------------------
    // XPBD contact constraint solver (position-level)
    // -----------------------------------------------------------------------

    /// Solve contact constraints using XPBD position corrections with compliance.
    ///
    /// For each contact point, computes the constraint violation C (penetration)
    /// and applies a position correction weighted by inverse mass, accumulating
    /// the Lagrange multiplier λ to prevent over-correction.
    fn xpbd_solve_contacts(
        &mut self,
        keys: &[ManifoldKey],
        lambdas: &mut BTreeMap<(ManifoldKey, usize), f64>,
        alpha_tilde: f64,
        slop: f64,
    ) {
        for key in keys {
            let manifold = match self.manifolds.get(key) {
                Some(m) => m,
                None => continue,
            };

            // Skip sensors
            let is_sensor = match (
                self.colliders.get(coll_ah(manifold.collider_a)),
                self.colliders.get(coll_ah(manifold.collider_b)),
            ) {
                (Some(a), Some(b)) => a.is_sensor || b.is_sensor,
                _ => false,
            };
            if is_sensor {
                continue;
            }

            let body_a = manifold.body_a;
            let body_b = manifold.body_b;
            let normal = manifold.normal;
            let num_points = manifold.points.len();

            let inv_mass_a = self
                .bodies
                .get(body_ah(body_a))
                .map(|b| b.inv_mass)
                .unwrap_or(0.0);
            let inv_mass_b = self
                .bodies
                .get(body_ah(body_b))
                .map(|b| b.inv_mass)
                .unwrap_or(0.0);
            let w_sum = inv_mass_a + inv_mass_b;
            if w_sum < EPSILON {
                continue;
            }

            for pi in 0..num_points {
                let mp = &self.manifolds[key].points[pi];
                let initial_depth = mp.depth;

                // Constraint C = -penetration (negative when overlapping).
                // We want to drive C to 0 from below.
                let lambda_prev = lambdas.get(&(*key, pi)).copied().unwrap_or(0.0);

                // Effective penetration accounting for corrections already applied.
                // Each unit of lambda corresponds to w_sum units of separation gained.
                let current_pen = (initial_depth - lambda_prev * w_sum - slop).max(0.0);
                if current_pen < EPSILON {
                    continue;
                }

                // XPBD: Δλ = (penetration - α̃·λ) / (w + α̃)
                // This drives penetration to zero while respecting compliance.
                let denom = w_sum + alpha_tilde;
                if denom < EPSILON {
                    continue;
                }
                let delta_lambda = (current_pen - alpha_tilde * lambda_prev) / denom;

                // Clamp: accumulated lambda must stay non-negative (only push apart)
                let lambda_new = (lambda_prev + delta_lambda).max(0.0);
                let dl = lambda_new - lambda_prev;
                if dl.abs() < EPSILON {
                    continue;
                }

                lambdas.insert((*key, pi), lambda_new);
                if let Some(ba) = self.bodies.get_mut(body_ah(body_a))
                    && ba.is_dynamic()
                {
                    ba.position[0] -= dl * inv_mass_a * normal[0];
                    ba.position[1] -= dl * inv_mass_a * normal[1];
                }
                if let Some(bb) = self.bodies.get_mut(body_ah(body_b))
                    && bb.is_dynamic()
                {
                    bb.position[0] += dl * inv_mass_b * normal[0];
                    bb.position[1] += dl * inv_mass_b * normal[1];
                }
            }
        }
    }

    // -----------------------------------------------------------------------
    // XPBD joint constraint solver (position-level)
    // -----------------------------------------------------------------------

    /// Solve joint constraints at position level for XPBD.
    fn xpbd_solve_joints(&mut self, dt: f64, alpha_tilde: f64) {
        use crate::joint::JointType;

        let joint_data: Vec<_> = self
            .joints
            .iter()
            .map(|(_, j)| {
                (
                    j.body_a,
                    j.body_b,
                    j.joint_type.clone(),
                    j.local_anchor_a,
                    j.local_anchor_b,
                )
            })
            .collect();

        for (body_a, body_b, joint_type, anchor_a, anchor_b) in &joint_data {
            let (pos_a, rot_a, inv_mass_a) = match self.bodies.get(body_ah(*body_a)) {
                Some(b) => (b.position, b.rotation, b.inv_mass),
                None => continue,
            };
            let (pos_b, rot_b, inv_mass_b) = match self.bodies.get(body_ah(*body_b)) {
                Some(b) => (b.position, b.rotation, b.inv_mass),
                None => continue,
            };

            let w_sum = inv_mass_a + inv_mass_b;
            if w_sum < EPSILON {
                continue;
            }

            // World-space anchor positions
            let (sin_a, cos_a) = rot_a.sin_cos();
            let (sin_b, cos_b) = rot_b.sin_cos();
            let wa = [
                pos_a[0] + cos_a * anchor_a[0] - sin_a * anchor_a[1],
                pos_a[1] + sin_a * anchor_a[0] + cos_a * anchor_a[1],
            ];
            let wb = [
                pos_b[0] + cos_b * anchor_b[0] - sin_b * anchor_b[1],
                pos_b[1] + sin_b * anchor_b[0] + cos_b * anchor_b[1],
            ];

            match joint_type {
                JointType::Fixed | JointType::Revolute { .. } => {
                    // Distance constraint: anchors should coincide
                    let dx = wb[0] - wa[0];
                    let dy = wb[1] - wa[1];
                    let dist = (dx * dx + dy * dy).sqrt();
                    if dist < EPSILON {
                        continue;
                    }
                    let n = [dx / dist, dy / dist];
                    let c = dist;
                    let denom = w_sum + alpha_tilde;
                    let delta_lambda = -c / denom;

                    if let Some(ba) = self.bodies.get_mut(body_ah(*body_a))
                        && ba.is_dynamic()
                    {
                        ba.position[0] -= delta_lambda * inv_mass_a * n[0];
                        ba.position[1] -= delta_lambda * inv_mass_a * n[1];
                    }
                    if let Some(bb) = self.bodies.get_mut(body_ah(*body_b))
                        && bb.is_dynamic()
                    {
                        bb.position[0] += delta_lambda * inv_mass_b * n[0];
                        bb.position[1] += delta_lambda * inv_mass_b * n[1];
                    }
                }
                JointType::Distance { length } => {
                    let dx = wb[0] - wa[0];
                    let dy = wb[1] - wa[1];
                    let dist = (dx * dx + dy * dy).sqrt();
                    if dist < EPSILON {
                        continue;
                    }
                    let n = [dx / dist, dy / dist];
                    let c = dist - length;
                    let denom = w_sum + alpha_tilde;
                    let delta_lambda = -c / denom;

                    if let Some(ba) = self.bodies.get_mut(body_ah(*body_a))
                        && ba.is_dynamic()
                    {
                        ba.position[0] -= delta_lambda * inv_mass_a * n[0];
                        ba.position[1] -= delta_lambda * inv_mass_a * n[1];
                    }
                    if let Some(bb) = self.bodies.get_mut(body_ah(*body_b))
                        && bb.is_dynamic()
                    {
                        bb.position[0] += delta_lambda * inv_mass_b * n[0];
                        bb.position[1] += delta_lambda * inv_mass_b * n[1];
                    }
                }
                JointType::Spring {
                    rest_length,
                    stiffness,
                    damping: _,
                } => {
                    let dx = wb[0] - wa[0];
                    let dy = wb[1] - wa[1];
                    let dist = (dx * dx + dy * dy).sqrt();
                    if dist < EPSILON {
                        continue;
                    }
                    let n = [dx / dist, dy / dist];
                    let c = dist - rest_length;
                    // Spring compliance: α̃ = 1/(k·dt²)
                    let spring_alpha = if *stiffness > EPSILON && dt > EPSILON {
                        1.0 / (stiffness * dt * dt)
                    } else {
                        alpha_tilde
                    };
                    let denom = w_sum + spring_alpha;
                    let delta_lambda = -c / denom;

                    if let Some(ba) = self.bodies.get_mut(body_ah(*body_a))
                        && ba.is_dynamic()
                    {
                        ba.position[0] -= delta_lambda * inv_mass_a * n[0];
                        ba.position[1] -= delta_lambda * inv_mass_a * n[1];
                    }
                    if let Some(bb) = self.bodies.get_mut(body_ah(*body_b))
                        && bb.is_dynamic()
                    {
                        bb.position[0] += delta_lambda * inv_mass_b * n[0];
                        bb.position[1] += delta_lambda * inv_mass_b * n[1];
                    }
                }
                JointType::Rope { max_length } => {
                    let dx = wb[0] - wa[0];
                    let dy = wb[1] - wa[1];
                    let dist = (dx * dx + dy * dy).sqrt();
                    if dist <= *max_length || dist < EPSILON {
                        continue;
                    }
                    let n = [dx / dist, dy / dist];
                    let c = dist - max_length;
                    let denom = w_sum + alpha_tilde;
                    let delta_lambda = -c / denom;

                    if let Some(ba) = self.bodies.get_mut(body_ah(*body_a))
                        && ba.is_dynamic()
                    {
                        ba.position[0] -= delta_lambda * inv_mass_a * n[0];
                        ba.position[1] -= delta_lambda * inv_mass_a * n[1];
                    }
                    if let Some(bb) = self.bodies.get_mut(body_ah(*body_b))
                        && bb.is_dynamic()
                    {
                        bb.position[0] += delta_lambda * inv_mass_b * n[0];
                        bb.position[1] += delta_lambda * inv_mass_b * n[1];
                    }
                }
                _ => {
                    // Other joint types: fall through (Mouse, Prismatic, Wheel handled later)
                }
            }
        }
    }

    // -----------------------------------------------------------------------
    // XPBD velocity-level friction + restitution
    // -----------------------------------------------------------------------

    /// After XPBD position solve and velocity derivation, apply velocity-level
    /// friction and restitution corrections.
    fn xpbd_velocity_solve(&mut self, keys: &[ManifoldKey]) {
        use crate::material::CombineRule;

        fn combine_property(a: f64, b: f64, rule_a: CombineRule, rule_b: CombineRule) -> f64 {
            let rule = rule_a.max(rule_b);
            rule.combine(a, b)
        }

        for key in keys {
            let manifold = match self.manifolds.get(key) {
                Some(m) => m,
                None => continue,
            };

            let (restitution, friction) = match (
                self.colliders.get(coll_ah(manifold.collider_a)),
                self.colliders.get(coll_ah(manifold.collider_b)),
            ) {
                (Some(a), Some(b)) => {
                    if a.is_sensor || b.is_sensor {
                        continue;
                    }
                    (
                        combine_property(
                            a.material.restitution,
                            b.material.restitution,
                            a.material.restitution_combine,
                            b.material.restitution_combine,
                        ),
                        combine_property(
                            a.material.friction,
                            b.material.friction,
                            a.material.friction_combine,
                            b.material.friction_combine,
                        ),
                    )
                }
                _ => continue,
            };

            let normal = manifold.normal;
            let body_a = manifold.body_a;
            let body_b = manifold.body_b;

            let (vel_a, inv_mass_a) = match self.bodies.get(body_ah(body_a)) {
                Some(b) => (b.linear_velocity, b.inv_mass),
                None => continue,
            };
            let (vel_b, inv_mass_b) = match self.bodies.get(body_ah(body_b)) {
                Some(b) => (b.linear_velocity, b.inv_mass),
                None => continue,
            };

            let w_sum = inv_mass_a + inv_mass_b;
            if w_sum < EPSILON {
                continue;
            }

            // Relative velocity at contact
            let rel_vel = [vel_b[0] - vel_a[0], vel_b[1] - vel_a[1]];
            let vn = rel_vel[0] * normal[0] + rel_vel[1] * normal[1];

            // Restitution: if bodies are approaching along normal, apply bounce.
            // Uses the derived velocity (from position delta) which already
            // reflects the XPBD position correction.
            if vn < -EPSILON {
                let delta_vn = -vn * (1.0 + restitution);
                let j = delta_vn / w_sum;

                if let Some(ba) = self.bodies.get_mut(body_ah(body_a))
                    && ba.is_dynamic()
                {
                    ba.linear_velocity[0] -= j * inv_mass_a * normal[0];
                    ba.linear_velocity[1] -= j * inv_mass_a * normal[1];
                }
                if let Some(bb) = self.bodies.get_mut(body_ah(body_b))
                    && bb.is_dynamic()
                {
                    bb.linear_velocity[0] += j * inv_mass_b * normal[0];
                    bb.linear_velocity[1] += j * inv_mass_b * normal[1];
                }
            }

            // Friction: tangential velocity damping
            if friction > EPSILON {
                // Re-read velocities after restitution
                let vel_a = self
                    .bodies
                    .get(body_ah(body_a))
                    .map(|b| b.linear_velocity)
                    .unwrap_or([0.0, 0.0]);
                let vel_b = self
                    .bodies
                    .get(body_ah(body_b))
                    .map(|b| b.linear_velocity)
                    .unwrap_or([0.0, 0.0]);
                let rel_vel = [vel_b[0] - vel_a[0], vel_b[1] - vel_a[1]];
                let vn_after = rel_vel[0] * normal[0] + rel_vel[1] * normal[1];
                let tangent_vel = [
                    rel_vel[0] - vn_after * normal[0],
                    rel_vel[1] - vn_after * normal[1],
                ];
                let vt = (tangent_vel[0] * tangent_vel[0] + tangent_vel[1] * tangent_vel[1]).sqrt();

                if vt > EPSILON {
                    let tangent = [tangent_vel[0] / vt, tangent_vel[1] / vt];
                    // Coulomb friction: clamp tangential impulse by μ * normal impulse
                    // For XPBD, use a simple friction model: remove tangential velocity
                    // scaled by friction coefficient
                    let delta_vt = (-vt * friction).max(-vt); // don't reverse direction
                    let j = delta_vt / w_sum;

                    if let Some(ba) = self.bodies.get_mut(body_ah(body_a))
                        && ba.is_dynamic()
                    {
                        ba.linear_velocity[0] -= j * inv_mass_a * tangent[0];
                        ba.linear_velocity[1] -= j * inv_mass_a * tangent[1];
                    }
                    if let Some(bb) = self.bodies.get_mut(body_ah(body_b))
                        && bb.is_dynamic()
                    {
                        bb.linear_velocity[0] += j * inv_mass_b * tangent[0];
                        bb.linear_velocity[1] += j * inv_mass_b * tangent[1];
                    }
                }
            }
        }
    }

    // -----------------------------------------------------------------------
    // Collision event generation
    // -----------------------------------------------------------------------

    fn generate_events(&mut self) -> Vec<CollisionEvent> {
        let mut events = Vec::new();

        let current_keys: BTreeSet<ManifoldKey> = self.manifolds.keys().copied().collect();

        for key in &current_keys {
            if !self.prev_manifold_keys.contains(key) {
                events.push(CollisionEvent::Started {
                    collider_a: key.0,
                    collider_b: key.1,
                });
            } else {
                events.push(CollisionEvent::Ongoing {
                    collider_a: key.0,
                    collider_b: key.1,
                });
            }
        }

        for key in &self.prev_manifold_keys {
            if !current_keys.contains(key) {
                events.push(CollisionEvent::Stopped {
                    collider_a: key.0,
                    collider_b: key.1,
                });
            }
        }

        self.prev_manifold_keys = current_keys;
        events
    }

    // -----------------------------------------------------------------------
    // Simulation islands — union-find based grouping and island-level sleep
    // -----------------------------------------------------------------------

    fn build_islands_and_sleep(&mut self, dt: f64) {
        use std::collections::BTreeMap as Map;

        // Collect all body handles and assign indices
        let body_handles: Vec<crate::body::BodyHandle> =
            self.bodies.iter().map(|(_, b)| b.handle).collect();
        let body_count = body_handles.len();
        if body_count == 0 {
            return;
        }

        // Build handle -> index map
        let mut handle_to_idx: Map<crate::body::BodyHandle, u32> = Map::new();
        for (i, &h) in body_handles.iter().enumerate() {
            handle_to_idx.insert(h, i as u32);
        }

        // Reset island manager
        self.island_manager.reset(body_count);

        // Union bodies connected by contacts
        for manifold in self.manifolds.values() {
            if let (Some(&ia), Some(&ib)) = (
                handle_to_idx.get(&manifold.body_a),
                handle_to_idx.get(&manifold.body_b),
            ) {
                self.island_manager.union(ia, ib);
            }
        }

        // Union bodies connected by joints
        for (_, joint) in self.joints.iter() {
            if let (Some(&ia), Some(&ib)) = (
                handle_to_idx.get(&joint.body_a),
                handle_to_idx.get(&joint.body_b),
            ) {
                self.island_manager.union(ia, ib);
            }
        }

        // Assign island IDs and collect per-island info
        // island_root -> list of body indices in that island
        let mut islands: Map<u32, Vec<u32>> = Map::new();
        for i in 0..body_count {
            let root = self.island_manager.find(i as u32);
            islands.entry(root).or_default().push(i as u32);
        }

        // For each island, check if ALL dynamic bodies are below velocity threshold
        for members in islands.values() {
            let mut all_slow = true;
            let mut has_dynamic = false;

            for &idx in members {
                let handle = body_handles[idx as usize];
                let rb = match self.bodies.get(body_ah(handle)) {
                    Some(b) => b,
                    None => continue,
                };
                if !rb.is_dynamic() || rb.inv_mass == 0.0 {
                    continue;
                }
                has_dynamic = true;
                let lin_speed = (rb.linear_velocity[0] * rb.linear_velocity[0]
                    + rb.linear_velocity[1] * rb.linear_velocity[1])
                    .sqrt();
                let ang_speed = rb.angular_velocity.abs();
                if lin_speed >= SLEEP_VELOCITY_THRESHOLD || ang_speed >= SLEEP_VELOCITY_THRESHOLD {
                    all_slow = false;
                    break;
                }
            }

            if !has_dynamic {
                continue;
            }

            // Update sleep timers for the whole island atomically
            for &idx in members {
                let handle = body_handles[idx as usize];
                let rb = match self.bodies.get_mut(body_ah(handle)) {
                    Some(b) => b,
                    None => continue,
                };
                if !rb.is_dynamic() || rb.inv_mass == 0.0 {
                    continue;
                }
                // Assign island_id for external inspection
                rb.island_id = self.island_manager.find(idx);

                if all_slow {
                    rb.sleep_timer += dt;
                    if rb.sleep_timer >= SLEEP_TIME_THRESHOLD {
                        rb.is_sleeping = true;
                    }
                } else {
                    // Any body in the island is fast — wake the whole island
                    rb.sleep_timer = 0.0;
                    rb.is_sleeping = false;
                }
            }
        }
    }
}
