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

const scratch_local = new Float64Array(3);
const scratch_local_aabb = new Float64Array(6);

/**
 * AbstractShape3D adapter that re-frames a shape's `support` (and bounding box)
 * into world space under a rigid pose (position + unit quaternion). The
 * underlying shape itself is never copied or transformed — we rotate the GJK
 * query direction into body space, ask the shape, then rotate the support
 * point back to world.
 *
 * Existing TransformedShape3D solves a more general case (arbitrary matrix
 * including scale) and crucially does *not* rotate the query direction into
 * body space before calling support — that path is correct for unscaled
 * identity rotations but wrong for rotated bodies, which is what narrowphase
 * needs.
 *
 * Reusable: physics narrowphase keeps two long-lived instances and calls
 * {@link setup} each pair. No per-pair allocation.
 *
 * @author Alex Goldring
 * @copyright Company Named Limited (c) 2026
 */
export class PosedShape extends AbstractShape3D {

    constructor() {
        super();

        /** @type {AbstractShape3D|null} */
        this.shape = null;

        this.px = 0; this.py = 0; this.pz = 0;
        this.qx = 0; this.qy = 0; this.qz = 0; this.qw = 1;
    }

    /**
     * Bind a shape and a world pose. Subsequent calls to {@link support} and
     * {@link compute_bounding_box} return results in world space.
     *
     * @param {AbstractShape3D} shape
     * @param {Vector3|{x:number,y:number,z:number}} position
     * @param {Quaternion|{x:number,y:number,z:number,w:number}} rotation
     */
    setup(shape, position, rotation) {
        this.shape = shape;
        this.px = position.x; this.py = position.y; this.pz = position.z;
        this.qx = rotation.x; this.qy = rotation.y; this.qz = rotation.z; this.qw = rotation.w;
    }

    /**
     * @param {number[]|Float32Array|Float64Array} result
     * @param {number} result_offset
     * @param {number} dx world-space query direction x (unit)
     * @param {number} dy world-space query direction y
     * @param {number} dz world-space query direction z
     */
    support(result, result_offset, dx, dy, dz) {
        // Inline q·v·q* math: PosedShape.support is the hottest GJK call
        // (twice per iteration of the body-level fallback, once per
        // triangle for concave dispatch). V8 declines to inline
        // v3_quat3_apply at 9-arg call sites, so the math is repeated
        // inline here at the cost of ~7 extra lines per direction.
        // See core/geom/vec3/v3_quat3_apply.js for the canonical form
        // — this is a literal expansion of it.
        const qx = this.qx, qy = this.qy, qz = this.qz, qw = this.qw;

        // Rotate world direction into body frame via the conjugate
        // quaternion: v_local = q* · v_world · q.  Conjugate xyz are
        // negated (cx, cy, cz below).
        const tdx = qw * dx - qy * dz + qz * dy;
        const tdy = qw * dy - qz * dx + qx * dz;
        const tdz = qw * dz - qx * dy + qy * dx;
        const tdw = qx * dx + qy * dy + qz * dz;

        const ldx = tdx * qw + tdw * qx + tdy * qz - tdz * qy;
        const ldy = tdy * qw + tdw * qy + tdz * qx - tdx * qz;
        const ldz = tdz * qw + tdw * qz + tdx * qy - tdy * qx;

        // Ask the local shape for its support point in body-local space.
        this.shape.support(scratch_local, 0, ldx, ldy, ldz);

        // Rotate the body-local support point back to world: v_world =
        // q · v_local · q*.
        const lsx = scratch_local[0], lsy = scratch_local[1], lsz = scratch_local[2];

        const tsx =  qw * lsx + qy * lsz - qz * lsy;
        const tsy =  qw * lsy + qz * lsx - qx * lsz;
        const tsz =  qw * lsz + qx * lsy - qy * lsx;
        const tsw = -qx * lsx - qy * lsy - qz * lsz;

        result[result_offset]     = tsx * qw - tsw * qx - tsy * qz + tsz * qy + this.px;
        result[result_offset + 1] = tsy * qw - tsw * qy - tsz * qx + tsx * qz + this.py;
        result[result_offset + 2] = tsz * qw - tsw * qz - tsx * qy + tsy * qx + this.pz;
    }

    compute_bounding_box(result) {
        this.shape.compute_bounding_box(scratch_local_aabb);
        aabb3_transform_oriented(
            result, 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],
            this.px, this.py, this.pz,
            this.qx, this.qy, this.qz, this.qw
        );
    }
}
