import { v3_quat3_apply } from "../../../core/geom/vec3/v3_quat3_apply.js";
import { v3_quat3_apply_inverse } from "../../../core/geom/vec3/v3_quat3_apply_inverse.js";
import { body_id_index } from "../body/BodyStorage.js";
import {
    SBS_AV_X,
    SBS_AV_Y,
    SBS_AV_Z,
    SBS_INV_I_X,
    SBS_INV_I_Y,
    SBS_INV_I_Z,
    SBS_INV_MASS,
    SBS_LV_X,
    SBS_LV_Y,
    SBS_LV_Z,
    SBS_QW,
    SBS_QX,
    SBS_QY,
    SBS_QZ,
    SBS_STRIDE,
} from "../body/SolverBodyState.js";
import { CONTACT_STRIDE } from "../contact/ManifoldStore.js";
import { is_sensor } from "../ecs/is_sensor.js";
import { world_inverse_inertia_apply_raw } from "../inertia/world_inverse_inertia.js";
import { redetect_pair_geometry } from "../narrowphase/narrowphase_step.js";
import { friction_cone_clamp } from "./friction_cone.js";

/**
 * # TGS split-impulse contact solver (staged)
 *
 * Temporal Gauss-Seidel with Catto-2018 split impulse. The solver runs as a
 * sequence of stages driven by `PhysicsSystem.fixedUpdate`, which owns the
 * substep loop (it has to — velocity/position integration spans every awake
 * body, not just contacts):
 *
 *   prepare_contacts(manifolds, system, h)         // once per outer step
 *   for each substep:
 *       (system integrates gravity by h)
 *       redetect_concave_contacts(manifolds, system) // concave: fresh narrowphase
 *       refresh_contacts(manifolds, system)        // convex: analytic re-derive
 *       warm_start_contacts(manifolds, system)     // replay impulse — per substep!
 *       solve_velocity(manifolds, system, iters)   // non-penetration + friction
 *       solve_position(manifolds, system, pos_iters)
 *       (system integrates position by h, folding pseudo-velocity)
 *   apply_restitution(manifolds, system)           // once, after the loop
 *
 * The three concerns are fully decoupled (Phases 1–2) and substepping
 * (Phase 3) re-runs only the velocity + position solve per substep —
 * narrowphase runs once per outer step. Each substep re-derives the
 * current penetration analytically from the bodies' moved poses and the
 * contact anchors captured at prepare time, so the position correction
 * adapts as the stack settles (the mechanism behind TGS stack stability)
 * without paying for narrowphase N times.
 *
 * Why staged module functions sharing module scratch, rather than one call:
 * the substep loop interleaves solver stages with whole-body integration
 * that lives in the system. The stages communicate through module-scoped
 * scratch + `g_*` state set by `prepare_contacts`; they must be called in
 * order, prepare first, within a single outer step. Single-threaded and
 * deterministic, so the shared state is safe.
 *
 * Contacts from all islands are flattened into one scratch array. Islands
 * are independent (they share no bodies), so a single flat Gauss-Seidel
 * sweep gives the same result as per-island sweeps — the island partition
 * is still built and used by the sleep test, just not needed here.
 */

/**
 * A pair is "sensor-only" when either side {@link is_sensor} (the body OR its
 * primary collider carries the IsSensor flag). The manifold still exists (so
 * Begin/Stay/End events fire from the manifold-diff pass) but no impulse
 * is applied.
 *
 * @param {RigidBody} rbA
 * @param {Collider} colA
 * @param {RigidBody} rbB
 * @param {Collider} colB
 * @returns {boolean}
 */
function pair_is_sensor(rbA, colA, rbB, colB) {
    return is_sensor(rbA, colA) || is_sensor(rbB, colB);
}

/**
 * Velocity-iteration count per substep. With substepping the per-substep
 * count can be lower than a single-step PGS solver would need, because the
 * outer loop revisits the contact set `substeps` times.
 * @type {number}
 */
const DEFAULT_VELOCITY_ITERATIONS = 10;

/**
 * Position-iteration count per substep (split-impulse pseudo-velocity pass).
 * @type {number}
 */
const DEFAULT_POSITION_ITERATIONS = 2;

/**
 * Penetration allowed without applying position correction. Eliminates
 * micro-jitter at near-zero overlap.
 * @type {number}
 */
const PENETRATION_SLOP = 0.005;

/**
 * SPOOK contact stiffness `k`. Effectively infinite for rigid-body
 * contact: what the solver actually sees is the regularization
 * `eps = 4 / (h² · k · (1 + 4d))`, negligible at `k = 1e12` but in place
 * as a continuous compliance dial for future soft contacts. Lacoursière
 * 2007; same formulation as cannon-es / AgX.
 * @type {number}
 */
const CONTACT_STIFFNESS = 1e12;

/**
 * SPOOK contact relaxation `d`. Chosen so `a = 4 / (h(1 + 4d)) = 0.2 / h`,
 * numerically matching the prior Baumgarte β = 0.2 gain. `4/(1+4d)=0.2` →
 * `d = 4.75`. Note `h` here is the SUBSTEP size: position correction is
 * applied per substep, so the gain is derived from the substep `dt`.
 * @type {number}
 */
const CONTACT_RELAXATION = 4.75;

/**
 * Maximum magnitude of the position-correction velocity bias, in m/s.
 * Belt-and-braces against inflated EPA depths (PLAN.md caveat) driving the
 * bias to tens of m/s. Removed once closed-form triangle solvers land.
 * @type {number}
 */
const MAX_POSITION_BIAS = 3;

/**
 * Velocity below which restitution is suppressed (no micro-bounce buzz on
 * resting stacks).
 * @type {number}
 */
const RESTITUTION_VELOCITY_THRESHOLD = 1.0;

/**
 * Per-contact pre-step scratch stride: 23 doubles.
 *   0..2 : localWA  (A's contact witness, in A's LOCAL frame — constant)
 *   3..5 : localWB  (B's contact witness, in B's LOCAL frame — constant)
 *   6..8 : rA       (lever from A's COM to the contact midpoint — refreshed)
 *   9..11: rB       (lever from B's COM to the contact midpoint — refreshed)
 *   12..14: t1      (tangent 1, unit, world — constant)
 *   15..17: t2      (tangent 2, unit, world — constant)
 *   18   : m_eff_n
 *   19   : m_eff_t1
 *   20   : m_eff_t2
 *   21   : rest_bias       (restitution: `e · vn_approach`, ≤ 0; 0 if not bouncing)
 *   22   : bias_position   (depth-correction bias — refreshed each substep)
 *
 * `localWA` / `localWB` are the per-body contact witnesses expressed in body
 * local frames at prepare time. Each substep, `refresh_contacts` rotates
 * them back to world by the body's current pose to recover the moved contact
 * points, re-derives the current penetration depth (anchored on the trusted
 * prepare-time depth via a delta, so it's sign-robust), and rebuilds the
 * impulse lever arms `rA` / `rB` and the position bias.
 * @type {number}
 */
const PRE_STRIDE = 23;

/** Per-contact index list — 4 uint32 per contact: slot, idx, idxA, idxB. */
const INDEX_STRIDE = 4;

/**
 * Per-body pseudo-velocity stride: 3 linear + 3 angular doubles. Owned by
 * PhysicsSystem (`system.__pseudo_velocity`); zeroed each substep before
 * the position pass and folded into the pose by `integrate_position`.
 * @type {number}
 */
const PSEUDO_STRIDE = 6;

