import { assert } from "../../../core/assert.js";
import { BinaryBuffer } from "../../../core/binary/BinaryBuffer.js";
import { BVH } from "../../../core/bvh2/bvh3/BVH.js";
import { returnTrue } from "../../../core/function/returnTrue.js";
import { aabb3_transform_oriented } from "../../../core/geom/3d/aabb/aabb3_transform_oriented.js";
import Vector3 from "../../../core/geom/Vector3.js";
import { ResourceAccessKind } from "../../../core/model/ResourceAccessKind.js";
import { ResourceAccessSpecification } from "../../../core/model/ResourceAccessSpecification.js";
import { System } from "../../ecs/System.js";
import { Transform } from "../../ecs/transform/Transform.js";
import { Interpolated } from "../../interpolation/Interpolated.js";
import { body_id_index, BodyStorage } from "../body/BodyStorage.js";
import { SBS_STRIDE, SolverBodyState } from "../body/SolverBodyState.js";
import { compute_fat_world_aabb } from "../broadphase/compute_fat_world_aabb.js";
import { generate_pairs } from "../broadphase/generate_pairs.js";
import { PairList } from "../broadphase/PairList.js";
import { ccd_resolve } from "../ccd/linear_sweep.js";
import { solve_joints } from "../constraint/solve_constraints.js";
import { ManifoldStore } from "../contact/ManifoldStore.js";
import { ContactEventBuffer, ContactEventKind } from "../events/ContactEventBuffer.js";
import { diff_manifolds } from "../events/diff_manifolds.js";
import { world_inverse_inertia_apply } from "../inertia/world_inverse_inertia.js";
import { integrate_position } from "../integration/integrate_position.js";
import { integrate_velocity_forces, integrate_velocity_gravity } from "../integration/integrate_velocity.js";
import { IslandBuilder } from "../island/IslandBuilder.js";
import { narrowphase_step } from "../narrowphase/narrowphase_step.js";
import { overlap_shape as overlap_shape_query } from "../queries/overlap_shape.js";
import { raycast as raycast_query } from "../queries/raycast.js";
import { shape_cast as shape_cast_query } from "../queries/shape_cast.js";
import {
    apply_restitution,
    prepare_contacts,
    redetect_concave_contacts,
    refresh_contacts,
    solve_position,
    solve_velocity,
    warm_start_contacts,
} from "../solver/solve_contacts.js";
import { BodyKind } from "./BodyKind.js";
import { Collider, COLLIDER_UNBOUND } from "./Collider.js";
import { JOINT_UNALLOCATED, JOINT_WORLD } from "./Joint.js";
import { PhysicsEvents } from "./PhysicsEvents.js";
import { RIGID_BODY_UNALLOCATED, RigidBody } from "./RigidBody.js";
import { RigidBodyFlags } from "./RigidBodyFlags.js";
import { SleepState } from "./SleepState.js";

/**
 * Scratch for {@link applyImpulseAt}'s angular delta calculation.
 * @type {Float64Array}
 */
const scratch_angular_delta = new Float64Array(3);

/**
 * Reusable scratch buffer for world-AABB construction so the link path is
 * allocation-free in steady state.
 * @type {Float64Array}
 */
const scratch_world_aabb = new Float64Array(6);

/**
 * Reusable scratch buffer for the local AABB returned by
 * {@link AbstractShape3D#compute_bounding_box}.
 * @type {Float64Array}
 */
const scratch_local_aabb = new Float64Array(6);

/**
 * Rigid-body physics system.
 *
 * v1 scope: pool + active list + two BVHs (static / dynamic), Transform sync
 * contract, and the user-facing API surface (gravity, force / impulse, layer
 * filtering, wake / sleep, contact filter). The per-step simulation pipeline
 * (broadphase pair generation, narrowphase, solver, islands, sleep test,
 * contact-event emission) is built on top of this skeleton in subsequent
 * iterations.
 *
 * Dependency tuple is `(RigidBody, Collider, Transform)` — every body has
 * exactly one collider on the same entity. Compound bodies via child collider
 * entities are an extension point handled by a follow-up sub-observer.
 *
 * @author Alex Goldring
 * @copyright Company Named Limited (c) 2026
 */
export class PhysicsSystem extends System {

