import { quat3_multiply } from "../../../core/geom/3d/quaternion/quat3_multiply.js";
import { quat3_to_matrix3 } from "../../../core/geom/3d/quaternion/quat3_to_matrix3.js";
import { v3_quat3_apply } from "../../../core/geom/vec3/v3_quat3_apply.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 { JOINT_WORLD } from "../ecs/Joint.js";
import { SleepState } from "../ecs/SleepState.js";
import { world_inverse_inertia_apply_raw } from "../inertia/world_inverse_inertia.js";
import { DofMode } from "./DofMode.js";

/**
 * # 6-DOF constraint (joint) solver
 *
 * Runs alongside the contact solver inside the same TGS substep loop: each
 * substep the joints are warm-started and their velocity rows solved, sharing
 * the contact solver's per-substep / warm-start / SPOOK-bias model so the two
 * constraint families converge together (a body touched by both a contact and
 * a joint sees a single coupled Gauss-Seidel sweep across the substep loop).
 *
 * A joint is a configurable 6-DOF constraint ({@link Joint}); each relative
 * degree of freedom is independently locked / free / limited / springy /
 * motorised. This module implements **LOCKED** and **LIMITED** DOFs (linear
 * and angular). LOCKED + FREE express the ball-socket, hinge, weld, and
 * prismatic joints; LIMITED adds slider end-stops and joint range-of-motion.
 * Springs and motors slot into the same per-DOF row model as they land.
 *
 * ## One row model, parameterised by mode
 *
 * Every DOF resolves to the same scalar velocity row, contact-shaped: for a
 * unit axis `d` and lever arms `rA = anchorA − comA`, `rB = anchorB − comB`,
 *   `K = invMA + invMB + (rA×d)·Iw_A·(rA×d) + (rB×d)·Iw_B·(rB×d)`  (linear)
 *   `K = d·Iw_A·d + d·Iw_B·d`                                       (angular)
 *   `vrel = (vA + ωA×rA − vB − ωB×rB) · d`   (linear, A−B)
 *   `vrel = (ωB − ωA) · d`                   (angular, B−A)
 *   `λ = clamp(λ_acc − (1/K)·(vrel + bias)) − λ_acc`,
 * applied as `±λ·d`. The **mode** picks only two row parameters:
 *   - LOCKED  → `bias = β/h · error`, impulse clamp `(−∞, +∞)` (bilateral).
 *   - LIMITED → row active only when the DOF position is at/beyond a bound;
 *     `bias = β/h · (position − bound)`, impulse clamp one-sided
 *     (`[0, +∞)` at the lower stop, `(−∞, 0]` at the upper). The clamp makes
 *     it a unilateral push-out-of-violation constraint, exactly like a contact
 *     normal — the limit can only resist crossing the stop, never pull the DOF
 *     back into range.
 * The warm-start and the iteration loop are mode-agnostic; they read the
 * per-DOF `(active, effMass, bias, clampLo, clampHi)` filled by the mode-aware
 * setup. Geometry (anchors / lever arms / position error) is recomputed from
 * the current pose every substep, so the joint tracks the bodies as they move.
 *
 * The "position" along a DOF is `C · axis` for linear (signed anchor offset in
 * metres) and `e · axis` for angular, where `e = 2·sign(qD.w)·qD.xyz` is the
 * small-angle rotation vector of the relative rotation `qD = conj(qA)⊗qB`,
 * expressed in frame-A-local coordinates (so error component `k` pairs with
 * frame axis `k`). The angular position is therefore a first-order (small
 * angle) measure: accurate for the modest ranges of slider/hinge end-stops,
 * but it under-reports large angles (`2·sin(θ/2)` vs `θ`). Accurate wide-cone
 * range-of-motion (ragdoll shoulders) needs the swing-twist decomposition,
 * tracked as the follow-up; angular LIMITED here is the moderate-range cut.
 * The decomposition itself is already implemented — reuse
 * `Quaternion.computeSwingAndTwist(axis, swing, twist)` /
 * `computeTwistAngle(axis)` (`core/geom/Quaternion.js`) on the relative
 * rotation `qD` rather than writing a new one.
 *
 * @author Alex Goldring
 * @copyright Company Named Limited (c) 2026
 */

/**
 * Maximum SPOOK position-correction bias for a joint row, m/s — belt-and-braces
 * against a wildly-violated constraint (e.g. a freshly-teleported body)
 * yanking the solve. Equality joints sit near zero error at steady state, so
 * this only clamps transients.
 * @type {number}
 */
const MAX_JOINT_BIAS = 4;