let scratch_pre = new Float64Array(64 * PRE_STRIDE);
let scratch_idx = new Uint32Array(64 * INDEX_STRIDE);
let scratch_mu = new Float64Array(64);
let scratch_pos_jn = new Float64Array(64);

/**
 * Per-contact maximum normal impulse seen across the whole outer step's
 * velocity solving (all substeps, all iterations). Reset in
 * {@link prepare_contacts}, updated in {@link solve_velocity}, read by
 * {@link apply_restitution}.
 *
 * Restitution must fire whenever a contact *was* compressive at some point
 * in the step, even if its accumulated impulse later relaxes back to ~0 — a
 * transient collision (ball bouncing, head-on hit) under per-substep
 * warm-start ends the loop with `j_n ≈ 0` because there's no sustained load
 * to hold the impulse up. Gating on this running max (Box2D-v3
 * `maxNormalImpulse`) rather than the end-of-loop `j_n` is what makes
 * bounces fire.
 * @type {Float64Array}
 */
let scratch_max_jn = new Float64Array(64);

/**
 * Per-contact flag: 1 if the contact's pair involves a concave body, else 0.
 * Concave contacts take the per-substep re-detection path
 * ({@link redetect_concave_contacts}) — their feature genuinely changes as
 * the body rocks, so the analytic refresh that freezes it would pump energy.
 * Convex contacts (flag 0) take the cheap analytic {@link refresh_contacts}.
 * @type {Uint8Array}
 */
let scratch_concave = new Uint8Array(64);

/**
 * Distinct concave-involved manifold slots touched this step, with the body
 * indices needed to fetch their collider lists for re-detection. Parallel
 * arrays, `g_concave_slot_count` valid entries, filled by
 * {@link prepare_contacts}.
 */
let concave_slot = new Uint32Array(32);
let concave_slot_idxA = new Uint32Array(32);
let concave_slot_idxB = new Uint32Array(32);
let g_concave_slot_count = 0;

/**
 * Shared cross-stage state, set by {@link prepare_contacts} and read by the
 * per-substep stages within the same outer step. Single-threaded, so plain
 * module variables are safe.
 */
let g_contact_count = 0;
let g_spook_a = 0;
let g_spook_eps = 0;

/** Scratch for re-deriving contact world points in {@link refresh_contacts}. */
const scratch_cp = new Float64Array(6);

function ensure_capacity(n) {
    if (scratch_pre.length < n * PRE_STRIDE) {
        scratch_pre = new Float64Array(n * PRE_STRIDE * 2);
    }
    if (scratch_idx.length < n * INDEX_STRIDE) {
        scratch_idx = new Uint32Array(n * INDEX_STRIDE * 2);
    }
    if (scratch_mu.length < n) {
        scratch_mu = new Float64Array(n * 2);
    }
    if (scratch_pos_jn.length < n) {
        scratch_pos_jn = new Float64Array(n * 2);
    }
    if (scratch_max_jn.length < n) {
        scratch_max_jn = new Float64Array(n * 2);
    }
    if (scratch_concave.length < n) {
        scratch_concave = new Uint8Array(n * 2);
    }
}

function ensure_concave_slot_capacity(n) {
    if (concave_slot.length < n) {
        concave_slot = new Uint32Array(n * 2);
        concave_slot_idxA = new Uint32Array(n * 2);
        concave_slot_idxB = new Uint32Array(n * 2);
    }
}

/**
 * Build an orthonormal tangent basis perpendicular to a unit normal.
 *
 * @param {Float64Array} out  6 floats: t1.xyz then t2.xyz
 * @param {number} off
 * @param {number} nx
 * @param {number} ny
 * @param {number} nz
 */
function build_tangents(out, off, nx, ny, nz) {
    const ax = nx < 0 ? -nx : nx;
    const ay = ny < 0 ? -ny : ny;
    const az = nz < 0 ? -nz : nz;

    let rx, ry, rz;

    if (ax <= ay && ax <= az) {
        rx = 1;
        ry = 0;
        rz = 0;
    } else if (ay <= az) {
        rx = 0;
        ry = 1;
        rz = 0;
    } else {
        rx = 0;
        ry = 0;
        rz = 1;
    }

    let t1x = ny * rz - nz * ry;
    let t1y = nz * rx - nx * rz;
    let t1z = nx * ry - ny * rx;

    const inv = 1 / Math.sqrt(t1x * t1x + t1y * t1y + t1z * t1z);

    t1x *= inv;
    t1y *= inv;
    t1z *= inv;

    const t2x = ny * t1z - nz * t1y;
    const t2y = nz * t1x - nx * t1z;
    const t2z = nx * t1y - ny * t1x;

    out[off] = t1x;
    out[off + 1] = t1y;
    out[off + 2] = t1z;

    out[off + 3] = t2x;
    out[off + 4] = t2y;
    out[off + 5] = t2z;
}

// Friction / restitution are no longer combined here: the narrowphase combines
// the specific source-collider pair's coefficients per contact (so compound
// bodies with mixed-material colliders are honoured) and stores the result in
// the manifold (CONTACT_STRIDE offsets 14 / 15). prepare_contacts reads them
// per contact. See engine/physics/contact/combine_material.js.

const scratch_clamp = new Float64Array(2);
const scratch_inertia_a = new Float64Array(3);
const scratch_inertia_b = new Float64Array(3);

/**
 * Apply an impulse `P` at body-relative offset `r` to persistent velocity
 * (Δv = P/m, Δω = I⁻¹·(r × P)), reading and writing the body's
 * {@link SolverBodyState} span instead of its `RigidBody` / `Transform`
 * objects — flat typed-array access, no dereference, on the hot path.
 *
 * @param {Float64Array} ss   solver-body-state data array
 * @param {number} base       `body_index * SBS_STRIDE`
 * @param {number} invM
 * @param {number} rx
 * @param {number} ry
 * @param {number} rz
 * @param {number} Px
 * @param {number} Py
 * @param {number} Pz
 * @param {number} sign +1 for body A, -1 for body B
 * @param {Float64Array} scratch_inertia
 */
function apply_impulse_to_body(
    ss, base, invM,
    rx, ry, rz,
    Px, Py, Pz,
    sign,
    scratch_inertia
) {

    if (invM === 0) {
        return;
    }

    const sPx = sign * Px;
    const sPy = sign * Py;
    const sPz = sign * Pz;

    ss[base + SBS_LV_X] += sPx * invM;
    ss[base + SBS_LV_Y] += sPy * invM;
    ss[base + SBS_LV_Z] += sPz * invM;

    const tx = ry * sPz - rz * sPy;
    const ty = rz * sPx - rx * sPz;
    const tz = rx * sPy - ry * sPx;

    world_inverse_inertia_apply_raw(
        scratch_inertia, 0,
        ss[base + SBS_INV_I_X], ss[base + SBS_INV_I_Y], ss[base + SBS_INV_I_Z],
        ss[base + SBS_QX], ss[base + SBS_QY], ss[base + SBS_QZ], ss[base + SBS_QW],
        tx, ty, tz
    );

    ss[base + SBS_AV_X] += scratch_inertia[0];
    ss[base + SBS_AV_Y] += scratch_inertia[1];
    ss[base + SBS_AV_Z] += scratch_inertia[2];
}