    constructor() {
        super();

        this.dependencies = [RigidBody, Transform];

        this.components_used = [
            ResourceAccessSpecification.from(RigidBody, ResourceAccessKind.Read | ResourceAccessKind.Write),
            ResourceAccessSpecification.from(Collider, ResourceAccessKind.Read),
            ResourceAccessSpecification.from(Transform, ResourceAccessKind.Read | ResourceAccessKind.Write),
        ];

        /**
         * @type {BodyStorage}
         * @readonly
         */
        this.storage = new BodyStorage();

        /**
         * @type {BVH}
         */
        this.staticBvh = new BVH();

        /**
         * @type {BVH}
         */
        this.dynamicBvh = new BVH();

        /**
         * Persistent contact-manifold cache. One slot per active pair.
         * @type {ManifoldStore}
         * @readonly
         */
        this.manifolds = new ManifoldStore();

        /**
         * Per-frame list of broadphase-overlapping pairs (canonical
         * `(min, max)`). Cleared at the top of each step.
         * @type {PairList}
         */
        this.pairs = new PairList();

        /**
         * Per-frame contact-event buffer. Populated by the manifold diff
         * pass at end-of-step and consumed by the dispatch pass.
         * @type {ContactEventBuffer}
         */
        this.contactEvents = new ContactEventBuffer();

        /**
         * Per-frame island partitioning of the awake-body + contact graph.
         * Rebuilt after narrowphase, consumed by the solver (and, in the
         * follow-up slice, by the per-island atomic sleep test). Bodies
         * static and kinematic act as constraint anchors and do not enlarge
         * islands.
         * @type {IslandBuilder}
         */
        this.islands = new IslandBuilder();

        /**
         * Data-oriented mirror of the per-body solver hot state (velocity,
         * inverse mass / inertia, orientation), packed into one ArrayBuffer
         * and indexed by body slot. Gathered from the `RigidBody` /
         * `Transform` components once per step (after islands are built),
         * mutated in place by the TGS substep loop, and the persistent
         * velocity scattered back at the end. Keeps the solver's hottest inner
         * loop free of component-object dereferences.
         * @type {SolverBodyState}
         */
        this.__solver_state = new SolverBodyState();

        /**
         * Velocity-squared threshold below which a body is eligible to start
         * accumulating sleep time. Combined linear + angular kinetic-ish
         * metric: `vx²+vy²+vz² + ωx²+ωy²+ωz²`. Default 0.01 corresponds to
         * ~0.1 m/s linear or ~0.1 rad/s angular.
         * @type {number}
         */
        this.sleepVelocitySqrThreshold = 0.01;

        /**
         * Seconds of below-threshold motion before a body is moved to the
         * sleeping set. Box2D default is 0.5 s.
         * @type {number}
         */
        this.sleepTimeThreshold = 0.5;

        /**
         * Number of TGS substeps per `fixedUpdate`. Each substep re-runs the
         * velocity + position solve at `dt / substeps` against contacts whose
         * penetration is re-derived analytically from the bodies' moved poses
         * — narrowphase still runs once per outer step. Higher counts buy
         * stack stability, high-mass-ratio robustness, and smoother
         * trajectories at a near-linear solver cost (sleeping islands are
         * unaffected — they never enter the loop).
         *
         * `1` reproduces the non-substepped (single-step) solve.
         * @type {number}
         */
        this.substeps = 4;

        /**
         * Velocity iterations per substep. Lower than a single-step solver
         * would need, because the substep loop revisits the contact set
         * `substeps` times.
         * @type {number}
         */
        this.velocityIterations = 4;

        /**
         * Position (split-impulse) iterations per substep.
         * @type {number}
         */
        this.positionIterations = 1;

        /**
         * Reusable contact-event payload. Listeners must copy any fields they
         * intend to retain past their own scope. Reset before each dispatch.
         * @private
         */
        this.__contact_payload = {
            entityA: -1,
            entityB: -1,
            kind: 0,
            depth: 0,
            normal: new Vector3(),
            point: new Vector3(),
        };

        /**
         * World gravity, m/s². Applied each step scaled by per-body gravityScale.
         * @readonly
         * @type {Vector3}
         */
        this.gravity = new Vector3(0, -9.81, 0);

        /**
         * Optional global contact filter. Called for each surviving broadphase
         * pair; returning `false` discards the pair.
         * @type {((entityA: number, entityB: number, colliderA: Collider, colliderB: Collider) => boolean) | null}
         * @private
         */
        this.__contact_filter = null;

        /**
         * Per-body component side-tables indexed by body index (NOT packed id).
         * Sparse arrays — populated at link, cleared at unlink. The hot loop
         * iterates `storage.awake_at(i)` and dereferences these.
         * @type {RigidBody[]}
         */
        this.__bodies = [];

        /**
         * @type {Transform[]}
         * @readonly
         */
        this.__transforms = [];

        /**
         * Per-body list of attached colliders. Each entry stores the
         * Collider component, its world Transform, the entity that owns
         * it (the body entity for same-entity attachments, a child entity
         * for compound bodies), and the BVH leaf id assigned at attach
         * time. A body may have zero or more attached colliders.
         *
         * @type {Array<Array<{collider: Collider, transform: Transform, entity: number, bvhNode: number}>>}
         */
        this.__body_collider_lists = [];

        /**
         * Live {@link Joint} (6-DOF constraint) instances, in a sparse array
         * indexed by joint id. Solved alongside contacts inside the TGS
         * substep loop. Holes (unlinked joints) are `undefined`.
         * @type {Joint[]}
         */
        this.__joints = [];

        /**
         * Lowest free index in {@link __joints} for slot reuse on link.
         * @private
         * @type {number}
         */
        this.__joint_free = [];

        /**
         * Velocity iterations per substep for the joint solver. Joints (and
         * especially joint chains) want a few more iterations than contacts to
         * propagate impulses along the chain; cheap because joints are far
         * fewer than contacts.
         * @type {number}
         */
        this.jointIterations = 8;

        /**
         * Per-body pseudo-velocity for the Catto split-impulse position
         * pass (TGS Phase 1). Flat layout, 6 doubles per body slot index:
         * `[lin.x, lin.y, lin.z, ang.x, ang.y, ang.z]`. Sized to
         * `storage.high_water_mark * 6` at the top of each fixedUpdate
         * and zeroed in place so unwritten slots contribute nothing to
         * `integrate_position`.
         *
         * The solver writes during its position pass; `integrate_position`
         * reads and folds into the pose update on the same tick, then
         * the next tick's zero-pass wipes the state. It NEVER lands in
         * `linearVelocity` / `angularVelocity` — that's the point of
         * split impulse: depth correction does not contaminate persistent
         * velocity (so a `restitution = 0` impact stops cleanly).
         *
         * @type {Float64Array}
         */
        this.__pseudo_velocity = new Float64Array(0);

        /**
         * Master switch for the continuous-collision pass. When false the
         * {@link RigidBodyFlags.CCD} flag is ignored and no swept queries run.
         * @type {boolean}
         */
        this.ccdEnabled = true;

        /**
         * Start-of-step world positions for CCD-flagged bodies — 3 doubles per
         * body slot index (`[x, y, z]`). Captured in Stage 1 before the substep
         * loop integrates poses; the CCD pass ({@link ccd_resolve}) sweeps from
         * here to the final pose. Grows to `storage.high_water_mark * 3`; only
         * CCD-flagged slots are written each step.
         * @type {Float64Array}
         */
        this.__ccd_start_pos = new Float64Array(0);


        /**
         * Optional shared interpolation log to produce per-step pose snapshots
         * into (the {@link InterpolationSystem}'s log). When set — and only then —
         * each fixedUpdate restores every awake {@link Interpolated} body's live
         * components from the previous tick (undoing render-time interpolation so
         * the sim reads authoritative state), then records the post-step state
         * under the current `entityManager.fixedStepTick`. Null on a headless /
         * non-rendering world, where the producer work is skipped entirely.
         * @type {InterpolationLog|null}
         */
        this.interpolationLog = null;

        /**
         * Reusable decode buffer for restoring interpolated snapshots.
         * @private
         * @type {BinaryBuffer}
         */
        this.__interp_scratch = new BinaryBuffer();
        this.__interp_scratch.fromArrayBuffer(new ArrayBuffer(256));
    }

    /**
     * Symmetric layer/mask check + optional user callback. Called per
     * candidate pair during broadphase. Returns `true` to accept the pair.
     *
     * Layer/mask rule: pair (A, B) collides iff
     *   `(A.layer & B.mask) !== 0 && (B.layer & A.mask) !== 0`.
     * The user's contact-filter callback is consulted only for pairs that
     * pass the bitmask test (cheap gate first).
     *
     * @private
     * @param {number} idA packed body id
     * @param {number} idB packed body id
     * @returns {boolean}
     */
    __pair_filter(idA, idB) {
        const idxA = body_id_index(idA);
        const idxB = body_id_index(idB);

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

        if (rbA === undefined || rbB === undefined) {
            return false;
        }

        // Layer/mask gate (symmetric).
        if (((rbA.layer & rbB.mask) | 0) === 0) {
            return false;
        }
        if (((rbB.layer & rbA.mask) | 0) === 0) {
            return false;
        }

        // User callback gate, if installed.
        const fn = this.__contact_filter;
        if (fn !== null) {
            const entA = this.storage.entity_at(idxA);
            const entB = this.storage.entity_at(idxB);
            const colA = this.__primary_collider(idxA);
            const colB = this.__primary_collider(idxB);
            if (!fn(entA, entB, colA, colB)) return false;
        }
        return true;
    }

    /**
     * First attached collider of a body, or `null` if none. Used for
     * body-level checks that aren't yet per-collider: the sensor flag, the
     * concave-shape dispatch flag, and the contact-filter callback's
     * `colliderA` / `colliderB` arguments. Contact **materials** (friction /
     * restitution) are NOT read here — the narrowphase tracks the specific
     * source collider on each side of every contact and stamps the combined
     * coefficients into the manifold per contact, so mixed-material compound
     * bodies are accurate. (Per-contact source colliders for the filter
     * callback / sensor flag remain a follow-up.)
     *
     * @private
     * @param {number} body_idx
     * @returns {Collider|null}
     */
    __primary_collider(body_idx) {
        const list = this.__body_collider_lists[body_idx];
        return (list !== undefined && list.length > 0) ? list[0].collider : null;
    }

    /**
     * Resize {@link __pseudo_velocity} to cover every live body slot and
     * zero its contents. Called at the top of each fixedUpdate so the
     * split-impulse position pass starts from a clean state — bodies the
     * solver doesn't touch contribute zero pseudo-velocity to
     * {@link integrate_position}.
     *
     * Doubles on growth like the other physics scratches; never shrinks.
     *
     * @private
     */
    __reset_pseudo_velocity() {
        const required = this.storage.high_water_mark * 6;
        if (this.__pseudo_velocity.length < required) {
            this.__pseudo_velocity = new Float64Array(required * 2);
        } else if (required > 0) {
            this.__pseudo_velocity.fill(0, 0, required);
        }
    }

    /**
     * Replace the world gravity vector. Effective on the next step.
     * @param {Vector3|{x:number,y:number,z:number}} v
     */
    setGravity(v) {
        this.gravity.set(v.x, v.y, v.z);
    }

    /**
     * Install (or remove with `null`) the contact filter callback.
     * @param {((entityA:number, entityB:number, colliderA:Collider, colliderB:Collider) => boolean) | null} fn
     */
    setContactFilter(fn) {
        this.__contact_filter = fn;
    }

