import { v3_array_copy } from "../../../core/geom/vec3/v3_array_copy.js";
import { v3_cross } from "../../../core/geom/vec3/v3_cross.js";
import { v3_dot } from "../../../core/geom/vec3/v3_dot.js";
import { v3_length_sqr } from "../../../core/geom/vec3/v3_length_sqr.js";
import { v3_subtract } from "../../../core/geom/vec3/v3_subtract.js";
import { v3_triple_cross_product } from "../../../core/geom/vec3/v3_triple_cross_product.js";

/**
 * Robust GJK + EPA penetration for convex shapes — the narrowphase's
 * convex-fallback and concave-per-triangle penetration query. Replaced the
 * `gjk.js` `gjk_with_axis` + `expanding_polytope_algorithm.js` pair, which
 * returned a non-minimal axis for polytopes because its GJK handed EPA a
 * degenerate (origin-on-face) simplex (see memory: feedback_epa_unreliable_polytopes).
 *
 * Structure follows Bullet's btGjkEpa2: GJK produces a rank-4 simplex that
 * strictly encloses the origin; EPA builds outward-oriented faces from it and
 * expands toward the closest face, rebuilding the horizon each step. Operates on
 * any support provider with `support(out, off, dx, dy, dz)` in WORLD space (e.g.
 * {@link PosedShape}); the penetration normal + depth come from the Minkowski
 * difference's closest face to the origin. No cross-frame state → reset-and-
 * resimulate determinism preserved.
 *
 * Allocation-free hot path: the GJK simplex lives in a flat `Float64Array(12)`
 * and the EPA polytope in pooled flat typed arrays (vertices, face vertex-indices,
 * face normals, face distances, horizon edges). All vector math goes through
 * `v3_*` out-parameter helpers writing into module scratch — no per-call arrays
 * or face objects. The pools are module-level scratch reused across calls; the
 * engine is single-threaded and GJK/EPA is never re-entered, matching the rest
 * of the physics core's scratch convention.
 *
 * @author Alex Goldring
 * @copyright Company Named Limited (c) 2026
 */

const GJK_MAX_ITER = 64;
const EPA_MAX_ITER = 64;
const EPA_EPS = 1e-8;
const DIR_EPS = 1e-18;

// EPA pool capacities. EPA adds at most one vertex per iteration, so vertices are
// bounded by 4 + EPA_MAX_ITER (= 68), faces by Euler's 2V−4 (= 132) and horizon
// edges by 3V−6 (= 198). These caps clear those bounds with margin, so for the
// convex inputs the narrowphase feeds they are never reached; the guards below
// degrade to "use the current best face" purely as a defence against degenerate
// input rather than growing the pools.
const VERT_CAP = 128;
const FACE_CAP = 256;
const EDGE_CAP = 256;

// --- support scratch ---
const _sA = new Float64Array(3);
const _sB = new Float64Array(3);
const _w = new Float64Array(3); // newly sampled support point

// --- GJK simplex + search direction ---
const SIMPLEX = new Float64Array(12); // 4 points, 3 floats each; slot 0 = newest (a)
const DIR = new Float64Array(3);

// --- vector scratch (shared; GJK and EPA never run concurrently) ---
const _ab = new Float64Array(3);
const _ac = new Float64Array(3);
const _ad = new Float64Array(3);
const _abc = new Float64Array(3);
const _acd = new Float64Array(3);
const _adb = new Float64Array(3);
const _t = new Float64Array(3);

// --- EPA polytope pools ---
const VERTS = new Float64Array(VERT_CAP * 3);
const FACE_IDX = new Int32Array(FACE_CAP * 3); // vertex indices, 3 per face
const FACE_N = new Float64Array(FACE_CAP * 3); // outward unit normal, 3 per face
const FACE_DIST = new Float64Array(FACE_CAP); // perpendicular distance from origin
const EDGE = new Int32Array(EDGE_CAP * 2); // horizon edges, 2 vertex indices each
const CEN = new Float64Array(3); // polytope centroid (for outward orientation)

/**
 * A convex shape exposing a world-space support mapping: `support(out, off, dx,
 * dy, dz)` writes the farthest point of the shape in direction `(dx, dy, dz)`
 * into `out[off..off+2]` (e.g. {@link PosedShape}).
 *
 * @typedef {{ support: function(Float64Array, number, number, number, number): void }} SupportProvider
 */

