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 { body_id_index } from "../body/BodyStorage.js";
import { gjk } from "../gjk/gjk.js";
import { mpr } from "../gjk/mpr.js";
import { PosedShape } from "../narrowphase/PosedShape.js";

/**
 * Bisection halts when the search interval is below this fraction of
 * the sweep length. With `MAX_BISECTION = 32` and a unit-length sweep
 * the achieved precision is ≈ 1 / 2³² (well below any practical
 * world-scale tolerance), but we cap on relative tolerance so a
 * 10 m sweep doesn't insist on sub-micron precision.
 * @type {number}
 */
const TOI_TOLERANCE = 1e-4;

/**
 * Maximum bisection iterations. `log2(1 / TOI_TOLERANCE)` is enough
 * for any practical sweep; this is the hard upper bound to prevent
 * pathological inputs (NaN ray, etc.) from hanging the query.
 * @type {number}
 */
const MAX_BISECTION = 32;

/**
 * Coarse linear-search resolution for finding *any* overlap moment
 * along the per-candidate AABB-overlap interval (NOT the full sweep —
 * see `aabb_overlap_interval` for the analytical narrowing that runs
 * upstream). 32 substeps over the AABB-overlap window is dense enough
 * for any sweep where the shape's smallest extent is on the order of
 * its AABB's extent (true for spheres / boxes / capsules); the slab
 * intersection keeps that interval small even for kilometre-scale
 * sweeps, so this isn't the limit it would otherwise be.
 * @type {number}
 */
const COARSE_STEPS = 32;

const swept_aabb = new Float64Array(6);
const local_aabb = new Float64Array(6);
const initial_aabb = new Float64Array(6);
const target_local_aabb = new Float64Array(6);
const target_world_aabb = new Float64Array(6);

// GJK simplex buffer (used to detect overlap during the bisection).
const simplex_buf = new Float64Array(12);

// Scratch for MPR's MTV output — same vec3 convention as EPA: direction
// is "A's interior into B" with the magnitude being the depth. We use
// MPR for the contact-normal recovery at the kiss point because it
// converges reliably on smooth-vs-smooth supports where EPA hits its
// iteration cap before tightening (sphere-vs-sphere shallow overlap).
const mpr_result = new Float64Array(3);

/**
 * Long-lived PosedShape adapters for the swept shape and the
 * candidate's collider. Both are re-bound per query — the swept one
 * also gets its position field re-written every bisection step
 * without re-running `setup()`, since shape and rotation are
 * constant through the sweep.
 */
const swept_posed = new PosedShape();
const candidate_posed = new PosedShape();

/**
 * Buffer for the union of static-BVH and dynamic-BVH candidates that
 * overlap the swept AABB. Grows by doubling on the rare overflow.
 * @type {Uint32Array}
 */
let scratch_candidates = new Uint32Array(64);

/**
 * Sweep a convex shape along a ray and find the first body it would
 * hit. Used by character controllers (sweep a capsule along intended
 * movement to find blockers), high-speed projectile movement (avoid
 * tunnelling without paying for full per-body CCD), and any kinematic
 * "where would this body end up if I tried to move it?" query.
 *
 * Pipeline:
 *   1. Build the swept AABB: the shape's local-frame AABB rotated
 *      into world by `rotation`, translated to `ray.origin`, then
 *      stretched along `ray.direction * ray.tMax`.
 *   2. Query both broadphase BVHs (static + dynamic) for leaves whose
 *      AABB overlaps the swept volume.
 *   3. For each candidate, bisect the [0, best_t] interval for the
 *      smallest `t` at which the swept shape overlaps the candidate's
 *      collider. GJK on each midpoint.
 *
 * Picking `best_t` (rather than always running the full [0, tMax]
 * bisection) is the early-termination optimisation: once we have a
 * hit at some t, no later candidate can produce a smaller TOI by
 * being checked at its full [0, tMax] interval.
 *
 * Output on hit:
 *   - `result.t` — sweep distance to impact (in the same units as
 *     `ray.direction`'s magnitude; for a unit-direction ray, this is
 *     metres).
 *   - `result.position` — `ray.origin + t * ray.direction`, i.e. the
 *     centre of the swept shape at the moment of first contact. NOT
 *     a point on the target's surface; that would need narrowphase
 *     refinement which the broadphase-only architecture doesn't have
 *     wired up yet.
 *   - `result.normal` — target surface's outward normal at the kiss
 *     point (unit length), computed by running GJK + EPA against the
 *     just-overlapping configuration at the TOI. Falls back to
 *     `-ray.direction` on the rare case EPA degenerates (NaN / zero
 *     depth) — see comment near the EPA call.
 *   - `result.entity` / `result.body_id` — the hit body.
 *
 * Output on miss: untouched; same convention as `raycast`.
 *
 * @param {PhysicsSystem} system
 * @param {Ray3} ray  origin + unit direction + `tMax`
 * @param {AbstractShape3D} shape  shape being swept, in its local frame
 * @param {{x:number,y:number,z:number,w:number}} rotation  fixed orientation
 * @param {PhysicsSurfacePoint} result  populated on hit; untouched on miss
 * @param {(entity:number, collider:Collider)=>boolean} [filter] mandatory in
 *   contract; defaults to {@link returnTrue}
 * @returns {boolean}
 */