    /**
     * @returns {((entityA:number, entityB:number, colliderA:Collider, colliderB:Collider) => boolean) | null}
     */
    getContactFilter() {
        return this.__contact_filter;
    }

    /**
     * @private
     * @param {RigidBody} rb
     * @param {Collider} collider
     * @param {Transform} transform
     * @returns {number} BVH node id
     */
    __insert_into_broadphase(rb, collider, transform) {
        const shape = collider.shape;

        assert.notNull(shape, 'Collider.shape must be set before attaching');

        shape.compute_bounding_box(scratch_local_aabb);

        const p = transform.position;
        const q = transform.rotation;

        aabb3_transform_oriented(
            scratch_world_aabb, 0,
            scratch_local_aabb[0], scratch_local_aabb[1], scratch_local_aabb[2],
            scratch_local_aabb[3], scratch_local_aabb[4], scratch_local_aabb[5],
            p.x, p.y, p.z,
            q.x, q.y, q.z, q.w
        );

        const bvh = rb.kind === BodyKind.Static ? this.staticBvh : this.dynamicBvh;
        const node = bvh.allocate_node();

        bvh.node_set_aabb_primitive(
            node,
            scratch_world_aabb[0], scratch_world_aabb[1], scratch_world_aabb[2],
            scratch_world_aabb[3], scratch_world_aabb[4], scratch_world_aabb[5]
        );
        bvh.node_set_user_data(node, rb._bodyId);
        bvh.insert_leaf(node);

        return node;
    }

    /**
     * @private
     * @param {RigidBody} rb
     * @param {number} node
     */
    __remove_from_broadphase(rb, node) {
        const bvh = rb.kind === BodyKind.Static ? this.staticBvh : this.dynamicBvh;
        bvh.remove_leaf(node);
        bvh.release_node(node);
    }

    /**
     * Lifecycle entry: invoked when an entity gains the (RigidBody, Transform)
     * tuple. Allocates the body's slot in storage and seeds an empty collider
     * list. Colliders are attached separately via {@link attach_collider} (the
     * paired {@link ColliderObserverSystem} drives this from the dataset; in
     * tests, call manually).
     *
     * @param {RigidBody} rigidBody
     * @param {Transform} transform
     * @param {number} entity
     */
    link(rigidBody, transform, entity) {
        const packed = this.storage.allocate(entity);
        rigidBody._bodyId = packed;
        rigidBody.sleepState = SleepState.Awake;

        const index = body_id_index(packed);
        this.storage.set_kind(index, rigidBody.kind);
        this.storage.set_flags(index, rigidBody.flags);

        this.__bodies[index] = rigidBody;
        this.__transforms[index] = transform;
        this.__body_collider_lists[index] = [];

        // Static bodies do not need to live in the active list — they never move.
        if (rigidBody.kind === BodyKind.Static) {
            this.storage.mark_sleeping(index);
        }
    }

    /**
     * Detach every collider attached to this body, free its slot, and clear
     * the side-tables.
     *
     * @param {RigidBody} rigidBody
     * @param {Transform} transform
     * @param {number} entity
     */
    unlink(rigidBody, transform, entity) {
        const packed = rigidBody._bodyId;

        assert.equal(this.storage.is_valid(packed), true, 'unlink: stale or absent body id');

        // If the body is sleeping inside a multi-member sleep group, dissolve
        // the group by waking every member first. Otherwise the surviving
        // members would hold dangling indices in their sleep-group chain
        // pointing at a slot that has just been freed.
        if (rigidBody.sleep_group_next !== -1) {
            this.__wake_body(rigidBody);
        }

        const index = body_id_index(packed);
        const list = this.__body_collider_lists[index];
        if (list !== undefined) {
            for (let i = 0; i < list.length; i++) {
                const entry = list[i];
                if (entry.bvhNode !== COLLIDER_UNBOUND) {
                    this.__remove_from_broadphase(rigidBody, entry.bvhNode);
                }
                entry.collider._bvhNode = COLLIDER_UNBOUND;
                entry.collider._bodyId = -1;
            }
        }

        this.__bodies[index] = undefined;
        this.__transforms[index] = undefined;
        this.__body_collider_lists[index] = undefined;

        this.storage.free(packed);
        rigidBody._bodyId = RIGID_BODY_UNALLOCATED;
    }

    /**
     * Attach a collider to an existing body. The collider can live on the
     * same entity as the body (single-collider body) or on a child entity
     * (compound body). The world transform passed here is the collider's
     * own — for a same-entity collider it is the body's Transform; for a
     * child collider it is the child entity's Transform.
     *
     * Idempotent — re-attaching the same collider is a no-op.
     *
     * @param {number} body_entity entity that owns the body
     * @param {Collider} collider
     * @param {Transform} transform world transform of the collider
     * @param {number} [collider_entity] entity owning the collider (defaults to body_entity)
     */
    attach_collider(body_entity, collider, transform, collider_entity = body_entity) {
        // Find the body by walking the storage entity table. The body must
        // have been allocated via `link` before any colliders are attached.
        const body_index = this.__find_body_index_by_entity(body_entity);
        assert.notEqual(body_index, -1, `attach_collider: no body found for entity ${body_entity}`);

        const rb = this.__bodies[body_index];
        // Idempotent: skip if collider already attached.
        if (collider._bvhNode !== COLLIDER_UNBOUND) return;

        const node = this.__insert_into_broadphase(rb, collider, transform);

        collider._bvhNode = node;
        collider._bodyId = rb._bodyId;
        this.__body_collider_lists[body_index].push({
            collider, transform, entity: collider_entity, bvhNode: node,
        });
    }

    /**
     * Reverse of {@link attach_collider}. Idempotent.
     *
     * @param {number} body_entity
     * @param {Collider} collider
     */
    detach_collider(body_entity, collider) {
        if (collider._bvhNode === COLLIDER_UNBOUND) return;

        const body_index = this.__find_body_index_by_entity(body_entity);
        if (body_index === -1) return;

        const rb = this.__bodies[body_index];
        this.__remove_from_broadphase(rb, collider._bvhNode);

        const list = this.__body_collider_lists[body_index];
        for (let i = 0; i < list.length; i++) {
            if (list[i].collider === collider) {
                list.splice(i, 1);
                break;
            }
        }

        collider._bvhNode = COLLIDER_UNBOUND;
        collider._bodyId = -1;
    }

    /**
     * Resolve an entity to its body index, or -1 if no live body owns it.
     * O(1) via {@link BodyStorage#index_of_entity}'s entity → index map — used
     * on the collider attach / detach and joint link paths.
     *
     * @private
     * @param {number} entity
     * @returns {number} body index or -1
     */
    __find_body_index_by_entity(entity) {
        return this.storage.index_of_entity(entity);
    }

    /**
     * Register a {@link Joint} (6-DOF constraint). Resolves the joint's
     * entities to packed body ids and adds it to the active set, where it is
     * solved alongside contacts in the TGS substep loop. Body A must be a
     * linked body; body B is either a linked body or {@link JOINT_WORLD}
     * (anchoring A to a fixed world point — `localAnchorB` is then a world
     * coordinate).
     *
     * @param {Joint} joint
     */
    link_joint(joint) {
        const idxA = this.__find_body_index_by_entity(joint.entityA);
        assert.notEqual(idxA, -1, `link_joint: no body for entityA ${joint.entityA}`);
        joint._bodyIdA = this.__bodies[idxA]._bodyId;

        if (joint.entityB === JOINT_WORLD) {
            joint._bodyIdB = JOINT_WORLD;
        } else {
            const idxB = this.__find_body_index_by_entity(joint.entityB);
            assert.notEqual(idxB, -1, `link_joint: no body for entityB ${joint.entityB}`);
            joint._bodyIdB = this.__bodies[idxB]._bodyId;
        }

        // Reuse a freed slot if available so joint ids stay dense-ish.
        let id;
        if (this.__joint_free.length > 0) {
            id = this.__joint_free.pop();
        } else {
            id = this.__joints.length;
        }
        joint._jointId = id;
        this.__joints[id] = joint;
    }