/**
 * Minkowski-difference support written to `out[off..off+2]`:
 * support_A(d̂) − support_B(−d̂), world space.
 *
 * The direction is normalised first: the support contract assumes a UNIT vector
 * — `SphereShape3D`/`CapsuleShape3D` scale the raw direction by their radius
 * with no internal renormalisation — but GJK/EPA hand us arbitrary-magnitude
 * search directions (the seed `−s` and the cross/triple products from the
 * simplex evolution). Polytope supports are scale-invariant (vertex argmax), so
 * this only matters for curved providers, but it must hold for them. Matches the
 * sibling `minkowski_support.js`, which normalises for the same reason.
 *
 * @param {Float64Array|number[]} out destination for the CSO point
 * @param {number} off offset into `out`
 * @param {SupportProvider} A first support provider
 * @param {SupportProvider} B second support provider
 * @param {number} dx search-direction x (need not be unit length)
 * @param {number} dy search-direction y
 * @param {number} dz search-direction z
 * @returns {void}
 */
function cso_support(out, off, A, B, dx, dy, dz) {
    const len2 = dx * dx + dy * dy + dz * dz;
    if (len2 > 1e-30) {
        const inv = 1 / Math.sqrt(len2);
        dx *= inv;
        dy *= inv;
        dz *= inv;
    }
    A.support(_sA, 0, dx, dy, dz);
    B.support(_sB, 0, -dx, -dy, -dz);
    out[off] = _sA[0] - _sB[0];
    out[off + 1] = _sA[1] - _sB[1];
    out[off + 2] = _sA[2] - _sB[2];
}

/**
 * GJK intersection. Fills {@link SIMPLEX} with a rank-4 simplex enclosing the
 * origin and returns 4 on overlap; returns 0 if the shapes are separated.
 * `SIMPLEX` slot 0 is the most-recent point throughout (Muratori convention).
 *
 * @param {SupportProvider} A first support provider
 * @param {SupportProvider} B second support provider
 * @returns {number} 4 if the origin is enclosed (shapes overlap), 0 if separated
 */
function gjk(A, B) {
    cso_support(SIMPLEX, 0, A, B, 1, 0, 0);
    if (v3_length_sqr(SIMPLEX[0], SIMPLEX[1], SIMPLEX[2]) < DIR_EPS) {
        cso_support(SIMPLEX, 0, A, B, 0, 1, 0);
    }
    DIR[0] = -SIMPLEX[0];
    DIR[1] = -SIMPLEX[1];
    DIR[2] = -SIMPLEX[2];
    let count = 1;

    for (let iter = 0; iter < GJK_MAX_ITER; iter++) {
        if (v3_length_sqr(DIR[0], DIR[1], DIR[2]) < DIR_EPS) {
            return 0; // search direction collapsed → touching/separated
        }
        cso_support(_w, 0, A, B, DIR[0], DIR[1], DIR[2]);
        if (v3_dot(_w[0], _w[1], _w[2], DIR[0], DIR[1], DIR[2]) < 0) {
            return 0; // farthest point short of the origin → separated
        }
        // push the new point to slot 0, shifting the existing points down one slot
        // (copyWithin is memmove-style: the overlapping forward copy is well-defined)
        SIMPLEX.copyWithin(3, 0, count * 3);
        SIMPLEX[0] = _w[0];
        SIMPLEX[1] = _w[1];
        SIMPLEX[2] = _w[2];
        count++;

        count = do_simplex(count);
        if (count === 0) {
            return 4; // origin enclosed by the tetrahedron
        }
    }
    return 0;
}

/**
 * Evolve the simplex toward the origin. Reorders {@link SIMPLEX} in place and
 * writes the next search direction into {@link DIR}. Returns the new simplex size,
 * or 0 when a tetrahedron encloses the origin.
 *
 * @param {number} count current simplex size (2, 3, or 4)
 * @returns {number} the new simplex size (1–3), or 0 when the origin is enclosed
 */
function do_simplex(count) {
    if (count === 2) {
        return line_case();
    }
    if (count === 3) {
        return triangle_case();
    }
    return tetra_case();
}