/**
 * Baumgarte / SPOOK position-correction gain `β`. The per-substep gain is
 * `β / dt_sub`; with `β = 0.2` this matches the contact solver's position
 * stiffness. Joints are bilateral equality constraints, so this only nudges
 * residual position error toward zero (no clamp interplay to destabilise).
 * @type {number}
 */
const JOINT_BAUMGARTE = 0.2;

const scratch_inertia = new Float64Array(3);
const scratch_anchor = new Float64Array(3);
/** Frame-A world basis (rows = A's frame axes in world), from quat3_to_matrix3. */
const scratch_frame_a = new Float64Array(9);
/** Scratch quaternion for frame composition / relative rotation. */
const scratch_q = new Float64Array(4);
/** Angular position error `e` per frame axis (frame-A-local small-angle vector). */
const scratch_ang_err = new Float64Array(3);

// Per-DOF row parameters, filled per joint by the mode-aware setup and consumed
// by the mode-agnostic warm-start + iteration. Indexed 0..2 (the three frame
// axes); the linear set drives imp[0..2], the angular set drives imp[3..5].
const lin_active = new Uint8Array(3);
const lin_eff = new Float64Array(3);
const lin_bias = new Float64Array(3);
const lin_clo = new Float64Array(3);
const lin_chi = new Float64Array(3);
const lin_gamma = new Float64Array(3);
const ang_active = new Uint8Array(3);
const ang_eff = new Float64Array(3);
const ang_bias = new Float64Array(3);
const ang_clo = new Float64Array(3);
const ang_chi = new Float64Array(3);
const ang_gamma = new Float64Array(3);

/**
 * Angular effective-mass contribution of one body about a unit world axis:
 * `a · Iw⁻¹ · a`. Zero for non-dynamic / rotation-locked bodies (whose
 * {@link SolverBodyState} inverse-inertia diagonal is zero).
 * @param {Float64Array} ss solver-body-state data array
 * @param {number} base `body_index * SBS_STRIDE`
 * @returns {number}
 */
function angular_axis_effective_mass(ss, base, ax, ay, az) {
    const iix = ss[base + SBS_INV_I_X], iiy = ss[base + SBS_INV_I_Y], iiz = ss[base + SBS_INV_I_Z];
    if (iix === 0 && iiy === 0 && iiz === 0) return 0;
    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], ax, ay, az);
    return ax * scratch_inertia[0] + ay * scratch_inertia[1] + az * scratch_inertia[2];
}

/**
 * Apply a pure angular impulse `sign·λ·axis`: Δω = Iw⁻¹·(sign·λ·axis).
 * @param {Float64Array} ss solver-body-state data array
 * @param {number} base `body_index * SBS_STRIDE`
 * @param {number} ax @param {number} ay @param {number} az
 * @param {number} lambda @param {number} sign
 */
function apply_angular_impulse(ss, base, ax, ay, az, lambda, sign) {
    const iix = ss[base + SBS_INV_I_X], iiy = ss[base + SBS_INV_I_Y], iiz = ss[base + SBS_INV_I_Z];
    if (iix === 0 && iiy === 0 && iiz === 0) return;
    const L = sign * lambda;
    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], ax * L, ay * L, az * L);
    ss[base + SBS_AV_X] += scratch_inertia[0];
    ss[base + SBS_AV_Y] += scratch_inertia[1];
    ss[base + SBS_AV_Z] += scratch_inertia[2];
}

/**
 * Effective-mass denominator contribution of one body along a unit axis:
 * `invM + (r×d)·Iw⁻¹·(r×d)`. Static / kinematic bodies (invM === 0) contribute
 * nothing.
 *
 * @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} dx @param {number} dy @param {number} dz
 * @returns {number}
 */
function axis_effective_mass(ss, base, invM, rx, ry, rz, dx, dy, dz) {
    if (invM === 0) return 0;
    let k = invM;
    const iix = ss[base + SBS_INV_I_X], iiy = ss[base + SBS_INV_I_Y], iiz = ss[base + SBS_INV_I_Z];
    if (iix !== 0 || iiy !== 0 || iiz !== 0) {
        const cx = ry * dz - rz * dy;
        const cy = rz * dx - rx * dz;
        const cz = rx * dy - ry * dx;
        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], cx, cy, cz);
        k += cx * scratch_inertia[0] + cy * scratch_inertia[1] + cz * scratch_inertia[2];
    }
    return k;
}

/**
 * Apply impulse `P` at body-relative offset `r`: Δv = P·invM, Δω = Iw⁻¹·(r×P).
 * Reads / writes the body's {@link SolverBodyState} span.
 *
 * @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 / −1
 */