    /**
     * Remove a previously {@link link_joint}'d joint from the active set.
     * Idempotent.
     * @param {Joint} joint
     */
    unlink_joint(joint) {
        const id = joint._jointId;
        if (id === JOINT_UNALLOCATED || this.__joints[id] !== joint) return;
        this.__joints[id] = undefined;
        this.__joint_free.push(id);
        joint._jointId = JOINT_UNALLOCATED;
    }

    /**
     * Resolve a packed body id to its entity, or `-1` if the id is stale.
     * @param {number} packed_body_id
     * @returns {number}
     */
    entityOf(packed_body_id) {
        if (!this.storage.is_valid(packed_body_id)) return -1;
        return this.storage.entity_at(body_id_index(packed_body_id));
    }

    /**
     * Number of live bodies (regardless of awake/sleeping state).
     * @returns {number}
     */
    get bodyCount() {
        return this.storage.size;
    }

    /**
     * Apply an instantaneous change of momentum at the body's centre of mass.
     * Linear-only — see {@link applyImpulseAt} for an off-centre impulse that
     * also produces angular response.
     *
     * Wakes the body if it is asleep.
     *
     * @param {RigidBody} rigidBody
     * @param {Vector3|{x:number,y:number,z:number}} impulse
     */
    applyImpulse(rigidBody, impulse) {
        if (rigidBody.kind !== BodyKind.Dynamic) {
            return;
        }
        const inv_m = rigidBody.mass > 0 ? 1 / rigidBody.mass : 0;
        rigidBody.linearVelocity.addScaled(impulse, inv_m);

        this.__wake_body(rigidBody);
    }

    /**
     * Apply an instantaneous change of momentum at a specific world-space point.
     * Off-centre impulses produce both linear (Δv = P/m) and angular
     * (Δω = I_w⁻¹·(r × P)) response.
     *
     * Wakes the body if it is asleep.
     *
     * @param {RigidBody} rigidBody
     * @param {Transform} transform body's current world Transform (used for r and I_w)
     * @param {Vector3|{x:number,y:number,z:number}} impulse
     * @param {Vector3|{x:number,y:number,z:number}} worldPoint
     */
    applyImpulseAt(rigidBody, transform, impulse, worldPoint) {
        if (rigidBody.kind !== BodyKind.Dynamic) {
            return;
        }
        const inv_m = rigidBody.mass > 0 ? 1 / rigidBody.mass : 0;

        rigidBody.linearVelocity.set(
            rigidBody.linearVelocity.x + impulse.x * inv_m,
            rigidBody.linearVelocity.y + impulse.y * inv_m,
            rigidBody.linearVelocity.z + impulse.z * inv_m
        );

        const rx = worldPoint.x - transform.position.x;
        const ry = worldPoint.y - transform.position.y;
        const rz = worldPoint.z - transform.position.z;

        // Δω = I_w⁻¹ · (r × P)
        const tx = ry * impulse.z - rz * impulse.y;
        const ty = rz * impulse.x - rx * impulse.z;
        const tz = rx * impulse.y - ry * impulse.x;

        world_inverse_inertia_apply(scratch_angular_delta, 0, rigidBody.inverseInertiaLocal, transform.rotation, tx, ty, tz);

        rigidBody.angularVelocity.set(
            rigidBody.angularVelocity.x + scratch_angular_delta[0],
            rigidBody.angularVelocity.y + scratch_angular_delta[1],
            rigidBody.angularVelocity.z + scratch_angular_delta[2]
        );

        this.__wake_body(rigidBody);
    }

    /**
     * Accumulate a continuous torque (world-space) for integration on the
     * next fixedUpdate. Pairs with {@link applyForce} for the rotational case.
     *
     * Wakes the body if asleep.
     *
     * @param {RigidBody} rigidBody
     * @param {Vector3|{x:number,y:number,z:number}} torque
     */
    applyTorque(rigidBody, torque) {
        if (rigidBody.kind !== BodyKind.Dynamic) {
            return;
        }
        rigidBody.accumulatedTorque.set(
            rigidBody.accumulatedTorque.x + torque.x,
            rigidBody.accumulatedTorque.y + torque.y,
            rigidBody.accumulatedTorque.z + torque.z
        );
        this.__wake_body(rigidBody);
    }

    /**
     * Apply a continuous force at a specific world-space point. The force
     * generates both a linear acceleration (F/m) and a torque (r × F) about
     * the body's centre of mass.
     *
     * Wakes the body if asleep.
     *
     * @param {RigidBody} rigidBody
     * @param {Transform} transform body's current world Transform
     * @param {Vector3|{x:number,y:number,z:number}} force
     * @param {Vector3|{x:number,y:number,z:number}} worldPoint
     */
    applyForceAt(rigidBody, transform, force, worldPoint) {
        if (rigidBody.kind !== BodyKind.Dynamic) {
            return;
        }
        rigidBody.accumulatedForce.set(
            rigidBody.accumulatedForce.x + force.x,
            rigidBody.accumulatedForce.y + force.y,
            rigidBody.accumulatedForce.z + force.z
        );

        const rx = worldPoint.x - transform.position.x;
        const ry = worldPoint.y - transform.position.y;
        const rz = worldPoint.z - transform.position.z;

        rigidBody.accumulatedTorque.set(
            rigidBody.accumulatedTorque.x + (ry * force.z - rz * force.y),
            rigidBody.accumulatedTorque.y + (rz * force.x - rx * force.z),
            rigidBody.accumulatedTorque.z + (rx * force.y - ry * force.x)
        );

        this.__wake_body(rigidBody);
    }

    /**
     * Accumulate a continuous force to be integrated next fixedUpdate step.
     * Wakes the body if asleep.
     *
     * @param {RigidBody} rigidBody
     * @param {Vector3|{x:number,y:number,z:number}} force
     */
    applyForce(rigidBody, force) {
        if (rigidBody.kind !== BodyKind.Dynamic) {
            return;
        }
        rigidBody.accumulatedForce.add(force);
        this.__wake_body(rigidBody);
    }

    /**
     * Replace the linear velocity. Wakes the body if asleep.
     *
     * @param {RigidBody} rigidBody
     * @param {Vector3|{x:number,y:number,z:number}} v
     */
    setLinearVelocity(rigidBody, v) {
        rigidBody.linearVelocity.copy(v);
        if (rigidBody.kind === BodyKind.Dynamic) {
            this.__wake_body(rigidBody);
        }
    }

    /**
     * Teleport a body to a new pose, bypassing integration: writes the body's
     * Transform directly and wakes it. For an interpolated body this also flags a
     * render `snap` on its {@link Interpolated} component, so the producer keeps
     * this pose (rather than restoring the previous tick over it) and the
     * renderer shows the new pose without sliding across the jump.
     *
     * This is the authoritative way to reposition an interpolated body — a raw
     * `Transform` write would be undone by the per-step restore. Velocity is left
     * as-is; zero it via {@link setLinearVelocity} if the teleport should also
     * stop the body.
     *
     * @param {RigidBody} rigidBody
     * @param {Vector3|{x:number,y:number,z:number}} position world position
     * @param {Quaternion|{x:number,y:number,z:number,w:number}} rotation world unit-quaternion rotation
     */
    setPose(rigidBody, position, rotation) {
        const idx = body_id_index(rigidBody._bodyId);
        const transform = this.__transforms[idx];
        if (transform === undefined) {
            return;
        }

        transform.position.set(position.x, position.y, position.z);
        transform.rotation.set(rotation.x, rotation.y, rotation.z, rotation.w);

        this.__wake_body(rigidBody);

        // Flag a snap on the body's Interpolated component, if any, so the
        // interpolation producer (restore) and consumer (blend) treat this pose
        // as authoritative this frame instead of interpolating across the jump.
        const em = this.entityManager;
        if (em !== null && em !== undefined) {
            const dataset = em.dataset;
            if (dataset !== null && dataset !== undefined) {
                const entity = this.storage.entity_at(idx);
                const interpolated = dataset.getComponent(entity, Interpolated);
                if (interpolated !== undefined && interpolated !== null) {
                    interpolated.snap = true;
                }
            }
        }
    }