/**
 * Line (2-point) simplex sub-case: pick the Voronoi region of segment [a, b] the
 * origin projects into and set the next search direction in {@link DIR}.
 *
 * @returns {number} the new simplex size (1 keeps [a], 2 keeps [a, b])
 */
function line_case() {
    const ax = SIMPLEX[0], ay = SIMPLEX[1], az = SIMPLEX[2];
    const bx = SIMPLEX[3], by = SIMPLEX[4], bz = SIMPLEX[5];
    v3_subtract(_ab, 0, bx, by, bz, ax, ay, az); // ab = b − a
    const aox = -ax, aoy = -ay, aoz = -az; // ao = −a

    if (v3_dot(_ab[0], _ab[1], _ab[2], aox, aoy, aoz) > 0) {
        v3_triple_cross_product(DIR, 0, _ab[0], _ab[1], _ab[2], aox, aoy, aoz); // (ab×ao)×ab
        if (v3_length_sqr(DIR[0], DIR[1], DIR[2]) < DIR_EPS) {
            // origin on the line — pick any perpendicular to ab
            if (Math.abs(_ab[0]) < 0.9) {
                v3_cross(DIR, 0, _ab[0], _ab[1], _ab[2], 1, 0, 0);
            } else {
                v3_cross(DIR, 0, _ab[0], _ab[1], _ab[2], 0, 1, 0);
            }
        }
        return 2; // keep [a, b]
    }
    DIR[0] = aox;
    DIR[1] = aoy;
    DIR[2] = aoz;
    return 1; // keep [a]
}

/**
 * Triangle (3-point) simplex sub-case: classify the origin against triangle
 * [a, b, c]'s edges and faces, reorder {@link SIMPLEX}, and set the next search
 * direction in {@link DIR}.
 *
 * @returns {number} the new simplex size (1, 2, or 3)
 */
function triangle_case() {
    const ax = SIMPLEX[0], ay = SIMPLEX[1], az = SIMPLEX[2];
    const bx = SIMPLEX[3], by = SIMPLEX[4], bz = SIMPLEX[5];
    const cx = SIMPLEX[6], cy = SIMPLEX[7], cz = SIMPLEX[8];
    v3_subtract(_ab, 0, bx, by, bz, ax, ay, az);
    v3_subtract(_ac, 0, cx, cy, cz, ax, ay, az);
    const aox = -ax, aoy = -ay, aoz = -az;
    v3_cross(_abc, 0, _ab[0], _ab[1], _ab[2], _ac[0], _ac[1], _ac[2]); // abc = ab×ac

    // outside edge ac?  (abc×ac) · ao > 0
    v3_cross(_t, 0, _abc[0], _abc[1], _abc[2], _ac[0], _ac[1], _ac[2]);
    if (v3_dot(_t[0], _t[1], _t[2], aox, aoy, aoz) > 0) {
        if (v3_dot(_ac[0], _ac[1], _ac[2], aox, aoy, aoz) > 0) {
            // keep [a, c]: move c into slot 1
            v3_array_copy(SIMPLEX, 3, SIMPLEX, 6);
            v3_triple_cross_product(DIR, 0, _ac[0], _ac[1], _ac[2], aox, aoy, aoz); // (ac×ao)×ac
            return 2;
        }
        return star_case();
    }

    // outside edge ab?  (ab×abc) · ao > 0
    v3_cross(_t, 0, _ab[0], _ab[1], _ab[2], _abc[0], _abc[1], _abc[2]);
    if (v3_dot(_t[0], _t[1], _t[2], aox, aoy, aoz) > 0) {
        return star_case();
    }

    // origin within the triangle's prism — above or below the face
    if (v3_dot(_abc[0], _abc[1], _abc[2], aox, aoy, aoz) > 0) {
        DIR[0] = _abc[0];
        DIR[1] = _abc[1];
        DIR[2] = _abc[2];
    } else {
        // below: reorder to [a, c, b] so the winding faces the origin; DIR = −abc
        SIMPLEX[3] = cx;
        SIMPLEX[4] = cy;
        SIMPLEX[5] = cz;
        SIMPLEX[6] = bx;
        SIMPLEX[7] = by;
        SIMPLEX[8] = bz;
        DIR[0] = -_abc[0];
        DIR[1] = -_abc[1];
        DIR[2] = -_abc[2];
    }
    return 3; // keep the triangle
}

