import { aabb3_transform_oriented } from "../../../core/geom/3d/aabb/aabb3_transform_oriented.js";

const scratch_local = new Float64Array(6);

/**
 * Constant inflation applied to a world AABB so a small motion doesn't churn
 * the broadphase. Tuned to Box2D / Bullet conventions.
 * @type {number}
 */
const FAT_LINEAR = 0.05;

/**
 * Per-second-of-velocity inflation. Two seconds of motion is a generous look-ahead
 * — `node_move_aabb` only re-inserts when the inflated AABB no longer encloses
 * the actual one, so this is a hint, not an upper bound.
 * @type {number}
 */
const FAT_VELOCITY_MULTIPLIER = 2;

/**
 * Compute the inflated world AABB of a shape under a pose, padded by both a
 * constant slack and a velocity-proportional swept extent. The output is the
 * thing to hand to `BVH#node_move_aabb`.
 *
 * @param {number[]|Float32Array|Float64Array} result
 * @param {number} result_offset
 * @param {AbstractShape3D} shape
 * @param {Transform} transform
 * @param {number} vx world linear velocity x
 * @param {number} vy world linear velocity y
 * @param {number} vz world linear velocity z
 * @param {number} dt step size
 */
export function compute_fat_world_aabb(
    result, result_offset,
    shape, transform,
    vx, vy, vz, dt
) {
    shape.compute_bounding_box(scratch_local);

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

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

    const speed = Math.sqrt(vx * vx + vy * vy + vz * vz);
    const pad = FAT_LINEAR + FAT_VELOCITY_MULTIPLIER * speed * dt;

    result[result_offset] -= pad;
    result[result_offset + 1] -= pad;
    result[result_offset + 2] -= pad;
    result[result_offset + 3] += pad;
    result[result_offset + 4] += pad;
    result[result_offset + 5] += pad;
}