    /**
     * Force the body awake. Static bodies are ignored.
     * @param {RigidBody} rigidBody
     */
    wake(rigidBody) {
        this.__wake_body(rigidBody);
    }

    /**
     * Force the body asleep. Dynamic bodies will not re-enter the active list
     * until a wake event occurs.
     * @param {RigidBody} rigidBody
     */
    sleep(rigidBody) {
        if (rigidBody.kind !== BodyKind.Dynamic) {
            return;
        }
        if (rigidBody.sleepState === SleepState.Sleeping) {
            return;
        }
        rigidBody.sleepState = SleepState.Sleeping;
        const index = body_id_index(rigidBody._bodyId);
        this.storage.mark_sleeping(index);
    }

    /**
     * Wake a body and atomically wake every other body it was last sleeping
     * with (its "sleep group"). Sleep groups are circular doubly-linked lists
     * threaded through every member of an island when it sleeps atomically;
     * waking any one member walks the chain and wakes the rest in the same
     * call.
     *
     * Without this, a 100-block stack hit at the base would wake one block
     * per frame as the broadphase propagated awareness up the stack — a
     * visible ~1.6 s wave at 60 fps. Atomic wake eliminates the wave.
     *
     * No-op for non-dynamic bodies. Idempotent for already-awake bodies.
     * @private
     * @param {RigidBody} rb
     */
    __wake_body(rb) {
        if (rb.kind !== BodyKind.Dynamic) return;
        if (rb.sleepState === SleepState.Awake) return;
        const index = body_id_index(rb._bodyId);

        // Remember the next-in-chain before clearing the body's own pointers;
        // the rest of the group is reached by walking forward from there.
        const start_next = rb.sleep_group_next;

        rb.sleepState = SleepState.Awake;
        rb.sleep_timer = 0;
        rb.sleep_group_next = -1;
        rb.sleep_group_prev = -1;
        this.storage.mark_awake(index);

        if (start_next === -1 || start_next === index) return;

        // Walk the (now-broken) chain forward until we loop back. The chain
        // is circular so we know when to stop; defensive `-1` guards against
        // corruption from a body being unlinked mid-sleep-group.
        let cur = start_next;
        while (cur !== -1 && cur !== index) {
            const cur_rb = this.__bodies[cur];
            if (cur_rb === undefined) break;
            const nxt = cur_rb.sleep_group_next;
            cur_rb.sleepState = SleepState.Awake;
            cur_rb.sleep_timer = 0;
            cur_rb.sleep_group_next = -1;
            cur_rb.sleep_group_prev = -1;
            this.storage.mark_awake(cur);
            cur = nxt;
        }
    }

    /**
     * Atomically put every body in a contiguous range of island members to
     * sleep. Members are threaded into a circular doubly-linked list so any
     * future `wake` on any member walks the chain and revives them all.
     *
     * Velocities are zeroed because the body is by definition at rest at
     * this point — the alternative (storing residual velocities for "softer"
     * wake) is what Bullet does, but for a deterministic game-physics target
     * fully resetting is simpler and avoids drift while sleeping.
     *
     * @private
     * @param {Uint32Array} member_array view (or full array) of body indices
     * @param {number} start
     * @param {number} end
     */
    __atomic_sleep_island_range(member_array, start, end) {
        const count = end - start;
        if (count === 0) return;
        const bodies = this.__bodies;
        const storage = this.storage;

        if (count === 1) {
            const idx = member_array[start];
            const rb = bodies[idx];
            if (rb === undefined) return;
            rb.sleep_group_next = -1;
            rb.sleep_group_prev = -1;
            rb.sleepState = SleepState.Sleeping;
            rb.linearVelocity[0] = 0;
            rb.linearVelocity[1] = 0;
            rb.linearVelocity[2] = 0;
            rb.angularVelocity[0] = 0;
            rb.angularVelocity[1] = 0;
            rb.angularVelocity[2] = 0;
            storage.mark_sleeping(idx);
            return;
        }

        for (let i = 0; i < count; i++) {
            const idx = member_array[start + i];
            const rb = bodies[idx];
            if (rb === undefined) continue;
            const next_idx = member_array[start + ((i + 1) % count)];
            const prev_idx = member_array[start + ((i - 1 + count) % count)];
            rb.sleep_group_next = next_idx;
            rb.sleep_group_prev = prev_idx;
            rb.sleepState = SleepState.Sleeping;
            rb.linearVelocity[0] = 0;
            rb.linearVelocity[1] = 0;
            rb.linearVelocity[2] = 0;
            rb.angularVelocity[0] = 0;
            rb.angularVelocity[1] = 0;
            rb.angularVelocity[2] = 0;
            storage.mark_sleeping(idx);
        }
    }

    /**
     * Broadphase raycast against both BVHs. Fills `result` with the nearest
     * hit and returns `true` on hit, `false` on miss.
     *
     * Narrowphase refinement against the actual shape geometry is a
     * follow-up — for now `result.t` is the distance to the leaf's
     * inflated AABB and `result.normal` is the AABB face normal. Both are
     * exact for AABB-shaped colliders.
     *
     * @param {Ray3} ray  origin + unit direction + `tMax`
     * @param {PhysicsSurfacePoint} result populated on hit; untouched on miss
     * @param {(entity:number, collider:Collider)=>boolean} [filter] defaults
     *   to {@link returnTrue} (accept every candidate)
     * @returns {boolean}
     */
    raycast(ray, result, filter = returnTrue) {
        return raycast_query(this, ray, result, filter);
    }

    /**
     * Sweep a convex shape along a ray and find the first body it
     * would hit. The shape starts at `ray.origin` oriented by
     * `rotation` and translates along `ray.direction` for up to
     * `ray.tMax` units. Returns the nearest impact within that
     * interval.
     *
     * The "swept AABB" broadphase finds candidate bodies whose BVH
     * leaves overlap the shape's swept volume; the narrowphase then
     * bisects [0, t] on GJK overlap to find the time-of-impact for
     * each candidate. Best-t early termination skips candidates that
     * can't tighten the answer.
     *
     * @param {Ray3} ray  origin + unit direction + `tMax`
     * @param {AbstractShape3D} shape
     * @param {{x:number,y:number,z:number,w:number}} rotation
     * @param {PhysicsSurfacePoint} result populated on hit; untouched on miss
     * @param {(entity:number, collider:Collider)=>boolean} [filter]
     * @returns {boolean}
     */
    shapeCast(ray, shape, rotation, result, filter = returnTrue) {
        return shape_cast_query(this, ray, shape, rotation, result, filter);
    }

    /**
     * Speculative overlap query: find all bodies whose collider would
     * overlap the given convex shape placed at the given world pose,
     * without mutating the simulation. Intended for kinematic /
     * character controllers that need to test "would I collide if I
     * moved here?" before committing the move.
     *
     * Pipeline:
     *   1. The shape's world AABB is computed from `position` + `rotation`.
     *   2. Both broadphase trees (static + dynamic) are queried for
     *      bodies whose leaf AABB overlaps that envelope.
     *   3. Each candidate is GJK-tested in world frame. Convex
     *      candidates run one GJK call; concave candidates (heightmap,
     *      mesh) run per-triangle GJK via the decomposition path.
     *   4. The optional `filter` is consulted before the GJK test —
     *      useful for skipping the caller's own body, allies, sensors,
     *      etc.
     *
     * The output buffer is filled with overlapping bodies' `body_id`
     * values (uint32 with packed generation), starting at
     * `output_offset`. The caller is responsible for sizing the buffer;
     * IDs past its end are dropped silently and the count caps at the
     * available space.
     *
     * The query shape must be convex. Concave query shapes throw —
     * they're typically static terrain and not used as kinematic
     * probes; the M×N triangle-pair cost wouldn't be worth the rare
     * use case.
     *
     * @param {AbstractShape3D} shape convex query shape, in its local frame
     * @param {{x:number,y:number,z:number}} position world position of
     *   the query shape
     * @param {{x:number,y:number,z:number,w:number}} rotation world
     *   rotation (unit quaternion)
     * @param {Uint32Array|number[]} output buffer to receive body_ids
     * @param {number} output_offset starting index in output
     * @param {(entity:number, collider:Collider)=>boolean} [filter]
     *   defaults to {@link returnTrue}
     * @returns {number} number of overlapping bodies written
     */
    overlap(shape, position, rotation, output, output_offset, filter = returnTrue) {
        return overlap_shape_query(this, shape, position, rotation, output, output_offset, filter);
    }