/**
 * Apply a position-pass impulse `P` at body-relative offset `r` to the
 * body's pseudo-velocity (linear + angular) in the `pseudo_velocity` flat
 * buffer. Mirrors {@link apply_impulse_to_body} but the result is consumed
 * by `integrate_position` the same substep and never persists. Inverse
 * inertia / orientation are read from the body's {@link SolverBodyState} span.
 *
 * @param {Float64Array} pseudo_velocity stride = {@link PSEUDO_STRIDE}
 * @param {number} pbase offset = `body_index * PSEUDO_STRIDE`
 * @param {Float64Array} ss solver-body-state data array
 * @param {number} sbase `body_index * SBS_STRIDE`
 * @param {number} invM
 * @param {number} rx
 * @param {number} ry
 * @param {number} rz
 * @param {number} Px
 * @param {number} Py
 * @param {number} Pz
 * @param {number} sign +1 for body A, -1 for body B
 * @param {Float64Array} scratch_inertia
 */
function apply_impulse_to_pseudo(
    pseudo_velocity, pbase,
    ss, sbase, invM,
    rx, ry, rz,
    Px, Py, Pz,
    sign,
    scratch_inertia
) {
    if (invM === 0) {
        return;
    }

    const sPx = sign * Px;
    const sPy = sign * Py;
    const sPz = sign * Pz;

    pseudo_velocity[pbase] += sPx * invM;
    pseudo_velocity[pbase + 1] += sPy * invM;
    pseudo_velocity[pbase + 2] += sPz * invM;

    const tx = ry * sPz - rz * sPy;
    const ty = rz * sPx - rx * sPz;
    const tz = rx * sPy - ry * sPx;

    world_inverse_inertia_apply_raw(
        scratch_inertia, 0,
        ss[sbase + SBS_INV_I_X], ss[sbase + SBS_INV_I_Y], ss[sbase + SBS_INV_I_Z],
        ss[sbase + SBS_QX], ss[sbase + SBS_QY], ss[sbase + SBS_QZ], ss[sbase + SBS_QW],
        tx, ty, tz
    );

    pseudo_velocity[pbase + 3] += scratch_inertia[0];
    pseudo_velocity[pbase + 4] += scratch_inertia[1];
    pseudo_velocity[pbase + 5] += scratch_inertia[2];
}

/**
 * Quadratic-form contribution of one body to the constraint effective mass
 * along a unit axis: `(r × axis)^T · I⁻¹_world · (r × axis)`. Reads the body's
 * inverse-inertia diagonal + orientation from its {@link SolverBodyState} span.
 * Non-dynamic bodies carry a zero inverse-inertia diagonal there, so the
 * zero-guard returns 0 for them — matching the object path's `kind` guard.
 *
 * @param {Float64Array} ss solver-body-state data array
 * @param {number} base `body_index * SBS_STRIDE`
 * @param {number} rx
 * @param {number} ry
 * @param {number} rz
 * @param {number} ax
 * @param {number} ay
 * @param {number} az
 * @param {Float64Array} scratch_inertia
 * @returns {number}
 */
function angular_jacobian_contribution(
    ss, base,
    rx, ry, rz,
    ax, ay, az,
    scratch_inertia
) {
    const iix = ss[base + SBS_INV_I_X];
    const iiy = ss[base + SBS_INV_I_Y];
    const iiz = ss[base + SBS_INV_I_Z];

    if (iix === 0 && iiy === 0 && iiz === 0) {
        return 0;
    }

    const rxax = ry * az - rz * ay;
    const rxay = rz * ax - rx * az;
    const rxaz = rx * ay - ry * ax;

    world_inverse_inertia_apply_raw(
        scratch_inertia, 0,
        iix, iiy, iiz,
        ss[base + SBS_QX], ss[base + SBS_QY], ss[base + SBS_QZ], ss[base + SBS_QW],
        rxax, rxay, rxaz
    );

    return rxax * scratch_inertia[0]
        + rxay * scratch_inertia[1]
        + rxaz * scratch_inertia[2];
}

/**
 * Flat count of all touched contacts across every island, plus the slot
 * list to iterate. Islands are concatenated densely in `contact_data`;
 * `contact_offsets[island_count]` is the end of the last island.
 *
 * @param {PhysicsSystem} system
 * @returns {{slot_list: Uint32Array, total_slots: number}|null}
 */
function island_slot_range(system) {
    const islands = system.islands;

    if (islands === undefined || islands === null) {
        return null;
    }

    const island_count = islands.island_count;

    if (island_count === 0) {
        return null;
    }

    return {
        slot_list: islands.contact_data,
        total_slots: islands.contact_offsets[island_count],
    };
}

/**
 * Stage 1 — prepare the contact constraints for an outer step.
 *
 * Packs every touched, non-sensor contact into the flat scratch arrays:
 * local-frame witness anchors, tangent basis, effective masses, friction,
 * the restitution approach velocity, and the warm-start replay (applied
 * once here, not per substep). Computes the SPOOK gains from the SUBSTEP
 * size `dt_sub` because the position correction runs once per substep.
 *
 * @param {ManifoldStore} manifolds
 * @param {PhysicsSystem} system
 * @param {number} dt_sub substep size `dt / substeps`
 * @returns {number} number of contacts prepared (also stored module-side)
 */