function apply_impulse(ss, base, invM, rx, ry, rz, Px, Py, Pz, sign) {
    if (invM === 0) return;
    const sPx = sign * Px, sPy = sign * Py, 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];
}

/**
 * Clamp the SPOOK position bias to ±{@link MAX_JOINT_BIAS}.
 * @param {number} b @returns {number}
 */
function clamp_bias(b) {
    if (b > MAX_JOINT_BIAS) return MAX_JOINT_BIAS;
    if (b < -MAX_JOINT_BIAS) return -MAX_JOINT_BIAS;
    return b;
}

/**
 * Swing-twist decomposition of the relative rotation `qD = conj(qA)⊗qB`, giving
 * the per-frame-axis angular positions used by wide-cone angular DOFs.
 *
 * Decomposes `qD = swing ⊗ twist`, with **twist** the rotation about frame
 * axis X (the bone / twist axis) and **swing** the residual tilt (a rotation
 * about an axis in the YZ plane that carries X to its new direction). Writes,
 * into `out`:
 *   - `out[0]` = signed twist angle about X, in `(−π, π]` — *exact*, so a twist
 *     limit engages at the true angle, not the `2·sin(θ/2)` small-angle proxy;
 *   - `out[1]`, `out[2]` = the swing angle distributed over Y and Z
 *     (`φ·axis`, φ the true swing angle), so independent Y/Z swing limits form
 *     an accurate (box-shaped) cone that holds at large tilt.
 * Reduces continuously to the small-angle vector near identity. Pure scratch /
 * locals — no allocation, suitable for the per-substep hot loop. (The Quaternion
 * class has an object-based `computeSwingAndTwist`; this is the inlined,
 * allocation-free form — see the bench that justifies it.)
 *
 * @param {number} dx @param {number} dy @param {number} dz @param {number} dw  qD
 * @param {Float64Array} out  length-3 destination (frame-axis positions)
 */
export function swing_twist_error(dx, dy, dz, dw, out) {
    // Shortest arc: work with the hemisphere where dw ≥ 0.
    if (dw < 0) { dx = -dx; dy = -dy; dz = -dz; dw = -dw; }

    const nxt = Math.sqrt(dx * dx + dw * dw); // |twist| (before normalising)
    let sy, sz, sw;
    if (nxt < 1e-12) {
        // dw ≈ 0 and dx ≈ 0: a ~180° pure swing — twist is undefined, take 0.
        out[0] = 0;
        // Swing is qD itself; distribute its angle over Y/Z below.
        sy = dy; sz = dz; sw = dw;
    } else {
        out[0] = 2 * Math.atan2(dx, dw); // exact signed twist about X
        const tx = dx / nxt, tw = dw / nxt; // normalised twist (tx,0,0,tw)
        // swing = qD ⊗ conj(twist); its X component is identically 0.
        sy = dy * tw - dz * tx;
        sz = dy * tx + dz * tw;
        sw = dw * tw + dx * tx;
    }

    if (sw < 0) { sy = -sy; sz = -sz; sw = -sw; }
    const syz = Math.sqrt(sy * sy + sz * sz);
    if (syz < 1e-9) {
        // Near-zero swing: small-angle vector (continuous with φ·axis as φ→0).
        out[1] = 2 * sy;
        out[2] = 2 * sz;
    } else {
        const phi = 2 * Math.atan2(syz, sw); // exact swing angle in [0, π]
        const s = phi / syz;
        out[1] = sy * s;
        out[2] = sz * s;
    }
}