    /**
     * Wake any sleeping body that appears in this step's broadphase pair list.
     * A pair means the BVH AABBs overlap — even if the sleeper hasn't moved,
     * an awake neighbour has come into contact range and the sleeper must
     * participate in narrowphase / solve.
     * @private
     */
    __wake_pairs() {
        const list = this.pairs;
        const n = list.count;
        const bodies = this.__bodies;
        for (let i = 0; i < n; i++) {
            const idA = list.get_a(i);
            const idB = list.get_b(i);
            const idxA = body_id_index(idA);
            const idxB = body_id_index(idB);
            const a = bodies[idxA];
            const b = bodies[idxB];
            if (a !== undefined && a.sleepState === SleepState.Sleeping) {
                this.__wake_body(a);
            }
            if (b !== undefined && b.sleepState === SleepState.Sleeping) {
                this.__wake_body(b);
            }
        }
    }

    /**
     * Wake propagation across joints: if a joint connects an awake body to a
     * sleeping one, wake the sleeper so the constraint stays coupled (a joint
     * with one body asleep and one awake would otherwise be skipped by the
     * solver, letting the awake side drift). A common trigger is a
     * kinematic/motor-driven body pulling on a sleeping chain.
     *
     * Bodies that slept together as one island share a sleep group, so the
     * usual atomic wake already revives the whole chain when any member is
     * hit; this catches the cases that atomic wake doesn't (joint spanning
     * separate groups, kinematic driver).
     * @private
     */
    __wake_joints() {
        const joints = this.__joints;
        const n = joints.length;
        if (n === 0) return;
        const bodies = this.__bodies;
        const storage = this.storage;
        for (let i = 0; i < n; i++) {
            const joint = joints[i];
            if (joint === undefined || joint === null) continue;
            if (joint._bodyIdB === JOINT_WORLD) continue;
            if (!storage.is_valid(joint._bodyIdA) || !storage.is_valid(joint._bodyIdB)) continue;
            const a = bodies[body_id_index(joint._bodyIdA)];
            const b = bodies[body_id_index(joint._bodyIdB)];
            if (a === undefined || b === undefined) continue;
            const aSleep = a.sleepState === SleepState.Sleeping;
            const bSleep = b.sleepState === SleepState.Sleeping;
            if (aSleep === bSleep) continue; // both awake or both asleep — leave as is
            if (aSleep) this.__wake_body(a); else this.__wake_body(b);
        }
    }

    /**
     * Per-island atomic sleep test. Walks each island once and applies the
     * decision uniformly across all members:
     *
     *  - If any member carries {@link RigidBodyFlags.DisableSleep}, the
     *    entire island is exempt; every member's sleep_timer is reset.
     *  - If `max(|v|² + |ω|²)` across all members is below
     *    {@link sleepVelocitySqrThreshold}, every member's sleep_timer is
     *    incremented by `dt`. When the smallest member's timer crosses
     *    {@link sleepTimeThreshold}, the whole island sleeps atomically in
     *    the same step (members get threaded into a sleep-group chain so
     *    {@link __wake_body} can wake them all in one call).
     *  - Otherwise the island has at least one active member, so every
     *    member's timer is reset.
     *
     * This is the design-plan atomic-island sleep — replaces the per-body
     * approximation that lived in this slot during the previous slice.
     * Weakly-connected piles no longer chatter awake when a single member
     * blips above threshold; piles fall asleep and wake up as one.
     *
     * @private
     * @param {number} dt
     */
    __sleep_test(dt) {
        const threshold_sqr = this.sleepVelocitySqrThreshold;
        const time_threshold = this.sleepTimeThreshold;
        const bodies = this.__bodies;
        const islands = this.islands;
        const island_count = islands.island_count;
        const body_offsets = islands.body_offsets;
        const body_data = islands.body_data;

        for (let isl = 0; isl < island_count; isl++) {
            const start = body_offsets[isl];
            const end = body_offsets[isl + 1];
            if (end === start) continue;

            // Pass 1: find max v² + check DisableSleep across the island.
            let max_v_sqr = 0;
            let any_disable_sleep = false;
            for (let i = start; i < end; i++) {
                const idx = body_data[i];
                const rb = bodies[idx];
                if (rb === undefined) continue;
                if ((rb.flags & RigidBodyFlags.DisableSleep) !== 0) {
                    any_disable_sleep = true;
                    break;
                }
                const lv = rb.linearVelocity;
                const av = rb.angularVelocity;
                const v_sqr = lv[0] * lv[0] + lv[1] * lv[1] + lv[2] * lv[2]
                    + av[0] * av[0] + av[1] * av[1] + av[2] * av[2];
                if (v_sqr > max_v_sqr) max_v_sqr = v_sqr;
            }

            if (any_disable_sleep) {
                // Whole island is exempt — reset every member's timer.
                for (let i = start; i < end; i++) {
                    const rb = bodies[body_data[i]];
                    if (rb !== undefined) rb.sleep_timer = 0;
                }
                continue;
            }

            if (max_v_sqr < threshold_sqr) {
                // Island is at rest — increment every member's timer; if the
                // slowest-stabilising member has crossed the time threshold,
                // every member has (they were incremented together this step),
                // so atomic-sleep the island.
                let min_timer = Infinity;
                for (let i = start; i < end; i++) {
                    const rb = bodies[body_data[i]];
                    if (rb === undefined) continue;
                    rb.sleep_timer += dt;
                    if (rb.sleep_timer < min_timer) min_timer = rb.sleep_timer;
                }
                if (min_timer >= time_threshold) {
                    this.__atomic_sleep_island_range(body_data, start, end);
                }
            } else {
                // At least one member is active — reset every timer.
                for (let i = start; i < end; i++) {
                    const rb = bodies[body_data[i]];
                    if (rb !== undefined) rb.sleep_timer = 0;
                }
            }
        }
    }