export function prepare_contacts(manifolds, system, dt_sub) {
    g_contact_count = 0;
    // Reset BEFORE the early returns below: on a step with awake bodies but zero
    // contacts/islands, an early return must not leave a stale count from a prior
    // step that redetect_concave_contacts would then act on against evicted slots.
    g_concave_slot_count = 0;

    if (dt_sub <= 0) {
        return 0;
    }

    const range = island_slot_range(system);

    if (range === null) {
        return 0;
    }

    const slot_list = range.slot_list;
    const total_slots = range.total_slots;

    let contact_total = 0;

    for (let i = 0; i < total_slots; i++) {
        contact_total += manifolds.contact_count(slot_list[i]);
    }

    if (contact_total === 0) {
        return 0;
    }

    ensure_capacity(contact_total);

    const denom = 1 + 4 * CONTACT_RELAXATION;
    g_spook_a = 4 / (dt_sub * denom);
    g_spook_eps = 4 / (dt_sub * dt_sub * CONTACT_STIFFNESS * denom);

    const data = manifolds.data_buffer;
    const pre = scratch_pre;
    const idx = scratch_idx;
    const mus = scratch_mu;
    const pos_jn = scratch_pos_jn;
    const ss = system.__solver_state.data;

    let c = 0;
    for (let i = 0; i < total_slots; i++) {
        const slot = slot_list[i];

        const idxA = body_id_index(manifolds.bodyA(slot));
        const idxB = body_id_index(manifolds.bodyB(slot));

        const baseA = idxA * SBS_STRIDE;
        const baseB = idxB * SBS_STRIDE;

        const rbA = system.__bodies[idxA];
        const rbB = system.__bodies[idxB];

        const trA = system.__transforms[idxA];
        const trB = system.__transforms[idxB];

        const colA = system.__primary_collider(idxA);
        const colB = system.__primary_collider(idxB);

        if (colA === null || colB === null) {
            continue;
        }

        if (pair_is_sensor(rbA, colA, rbB, colB)) {
            continue;
        }

        // A pair is "concave" when either side's shape is non-convex. Those
        // contacts take the per-substep re-detection path (the contact
        // feature moves as the body rocks); convex pairs keep the cheap
        // analytic refresh. Recorded once per slot for redetect_concave_contacts.
        const slot_concave = (colA.shape.is_convex === false) || (colB.shape.is_convex === false);

        if (slot_concave) {
            ensure_concave_slot_capacity(g_concave_slot_count + 1);
            concave_slot[g_concave_slot_count] = slot;
            concave_slot_idxA[g_concave_slot_count] = idxA;
            concave_slot_idxB[g_concave_slot_count] = idxB;
            g_concave_slot_count++;
        }

        const invMA = ss[baseA + SBS_INV_MASS];
        const invMB = ss[baseB + SBS_INV_MASS];

        const cc = manifolds.contact_count(slot);
        const slot_off = manifolds.slot_data_offset(slot);

        const pAx = trA.position.x, pAy = trA.position.y, pAz = trA.position.z;
        const pBx = trB.position.x, pBy = trB.position.y, pBz = trB.position.z;
        const qA = trA.rotation, qB = trB.rotation;

        for (let k = 0; k < cc; k++) {
            const off = slot_off + k * CONTACT_STRIDE;

            // Per-contact combined materials (stamped by the narrowphase from
            // this contact's specific source-collider pair).
            const friction_combined = data[off + 14];
            const restitution_combined = data[off + 15];

            const wax = data[off], way = data[off + 1], waz = data[off + 2];
            const wbx = data[off + 3], wby = data[off + 4], wbz = data[off + 5];
            const nx = data[off + 6], ny = data[off + 7], nz = data[off + 8];

            const px = (wax + wbx) * 0.5;
            const py = (way + wby) * 0.5;
            const pz = (waz + wbz) * 0.5;

            const rax = px - pAx, ray = py - pAy, raz = pz - pAz;
            const rbx = px - pBx, rby = py - pBy, rbz = pz - pBz;

            const pre_off = c * PRE_STRIDE;

            // Per-body witness anchors in LOCAL frame (constant across the
            // outer step). localW = R⁻¹ · (witness − COM).
            v3_quat3_apply_inverse(pre, pre_off, wax - pAx, way - pAy, waz - pAz, qA[0], qA[1], qA[2], qA[3]);
            v3_quat3_apply_inverse(pre, pre_off + 3, wbx - pBx, wby - pBy, wbz - pBz, qB[0], qB[1], qB[2], qB[3]);

            // Lever arms from COM to the contact midpoint (refreshed each
            // substep; seeded here from prepare-time pose).
            pre[pre_off + 6] = rax;
            pre[pre_off + 7] = ray;
            pre[pre_off + 8] = raz;
            pre[pre_off + 9] = rbx;
            pre[pre_off + 10] = rby;
            pre[pre_off + 11] = rbz;

            build_tangents(pre, pre_off + 12, nx, ny, nz);
            const t1x = pre[pre_off + 12], t1y = pre[pre_off + 13], t1z = pre[pre_off + 14];
            const t2x = pre[pre_off + 15], t2y = pre[pre_off + 16], t2z = pre[pre_off + 17];

            const k_n = invMA + invMB
                + angular_jacobian_contribution(ss, baseA, rax, ray, raz, nx, ny, nz, scratch_inertia_a)
                + angular_jacobian_contribution(ss, baseB, rbx, rby, rbz, nx, ny, nz, scratch_inertia_b);
            const k_t1 = invMA + invMB
                + angular_jacobian_contribution(ss, baseA, rax, ray, raz, t1x, t1y, t1z, scratch_inertia_a)
                + angular_jacobian_contribution(ss, baseB, rbx, rby, rbz, t1x, t1y, t1z, scratch_inertia_b);
            const k_t2 = invMA + invMB
                + angular_jacobian_contribution(ss, baseA, rax, ray, raz, t2x, t2y, t2z, scratch_inertia_a)
                + angular_jacobian_contribution(ss, baseB, rbx, rby, rbz, t2x, t2y, t2z, scratch_inertia_b);

            const k_n_eff = k_n + g_spook_eps;
            pre[pre_off + 18] = k_n_eff > 0 ? 1 / k_n_eff : 0;
            pre[pre_off + 19] = k_t1 > 0 ? 1 / k_t1 : 0;
            pre[pre_off + 20] = k_t2 > 0 ? 1 / k_t2 : 0;

            // Restitution approach velocity (captured once, this is the
            // closing speed entering the step). n is B → A; vn < 0 closing.
            const vAx_at = ss[baseA + SBS_LV_X] + ss[baseA + SBS_AV_Y] * raz - ss[baseA + SBS_AV_Z] * ray;
            const vAy_at = ss[baseA + SBS_LV_Y] + ss[baseA + SBS_AV_Z] * rax - ss[baseA + SBS_AV_X] * raz;
            const vAz_at = ss[baseA + SBS_LV_Z] + ss[baseA + SBS_AV_X] * ray - ss[baseA + SBS_AV_Y] * rax;

            const vBx_at = ss[baseB + SBS_LV_X] + ss[baseB + SBS_AV_Y] * rbz - ss[baseB + SBS_AV_Z] * rby;
            const vBy_at = ss[baseB + SBS_LV_Y] + ss[baseB + SBS_AV_Z] * rbx - ss[baseB + SBS_AV_X] * rbz;
            const vBz_at = ss[baseB + SBS_LV_Z] + ss[baseB + SBS_AV_X] * rby - ss[baseB + SBS_AV_Y] * rbx;

            const vn_pre = (vAx_at - vBx_at) * nx + (vAy_at - vBy_at) * ny + (vAz_at - vBz_at) * nz;

            let rest_bias = 0;
            if (vn_pre < -RESTITUTION_VELOCITY_THRESHOLD) {
                rest_bias = restitution_combined * vn_pre;
            }
            pre[pre_off + 21] = rest_bias;
            pre[pre_off + 22] = 0; // bias_position — filled by refresh_contacts

            mus[c] = friction_combined;
            pos_jn[c] = 0;
            scratch_max_jn[c] = 0;
            scratch_concave[c] = slot_concave ? 1 : 0;

            idx[c * INDEX_STRIDE] = slot;
            idx[c * INDEX_STRIDE + 1] = k;
            idx[c * INDEX_STRIDE + 2] = idxA;
            idx[c * INDEX_STRIDE + 3] = idxB;

            // Warm-start is NOT applied here: under TGS it must be replayed
            // *every substep* (see warm_start_contacts) so that, per substep,
            // the cached impulse balances exactly one substep of gravity.
            // Applying the cached impulse once against a full frame of
            // gravity (the non-substepped pattern) over-pushes resting
            // contacts and jitters / explodes deep stacks.

            c++;
        }
    }

    g_contact_count = c;
    return c;
}

/**
 * Stage 1b (per substep) — warm-start: replay the cached accumulated
 * impulses `(j_n, j_t1, j_t2)` onto persistent velocity using the current
 * (refreshed) lever arms and tangents. Run once per substep, after
 * {@link refresh_contacts} and before {@link solve_velocity}.
 *
 * Per-substep warm-start is the crux of stable TGS: the stored impulse is a
 * per-substep quantity (≈ the impulse to counter one substep of gravity), so
 * replaying it each substep balances that substep's `integrate_velocity_gravity`
 * and a resting contact holds at zero velocity. `solve_velocity` then only
 * has to correct the residual, which converges in a few iterations even for
 * deep chains because each substep carries just `h` of gravity.
 *
 * @param {ManifoldStore} manifolds
 * @param {PhysicsSystem} system
 */