/**
 * Fill the row parameters for one DOF given its mode, signed position and
 * signed velocity.
 *
 * Writes `active / eff / bias / clampLo / clampHi` at index `k` of the supplied
 * arrays and, for LIMITED, resets the warm-start impulse when the active bound
 * flipped (a stale opposite-side impulse must not warm-start).
 *
 * ## LIMITED is a speculative (β = 1) one-sided velocity constraint
 *
 * Rather than waiting for the DOF to cross a stop and then pushing back with a
 * Baumgarte bias — which injects real velocity and makes the stop *bounce* when
 * nothing else loads the joint — the limit row is always live and uses the full
 * `(position − bound)/h` as its bias. With the impulse clamped to one side this
 * removes exactly the approach velocity that would overshoot, so the DOF
 * **lands on the bound** with zero relative velocity (an inelastic stop, no
 * penetration to recover, no rebound). When the DOF is far from the bound the
 * large same-signed bias drives the clamp to zero, so the row is a no-op — it
 * self-gates on approach. At a stop held by a sustained load (gravity on a
 * slider) the per-substep load and the per-substep warm-start cancel, so the
 * DOF rests exactly on the bound, mirroring a resting contact.
 *
 * Only the *push-out* side of the bias is clamped (to {@link MAX_JOINT_BIAS}),
 * so a body teleported deep past a stop is eased out rather than yanked; the
 * gating side is left unbounded.
 *
 * The active bound is the violated one, else — within range — the one the DOF
 * is moving toward (`vel` sign). LOCKED is unchanged: a bilateral row with a
 * `β/h` Baumgarte bias and no clamp.
 *
 * ## MOTOR drives the relative velocity toward a target
 *
 * A motor is a velocity row biased to the target (`bias = −target`, so the
 * solve drives the relative velocity to the target) with the impulse clamped to
 * `±maxForce·h` — a force-limited servo. With an unbounded force budget it is a
 * perfect velocity drive; with a finite one it pulls toward the target as hard
 * as it can (a weak motor cannot overcome a heavier load). The target follows
 * the row's sign convention: linear is A−B along the frame axis, angular is
 * B−A about it.
 *
 * ## SPRING is a SPOOK soft (compliant) constraint
 *
 * A spring drives the DOF position toward zero with stiffness `k` (N/m or
 * N·m/rad) and damping `c`, as a *regularised* row rather than a hard one. The
 * implicit-Euler soft constraint (Catto, "Soft Constraints") gives, per substep
 * `h`, with `denom = c + h·k`:
 *   `γ = 1 / (h·denom)`         the compliance / regularisation (inverse mass),
 *   `bias = (k/denom)·C`        the restoring velocity, and a softened effective
 *   `effMass = 1/(K + γ)`       mass — so the row solves
 *   `λ = −effMass·(vrel + bias + γ·λ_accum)`.
 * The `γ·λ_accum` term is what makes it compliant (the constraint yields in
 * proportion to the force it carries), so a spring under load settles at a
 * deflection rather than snapping rigid. Stiffness-only (`c = 0`) oscillates
 * undamped; damping-only (`k = 0`) is a pure damper; both zero is inert. The
 * row is bilateral (no clamp). This is the suspension / bungee / soft-return
 * element, and the soft basis for cone-twist later. The angular position uses
 * the same small-angle measure as LIMITED (good for modest ranges).
 *
 * @param {Joint} joint   the joint (per-DOF config + warm-start impulse read here)
 * @param {number} dofIndex  DOF 0..5 (selects mode / limits / motor / imp slot)
 * @param {number} k      axis index 0..2 (output-array slot for this DOF group)
 * @param {number} keff   raw effective-mass denominator `K` (0 ⇒ no row); the
 *   row's effective mass is `1/K`, or `1/(K+γ)` softened for a spring.
 * @param {number} pos    signed DOF position (`C·axis` linear / `e·axis` angular)
 * @param {number} vel    signed DOF velocity along the same axis (selects the
 *   approached bound for an in-range LIMITED DOF)
 * @param {number} spook_locked  Baumgarte gain `β / dt_sub` (LOCKED)
 * @param {number} spook_spec    speculative gain `1 / dt_sub` (LIMITED)
 * @param {number} dt_sub  substep size (MOTOR impulse cap = `maxForce·dt_sub`;
 *   SPRING compliance derives from it)
 * @param {Uint8Array} active @param {Float64Array} effOut
 * @param {Float64Array} biasOut @param {Float64Array} cloOut @param {Float64Array} chiOut
 * @param {Float64Array} gammaOut  per-row regularisation (0 for non-spring rows)
 */