/**
 * Edge region shared by the two triangle edges that fold back toward [a]: keep
 * segment [a, b] or vertex [a] and set the next search direction in {@link DIR}.
 *
 * @returns {number} the new simplex size (1 keeps [a], 2 keeps [a, b])
 */
function star_case() {
    const ax = SIMPLEX[0], ay = SIMPLEX[1], az = SIMPLEX[2];
    const bx = SIMPLEX[3], by = SIMPLEX[4], bz = SIMPLEX[5];
    v3_subtract(_ab, 0, bx, by, bz, ax, ay, az);
    const aox = -ax, aoy = -ay, aoz = -az;

    if (v3_dot(_ab[0], _ab[1], _ab[2], aox, aoy, aoz) > 0) {
        v3_triple_cross_product(DIR, 0, _ab[0], _ab[1], _ab[2], aox, aoy, aoz);
        return 2; // keep [a, b]
    }
    DIR[0] = aox;
    DIR[1] = aoy;
    DIR[2] = aoz;
    return 1; // keep [a]
}

/**
 * Tetrahedron (4-point) simplex sub-case: if the origin lies outside one of the
 * three faces incident to [a], reorder {@link SIMPLEX} to that triangle and
 * recurse into {@link triangle_case}; otherwise the origin is enclosed.
 *
 * @returns {number} the new simplex size (1–3), or 0 when the origin is enclosed
 */
function tetra_case() {
    const ax = SIMPLEX[0], ay = SIMPLEX[1], az = SIMPLEX[2];
    const bx = SIMPLEX[3], by = SIMPLEX[4], bz = SIMPLEX[5];
    const cx = SIMPLEX[6], cy = SIMPLEX[7], cz = SIMPLEX[8];
    const dx = SIMPLEX[9], dy = SIMPLEX[10], dz = SIMPLEX[11];
    v3_subtract(_ab, 0, bx, by, bz, ax, ay, az);
    v3_subtract(_ac, 0, cx, cy, cz, ax, ay, az);
    v3_subtract(_ad, 0, dx, dy, dz, ax, ay, az);
    const aox = -ax, aoy = -ay, aoz = -az;
    v3_cross(_abc, 0, _ab[0], _ab[1], _ab[2], _ac[0], _ac[1], _ac[2]);
    v3_cross(_acd, 0, _ac[0], _ac[1], _ac[2], _ad[0], _ad[1], _ad[2]);
    v3_cross(_adb, 0, _ad[0], _ad[1], _ad[2], _ab[0], _ab[1], _ab[2]);

    if (v3_dot(_abc[0], _abc[1], _abc[2], aox, aoy, aoz) > 0) {
        // outside face abc → keep [a, b, c] (slots already correct)
        return triangle_case();
    }
    if (v3_dot(_acd[0], _acd[1], _acd[2], aox, aoy, aoz) > 0) {
        // outside face acd → reorder to [a, c, d]
        SIMPLEX[3] = cx;
        SIMPLEX[4] = cy;
        SIMPLEX[5] = cz;
        SIMPLEX[6] = dx;
        SIMPLEX[7] = dy;
        SIMPLEX[8] = dz;
        return triangle_case();
    }
    if (v3_dot(_adb[0], _adb[1], _adb[2], aox, aoy, aoz) > 0) {
        // outside face adb → reorder to [a, d, b]
        SIMPLEX[3] = dx;
        SIMPLEX[4] = dy;
        SIMPLEX[5] = dz;
        SIMPLEX[6] = bx;
        SIMPLEX[7] = by;
        SIMPLEX[8] = bz;
        return triangle_case();
    }
    return 0; // origin inside the tetrahedron
}

/**
 * Append a face spanning vertices (i, j, k) with an OUTWARD unit normal (away
 * from the polytope centroid {@link CEN}); winding is flipped if needed to keep
 * the normal outward.
 *
 * @param {number} i first vertex index into {@link VERTS}
 * @param {number} j second vertex index into {@link VERTS}
 * @param {number} k third vertex index into {@link VERTS}
 * @param {number} face_count current number of faces (the new face's slot)
 * @returns {number} the new face count
 */
