import { bvh_query_user_data_overlaps_aabb } from "../../../core/bvh2/bvh3/query/bvh_query_user_data_overlaps_aabb.js";
import { aabb3_transform_oriented } from "../../../core/geom/3d/aabb/aabb3_transform_oriented.js";
import { aabb_world_to_local } from "./decomposition/aabb_world_to_local.js";
import { build_convex_decomposition } from "./convex_decomposition.js";
import { convex_convex_manifold, CONVEX_CONVEX_OUT_LENGTH } from "./convex_convex_manifold.js";

/**
 * Contact generation between two triangulated meshes, each treated as the union
 * of its CONVEX PIECES (from greedy tet-merge — see {@link build_convex_decomposition}).
 * Concave-vs-concave reduced to convex piece-vs-piece, the standard
 * convex-decomposition approach.
 *
 * Only piece pairs whose AABBs overlap reach the SAT + face-clip manifold:
 * project mesh B's world AABB into A's local frame and query A's piece BVH; for
 * each candidate piece of A, project its world AABB into B's local frame and
 * query B's piece BVH. One top-level broadphase leaf per body; the per-shape
 * piece BVHs do the culling. Merging keeps the piece count (and the per-pair
 * SAT cost) far below the raw tet count — the path to large tet counts.
 *
 * The decomposition is built once per shape and cached (shapes are immutable,
 * shared, and the decomposition is in the shape's local frame). Per-frame, no
 * cross-frame state → reset-and-resimulate determinism preserved.
 *
 * @author Alex Goldring
 * @copyright Company Named Limited (c) 2026
 */

/** Stop emitting once the candidate buffer is nearly full (capped at 4
 *  downstream); a localised resting contact never approaches this. */
const MAX_EMIT = 60;

const piece_out = new Float64Array(CONVEX_CONVEX_OUT_LENGTH);
let cand_a = new Uint32Array(4096);
let cand_b = new Uint32Array(4096);
const local_bbox = new Float64Array(6);
const world_aabb = new Float64Array(6);
const query_aabb = new Float64Array(6);

/** Build + cache the convex decomposition on the (immutable, shared) shape. */
function get_decomposition(shape) {
    let d = shape.__convex_decomposition;
    if (d === undefined) {
        d = build_convex_decomposition(shape);
        shape.__convex_decomposition = d;
    }
    return d;
}

function ensure(buf, n) { return buf.length >= n ? buf : new Uint32Array(n); }

/**
 * Append the mesh-vs-mesh contacts via `append`, the narrowphase's
 * `append_contact(count, wax,way,waz, wbx,wby,wbz, nx,ny,nz, depth, fid) → newCount`.
 *
 * @returns {number} new candidate count
 */
export function mesh_mesh_tet_contacts(count, shapeA, trA, shapeB, trB, append) {
    const decompA = get_decomposition(shapeA);
    const decompB = get_decomposition(shapeB);
    const piecesA = decompA.pieces;
    const piecesB = decompB.pieces;

    // Candidate pieces of A: those whose AABB overlaps B's world AABB (in A-local).
    shapeB.compute_bounding_box(local_bbox);
    aabb3_transform_oriented(
        world_aabb, 0,
        local_bbox[0], local_bbox[1], local_bbox[2], local_bbox[3], local_bbox[4], local_bbox[5],
        trB.position.x, trB.position.y, trB.position.z,
        trB.rotation.x, trB.rotation.y, trB.rotation.z, trB.rotation.w
    );
    aabb_world_to_local(
        query_aabb, 0, world_aabb,
        trA.position.x, trA.position.y, trA.position.z,
        trA.rotation.x, trA.rotation.y, trA.rotation.z, trA.rotation.w
    );

    cand_a = ensure(cand_a, piecesA.length);
    const nA = bvh_query_user_data_overlaps_aabb(cand_a, 0, decompA.bvh, query_aabb);

    for (let i = 0; i < nA; i++) {
        const pieceA = piecesA[cand_a[i]];

        // Candidate pieces of B near this piece of A.
        const ab = pieceA.aabb;
        aabb3_transform_oriented(
            world_aabb, 0,
            ab[0], ab[1], ab[2], ab[3], ab[4], ab[5],
            trA.position.x, trA.position.y, trA.position.z,
            trA.rotation.x, trA.rotation.y, trA.rotation.z, trA.rotation.w
        );
        aabb_world_to_local(
            query_aabb, 0, world_aabb,
            trB.position.x, trB.position.y, trB.position.z,
            trB.rotation.x, trB.rotation.y, trB.rotation.z, trB.rotation.w
        );
        cand_b = ensure(cand_b, piecesB.length);
        const nB = bvh_query_user_data_overlaps_aabb(cand_b, 0, decompB.bvh, query_aabb);

        for (let j = 0; j < nB; j++) {
            const pieceB = piecesB[cand_b[j]];
            if (!convex_convex_manifold(piece_out, pieceA, trA.position, trA.rotation, pieceB, trB.position, trB.rotation)) {
                continue;
            }
            const cc = piece_out[3] | 0;
            const nx = piece_out[0], ny = piece_out[1], nz = piece_out[2];
            for (let k = 0; k < cc; k++) {
                const base = 4 + k * 7;
                count = append(count,
                    piece_out[base], piece_out[base + 1], piece_out[base + 2],
                    piece_out[base + 3], piece_out[base + 4], piece_out[base + 5],
                    nx, ny, nz, piece_out[base + 6], 0);
            }
            if (count >= MAX_EMIT) return count;
        }
    }
    return count;
}
