import { bvh_query_user_data_overlaps_aabb } from "../../../core/bvh2/bvh3/query/bvh_query_user_data_overlaps_aabb.js";
import { returnTrue } from "../../../core/function/returnTrue.js";
import { aabb3_transform_oriented } from "../../../core/geom/3d/aabb/aabb3_transform_oriented.js";
import { Triangle3D } from "../../../core/geom/3d/shape/Triangle3D.js";
import { body_id_index } from "../body/BodyStorage.js";
import { gjk } from "../gjk/gjk.js";
import { aabb_world_to_local } from "../narrowphase/decomposition/aabb_world_to_local.js";
import { decompose_to_triangles } from "../narrowphase/decomposition/decompose_to_triangles.js";
import { TRIANGLE_FLOAT_STRIDE } from "../narrowphase/decomposition/triangle_buffer_layout.js";
import { PosedShape } from "../narrowphase/PosedShape.js";

/**
 * Scratch state — module-scoped to avoid per-query allocation. Safe
 * because PhysicsSystem queries run on the main thread, sequentially.
 */
const local_aabb = new Float64Array(6);
const world_aabb = new Float64Array(6);
const concave_query_aabb = new Float64Array(6);
const simplex_buf = new Float64Array(12);

const query_posed = new PosedShape();
const candidate_posed = new PosedShape();
const triangle_shape = new Triangle3D();

/**
 * Maximum triangles a concave candidate can emit per overlap pair.
 * Same rationale as the narrowphase's `MAX_TRIANGLES_PER_PAIR`: the
 * broadphase has already bounded the query AABB to the query shape's
 * envelope, so a single candidate typically yields tens of triangles
 * (a heightmap's count scales O(N²) with its `tessellation`, still
 * inside the buffer for a bounded query at moderate tessellation).
 * Excess triangles are dropped by the enumerator's bounds check —
 * worst case is a missed overlap on a far edge of the candidate's
 * geometry, recovered next query.
 * @type {number}
 */
const MAX_TRIANGLES_PER_PAIR = 1024;

const triangle_buffer = new Float64Array(MAX_TRIANGLES_PER_PAIR * TRIANGLE_FLOAT_STRIDE);

/**
 * Broadphase candidate buffer. Grows by doubling on overflow.
 * @type {Uint32Array}
 */
let scratch_candidates = new Uint32Array(64);

/**
 * Test what bodies overlap a convex shape placed at a given pose. Each
 * overlapping body's `body_id` is written to `output` starting at
 * `output_offset`; the function returns the number of body ids written.
 *
 * Use case: speculative physics queries for kinematic / character
 * controllers. An external system can ask "would my body collide with
 * anything if I moved it here?" without committing a tick of
 * simulation. The output is a flat list of body ids so the caller can
 * decide what to do per hit (skip, push, slide, etc.).
 *
 * The pipeline mirrors the narrowphase pair test:
 *   1. Build the query shape's world AABB.
 *   2. Pull candidates from both broadphase trees that overlap that AABB.
 *   3. For each candidate, run GJK in world frame. Convex candidates
 *      go through one GJK call; concave candidates (heightmap / mesh)
 *      go through the per-triangle decomposition path.
 *   4. Apply the optional `filter` callback (same signature as in
 *      raycast / shapeCast) before the GJK test — early-out on bodies
 *      the caller already wants to skip (themselves, allies, etc.).
 *
 * The query shape must be convex (`is_convex === true`). Concave shapes
 * are typically static terrain and not used as kinematic query
 * probes; rejecting them avoids the M×N triangle-pair cost.
 *
 * @param {PhysicsSystem} system
 * @param {AbstractShape3D} shape  query shape, convex; expressed in
 *   its own 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
 *   of the query shape (unit quaternion)
 * @param {Uint32Array|number[]} output buffer to write body_ids into.
 *   Caller is responsible for sizing it; ids past its end are dropped.
 * @param {number} output_offset float-index in output to start writing at
 * @param {(entity:number, collider:Collider)=>boolean} [filter]
 *   defaults to {@link returnTrue} (accept every candidate)
 * @returns {number} number of overlapping bodies written
 * @throws {Error} if `shape.is_convex === false`
 */