function add_face(i, j, k, face_count) {
    const ax = VERTS[i * 3], ay = VERTS[i * 3 + 1], az = VERTS[i * 3 + 2];
    v3_subtract(_ab, 0, VERTS[j * 3], VERTS[j * 3 + 1], VERTS[j * 3 + 2], ax, ay, az);
    v3_subtract(_ac, 0, VERTS[k * 3], VERTS[k * 3 + 1], VERTS[k * 3 + 2], ax, ay, az);
    v3_cross(_t, 0, _ab[0], _ab[1], _ab[2], _ac[0], _ac[1], _ac[2]);

    let nx = _t[0], ny = _t[1], nz = _t[2];
    const len = Math.sqrt(nx * nx + ny * ny + nz * nz) || 1;
    const inv = 1 / len;
    nx *= inv;
    ny *= inv;
    nz *= inv;

    // orient outward: the normal must point away from the interior (centroid)
    if (v3_dot(nx, ny, nz, ax - CEN[0], ay - CEN[1], az - CEN[2]) < 0) {
        nx = -nx;
        ny = -ny;
        nz = -nz;
        const swap = j;
        j = k;
        k = swap;
    }

    const fb = face_count * 3;
    FACE_IDX[fb] = i;
    FACE_IDX[fb + 1] = j;
    FACE_IDX[fb + 2] = k;
    FACE_N[fb] = nx;
    FACE_N[fb + 1] = ny;
    FACE_N[fb + 2] = nz;
    FACE_DIST[face_count] = v3_dot(nx, ny, nz, ax, ay, az); // ≥ 0 once outward
    return face_count + 1;
}

/**
 * Add a horizon edge (i, j) with cancellation: if the reverse edge (j, i) is
 * already present, both are interior to the visible region and drop out.
 * Order is irrelevant to the horizon, so removal is an O(1) swap-with-last.
 *
 * @param {number} edge_count current number of horizon edges in {@link EDGE}
 * @param {number} i edge start (vertex index)
 * @param {number} j edge end (vertex index)
 * @returns {number} the new edge count
 */
function add_edge(edge_count, i, j) {
    for (let k = 0; k < edge_count; k++) {
        if (EDGE[k * 2] === j && EDGE[k * 2 + 1] === i) {
            const last = edge_count - 1;
            EDGE[k * 2] = EDGE[last * 2];
            EDGE[k * 2 + 1] = EDGE[last * 2 + 1];
            return last;
        }
    }
    if (edge_count >= EDGE_CAP) {
        return edge_count; // horizon pool full (unreachable for convex inputs)
    }
    EDGE[edge_count * 2] = i;
    EDGE[edge_count * 2 + 1] = j;
    return edge_count + 1;
}

/**
 * Copy a face record (vertex indices, normal, distance) from slot `src` to slot
 * `dst` — used to compact surviving faces over removed ones.
 *
 * @param {number} dst destination face slot
 * @param {number} src source face slot
 * @returns {void}
 */
function copy_face(dst, src) {
    const db = dst * 3, sb = src * 3;
    FACE_IDX[db] = FACE_IDX[sb];
    FACE_IDX[db + 1] = FACE_IDX[sb + 1];
    FACE_IDX[db + 2] = FACE_IDX[sb + 2];
    FACE_N[db] = FACE_N[sb];
    FACE_N[db + 1] = FACE_N[sb + 1];
    FACE_N[db + 2] = FACE_N[sb + 2];
    FACE_DIST[dst] = FACE_DIST[src];
}

/**
 * EPA over the Minkowski difference, seeded by the rank-4 enclosing simplex left
 * in {@link SIMPLEX} by {@link gjk}. Writes the unit penetration normal (B→A)
 * into `out` and returns the depth.
 *
 * @param {SupportProvider} A first support provider
 * @param {SupportProvider} B second support provider
 * @param {Float64Array|number[]} out destination for the unit penetration normal (B→A)
 * @returns {number} penetration depth, or 0 if the polytope degenerated
 */