export function warm_start_contacts(manifolds, system) {
    const count = g_contact_count;
    if (count === 0) return;

    const data = manifolds.data_buffer;
    const pre = scratch_pre;
    const idx = scratch_idx;
    const ss = system.__solver_state.data;

    for (let ci = 0; ci < count; ci++) {
        const slot = idx[ci * INDEX_STRIDE];
        const cidx = idx[ci * INDEX_STRIDE + 1];

        const baseA = idx[ci * INDEX_STRIDE + 2] * SBS_STRIDE;
        const baseB = idx[ci * INDEX_STRIDE + 3] * SBS_STRIDE;

        const invMA = ss[baseA + SBS_INV_MASS];
        const invMB = ss[baseB + SBS_INV_MASS];

        const pre_off = ci * PRE_STRIDE;

        const rax = pre[pre_off + 6], ray = pre[pre_off + 7], raz = pre[pre_off + 8];
        const rbx = pre[pre_off + 9], rby = pre[pre_off + 10], rbz = pre[pre_off + 11];

        const t1x = pre[pre_off + 12], t1y = pre[pre_off + 13], t1z = pre[pre_off + 14];
        const t2x = pre[pre_off + 15], t2y = pre[pre_off + 16], t2z = pre[pre_off + 17];

        const slot_off = manifolds.slot_data_offset(slot);
        const off = slot_off + cidx * CONTACT_STRIDE;
        const nx = data[off + 6], ny = data[off + 7], nz = data[off + 8];

        const j_n = data[off + 10];
        const j_t1 = data[off + 11];
        const j_t2 = data[off + 12];

        const Px = nx * j_n + t1x * j_t1 + t2x * j_t2;
        const Py = ny * j_n + t1y * j_t1 + t2y * j_t2;
        const Pz = nz * j_n + t1z * j_t1 + t2z * j_t2;

        apply_impulse_to_body(ss, baseA, invMA, rax, ray, raz, Px, Py, Pz, +1, scratch_inertia_a);
        apply_impulse_to_body(ss, baseB, invMB, rbx, rby, rbz, Px, Py, Pz, -1, scratch_inertia_b);
    }
}

/**
 * Stage 2 (per substep) — re-derive each contact's geometry from the
 * bodies' current poses and the local witness anchors captured at prepare.
 *
 * For each contact:
 *  - rotate the stored local witnesses back to world by the current pose to
 *    get the moved contact points `cpA`, `cpB`;
 *  - current penetration `depth_now = depth0 − Δseparation`, where
 *    `Δseparation = (cpA − wA − (cpB − wB)) · n` is the change since prepare.
 *    Anchoring on the trusted prepare-time depth makes the sign convention
 *    irrelevant — only the analytic delta uses the anchors;
 *  - rebuild the impulse levers `rA`/`rB` from the moved midpoint;
 *  - recompute the position-correction bias from `depth_now`.
 *
 * The contact normal and tangents are held fixed for the outer step (valid
 * for the small per-step rotation), so they are not recomputed here.
 *
 * @param {ManifoldStore} manifolds
 * @param {Transform[]} transforms
 */
export function refresh_contacts(manifolds, transforms) {
    const count = g_contact_count;

    if (count === 0) {
        return;
    }

    const data = manifolds.data_buffer;
    const pre = scratch_pre;
    const idx = scratch_idx;
    const cp = scratch_cp;
    const spook_a = g_spook_a;

    for (let ci = 0; ci < count; ci++) {
        // Concave contacts are refreshed by redetect_concave_contacts (fresh
        // narrowphase geometry each substep), not by the analytic rotation of
        // frozen anchors — their feature moves as the body rocks.
        if (scratch_concave[ci] === 1) {
            continue;
        }

        const slot = idx[ci * INDEX_STRIDE];
        const cidx = idx[ci * INDEX_STRIDE + 1];
        const idxA = idx[ci * INDEX_STRIDE + 2];
        const idxB = idx[ci * INDEX_STRIDE + 3];

        const trA = transforms[idxA];
        const trB = transforms[idxB];

        const slot_off = manifolds.slot_data_offset(slot);
        const off = slot_off + cidx * CONTACT_STRIDE;

        const wax = data[off], way = data[off + 1], waz = data[off + 2];
        const wbx = data[off + 3], wby = data[off + 4], wbz = data[off + 5];
        const nx = data[off + 6], ny = data[off + 7], nz = data[off + 8];

        const depth0 = data[off + 9];

        const pre_off = ci * PRE_STRIDE;

        const pAx = trA.position[0];
        const pAy = trA.position[1];
        const pAz = trA.position[2];

        const pBx = trB.position[0];
        const pBy = trB.position[1];
        const pBz = trB.position[2];

        const qA = trA.rotation;
        const qB = trB.rotation;

        // Current world contact points: COM + R · localWitness.
        v3_quat3_apply(cp, 0, pre[pre_off], pre[pre_off + 1], pre[pre_off + 2], qA[0], qA[1], qA[2], qA[3]);
        v3_quat3_apply(cp, 3, pre[pre_off + 3], pre[pre_off + 4], pre[pre_off + 5], qB[0], qB[1], qB[2], qB[3]);

        const cpAx = pAx + cp[0], cpAy = pAy + cp[1], cpAz = pAz + cp[2];
        const cpBx = pBx + cp[3], cpBy = pBy + cp[4], cpBz = pBz + cp[5];

        // Penetration re-derived as a delta from the trusted prepare depth.
        // Δsep = ((cpA − wA) − (cpB − wB)) · n.
        const dsep = ((cpAx - wax) - (cpBx - wbx)) * nx
            + ((cpAy - way) - (cpBy - wby)) * ny
            + ((cpAz - waz) - (cpBz - wbz)) * nz;

        const depth_now = depth0 - dsep;

        // Impulse levers from each COM to the current contact midpoint.
        const mx = (cpAx + cpBx) * 0.5;
        const my = (cpAy + cpBy) * 0.5;
        const mz = (cpAz + cpBz) * 0.5;

        pre[pre_off + 6] = mx - pAx;
        pre[pre_off + 7] = my - pAy;
        pre[pre_off + 8] = mz - pAz;
        pre[pre_off + 9] = mx - pBx;
        pre[pre_off + 10] = my - pBy;
        pre[pre_off + 11] = mz - pBz;

        let bias_position = 0;

        if (depth_now > PENETRATION_SLOP) {
            bias_position = -spook_a * (depth_now - PENETRATION_SLOP);

            if (bias_position < -MAX_POSITION_BIAS) {
                bias_position = -MAX_POSITION_BIAS;
            }
        }

        pre[pre_off + 22] = bias_position;
    }
}

/**
 * Stage 2b (per substep) — the concave counterpart of {@link refresh_contacts}.
 *
 * For each concave-involved slot, re-runs the narrowphase geometry at the
 * current substep pose ({@link redetect_pair_geometry}, which rewrites the
 * manifold's witness / normal / depth in place), then re-derives the solver
 * scratch (lever arms, tangent basis, effective masses, position bias) for
 * that slot's contacts from the fresh geometry. This is the whole point of
 * the concave path: a body rocking on a mesh changes which triangle (and
 * normal) it rests on within a single outer step, and freezing that — as the
 * convex analytic refresh does — pumps energy in. Re-detecting gives a
 * correct per-substep normal so the body settles.
 *
 * Cost is ~one narrowphase dispatch per concave slot per substep — acceptable
 * because concave-involved pairs are rare (the common concave case, a convex
 * body on static terrain, is convex on the moving side and never lands here).
 * The contact count is held fixed (redetect updates geometry only), so the
 * scratch stays aligned with prepare.
 *
 * Must run before {@link refresh_contacts} / {@link warm_start_contacts} each
 * substep. `rest_bias` (captured at prepare) and the `scratch_max_jn`
 * restitution gate are preserved.
 *
 * @param {ManifoldStore} manifolds
 * @param {PhysicsSystem} system  reads `__body_collider_lists`, `__bodies`,
 *   `__transforms`.
 */