export function overlap_shape(system, shape, position, rotation, output, output_offset, filter = returnTrue) {
    if (shape.is_convex === false) {
        throw new Error(`overlap_shape: query shape must be convex; received \`${shape.constructor.name}\` (is_convex=false)`);
    }

    // ── 1. Query shape's world AABB ─────────────────────────────────
    shape.compute_bounding_box(local_aabb);
    aabb3_transform_oriented(
        world_aabb, 0,
        local_aabb[0], local_aabb[1], local_aabb[2],
        local_aabb[3], local_aabb[4], local_aabb[5],
        position.x, position.y, position.z,
        rotation.x, rotation.y, rotation.z, rotation.w
    );

    // ── 2. Gather broadphase candidates ─────────────────────────────
    // The BVH query returns the TRUE overlap count and silently drops writes past
    // the buffer end, so an undersized buffer would leave the read loop pulling
    // `undefined` (garbage body ids) and silently MISS real overlaps. Grow by
    // doubling and re-query until both static + dynamic fit (queries are
    // deterministic, so this converges in one resize).
    let n_static, n_dynamic, n_total;
    for (;;) {
        n_static = bvh_query_user_data_overlaps_aabb(scratch_candidates, 0, system.staticBvh, world_aabb);
        n_dynamic = bvh_query_user_data_overlaps_aabb(scratch_candidates, n_static, system.dynamicBvh, world_aabb);
        n_total = n_static + n_dynamic;
        if (n_total <= scratch_candidates.length) break;
        scratch_candidates = new Uint32Array(Math.max(n_total, scratch_candidates.length * 2));
    }
    if (n_total === 0) return 0;

    // ── 3. Set up query PosedShape (constant across candidates) ─────
    query_posed.setup(shape, position, rotation);

    // ── 4. Per-candidate narrowphase ────────────────────────────────
    const output_capacity = output.length - output_offset;
    let count = 0;
    let cursor = output_offset;

    for (let i = 0; i < n_total; i++) {
        if (count >= output_capacity) break;

        const body_id = scratch_candidates[i];
        const body_idx = body_id_index(body_id);

        const entity = system.entityOf(body_id);
        if (entity < 0) continue;

        const collider = system.__primary_collider(body_idx);
        if (collider === null) continue;
        if (!filter(entity, collider)) continue;

        const candidate_tr = system.__transforms[body_idx];

        let overlaps = false;

        if (collider.shape.is_convex !== false) {
            candidate_posed.setup(collider.shape, candidate_tr.position, candidate_tr.rotation);
            overlaps = gjk(simplex_buf, query_posed, candidate_posed);
        } else {
            // Concave candidate: project the query's world AABB into
            // the candidate's body-local frame, decompose to triangles,
            // run per-triangle GJK until one overlap is found.
            aabb_world_to_local(
                concave_query_aabb, 0,
                world_aabb,
                candidate_tr.position.x, candidate_tr.position.y, candidate_tr.position.z,
                candidate_tr.rotation.x, candidate_tr.rotation.y, candidate_tr.rotation.z, candidate_tr.rotation.w
            );

            const tri_count = decompose_to_triangles(
                triangle_buffer, 0, collider.shape,
                concave_query_aabb[0], concave_query_aabb[1], concave_query_aabb[2],
                concave_query_aabb[3], concave_query_aabb[4], concave_query_aabb[5]
            );

            // Re-pose candidate as the concave body (the flyweight triangle is
            // rebound per iteration below; setup only stores the shape reference).
            candidate_posed.setup(triangle_shape, candidate_tr.position, candidate_tr.rotation);

            for (let t = 0; t < tri_count; t++) {
                triangle_shape.bind(triangle_buffer, t * TRIANGLE_FLOAT_STRIDE);
                if (gjk(simplex_buf, query_posed, candidate_posed)) {
                    overlaps = true;
                    break;
                }
            }
        }

        if (overlaps) {
            output[cursor] = body_id;
            cursor++;
            count++;
        }
    }

    return count;
}