    /**
     * Dispatch every buffered contact event to the entities involved, via the
     * dataset's per-entity event channel — `dataset.sendEvent(entity, name,
     * payload)`, once for each of the pair's entities. This is the only
     * contact-event path: a system with no dataset attached has nowhere to
     * deliver and emits nothing.
     *
     * Payload is a reused scratch object; listeners must copy anything they
     * intend to retain past the listener body.
     * @private
     */
    __dispatch_contact_events() {
        const events = this.contactEvents;
        const n = events.count;
        if (n === 0) return;

        const ecd = (this.entityManager !== null && this.entityManager !== undefined)
            ? this.entityManager.dataset
            : null;
        // No dataset → nowhere to deliver. The buffer is cleared by
        // diff_manifolds each step regardless, so nothing leaks.
        if (ecd === null || ecd === undefined) return;

        const manifolds = this.manifolds;
        const data = manifolds.data_buffer;

        const payload = this.__contact_payload;

        for (let i = 0; i < n; i++) {
            const kind = events.kind_at(i);
            const entA = events.entityA_at(i);
            const entB = events.entityB_at(i);
            const slot = events.slot_at(i);

            // Use the deepest contact of the manifold as the representative
            // point/normal/depth for the event. v1: contact 0 only.
            const slot_off = manifolds.slot_data_offset(slot);
            const has_contact = manifolds.contact_count(slot) > 0;

            // Scratch payload — write the point/normal components by index to
            // skip the Vector3 change-signal dispatch (nothing observes them).
            const pt = payload.point;
            const nm = payload.normal;
            if (has_contact) {
                const wax = data[slot_off], way = data[slot_off + 1], waz = data[slot_off + 2];
                const wbx = data[slot_off + 3], wby = data[slot_off + 4], wbz = data[slot_off + 5];
                pt[0] = (wax + wbx) * 0.5;
                pt[1] = (way + wby) * 0.5;
                pt[2] = (waz + wbz) * 0.5;
                nm[0] = data[slot_off + 6];
                nm[1] = data[slot_off + 7];
                nm[2] = data[slot_off + 8];
                payload.depth = data[slot_off + 9];
            } else {
                pt[0] = 0;
                pt[1] = 0;
                pt[2] = 0;
                nm[0] = 0;
                nm[1] = 0;
                nm[2] = 0;
                payload.depth = 0;
            }
            payload.entityA = entA;
            payload.entityB = entB;

            let event_name;
            if (kind === ContactEventKind.Begin) {
                event_name = PhysicsEvents.ContactBegin;
            } else if (kind === ContactEventKind.Stay) {
                event_name = PhysicsEvents.ContactStay;
            } else {
                event_name = PhysicsEvents.ContactEnd;
            }

            if (entA >= 0) ecd.sendEvent(entA, event_name, payload);
            if (entB >= 0) ecd.sendEvent(entB, event_name, payload);
        }
    }

    /**
     * Producer — restore pass. At the top of a fixed step, reset every awake
     * {@link Interpolated} body's live components to their authoritative state
     * from the previous tick's snapshot, undoing any render-time interpolation
     * the {@link InterpolationSystem} wrote between frames so the sim integrates
     * from truth, not an interpolated pose. A body with no previous snapshot
     * (first step ever, or just woken) is left as-is — its live state is already
     * authoritative. No-op unless {@link interpolationLog} is wired.
     * @private
     */
    __interp_restore() {
        const log = this.interpolationLog;
        const em = this.entityManager;
        if (log === null || em === null || em === undefined) return;
        const dataset = em.dataset;
        if (dataset === null || dataset === undefined) return;

        const prev_tick = em.fixedStepTick - 1;
        if (prev_tick < 0) return;

        const storage = this.storage;
        const count = storage.awake_count;
        const scratch = this.__interp_scratch;

        for (let i = 0; i < count; i++) {
            const idx = storage.awake_at(i);
            const entity = storage.entity_at(idx);
            const interpolated = dataset.getComponent(entity, Interpolated);
            if (interpolated === undefined || interpolated === null) continue;
            // A teleported body (snap set, e.g. via setPose) keeps its live pose
            // this step — restoring the previous tick would undo the teleport.
            if (interpolated.snap) continue;
            const key = interpolated.key;
            if (key < 0) continue;

            const interpolands = interpolated.interpolands;
            for (let k = 0; k < interpolands.length; k++) {
                const ip = interpolands[k];
                scratch.position = 0;
                // Snap to the previous tick's snapshot (both offsets equal → t irrelevant).
                const ok = log.interpolate(scratch, key, ip.type_id, prev_tick, prev_tick, 0, ip.interpolation_adapter);
                if (!ok) continue;
                const target = dataset.getComponent(entity, ip.component_class);
                if (target === undefined || target === null) continue;
                scratch.position = 0;
                ip.serialization_adapter.deserialize(scratch, target);
            }
        }
    }

    /**
     * Producer — record pass. At the end of a fixed step, snapshot every awake
     * {@link Interpolated} body's live components into the shared log under the
     * current `entityManager.fixedStepTick`. The render-time
     * {@link InterpolationSystem} blends consecutive ticks from these snapshots.
     * Only awake (moving) bodies are recorded, so the log stays sparse. No-op
     * unless {@link interpolationLog} is wired.
     * @private
     */
    __interp_record() {
        const log = this.interpolationLog;
        const em = this.entityManager;
        if (log === null || em === null || em === undefined) return;
        const dataset = em.dataset;
        if (dataset === null || dataset === undefined) return;

        const tick = em.fixedStepTick;
        const storage = this.storage;
        const count = storage.awake_count;

        log.begin_tick(tick);
        for (let i = 0; i < count; i++) {
            const idx = storage.awake_at(i);
            const entity = storage.entity_at(idx);
            const interpolated = dataset.getComponent(entity, Interpolated);
            if (interpolated === undefined || interpolated === null) continue;
            const key = interpolated.key;
            if (key < 0) continue;

            const interpolands = interpolated.interpolands;
            for (let k = 0; k < interpolands.length; k++) {
                const ip = interpolands[k];
                const target = dataset.getComponent(entity, ip.component_class);
                if (target === undefined || target === null) continue;
                const buf = log.begin_record(key, ip.type_id);
                ip.serialization_adapter.serialize(buf, target);
                log.end_record();
            }
        }
        log.end_tick();
    }