export function shape_cast(system, ray, shape, rotation, result, filter = returnTrue) {
    const ox = ray.origin_x;
    const oy = ray.origin_y;
    const oz = ray.origin_z;
    const dx = ray.direction_x;
    const dy = ray.direction_y;
    const dz = ray.direction_z;
    const tMax = ray.tMax;
    if (!(tMax > 0) || !Number.isFinite(tMax)) return false;

    const rx = rotation.x, ry = rotation.y, rz = rotation.z, rw = rotation.w;

    // ── 1. Swept AABB ──────────────────────────────────────────────
    shape.compute_bounding_box(local_aabb);
    // Rotation around ray.origin gives the shape's initial AABB in
    // world space.
    aabb3_transform_oriented(
        initial_aabb, 0,
        local_aabb[0], local_aabb[1], local_aabb[2],
        local_aabb[3], local_aabb[4], local_aabb[5],
        ox, oy, oz,
        rx, ry, rz, rw
    );
    // Stretch by the sweep displacement. Positive direction components
    // push the +face out; negative components push the −face out.
    swept_aabb[0] = initial_aabb[0];
    swept_aabb[1] = initial_aabb[1];
    swept_aabb[2] = initial_aabb[2];
    swept_aabb[3] = initial_aabb[3];
    swept_aabb[4] = initial_aabb[4];
    swept_aabb[5] = initial_aabb[5];
    if (dx > 0) swept_aabb[3] += dx * tMax; else swept_aabb[0] += dx * tMax;
    if (dy > 0) swept_aabb[4] += dy * tMax; else swept_aabb[1] += dy * tMax;
    if (dz > 0) swept_aabb[5] += dz * tMax; else swept_aabb[2] += dz * tMax;

    // ── 2. Broadphase: gather candidates from both trees ───────────
    // The BVH query returns the TRUE overlap count and silently drops writes past
    // the buffer end. An undersized buffer would make the narrowphase loop read
    // `undefined` candidates and silently MISS the true nearest hit (tunnelling on
    // a long sweep through a dense scene). Grow by doubling and re-query until
    // both fit (deterministic queries → 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, swept_aabb);
        n_dynamic = bvh_query_user_data_overlaps_aabb(scratch_candidates, n_static, system.dynamicBvh, swept_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 false;

    // ── 3. Narrowphase: per-candidate bisection ────────────────────
    //
    // Re-bind the swept shape's PosedShape once for shape + rotation;
    // we update `px/py/pz` per bisection midpoint without rerunning
    // setup (px/py/pz are public fields by contract).
    swept_posed.shape = shape;
    swept_posed.qx = rx; swept_posed.qy = ry; swept_posed.qz = rz; swept_posed.qw = rw;

    let best_t = tMax;
    let best_body_id = -1;
    let best_entity = -1;

    for (let i = 0; i < n_total; i++) {
        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 target_tr = system.__transforms[body_idx];
        candidate_posed.setup(collider.shape, target_tr.position, target_tr.rotation);

        // Already overlapping at t = 0? Return immediately — can't beat
        // this, and the bisection's "no overlap at lo, overlap at hi"
        // invariant doesn't hold.
        swept_posed.px = ox; swept_posed.py = oy; swept_posed.pz = oz;
        if (gjk(simplex_buf, swept_posed, candidate_posed)) {
            best_t = 0;
            best_body_id = body_id;
            best_entity = entity;
            break;
        }

        // ── Slab intersection: narrow the search interval analytically ─
        //
        // Compute the body's tight world AABB and intersect it with the
        // swept shape's AABB sliding along the sweep direction. The
        // result is the t-interval `[t_aabb_enter, t_aabb_exit]` during
        // which the two AABBs overlap. Outside this interval the actual
        // shapes can't overlap either (AABB is a superset of the
        // shape), so we get to skip all of [0, t_aabb_enter) and
        // (t_aabb_exit, best_t] without testing.
        //
        // This is the key to making `shapeCast` scale to long sweeps:
        // a 1 km cast of a 1 m sphere against another 1 m sphere has an
        // AABB-overlap window of ≈ 2 m (the sum of diameters along the
        // sweep axis), reducing the coarse-step grid from a 30 m
        // resolution (1000/32) to 6 cm (2/32).
        collider.shape.compute_bounding_box(target_local_aabb);
        aabb3_transform_oriented(
            target_world_aabb, 0,
            target_local_aabb[0], target_local_aabb[1], target_local_aabb[2],
            target_local_aabb[3], target_local_aabb[4], target_local_aabb[5],
            target_tr.position.x, target_tr.position.y, target_tr.position.z,
            target_tr.rotation.x, target_tr.rotation.y, target_tr.rotation.z, target_tr.rotation.w
        );

        let t_aabb_enter = -Infinity;
        let t_aabb_exit = Infinity;
        let skip_candidate = false;
        for (let ax = 0; ax < 3; ax++) {
            const init_min = initial_aabb[ax];
            const init_max = initial_aabb[ax + 3];
            const body_min = target_world_aabb[ax];
            const body_max = target_world_aabb[ax + 3];
            const d_ax = ax === 0 ? dx : (ax === 1 ? dy : dz);
            if (d_ax === 0) {
                // Static along this axis: AABBs must overlap in their
                // initial position or this candidate is unreachable.
                if (init_max < body_min || init_min > body_max) {
                    skip_candidate = true;
                    break;
                }
                continue;
            }
            // swept_min(t) = init_min + d·t, swept_max(t) = init_max + d·t.
            // Slab-enter: swept_max(t) = body_min  →  t = (body_min − init_max) / d
            // Slab-exit:  swept_min(t) = body_max  →  t = (body_max − init_min) / d
            const t1 = (body_min - init_max) / d_ax;
            const t2 = (body_max - init_min) / d_ax;
            const lo = t1 < t2 ? t1 : t2;
            const hi = t1 < t2 ? t2 : t1;
            if (lo > t_aabb_enter) t_aabb_enter = lo;
            if (hi < t_aabb_exit)  t_aabb_exit  = hi;
        }
        if (skip_candidate) continue;
        if (t_aabb_enter > t_aabb_exit) continue;

        // Clamp to the relevant t-window: [0, best_t]. Anything beyond
        // best_t can't tighten the answer.
        const t_start = t_aabb_enter > 0     ? t_aabb_enter : 0;
        const t_end   = t_aabb_exit  < best_t ? t_aabb_exit  : best_t;
        if (t_start >= t_end) continue;

        // Coarse-search within the narrowed interval to find any t for
        // which the shapes overlap (their AABBs do by construction; the
        // shapes themselves may not, e.g. two spheres passing alongside).
        const step_dt = (t_end - t_start) / COARSE_STEPS;
        let t_prev = t_start;
        let t_hi = -1;
        for (let s = 1; s <= COARSE_STEPS; s++) {
            const t = t_start + s * step_dt;
            swept_posed.px = ox + dx * t;
            swept_posed.py = oy + dy * t;
            swept_posed.pz = oz + dz * t;
            if (gjk(simplex_buf, swept_posed, candidate_posed)) {
                t_hi = t;
                break;
            }
            t_prev = t;
        }
        if (t_hi < 0) continue;

        // Bisect [t_prev, t_hi]: invariant is `gjk(t_lo) === false` and
        // `gjk(t_hi) === true`.
        let t_lo = t_prev;
        const stop_width = TOI_TOLERANCE * tMax;
        for (let bi = 0; bi < MAX_BISECTION && (t_hi - t_lo) > stop_width; bi++) {
            const t_mid = (t_lo + t_hi) * 0.5;
            swept_posed.px = ox + dx * t_mid;
            swept_posed.py = oy + dy * t_mid;
            swept_posed.pz = oz + dz * t_mid;
            if (gjk(simplex_buf, swept_posed, candidate_posed)) {
                t_hi = t_mid;
            } else {
                t_lo = t_mid;
            }
        }

        // Use t_lo (the just-SEPARATING side of the bisection) rather
        // than t_hi (just-overlapping). With the fixed GJK that no
        // longer flags 5 mm-clear gaps as overlap, the bisection is
        // tight; reporting t_lo gives kinematic callers a conservative
        // TOI — the swept shape at the reported moment is provably NOT
        // overlapping the target. The normal-probe below steps a few
        // mm past best_t to get a valid simplex for MPR.
        if (t_lo < best_t) {
            best_t = t_lo;
            best_body_id = body_id;
            best_entity = entity;
        }
    }

    if (best_body_id === -1) return false;

    const rp = result.position;
    const rn = result.normal;

    const swept_x = ox + dx * best_t;
    const swept_y = oy + dy * best_t;
    const swept_z = oz + dz * best_t;

    rp[0] = swept_x;
    rp[1] = swept_y;
    rp[2] = swept_z;

    // ── Contact-normal recovery via MPR ────────────────────────────
    //
    // Bisection converges with `swept_posed` somewhere within
    // `stop_width = TOI_TOLERANCE * tMax` of the true contact. At
    // exactly `best_t` the overlap depth is therefore sub-millimetre
    // on practical sweeps. EPA on smooth-vs-smooth supports at this
    // overlap can't tighten before its iteration cap and returns a
    // direction off by ~30° from truth. MPR is the better tool here:
    // it converges in 5-15 iterations on smooth surfaces, same MTV
    // output convention as EPA (direction = A's interior into B,
    // magnitude = depth).
    //
    // We still advance the swept centre by NORMAL_PROBE_OFFSET past
    // best_t (a few mm into the target) before sampling — the extra
    // overlap gives both MPR and the body-centre sign check a more
    // robust geometric basis. The reported `result.t` stays at
    // `best_t`; only the normal probe moves.
    //
    // Sign-validation: MPR's polytope can also settle on either side
    // of the origin for highly symmetric configurations, so we dot
    // against the body-centre axis and flip if needed (same pattern
    // as `narrowphase_step`'s EPA path). The stored normal is the
    // target's outward surface normal — `B → A` direction — i.e. the
    // negated (validated) MTV.
    const NORMAL_PROBE_OFFSET = 0.01;
    const probe_t = best_t + NORMAL_PROBE_OFFSET;
    const probe_x = ox + dx * probe_t;
    const probe_y = oy + dy * probe_t;
    const probe_z = oz + dz * probe_t;
    const best_body_idx = body_id_index(best_body_id);
    const best_collider = system.__primary_collider(best_body_idx);
    let normal_x = -dx, normal_y = -dy, normal_z = -dz;
    if (best_collider !== null) {
        const best_tr = system.__transforms[best_body_idx];
        candidate_posed.setup(best_collider.shape, best_tr.position, best_tr.rotation);
        swept_posed.px = probe_x;
        swept_posed.py = probe_y;
        swept_posed.pz = probe_z;
        if (mpr(mpr_result, 0, swept_posed, candidate_posed)) {
            let ex = mpr_result[0], ey = mpr_result[1], ez = mpr_result[2];
            const depth = Math.sqrt(ex * ex + ey * ey + ez * ez);
            if (depth > 0 && Number.isFinite(depth)) {
                const ab_x = best_tr.position.x - probe_x;
                const ab_y = best_tr.position.y - probe_y;
                const ab_z = best_tr.position.z - probe_z;
                if (ex * ab_x + ey * ab_y + ez * ab_z < 0) {
                    ex = -ex; ey = -ey; ez = -ez;
                }
                const inv = 1 / depth;
                normal_x = -ex * inv;
                normal_y = -ey * inv;
                normal_z = -ez * inv;
            }
        }
    }

    rn[0] = normal_x; rn[1] = normal_y; rn[2] = normal_z;
    result.t = best_t;
    result.entity = best_entity;
    result.body_id = best_body_id;
    return true;
}