export function redetect_concave_contacts(manifolds, system) {
    const ns = g_concave_slot_count;

    if (ns === 0) {
        return;
    }

    const lists = system.__body_collider_lists;

    // 1. Re-detect fresh geometry into the manifold for each concave slot.
    for (let s = 0; s < ns; s++) {
        redetect_pair_geometry(manifolds, concave_slot[s], lists[concave_slot_idxA[s]], lists[concave_slot_idxB[s]]);
    }

    // 2. Re-derive the solver scratch for every concave contact from the
    //    fresh manifold geometry (lever arms, tangents, effective masses,
    //    position bias). Mirrors prepare's per-contact setup but reads the
    //    just-updated witness / normal / depth instead of the prepare-time
    //    values; no local-frame anchors are needed since we re-detect rather
    //    than rotate frozen anchors.
    const count = g_contact_count;
    const data = manifolds.data_buffer;
    const pre = scratch_pre;
    const idx = scratch_idx;
    const spook_a = g_spook_a;
    const spook_eps = g_spook_eps;
    const ss = system.__solver_state.data;

    for (let ci = 0; ci < count; ci++) {
        if (scratch_concave[ci] === 0) {
            continue;
        }

        const slot = idx[ci * INDEX_STRIDE];
        const cidx = idx[ci * INDEX_STRIDE + 1];
        const idxA = idx[ci * INDEX_STRIDE + 2];
        const idxB = idx[ci * INDEX_STRIDE + 3];

        const baseA = idxA * SBS_STRIDE;
        const baseB = idxB * SBS_STRIDE;

        const trA = system.__transforms[idxA];
        const trB = system.__transforms[idxB];

        const invMA = ss[baseA + SBS_INV_MASS];
        const invMB = ss[baseB + SBS_INV_MASS];

        const slot_off = manifolds.slot_data_offset(slot);
        const off = slot_off + cidx * CONTACT_STRIDE;

        const wax = data[off], way = data[off + 1], waz = data[off + 2];
        const wbx = data[off + 3], wby = data[off + 4], wbz = data[off + 5];
        const nx = data[off + 6], ny = data[off + 7], nz = data[off + 8];
        const depth = data[off + 9];

        const px = (wax + wbx) * 0.5;
        const py = (way + wby) * 0.5;
        const pz = (waz + wbz) * 0.5;

        const rax = px - trA.position.x, ray = py - trA.position.y, raz = pz - trA.position.z;
        const rbx = px - trB.position.x, rby = py - trB.position.y, rbz = pz - trB.position.z;

        const pre_off = ci * PRE_STRIDE;
        pre[pre_off + 6] = rax;
        pre[pre_off + 7] = ray;
        pre[pre_off + 8] = raz;
        pre[pre_off + 9] = rbx;
        pre[pre_off + 10] = rby;
        pre[pre_off + 11] = rbz;

        build_tangents(pre, pre_off + 12, nx, ny, nz);
        const t1x = pre[pre_off + 12], t1y = pre[pre_off + 13], t1z = pre[pre_off + 14];
        const t2x = pre[pre_off + 15], t2y = pre[pre_off + 16], t2z = pre[pre_off + 17];

        const k_n = invMA + invMB
            + angular_jacobian_contribution(ss, baseA, rax, ray, raz, nx, ny, nz, scratch_inertia_a)
            + angular_jacobian_contribution(ss, baseB, rbx, rby, rbz, nx, ny, nz, scratch_inertia_b);
        const k_t1 = invMA + invMB
            + angular_jacobian_contribution(ss, baseA, rax, ray, raz, t1x, t1y, t1z, scratch_inertia_a)
            + angular_jacobian_contribution(ss, baseB, rbx, rby, rbz, t1x, t1y, t1z, scratch_inertia_b);
        const k_t2 = invMA + invMB
            + angular_jacobian_contribution(ss, baseA, rax, ray, raz, t2x, t2y, t2z, scratch_inertia_a)
            + angular_jacobian_contribution(ss, baseB, rbx, rby, rbz, t2x, t2y, t2z, scratch_inertia_b);

        const k_n_eff = k_n + spook_eps;
        pre[pre_off + 18] = k_n_eff > 0 ? 1 / k_n_eff : 0;
        pre[pre_off + 19] = k_t1 > 0 ? 1 / k_t1 : 0;
        pre[pre_off + 20] = k_t2 > 0 ? 1 / k_t2 : 0;

        let bias_position = 0;

        if (depth > PENETRATION_SLOP) {
            bias_position = -spook_a * (depth - PENETRATION_SLOP);

            if (bias_position < -MAX_POSITION_BIAS) {
                bias_position = -MAX_POSITION_BIAS;
            }

        }

        pre[pre_off + 22] = bias_position;
    }
}

/**
 * Stage 3 (per substep) — velocity iterations enforcing pure
 * non-penetration (`vn → 0`) plus Coulomb-disk friction. No bias: depth
 * correction is the position pass, restitution is the one-shot pass.
 *
 * @param {ManifoldStore} manifolds
 * @param {PhysicsSystem} system
 * @param {number} iters
 */