    fixedUpdate(dt) {
        // Producer: restore authoritative pose (undo render interpolation)
        // before the sim reads any Transform this step.
        this.__interp_restore();

        const gx = this.gravity.x;
        const gy = this.gravity.y;
        const gz = this.gravity.z;

        const storage = this.storage;
        const bodies = this.__bodies;
        const transforms = this.__transforms;
        const joints = this.__joints;
        const manifolds = this.manifolds;


        const count = storage.awake_count;

        // Stage 1: consume the per-frame force / torque accumulators into
        // velocity at the full `dt`, exactly once (a user force is a per-frame
        // budget that must land in full regardless of substep count). Gravity
        // is applied per substep below, so the trajectory integrates at the
        // substep rate and — crucially — each substep's gravity is balanced by
        // that substep's contact warm-start, keeping resting stacks at zero
        // velocity.
        for (let i = 0; i < count; i++) {
            const idx = storage.awake_at(i);

            const rb = bodies[idx];
            const tr = transforms[idx];

            integrate_velocity_forces(rb, tr, dt);
        }

        // Stage 2: refit each awake body's collider leaves at the current
        // pose, padded by the swept extent for the body's velocity. The fat
        // margin uses the post-force velocity; this frame's gravity increment
        // is a sub-millimetre slack difference, safely inside the margin.
        const lists = this.__body_collider_lists;
        for (let i = 0; i < count; i++) {

            const idx = storage.awake_at(i);
            const rb = bodies[idx];
            const list = lists[idx];

            if (list === undefined) {
                continue;
            }

            const lv = rb.linearVelocity;

            const list_length = list.length;

            for (let k = 0; k < list_length; k++) {

                const entry = list[k];

                compute_fat_world_aabb(
                    scratch_world_aabb, 0,
                    entry.collider.shape, entry.transform,
                    lv[0], lv[1], lv[2],
                    dt
                );

                this.dynamicBvh.node_move_aabb(entry.bvhNode, scratch_world_aabb);
            }
        }

        // Stage 3: broadphase pair generation. The fat AABBs cover the full
        // outer-step motion, so the pair set stays valid across all substeps
        // — broadphase runs once.
        generate_pairs(
            storage,
            this.dynamicBvh,
            this.staticBvh,
            manifolds,
            lists,
            this.pairs,
            this.__pair_filter,
            this,
        );

        // Stage 4: wake propagation — through broadphase pairs, then through
        // joints (a joint must not have one body awake and one asleep).
        this.__wake_pairs();
        this.__wake_joints();

        // Stage 5: narrowphase — once per outer step. The substep loop below
        // re-derives each contact's penetration analytically from the moved
        // poses rather than re-running geometry.
        narrowphase_step(this.pairs, manifolds, this.__body_collider_lists);

        // Stage 6: partition awake bodies + touched contacts into islands.
        // Consumed by the solver (flattened contact list) and the sleep test.
        this.islands.build(storage, manifolds, bodies, this.__body_collider_lists, joints);

        // Stage 7: TGS substep loop.
        //
        // prepare_contacts captures per-contact anchors / effective masses /
        // approach velocity (no warm-start — that's per-substep). Each substep
        // integrates gravity by `h`, re-derives contact geometry from the
        // current poses, replays warm-start, solves velocity (non-penetration
        // + friction) and position (pseudo-velocity), and integrates the pose
        // by `h`. Per-substep gravity + per-substep warm-start balance exactly
        // at a resting contact (each cancels `h` of the other), so stacks hold
        // at zero velocity and sleep; the position correction adapts as bodies
        // separate between substeps — the TGS stack-stability mechanism —
        // without re-running narrowphase. Restitution is applied once after
        // the loop.
        const N = this.substeps;
        const h = dt / N;
        const count_after_wake = storage.awake_count;

        // CCD: capture start-of-step positions for flagged bodies over the
        // post-wake awake set (poses are unchanged until the substep loop below
        // integrates them). The CCD pass after the solver sweeps from here to
        // each body's final pose. Reads the primary collider's transform so the
        // start matches the end the resolve pass reads. Zero-cost when no body
        // is flagged.
        const ccd_on = this.ccdEnabled;

        if (ccd_on) {

            const ccd_need = storage.high_water_mark * 3;

            if (this.__ccd_start_pos.length < ccd_need) {
                this.__ccd_start_pos = new Float64Array(ccd_need);
            }

            const ccd_start = this.__ccd_start_pos;

            for (let i = 0; i < count_after_wake; i++) {
                const idx = storage.awake_at(i);
                const rb = bodies[idx];

                if (rb.kind !== BodyKind.Dynamic) {
                    continue;
                }
                if ((rb.flags & RigidBodyFlags.CCD) === 0) {
                    continue;
                }

                const list = this.__body_collider_lists[idx];

                if (list === undefined || list.length === 0) {
                    continue;
                }

                const cp = list[0].transform.position;
                const cb = idx * 3;

                ccd_start[cb] = cp[0];
                ccd_start[cb + 1] = cp[1];
                ccd_start[cb + 2] = cp[2];
            }
        }

        // Size the pseudo-velocity buffer ONCE (it may reallocate on growth),
        // then capture the reference. Inside the loop we only zero its live
        // region per substep — re-capturing is unnecessary since it won't
        // reallocate again this step.
        this.__reset_pseudo_velocity();
        const pseudoVel = this.__pseudo_velocity;
        const pseudo_len = storage.high_water_mark * 6;

        // Gather the data-oriented solver state for every body the substep loop
        // will touch: the post-wake awake set (all dynamics, at their post-force
        // velocity) plus the static / kinematic anchors and jointed partners
        // referenced by this step's contacts and joints. From here the substep
        // loop reads / writes velocity + orientation through `ss_data` with no
        // component-object dereference; persistent velocity is scattered back
        // onto the RigidBodies after the solve.
        const solver_state = this.__solver_state;
        solver_state.begin(storage.high_water_mark);

        for (let i = 0; i < count_after_wake; i++) {
            const idx = storage.awake_at(i);
            solver_state.gather(idx, bodies[idx], transforms[idx]);
        }

        const island_contacts = this.islands.contact_data;
        const island_contact_total = this.islands.contact_offsets[this.islands.island_count];

        for (let i = 0; i < island_contact_total; i++) {
            const slot = island_contacts[i];

            const ia = body_id_index(manifolds.bodyA(slot));
            const ib = body_id_index(manifolds.bodyB(slot));

            solver_state.gather(ia, bodies[ia], transforms[ia]);
            solver_state.gather(ib, bodies[ib], transforms[ib]);
        }

        const joint_count = joints.length;
        for (let i = 0; i < joint_count; i++) {
            const joint = joints[i];

            if (joint === undefined || joint === null) {
                continue;
            }

            if (!storage.is_valid(joint._bodyIdA)) {
                continue;
            }

            const ia = body_id_index(joint._bodyIdA);

            solver_state.gather(ia, bodies[ia], transforms[ia]);

            if (joint._bodyIdB !== JOINT_WORLD && storage.is_valid(joint._bodyIdB)) {

                const ib = body_id_index(joint._bodyIdB);

                solver_state.gather(ib, bodies[ib], transforms[ib]);

            }
        }

        const ss_data = solver_state.data;

        prepare_contacts(manifolds, this, h);

        for (let s = 0; s < N; s++) {

            // Gravity (+ damping) for this substep.
            for (let i = 0; i < count_after_wake; i++) {
                const idx = storage.awake_at(i);
                integrate_velocity_gravity(ss_data, idx * SBS_STRIDE, bodies[idx], gx, gy, gz, h);
            }

            // Re-derive contact geometry at the current poses: concave pairs
            // re-run narrowphase (fresh feature/normal as the body rocks),
            // convex pairs rotate frozen anchors analytically. Then replay
            // the per-substep warm-start and solve velocity.
            redetect_concave_contacts(manifolds, this);
            refresh_contacts(manifolds, this.__transforms);
            warm_start_contacts(manifolds, this);
            solve_velocity(manifolds, this, this.velocityIterations);

            // Joints share the substep: warm-start + velocity-solve the 6-DOF
            // constraints on real velocity, coupled with the contacts above
            // (a body touched by both sees one substep of Gauss-Seidel across
            // contacts then joints). Position correction for locked DOFs is a
            // SPOOK bias inside this solve, so no separate joint position pass.
            if (joints.length > 0) {
                solve_joints(joints, this, h, this.jointIterations);
            }

            // Position correction writes pseudo-velocity (zeroed first so it
            // is a fresh per-substep correction), folded into the pose by the
            // position integrate and then discarded.
            pseudoVel.fill(0, 0, pseudo_len);
            solve_position(manifolds, this, this.positionIterations);

            for (let i = 0; i < count_after_wake; i++) {
                const idx = storage.awake_at(i);
                const rb = bodies[idx];
                const tr = transforms[idx];
                const base = idx * 6;
                integrate_position(ss_data, idx * SBS_STRIDE, rb, tr, h,
                    pseudoVel[base], pseudoVel[base + 1], pseudoVel[base + 2],
                    pseudoVel[base + 3], pseudoVel[base + 4], pseudoVel[base + 5]);
            }
        }

        // Stage 8: one-shot restitution, after the substep loop, keyed off
        // the approach velocity captured at prepare time.
        apply_restitution(manifolds, this);

        // Scatter the solved persistent linear / angular velocity back onto the
        // RigidBody components (pose was written through to the Transforms each
        // substep). After this the bodies are authoritative again for CCD,
        // interpolation recording, and the sleep test below.
        solver_state.scatter(bodies);

        // Stage 8.5: continuous collision — sweep CCD-flagged fast movers along
        // their net step translation and stop them at the first blocker, so they
        // can't tunnel through thin geometry between discrete steps. Runs on the
        // final post-solve poses, before the sleep test sees the clamped
        // velocities. No-op when no awake body is flagged.
        if (ccd_on) {
            ccd_resolve(this);
        }

        // Producer: record the post-step authoritative pose of every awake
        // interpolated body under this step's tick. Before the sleep test, so a
        // body that settles this step still records its final pose for the last
        // interpolation interval.
        this.__interp_record();

        // Stage 9: sleep test.
        this.__sleep_test(dt);

        // Stage 10: diff manifolds against the previous frame and dispatch
        // Begin / Stay / End events. MUST run before advance_frame, which
        // rolls the touched flags.
        diff_manifolds(manifolds, storage, this.contactEvents);
        this.__dispatch_contact_events();

        // Stage 11 (end-of-step): roll touched → prev_touched and evict slots
        // whose pair has not been touched within the grace window.
        manifolds.advance_frame();
    }
}

/**
 * @readonly
 * @type {boolean}
 */
PhysicsSystem.prototype.isPhysicsSystem = true;

// Re-export for convenience.
export { BodyKind, RigidBodyFlags };