function fill_row(joint, dofIndex, k, keff, pos, vel, spook_locked, spook_spec, dt_sub,
                  active, effOut, biasOut, cloOut, chiOut, gammaOut) {
    const mode = joint.dofMode[dofIndex];
    const imp = joint.dofImpulse;

    if (mode === DofMode.LOCKED) {
        if (keff === 0) { active[k] = 0; return; }
        effOut[k] = 1 / keff;
        biasOut[k] = clamp_bias(spook_locked * pos);
        cloOut[k] = -Infinity;
        chiOut[k] = Infinity;
        gammaOut[k] = 0;
        active[k] = 1;
        return;
    }
    if (mode === DofMode.LIMITED) {
        if (keff === 0) { imp[dofIndex] = 0; active[k] = 0; return; }
        const lower = joint.dofLowerLimit[dofIndex];
        const upper = joint.dofUpperLimit[dofIndex];
        // Select the bound: the violated one, else the one being approached.
        let bound, lower_side;
        if (pos <= lower) { bound = lower; lower_side = true; }
        else if (pos >= upper) { bound = upper; lower_side = false; }
        else { lower_side = vel <= 0; bound = lower_side ? lower : upper; }

        let b = spook_spec * (pos - bound);
        if (lower_side) {
            // Lower stop: impulse pushes the DOF up (+), clamp [0, +∞). The
            // push-out side of the bias is the negative one — bound it.
            cloOut[k] = 0; chiOut[k] = Infinity;
            if (imp[dofIndex] < 0) imp[dofIndex] = 0; // drop stale upper-side impulse
            if (b < -MAX_JOINT_BIAS) b = -MAX_JOINT_BIAS;
        } else {
            // Upper stop: impulse pushes the DOF down (−), clamp (−∞, 0]. The
            // push-out side of the bias is the positive one — bound it.
            cloOut[k] = -Infinity; chiOut[k] = 0;
            if (imp[dofIndex] > 0) imp[dofIndex] = 0; // drop stale lower-side impulse
            if (b > MAX_JOINT_BIAS) b = MAX_JOINT_BIAS;
        }
        effOut[k] = 1 / keff;
        biasOut[k] = b;
        gammaOut[k] = 0;
        active[k] = 1;
        return;
    }
    if (mode === DofMode.MOTOR) {
        if (keff === 0) { imp[dofIndex] = 0; active[k] = 0; return; }
        const maxImpulse = joint.dofMotorMaxForce[dofIndex] * dt_sub;
        if (maxImpulse <= 0) { imp[dofIndex] = 0; active[k] = 0; return; }
        effOut[k] = 1 / keff;
        // Drive the relative velocity to the target: λ = −eff·(vrel − target).
        biasOut[k] = -joint.dofMotorTargetVelocity[dofIndex];
        cloOut[k] = -maxImpulse;
        chiOut[k] = maxImpulse;
        gammaOut[k] = 0;
        active[k] = 1;
        return;
    }
    if (mode === DofMode.SPRING) {
        if (keff === 0) { imp[dofIndex] = 0; active[k] = 0; return; }
        const ks = joint.dofStiffness[dofIndex];
        const cs = joint.dofDamping[dofIndex];
        const denom = cs + dt_sub * ks;
        if (denom <= 0) { imp[dofIndex] = 0; active[k] = 0; return; } // inert (no k, no c)
        const gamma = 1 / (dt_sub * denom);
        effOut[k] = 1 / (keff + gamma);
        biasOut[k] = (ks / denom) * pos; // restoring velocity toward pos = 0
        cloOut[k] = -Infinity;
        chiOut[k] = Infinity;
        gammaOut[k] = gamma;
        active[k] = 1;
        return;
    }
    // FREE: no row this substep.
    imp[dofIndex] = 0;
    active[k] = 0;
}

/**
 * Solve every joint for one substep: recompute geometry at the current poses,
 * derive each DOF's row parameters from its mode, replay the per-substep
 * warm-start, and run `iters` velocity iterations.
 *
 * Called once per substep from `PhysicsSystem.fixedUpdate`, after the contact
 * solve so the two share the substep / warm-start cadence.
 *
 * @param {Joint[]} joints  live joints (sparse array; holes skipped)
 * @param {PhysicsSystem} system  reads `__bodies` / `__transforms` / index map
 * @param {number} dt_sub  substep size in seconds (the SPOOK gain is derived
 *   from it, matching the contact solver's per-substep position stiffness)
 * @param {number} iters  velocity iterations
 */
