import { BodyKind } from "../ecs/BodyKind.js";
import { quat_integrate } from "./quat_integrate.js";
import {
    SBS_QX, SBS_QY, SBS_QZ, SBS_QW,
    SBS_LV_X, SBS_LV_Y, SBS_LV_Z,
    SBS_AV_X, SBS_AV_Y, SBS_AV_Z,
} from "../body/SolverBodyState.js";

const scratch_q = new Float64Array(4);

/**
 * Integrate `transform.position` and `transform.rotation` from the body's
 * current linear and angular velocity over a step `dt`. Static bodies are
 * not touched.
 *
 * The trailing six arguments are the body's *pseudo-velocity*: the
 * position-pass output from the constraint solver (Catto split-impulse).
 * Pseudo-velocity exists only inside the step — it's folded into the
 * integration so the position update reflects depth correction, then
 * implicitly discarded. The body's persistent `linearVelocity` /
 * `angularVelocity` (which carry restitution + warm-start across steps)
 * are never contaminated by it.
 *
 * Callers with no position-pass output should pass zeros for the six
 * pseudo arguments — folding zero is free (one add per axis).
 *
 * KinematicPosition bodies are treated like Dynamic for the position
 * update — if the gameplay code is driving them, their velocity is
 * whatever the user wrote (typically zero); the step is effectively a
 * no-op in that case. KinematicVelocity bodies advance under their
 * user-set velocity.
 *
 * Persistent linear / angular velocity is read from the data-oriented
 * {@link SolverBodyState} span (`ss` / `base`); the pose remains authoritative
 * on the `Transform` and is written through `.set()` so its onChanged
 * subscribers and the per-substep concave re-detection keep seeing the moved
 * pose. The integrated orientation is mirrored back into the solver state so
 * the next substep's impulse loop evaluates world inertia against the current
 * frame.
 *
 * @param {Float64Array} ss solver-body-state data array
 * @param {number} base `body_index * SBS_STRIDE`
 * @param {RigidBody} rb
 * @param {Transform} transform
 * @param {number} dt
 * @param {number} ps_lin_x pseudo-linear-velocity x (0 when no contacts)
 * @param {number} ps_lin_y
 * @param {number} ps_lin_z
 * @param {number} ps_ang_x pseudo-angular-velocity x (0 when no contacts)
 * @param {number} ps_ang_y
 * @param {number} ps_ang_z
 */
export function integrate_position(
    ss, base, rb, transform, dt,
    ps_lin_x, ps_lin_y, ps_lin_z,
    ps_ang_x, ps_ang_y, ps_ang_z,
) {
    if (rb.kind === BodyKind.Static) {
        return;
    }

    const p = transform.position;

    // Combined integration velocity: persistent + pseudo. The pseudo
    // contribution exists only for this step and never lands in the velocity.
    const vx = ss[base + SBS_LV_X] + ps_lin_x;
    const vy = ss[base + SBS_LV_Y] + ps_lin_y;
    const vz = ss[base + SBS_LV_Z] + ps_lin_z;

    // The write goes through .set() so Transform's onChanged subscribers
    // (matrix recompose, parent/child sync, viewport position, fog-of-war
    // reveal) fire — pose stays authoritative on the Transform.
    p.set(
        p[0] + vx * dt,
        p[1] + vy * dt,
        p[2] + vz * dt
    );

    const wx = ss[base + SBS_AV_X] + ps_ang_x;
    const wy = ss[base + SBS_AV_Y] + ps_ang_y;
    const wz = ss[base + SBS_AV_Z] + ps_ang_z;
    if (wx !== 0 || wy !== 0 || wz !== 0) {
        const q = transform.rotation;
        quat_integrate(
            scratch_q,
            q[0], q[1], q[2], q[3],
            wx, wy, wz,
            dt
        );
        q.set(scratch_q[0], scratch_q[1], scratch_q[2], scratch_q[3]);
        // Mirror the moved orientation into the solver state.
        ss[base + SBS_QX] = scratch_q[0];
        ss[base + SBS_QY] = scratch_q[1];
        ss[base + SBS_QZ] = scratch_q[2];
        ss[base + SBS_QW] = scratch_q[3];
    }
}