export function solve_velocity(manifolds, system, iters) {
    const contact_count = g_contact_count;
    if (contact_count === 0) return;

    const data = manifolds.data_buffer;
    const pre = scratch_pre;
    const idx = scratch_idx;
    const mus = scratch_mu;
    const ss = system.__solver_state.data;

    for (let iter = 0; iter < iters; iter++) {
        for (let ci = 0; ci < contact_count; ci++) {

            const slot = idx[ci * INDEX_STRIDE];
            const cidx = idx[ci * INDEX_STRIDE + 1];

            const baseA = idx[ci * INDEX_STRIDE + 2] * SBS_STRIDE;
            const baseB = idx[ci * INDEX_STRIDE + 3] * SBS_STRIDE;

            const invMA = ss[baseA + SBS_INV_MASS];
            const invMB = ss[baseB + SBS_INV_MASS];

            const pre_off = ci * PRE_STRIDE;

            const rax = pre[pre_off + 6], ray = pre[pre_off + 7], raz = pre[pre_off + 8];
            const rbx = pre[pre_off + 9], rby = pre[pre_off + 10], rbz = pre[pre_off + 11];

            const t1x = pre[pre_off + 12], t1y = pre[pre_off + 13], t1z = pre[pre_off + 14];
            const t2x = pre[pre_off + 15], t2y = pre[pre_off + 16], t2z = pre[pre_off + 17];

            const m_eff_n = pre[pre_off + 18];
            const m_eff_t1 = pre[pre_off + 19];
            const m_eff_t2 = pre[pre_off + 20];

            const slot_off = manifolds.slot_data_offset(slot);
            const off = slot_off + cidx * CONTACT_STRIDE;
            const nx = data[off + 6], ny = data[off + 7], nz = data[off + 8];

            const vAx_at = ss[baseA + SBS_LV_X] + ss[baseA + SBS_AV_Y] * raz - ss[baseA + SBS_AV_Z] * ray;
            const vAy_at = ss[baseA + SBS_LV_Y] + ss[baseA + SBS_AV_Z] * rax - ss[baseA + SBS_AV_X] * raz;
            const vAz_at = ss[baseA + SBS_LV_Z] + ss[baseA + SBS_AV_X] * ray - ss[baseA + SBS_AV_Y] * rax;

            const vBx_at = ss[baseB + SBS_LV_X] + ss[baseB + SBS_AV_Y] * rbz - ss[baseB + SBS_AV_Z] * rby;
            const vBy_at = ss[baseB + SBS_LV_Y] + ss[baseB + SBS_AV_Z] * rbx - ss[baseB + SBS_AV_X] * rbz;
            const vBz_at = ss[baseB + SBS_LV_Z] + ss[baseB + SBS_AV_X] * rby - ss[baseB + SBS_AV_Y] * rbx;

            const dvx = vAx_at - vBx_at;
            const dvy = vAy_at - vBy_at;
            const dvz = vAz_at - vBz_at;

            // --- Normal impulse (non-penetration: drive vn → 0) ---
            const vn = dvx * nx + dvy * ny + dvz * nz;
            const j_n_accum = data[off + 10];
            const lambda_n = -m_eff_n * vn;
            const sum_n = j_n_accum + lambda_n;
            const new_j_n = sum_n > 0 ? sum_n : 0;
            const delta_j_n = new_j_n - j_n_accum;
            data[off + 10] = new_j_n;

            if (new_j_n > scratch_max_jn[ci]) {
                scratch_max_jn[ci] = new_j_n;
            }

            if (delta_j_n !== 0) {
                const Pnx = nx * delta_j_n;
                const Pny = ny * delta_j_n;
                const Pnz = nz * delta_j_n;

                apply_impulse_to_body(ss, baseA, invMA, rax, ray, raz, Pnx, Pny, Pnz, +1, scratch_inertia_a);
                apply_impulse_to_body(ss, baseB, invMB, rbx, rby, rbz, Pnx, Pny, Pnz, -1, scratch_inertia_b);
            }

            // --- Friction impulse (Coulomb disk in tangent plane) ---
            // Re-read velocity from the solver state: the normal impulse above
            // just mutated it, and friction must see the corrected velocity.
            const vAx2 = ss[baseA + SBS_LV_X] + ss[baseA + SBS_AV_Y] * raz - ss[baseA + SBS_AV_Z] * ray;
            const vAy2 = ss[baseA + SBS_LV_Y] + ss[baseA + SBS_AV_Z] * rax - ss[baseA + SBS_AV_X] * raz;
            const vAz2 = ss[baseA + SBS_LV_Z] + ss[baseA + SBS_AV_X] * ray - ss[baseA + SBS_AV_Y] * rax;

            const vBx2 = ss[baseB + SBS_LV_X] + ss[baseB + SBS_AV_Y] * rbz - ss[baseB + SBS_AV_Z] * rby;
            const vBy2 = ss[baseB + SBS_LV_Y] + ss[baseB + SBS_AV_Z] * rbx - ss[baseB + SBS_AV_X] * rbz;
            const vBz2 = ss[baseB + SBS_LV_Z] + ss[baseB + SBS_AV_X] * rby - ss[baseB + SBS_AV_Y] * rbx;

            const dvx2 = vAx2 - vBx2;
            const dvy2 = vAy2 - vBy2;
            const dvz2 = vAz2 - vBz2;

            const vt1 = dvx2 * t1x + dvy2 * t1y + dvz2 * t1z;
            const vt2 = dvx2 * t2x + dvy2 * t2y + dvz2 * t2z;

            const j_t1_accum = data[off + 11];
            const j_t2_accum = data[off + 12];

            const lambda_t1 = -m_eff_t1 * vt1;
            const lambda_t2 = -m_eff_t2 * vt2;

            const want_t1 = j_t1_accum + lambda_t1;
            const want_t2 = j_t2_accum + lambda_t2;

            const max_friction = mus[ci] * new_j_n;

            friction_cone_clamp(scratch_clamp, 0, want_t1, want_t2, max_friction);

            const new_j_t1 = scratch_clamp[0];
            const new_j_t2 = scratch_clamp[1];

            const delta_t1 = new_j_t1 - j_t1_accum;
            const delta_t2 = new_j_t2 - j_t2_accum;

            data[off + 11] = new_j_t1;
            data[off + 12] = new_j_t2;

            if (delta_t1 !== 0 || delta_t2 !== 0) {

                const Ptx = t1x * delta_t1 + t2x * delta_t2;
                const Pty = t1y * delta_t1 + t2y * delta_t2;
                const Ptz = t1z * delta_t1 + t2z * delta_t2;

                apply_impulse_to_body(ss, baseA, invMA, rax, ray, raz, Ptx, Pty, Ptz, +1, scratch_inertia_a);
                apply_impulse_to_body(ss, baseB, invMB, rbx, rby, rbz, Ptx, Pty, Ptz, -1, scratch_inertia_b);

            }
        }
    }
}

/**
 * Stage 4 (once, after the substep loop) — one-shot restitution
 * (Box2D-v3 `b2ApplyRestitution`, Catto 2018). Drives `vn → -e · vn_approach`
 * exactly once per closing contact, gated on (a) the contact having been
 * closing faster than the threshold at prepare, and (b) a compressive
 * normal impulse having formed during the velocity solve. The added impulse
 * accumulates into the same normal-impulse slot so it composes with
 * warm-start next frame.
 *
 * @param {ManifoldStore} manifolds
 * @param {PhysicsSystem} system
 */
export function apply_restitution(manifolds, system) {
    const contact_count = g_contact_count;
    if (contact_count === 0) return;

    const data = manifolds.data_buffer;
    const pre = scratch_pre;
    const idx = scratch_idx;
    const ss = system.__solver_state.data;

    for (let ci = 0; ci < contact_count; ci++) {
        const pre_off = ci * PRE_STRIDE;
        const rest_bias = pre[pre_off + 21];
        if (rest_bias === 0) continue;

        const slot = idx[ci * INDEX_STRIDE];
        const cidx = idx[ci * INDEX_STRIDE + 1];
        // Gate on the running max normal impulse, not the end-of-loop value:
        // a transient collision relaxes back to j_n ≈ 0 under per-substep
        // warm-start, but it WAS compressive, so it should still bounce.
        if (scratch_max_jn[ci] <= 0) continue;

        const slot_off = manifolds.slot_data_offset(slot);
        const off = slot_off + cidx * CONTACT_STRIDE;

        const j_n_accum = data[off + 10];

        const baseA = idx[ci * INDEX_STRIDE + 2] * SBS_STRIDE;
        const baseB = idx[ci * INDEX_STRIDE + 3] * SBS_STRIDE;
        const invMA = ss[baseA + SBS_INV_MASS];
        const invMB = ss[baseB + SBS_INV_MASS];

        const rax = pre[pre_off + 6], ray = pre[pre_off + 7], raz = pre[pre_off + 8];
        const rbx = pre[pre_off + 9], rby = pre[pre_off + 10], rbz = pre[pre_off + 11];
        const m_eff_n = pre[pre_off + 18];
        const nx = data[off + 6], ny = data[off + 7], nz = data[off + 8];

        const vAx_at = ss[baseA + SBS_LV_X] + ss[baseA + SBS_AV_Y] * raz - ss[baseA + SBS_AV_Z] * ray;
        const vAy_at = ss[baseA + SBS_LV_Y] + ss[baseA + SBS_AV_Z] * rax - ss[baseA + SBS_AV_X] * raz;
        const vAz_at = ss[baseA + SBS_LV_Z] + ss[baseA + SBS_AV_X] * ray - ss[baseA + SBS_AV_Y] * rax;
        const vBx_at = ss[baseB + SBS_LV_X] + ss[baseB + SBS_AV_Y] * rbz - ss[baseB + SBS_AV_Z] * rby;
        const vBy_at = ss[baseB + SBS_LV_Y] + ss[baseB + SBS_AV_Z] * rbx - ss[baseB + SBS_AV_X] * rbz;
        const vBz_at = ss[baseB + SBS_LV_Z] + ss[baseB + SBS_AV_X] * rby - ss[baseB + SBS_AV_Y] * rbx;
        const vn = (vAx_at - vBx_at) * nx + (vAy_at - vBy_at) * ny + (vAz_at - vBz_at) * nz;

        const lambda = -m_eff_n * (vn + rest_bias);
        const new_j_n = (j_n_accum + lambda) > 0 ? (j_n_accum + lambda) : 0;
        const delta = new_j_n - j_n_accum;
        data[off + 10] = new_j_n;

        if (delta !== 0) {
            const Px = nx * delta, Py = ny * delta, Pz = nz * delta;
            apply_impulse_to_body(ss, baseA, invMA, rax, ray, raz, Px, Py, Pz, +1, scratch_inertia_a);
            apply_impulse_to_body(ss, baseB, invMB, rbx, rby, rbz, Px, Py, Pz, -1, scratch_inertia_b);
        }
    }
}