export function solve_joints(joints, system, dt_sub, iters) {
    const n = joints.length;
    if (n === 0 || dt_sub <= 0) return;

    const spook_locked = JOINT_BAUMGARTE / dt_sub;
    const spook_spec = 1 / dt_sub;

    const storage = system.storage;
    const ss = system.__solver_state.data;

    for (let ji = 0; ji < n; ji++) {
        const joint = joints[ji];
        if (joint === undefined || joint === null) continue;

        // Generation-checked validity: if A's body was unlinked (and its slot
        // possibly reused by a different body), the stored packed id no longer
        // validates — skip the stale joint rather than attach to the wrong body.
        if (!storage.is_valid(joint._bodyIdA)) continue;
        const idxA = body_id_index(joint._bodyIdA);
        const baseA = idxA * SBS_STRIDE;
        const rbA = system.__bodies[idxA];
        if (rbA === undefined) continue;
        const trA = system.__transforms[idxA];

        const to_world = joint._bodyIdB === JOINT_WORLD;
        let rbB = null, trB = null, idxB = -1, baseB = -1;
        if (!to_world) {
            if (!storage.is_valid(joint._bodyIdB)) continue;
            idxB = body_id_index(joint._bodyIdB);
            baseB = idxB * SBS_STRIDE;
            rbB = system.__bodies[idxB];
            if (rbB === undefined) continue;
            trB = system.__transforms[idxB];
        }

        // Skip while a participating dynamic body sleeps — its velocity must
        // stay zero until woken. (Island-aware joint sleep is a follow-up; for
        // now jointed bodies that should stay coupled use DisableSleep.)
        if (rbA.sleepState === SleepState.Sleeping) continue;
        if (!to_world && rbB.sleepState === SleepState.Sleeping) continue;

        // Both bodies static/kinematic → nothing to solve.
        const invMA = ss[baseA + SBS_INV_MASS];
        const invMB = to_world ? 0 : ss[baseB + SBS_INV_MASS];
        if (invMA === 0 && invMB === 0) continue;

        const mode = joint.dofMode;
        const linConstrained = mode[0] !== DofMode.FREE || mode[1] !== DofMode.FREE || mode[2] !== DofMode.FREE;
        const angConstrained = mode[3] !== DofMode.FREE || mode[4] !== DofMode.FREE || mode[5] !== DofMode.FREE;
        if (!linConstrained && !angConstrained) continue;

        // World anchor A and its lever arm rA = R_A · localAnchorA.
        const la = joint.localAnchorA;
        v3_quat3_apply(scratch_anchor, 0, la.x, la.y, la.z,
            trA.rotation[0], trA.rotation[1], trA.rotation[2], trA.rotation[3]);
        const rAx = scratch_anchor[0], rAy = scratch_anchor[1], rAz = scratch_anchor[2];
        const anchorAx = trA.position.x + rAx;
        const anchorAy = trA.position.y + rAy;
        const anchorAz = trA.position.z + rAz;

        // World anchor B + lever arm rB. For a world anchor, localAnchorB is a
        // fixed world point and there is no body B lever.
        let rBx = 0, rBy = 0, rBz = 0;
        let anchorBx, anchorBy, anchorBz;
        const lb = joint.localAnchorB;
        if (to_world) {
            anchorBx = lb.x; anchorBy = lb.y; anchorBz = lb.z;
        } else {
            v3_quat3_apply(scratch_anchor, 0, lb.x, lb.y, lb.z,
                trB.rotation[0], trB.rotation[1], trB.rotation[2], trB.rotation[3]);
            rBx = scratch_anchor[0]; rBy = scratch_anchor[1]; rBz = scratch_anchor[2];
            anchorBx = trB.position.x + rBx;
            anchorBy = trB.position.y + rBy;
            anchorBz = trB.position.z + rBz;
        }

        // Position error C = anchorA − anchorB (its projection on a frame axis
        // is the signed DOF offset; locked axes drive it to zero, limited axes
        // bound it).
        const Cx = anchorAx - anchorBx;
        const Cy = anchorAy - anchorBy;
        const Cz = anchorAz - anchorBz;

        const imp = joint.dofImpulse;

        // === Frame A world axes — shared by the linear AND angular rows ===
        // Every DOF is expressed in the joint's frame on body A. Frame A =
        // body-A rotation ⊗ localBasisA; the rows of its matrix are the frame
        // axes in world (see quat3_to_matrix3).
        const lba = joint.localBasisA;
        quat3_multiply(scratch_q, 0, trA.rotation[0], trA.rotation[1], trA.rotation[2], trA.rotation[3], lba[0], lba[1], lba[2], lba[3]);
        const qAx = scratch_q[0], qAy = scratch_q[1], qAz = scratch_q[2], qAw = scratch_q[3];
        quat3_to_matrix3(scratch_frame_a, 0, qAx, qAy, qAz, qAw);
        const fa = scratch_frame_a;

        // --- Linear DOF row setup (frame axes). Position along axis k is
        //     `C · axis_k`; convention is A−B (impulse +to A / −to B). ---
        if (linConstrained) {
            for (let k = 0; k < 3; k++) {
                lin_active[k] = 0;
                const m = mode[k];
                if (m === DofMode.FREE) { imp[k] = 0; continue; }
                const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
                const keff = axis_effective_mass(ss, baseA, invMA, rAx, rAy, rAz, ax, ay, az)
                    + (to_world ? 0 : axis_effective_mass(ss, baseB, invMB, rBx, rBy, rBz, ax, ay, az));
                const pos = Cx * ax + Cy * ay + Cz * az;
                // Relative anchor velocity along the axis (A−B), for LIMITED
                // bound selection. Cheap; only the projection is used.
                let vel = 0;
                if (m === DofMode.LIMITED) {
                    const rvx = (ss[baseA + SBS_LV_X] + ss[baseA + SBS_AV_Y] * rAz - ss[baseA + SBS_AV_Z] * rAy) - (to_world ? 0 : (ss[baseB + SBS_LV_X] + ss[baseB + SBS_AV_Y] * rBz - ss[baseB + SBS_AV_Z] * rBy));
                    const rvy = (ss[baseA + SBS_LV_Y] + ss[baseA + SBS_AV_Z] * rAx - ss[baseA + SBS_AV_X] * rAz) - (to_world ? 0 : (ss[baseB + SBS_LV_Y] + ss[baseB + SBS_AV_Z] * rBx - ss[baseB + SBS_AV_X] * rBz));
                    const rvz = (ss[baseA + SBS_LV_Z] + ss[baseA + SBS_AV_X] * rAy - ss[baseA + SBS_AV_Y] * rAx) - (to_world ? 0 : (ss[baseB + SBS_LV_Z] + ss[baseB + SBS_AV_X] * rBy - ss[baseB + SBS_AV_Y] * rBx));
                    vel = rvx * ax + rvy * ay + rvz * az;
                }
                fill_row(joint, k, k, keff, pos, vel, spook_locked, spook_spec, dt_sub,
                    lin_active, lin_eff, lin_bias, lin_clo, lin_chi, lin_gamma);
            }
        } else {
            lin_active[0] = lin_active[1] = lin_active[2] = 0;
        }

        // --- Angular DOF row setup (frame axes). Position along axis k is the
        //     k-th component of the frame-local small-angle error vector `e`;
        //     convention is B−A (impulse +to B / −to A). ---
        if (angConstrained) {
            // Frame B world rotation (identity for a world anchor → locks A's
            // frame to world axes).
            let qBx = 0, qBy = 0, qBz = 0, qBw = 1;
            if (!to_world) {
                const lbb = joint.localBasisB;
                quat3_multiply(scratch_q, 0, trB.rotation[0], trB.rotation[1], trB.rotation[2], trB.rotation[3], lbb[0], lbb[1], lbb[2], lbb[3]);
                qBx = scratch_q[0]; qBy = scratch_q[1]; qBz = scratch_q[2]; qBw = scratch_q[3];
            }
            // qD = conj(qA) ⊗ qB → the per-axis angular position in frame-A
            // coords. Two measures: the cheap small-angle rotation vector
            // (default — exact at the origin, fine for welds / hinges / modest
            // limits) or, when the joint opts into `swingTwist`, the swing-twist
            // decomposition (X = twist, Y/Z = swing) with *exact* angles, for
            // wide cone-twist range-of-motion (ragdoll shoulders) where the
            // small-angle measure under-reports.
            quat3_multiply(scratch_q, 0, -qAx, -qAy, -qAz, qAw, qBx, qBy, qBz, qBw);
            if (joint.swingTwist) {
                swing_twist_error(scratch_q[0], scratch_q[1], scratch_q[2], scratch_q[3], scratch_ang_err);
            } else {
                const es = scratch_q[3] < 0 ? -2 : 2;
                scratch_ang_err[0] = es * scratch_q[0];
                scratch_ang_err[1] = es * scratch_q[1];
                scratch_ang_err[2] = es * scratch_q[2];
            }

            for (let k = 0; k < 3; k++) {
                ang_active[k] = 0;
                const m = mode[3 + k];
                if (m === DofMode.FREE) { imp[3 + k] = 0; continue; }
                const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
                const keff = angular_axis_effective_mass(ss, baseA, ax, ay, az)
                    + (to_world ? 0 : angular_axis_effective_mass(ss, baseB, ax, ay, az));
                const pos = scratch_ang_err[k];
                // Relative angular velocity about the axis (B−A), for LIMITED
                // bound selection.
                let vel = 0;
                if (m === DofMode.LIMITED) {
                    vel = -(ss[baseA + SBS_AV_X] * ax + ss[baseA + SBS_AV_Y] * ay + ss[baseA + SBS_AV_Z] * az);
                    if (!to_world) vel += ss[baseB + SBS_AV_X] * ax + ss[baseB + SBS_AV_Y] * ay + ss[baseB + SBS_AV_Z] * az;
                }
                fill_row(joint, 3 + k, k, keff, pos, vel, spook_locked, spook_spec, dt_sub,
                    ang_active, ang_eff, ang_bias, ang_clo, ang_chi, ang_gamma);
            }
        } else {
            ang_active[0] = ang_active[1] = ang_active[2] = 0;
        }

        if (!lin_active[0] && !lin_active[1] && !lin_active[2]
            && !ang_active[0] && !ang_active[1] && !ang_active[2]) continue;

        // --- Per-substep warm-start (frame axes): linear impulse `Σ imp[k]·aₖ`
        //     applied at the anchors, angular impulse `Σ imp[3+k]·aₖ` about the
        //     frame axes (+to B / −to A). ---
        {
            let Px = 0, Py = 0, Pz = 0;
            for (let k = 0; k < 3; k++) {
                if (!lin_active[k]) continue;
                const w = imp[k];
                Px += w * fa[3 * k]; Py += w * fa[3 * k + 1]; Pz += w * fa[3 * k + 2];
            }
            if (Px !== 0 || Py !== 0 || Pz !== 0) {
                apply_impulse(ss, baseA, invMA, rAx, rAy, rAz, Px, Py, Pz, +1);
                if (!to_world) apply_impulse(ss, baseB, invMB, rBx, rBy, rBz, Px, Py, Pz, -1);
            }
            let Lx = 0, Ly = 0, Lz = 0;
            for (let k = 0; k < 3; k++) {
                if (!ang_active[k]) continue;
                const w = imp[3 + k];
                Lx += w * fa[3 * k]; Ly += w * fa[3 * k + 1]; Lz += w * fa[3 * k + 2];
            }
            if (Lx !== 0 || Ly !== 0 || Lz !== 0) {
                if (!to_world) apply_angular_impulse(ss, baseB, Lx, Ly, Lz, 1, +1);
                apply_angular_impulse(ss, baseA, Lx, Ly, Lz, 1, -1);
            }
        }

        for (let it = 0; it < iters; it++) {
            // Linear rows (frame axes). The relative anchor velocity is
            // recomputed before each row so successive rows see the prior
            // impulses (Gauss-Seidel coupling).
            for (let k = 0; k < 3; k++) {
                if (!lin_active[k]) continue;
                const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
                const rvx = (ss[baseA + SBS_LV_X] + ss[baseA + SBS_AV_Y] * rAz - ss[baseA + SBS_AV_Z] * rAy) - (to_world ? 0 : (ss[baseB + SBS_LV_X] + ss[baseB + SBS_AV_Y] * rBz - ss[baseB + SBS_AV_Z] * rBy));
                const rvy = (ss[baseA + SBS_LV_Y] + ss[baseA + SBS_AV_Z] * rAx - ss[baseA + SBS_AV_X] * rAz) - (to_world ? 0 : (ss[baseB + SBS_LV_Y] + ss[baseB + SBS_AV_Z] * rBx - ss[baseB + SBS_AV_X] * rBz));
                const rvz = (ss[baseA + SBS_LV_Z] + ss[baseA + SBS_AV_X] * rAy - ss[baseA + SBS_AV_Y] * rAx) - (to_world ? 0 : (ss[baseB + SBS_LV_Z] + ss[baseB + SBS_AV_X] * rBy - ss[baseB + SBS_AV_Y] * rBx));
                const vrel = rvx * ax + rvy * ay + rvz * az;
                const old = imp[k];
                // `+ γ·old` regularises a spring row (γ = 0 otherwise → exact).
                let nv = old - lin_eff[k] * (vrel + lin_bias[k] + lin_gamma[k] * old);
                if (nv < lin_clo[k]) nv = lin_clo[k]; else if (nv > lin_chi[k]) nv = lin_chi[k];
                imp[k] = nv;
                const d = nv - old;
                if (d !== 0) {
                    apply_impulse(ss, baseA, invMA, rAx, rAy, rAz, d * ax, d * ay, d * az, +1);
                    if (!to_world) apply_impulse(ss, baseB, invMB, rBx, rBy, rBz, d * ax, d * ay, d * az, -1);
                }
            }

            // Angular rows (frame axes): drive ωB−ωA about each axis toward the
            // bias target, clamped per the row's bound.
            for (let k = 0; k < 3; k++) {
                if (!ang_active[k]) continue;
                const ax = fa[3 * k], ay = fa[3 * k + 1], az = fa[3 * k + 2];
                let wrel = -(ss[baseA + SBS_AV_X] * ax + ss[baseA + SBS_AV_Y] * ay + ss[baseA + SBS_AV_Z] * az);
                if (!to_world) wrel += ss[baseB + SBS_AV_X] * ax + ss[baseB + SBS_AV_Y] * ay + ss[baseB + SBS_AV_Z] * az;
                const old = imp[3 + k];
                let nv = old - ang_eff[k] * (wrel + ang_bias[k] + ang_gamma[k] * old);
                if (nv < ang_clo[k]) nv = ang_clo[k]; else if (nv > ang_chi[k]) nv = ang_chi[k];
                imp[3 + k] = nv;
                const d = nv - old;
                if (d !== 0) {
                    if (!to_world) apply_angular_impulse(ss, baseB, ax, ay, az, d, +1);
                    apply_angular_impulse(ss, baseA, ax, ay, az, d, -1);
                }
            }
        }
    }
}