function epa(A, B, out) {
    // seed the vertex pool with the simplex points
    v3_array_copy(VERTS, 0, SIMPLEX, 0);
    v3_array_copy(VERTS, 3, SIMPLEX, 3);
    v3_array_copy(VERTS, 6, SIMPLEX, 6);
    v3_array_copy(VERTS, 9, SIMPLEX, 9);
    let vert_count = 4;

    CEN[0] = (VERTS[0] + VERTS[3] + VERTS[6] + VERTS[9]) / 4;
    CEN[1] = (VERTS[1] + VERTS[4] + VERTS[7] + VERTS[10]) / 4;
    CEN[2] = (VERTS[2] + VERTS[5] + VERTS[8] + VERTS[11]) / 4;

    let face_count = 0;
    face_count = add_face(0, 1, 2, face_count);
    face_count = add_face(0, 2, 3, face_count);
    face_count = add_face(0, 3, 1, face_count);
    face_count = add_face(1, 3, 2, face_count);

    for (let iter = 0; iter < EPA_MAX_ITER; iter++) {
        // closest face to the origin
        let best = 0;
        for (let f = 1; f < face_count; f++) {
            if (FACE_DIST[f] < FACE_DIST[best]) {
                best = f;
            }
        }

        const bn = best * 3;
        cso_support(_w, 0, A, B, FACE_N[bn], FACE_N[bn + 1], FACE_N[bn + 2]);
        const wdist = v3_dot(FACE_N[bn], FACE_N[bn + 1], FACE_N[bn + 2], _w[0], _w[1], _w[2]) - FACE_DIST[best];
        if (wdist < EPA_EPS) {
            break; // converged onto the closest face
        }
        if (vert_count >= VERT_CAP) {
            break; // vertex pool full (unreachable: EPA_MAX_ITER caps vertices at 68)
        }

        const wi = vert_count;
        VERTS[wi * 3] = _w[0];
        VERTS[wi * 3 + 1] = _w[1];
        VERTS[wi * 3 + 2] = _w[2];
        vert_count++;

        // remove faces visible from w, compacting the survivors; collect the
        // horizon (edges on exactly one removed face) with cancellation
        let edge_count = 0;
        let kept = 0;
        for (let f = 0; f < face_count; f++) {
            const fn = f * 3;
            if (v3_dot(FACE_N[fn], FACE_N[fn + 1], FACE_N[fn + 2], _w[0], _w[1], _w[2]) - FACE_DIST[f] > EPA_EPS) {
                const i0 = FACE_IDX[fn], i1 = FACE_IDX[fn + 1], i2 = FACE_IDX[fn + 2];
                edge_count = add_edge(edge_count, i0, i1);
                edge_count = add_edge(edge_count, i1, i2);
                edge_count = add_edge(edge_count, i2, i0);
            } else {
                if (kept !== f) {
                    copy_face(kept, f);
                }
                kept++;
            }
        }
        face_count = kept;

        for (let e = 0; e < edge_count; e++) {
            if (face_count >= FACE_CAP) {
                break; // face pool full (unreachable for convex inputs)
            }
            face_count = add_face(EDGE[e * 2], EDGE[e * 2 + 1], wi, face_count);
        }
        if (face_count === 0) {
            break; // degenerate polytope (all faces visible + horizon cancelled)
        }
    }

    if (face_count === 0) {
        // Degenerate: no face survived. Report no usable axis (the caller treats a
        // non-positive depth as a miss) rather than reading a stale face and
        // throwing out of the narrowphase step.
        out[0] = 0;
        out[1] = 0;
        out[2] = 0;
        return 0;
    }

    let best = 0;
    for (let f = 1; f < face_count; f++) {
        if (FACE_DIST[f] < FACE_DIST[best]) {
            best = f;
        }
    }
    const bn = best * 3;
    out[0] = FACE_N[bn];
    out[1] = FACE_N[bn + 1];
    out[2] = FACE_N[bn + 2];
    return FACE_DIST[best];
}

/**
 * Penetration depth + normal between two convex support providers at world pose.
 * Writes the unit separation normal (B→A) into `out` and returns the depth
 * (positive on overlap, 0 if separated — `out` untouched on 0).
 *
 * @param {Float64Array|number[]} out destination for the unit normal, length ≥ 3
 * @param {SupportProvider} A world-space support provider (e.g. PosedShape)
 * @param {SupportProvider} B world-space support provider (e.g. PosedShape)
 * @returns {number} penetration depth, or 0 if separated
 */
export function gjk_epa_penetration(out, A, B) {
    if (gjk(A, B) < 4) {
        return 0;
    }
    const depth = epa(A, B, out);
    return depth > 0 && Number.isFinite(depth) ? depth : 0;
}