/**
 * Stage 5 (per substep) — split-impulse position correction. Normal-only
 * (friction in the pseudo-velocity pass is ill-defined). Reads the per-body
 * pseudo-velocity, applies a clamped normal impulse driven by the refreshed
 * `bias_position`, and writes the increment back into the pseudo buffer. The
 * pose integrator folds pseudo-velocity into `pos += v · dt` and discards it.
 *
 * The pseudo buffer must be zeroed by the caller before this stage each
 * substep (it's a per-substep correction). The position accumulator
 * `scratch_pos_jn` is likewise reset here per substep.
 *
 * @param {ManifoldStore} manifolds
 * @param {PhysicsSystem} system
 * @param {number} pos_iters
 */
export function solve_position(manifolds, system, pos_iters) {
    const contact_count = g_contact_count;
    if (contact_count === 0) return;

    const data = manifolds.data_buffer;
    const pre = scratch_pre;
    const idx = scratch_idx;
    const pos_jn = scratch_pos_jn;
    const pseudoVel = system.__pseudo_velocity;
    const ss = system.__solver_state.data;

    // Reset the position-impulse accumulator for this substep.
    for (let ci = 0; ci < contact_count; ci++) pos_jn[ci] = 0;

    for (let iter = 0; iter < pos_iters; iter++) {
        for (let ci = 0; ci < contact_count; ci++) {
            const pre_off = ci * PRE_STRIDE;
            const bias_p = pre[pre_off + 22];
            if (bias_p === 0) continue;

            const slot = idx[ci * INDEX_STRIDE];
            const cidx = idx[ci * INDEX_STRIDE + 1];
            const idxA = idx[ci * INDEX_STRIDE + 2];
            const idxB = idx[ci * INDEX_STRIDE + 3];
            const sbaseA = idxA * SBS_STRIDE;
            const sbaseB = idxB * SBS_STRIDE;
            const invMA = ss[sbaseA + SBS_INV_MASS];
            const invMB = ss[sbaseB + SBS_INV_MASS];

            const rax = pre[pre_off + 6], ray = pre[pre_off + 7], raz = pre[pre_off + 8];
            const rbx = pre[pre_off + 9], rby = pre[pre_off + 10], rbz = pre[pre_off + 11];
            const m_eff_n = pre[pre_off + 18];

            const slot_off = manifolds.slot_data_offset(slot);
            const off = slot_off + cidx * CONTACT_STRIDE;
            const nx = data[off + 6], ny = data[off + 7], nz = data[off + 8];

            const baseA = idxA * PSEUDO_STRIDE;
            const baseB = idxB * PSEUDO_STRIDE;

            const pslA_x = pseudoVel[baseA], pslA_y = pseudoVel[baseA + 1], pslA_z = pseudoVel[baseA + 2];
            const psaA_x = pseudoVel[baseA + 3], psaA_y = pseudoVel[baseA + 4], psaA_z = pseudoVel[baseA + 5];
            const pslB_x = pseudoVel[baseB], pslB_y = pseudoVel[baseB + 1], pslB_z = pseudoVel[baseB + 2];
            const psaB_x = pseudoVel[baseB + 3], psaB_y = pseudoVel[baseB + 4], psaB_z = pseudoVel[baseB + 5];

            const psvAx_at = pslA_x + psaA_y * raz - psaA_z * ray;
            const psvAy_at = pslA_y + psaA_z * rax - psaA_x * raz;
            const psvAz_at = pslA_z + psaA_x * ray - psaA_y * rax;
            const psvBx_at = pslB_x + psaB_y * rbz - psaB_z * rby;
            const psvBy_at = pslB_y + psaB_z * rbx - psaB_x * rbz;
            const psvBz_at = pslB_z + psaB_x * rby - psaB_y * rbx;

            const psdvx = psvAx_at - psvBx_at;
            const psdvy = psvAy_at - psvBy_at;
            const psdvz = psvAz_at - psvBz_at;
            const psvn = psdvx * nx + psdvy * ny + psdvz * nz;

            const jn_accum = pos_jn[ci];
            const lambda = -m_eff_n * (psvn + bias_p);
            const sum = jn_accum + lambda;
            const new_jn = sum > 0 ? sum : 0;
            const delta = new_jn - jn_accum;
            pos_jn[ci] = new_jn;

            if (delta !== 0) {
                const Px = nx * delta;
                const Py = ny * delta;
                const Pz = nz * delta;
                apply_impulse_to_pseudo(pseudoVel, baseA, ss, sbaseA, invMA, rax, ray, raz, Px, Py, Pz, +1, scratch_inertia_a);
                apply_impulse_to_pseudo(pseudoVel, baseB, ss, sbaseB, invMB, rbx, rby, rbz, Px, Py, Pz, -1, scratch_inertia_b);
            }
        }
    }
}

/**
 * Convenience single-step driver: prepare → refresh → velocity →
 * restitution → position, all at the full `dt` (one substep). Equivalent to
 * the Phase-2 solver. The substepped path in `PhysicsSystem.fixedUpdate`
 * calls the stages directly; this entry point exists for callers/tests that
 * want a one-shot solve.
 *
 * The position pass writes `system.__pseudo_velocity`; the caller must zero
 * that buffer before this call and fold it into the pose afterwards.
 *
 * @param {ManifoldStore} manifolds
 * @param {PhysicsSystem} system
 * @param {number} dt
 * @param {number} [iters]
 * @param {number} [pos_iters]
 */
export function solve_contacts(manifolds, system, dt,
                               iters = DEFAULT_VELOCITY_ITERATIONS,
                               pos_iters = DEFAULT_POSITION_ITERATIONS) {
    if (dt <= 0) return;
    if (prepare_contacts(manifolds, system, dt) === 0) return;
    redetect_concave_contacts(manifolds, system);
    refresh_contacts(manifolds, system.__transforms);
    warm_start_contacts(manifolds, system);
    solve_velocity(manifolds, system, iters);
    apply_restitution(manifolds, system);
    solve_position(manifolds, system, pos_iters);
}

export { DEFAULT_VELOCITY_ITERATIONS, DEFAULT_POSITION_ITERATIONS };
