import { v3_quat3_apply } from "../../../core/geom/vec3/v3_quat3_apply.js";
import { reduce_manifold_contacts } from "./reduce_manifold_contacts.js";

/**
 * Multi-point contact manifold between two convex polytopes with **polygon
 * faces**, computed entirely from the current poses — no cross-frame state, so
 * it preserves reset-and-resimulate (rollback) determinism.
 *
 * A piece is described by:
 *   - `vertices`     : Float32Array, flat (x,y,z) per vertex, local frame
 *   - `face_offsets` : Uint32Array length F+1, CSR offsets into `face_loops`
 *   - `face_loops`   : Uint32Array, each face's vertex indices, ordered CCW so
 *                      the face normal (from its first three loop verts) points
 *                      outward.
 * A triangle-faced piece (a tetrahedron) is just the special case where every
 * face loop has length 3; a merged/box piece has quads or larger polygons.
 * Handling polygon faces directly is what lets a flat face produce a full
 * face-on-face patch instead of mismatched triangle slivers.
 *
 * The separating axis comes from SAT (the structure mirrors Bullet's
 * `clipHullAgainstHull`, which clips around a `separatingNormal`). A GJK+EPA
 * axis source was prototyped — Bullet sources the normal from EITHER SAT OR
 * `btGjkPairDetector`, the source being orthogonal to the clip — but this
 * engine's EPA returns a non-minimal, non-scaling axis for polytopes (a 0.05 m
 * overlap reports ~0.8 m depth), so clipping around it produces a garbage
 * manifold. SAT is exact and, for the low-poly hulls authored as collision
 * proxies and the small pieces of a decomposed mesh, cheap. See memory:
 * feedback_epa_unreliable_polytopes.
 *
 * Algorithm (general form of {@link box_box_manifold}):
 *   1. SAT over every face normal of A, every face normal of B, and every
 *      edge(A)×edge(B) cross finds the minimum-translation axis `n` (B→A).
 *   2. Face-axis winner ⇒ reference face = the polygon that owns the axis;
 *      incident face = the polygon on the other piece most antiparallel to the
 *      reference outward normal. Clip the incident polygon against the reference
 *      polygon's side planes (3-D Sutherland–Hodgman) and keep penetrating points.
 *   3. Edge-axis winner ⇒ a single deepest support-pair contact.
 *   4. Reduce survivors to ≤ {@link MAX_CONTACTS} (deepest + spread).
 *
 * Output layout (matches {@link box_box_manifold}):
 *   out[0..2]            : world normal (B→A)
 *   out[3]               : contact count (0 .. MAX_CONTACTS)
 *   out[4 + k*7 + 0..2]  : world contact on A's surface
 *   out[4 + k*7 + 3..5]  : world contact on B's surface
 *   out[4 + k*7 + 6]     : penetration depth (positive)
 *
 * @author Alex Goldring
 * @copyright Company Named Limited (c) 2026
 */

const MAX_CONTACTS = 4;
const CONTACT_STRIDE = 7;
export const CONVEX_CONVEX_OUT_LENGTH = 4 + MAX_CONTACTS * CONTACT_STRIDE;

const INITIAL_V = 256; // initial world-vertex scratch capacity (verts per piece/hull)
const MAX_POLY = 64;   // polygon verts during clipping

const TIE_REL = 0.02;
const TIE_ABS = 1e-5;
const PARALLEL_EPS = 1e-9;

// --- scratch ----------------------------------------------------------------
// World-vertex buffers. `let` + grow-on-demand: a convex MESH routed as a single
// hull (get_mesh_convex_hull) can have far more than the initial capacity of
// surface vertices (a subdivided icosphere is hundreds). Without the grow, the
// excess writes were silently dropped and SAT/clip read NaN past the end,
// computing a truncated (wrong) manifold against part of the hull.
let wvA = new Float64Array(INITIAL_V * 3);
let wvB = new Float64Array(INITIAL_V * 3);
const poly_in = new Float64Array(MAX_POLY * 3);
const poly_out = new Float64Array(MAX_POLY * 3);
const cand = new Float64Array(MAX_POLY * 4);
const cenA = new Float64Array(3);
const cenB = new Float64Array(3);
const tmp_n = new Float64Array(3);

const sat = { overlap: Infinity, nx: 0, ny: 0, nz: 0, kind: -1, face: -1 };

// --- per-call geometry refs (single-thread re-entrancy, like the scratch) ---
let g_foA, g_flA, g_foB, g_flB, g_fA, g_fB, g_nA, g_nB;

function set_geom(A, B) {
    g_nA = A.vertices.length / 3; g_nB = B.vertices.length / 3;
    g_foA = A.face_offsets; g_flA = A.face_loops; g_fA = g_foA.length - 1;
    g_foB = B.face_offsets; g_flB = B.face_loops; g_fB = g_foB.length - 1;
    // Grow the world-vertex scratch to fit, so large hulls aren't truncated.
    if (wvA.length < g_nA * 3) wvA = new Float64Array(g_nA * 3);
    if (wvB.length < g_nB * 3) wvB = new Float64Array(g_nB * 3);
}

function load_world(out, verts, n, p, q) {
    for (let i = 0; i < n; i++) {
        v3_quat3_apply(out, i * 3, verts[i * 3], verts[i * 3 + 1], verts[i * 3 + 2], q.x, q.y, q.z, q.w);
        out[i * 3] += p.x; out[i * 3 + 1] += p.y; out[i * 3 + 2] += p.z;
    }
}

function centroid(out, wv, n) {
    let cx = 0, cy = 0, cz = 0;
    for (let i = 0; i < n; i++) { cx += wv[i * 3]; cy += wv[i * 3 + 1]; cz += wv[i * 3 + 2]; }
    out[0] = cx / n; out[1] = cy / n; out[2] = cz / n;
}

/**
 * Outward normal of a polygon face (Newell's method over the whole loop, world).
 * Robust against collinear consecutive vertices, unlike a first-three cross.
 */
function face_normal(out, wv, loops, off, end) {
    let nx = 0, ny = 0, nz = 0;
    for (let i = off; i < end; i++) {
        const p = loops[i] * 3, q = loops[i + 1 < end ? i + 1 : off] * 3;
        nx += (wv[p + 1] - wv[q + 1]) * (wv[p + 2] + wv[q + 2]);
        ny += (wv[p + 2] - wv[q + 2]) * (wv[p] + wv[q]);
        nz += (wv[p] - wv[q]) * (wv[p + 1] + wv[q + 1]);
    }
    out[0] = nx; out[1] = ny; out[2] = nz;
}

/** SAT axis test; true if the axis SEPARATES the pieces. */
function test_axis(ux, uy, uz, nA, nB, kind, face) {
    const len2 = ux * ux + uy * uy + uz * uz;
    if (len2 < PARALLEL_EPS) return false;
    const inv = 1 / Math.sqrt(len2);
    ux *= inv; uy *= inv; uz *= inv;

    let minA = Infinity, maxA = -Infinity;
    for (let i = 0; i < nA; i++) {
        const d = ux * wvA[i * 3] + uy * wvA[i * 3 + 1] + uz * wvA[i * 3 + 2];
        if (d < minA) minA = d; if (d > maxA) maxA = d;
    }
    let minB = Infinity, maxB = -Infinity;
    for (let i = 0; i < nB; i++) {
        const d = ux * wvB[i * 3] + uy * wvB[i * 3 + 1] + uz * wvB[i * 3 + 2];
        if (d < minB) minB = d; if (d > maxB) maxB = d;
    }
    if (maxA < minB || maxB < minA) return true;

    const overlap = Math.min(maxA, maxB) - Math.max(minA, minB);
    if (sat.kind === -1 || overlap < sat.overlap - (sat.overlap * TIE_REL + TIE_ABS)) {
        sat.overlap = overlap;
        const dcx = cenA[0] - cenB[0], dcy = cenA[1] - cenB[1], dcz = cenA[2] - cenB[2];
        const sign = (ux * dcx + uy * dcy + uz * dcz) >= 0 ? 1 : -1;
        sat.nx = ux * sign; sat.ny = uy * sign; sat.nz = uz * sign;
        sat.kind = kind; sat.face = face;
    }
    return false;
}

/**
 * SAT entry point — finds the minimum-translation axis itself, then clips.
 * For the SMALL convex pieces of a decomposed mesh.
 *
 * @param {Float64Array|number[]} out length ≥ CONVEX_CONVEX_OUT_LENGTH
 * @param {{vertices:Float32Array, face_offsets:Uint32Array, face_loops:Uint32Array}} A
 * @param {{x,y,z}} posA @param {{x,y,z,w}} rotA
 * @param {{vertices:Float32Array, face_offsets:Uint32Array, face_loops:Uint32Array}} B
 * @param {{x,y,z}} posB @param {{x,y,z,w}} rotB
 * @returns {boolean}
 */
export function convex_convex_manifold(out, A, posA, rotA, B, posB, rotB) {
    set_geom(A, B);

    load_world(wvA, A.vertices, g_nA, posA, rotA);
    load_world(wvB, B.vertices, g_nB, posB, rotB);
    centroid(cenA, wvA, g_nA);
    centroid(cenB, wvB, g_nB);

    sat.kind = -1; sat.overlap = Infinity;

    // A face normals.
    for (let f = 0; f < g_fA; f++) {
        face_normal(tmp_n, wvA, g_flA, g_foA[f], g_foA[f + 1]);
        if (test_axis(tmp_n[0], tmp_n[1], tmp_n[2], g_nA, g_nB, 0, f)) return false;
    }
    // B face normals.
    for (let f = 0; f < g_fB; f++) {
        face_normal(tmp_n, wvB, g_flB, g_foB[f], g_foB[f + 1]);
        if (test_axis(tmp_n[0], tmp_n[1], tmp_n[2], g_nA, g_nB, 1, f)) return false;
    }
    // Edge × edge crosses (face-loop edges; shared edges retest harmlessly).
    for (let fa = 0; fa < g_fA; fa++) {
        const sa = g_foA[fa], la = g_foA[fa + 1] - sa;
        for (let i = 0; i < la; i++) {
            const a0 = g_flA[sa + i] * 3, a1 = g_flA[sa + (i + 1) % la] * 3;
            const eax = wvA[a1] - wvA[a0], eay = wvA[a1 + 1] - wvA[a0 + 1], eaz = wvA[a1 + 2] - wvA[a0 + 2];
            for (let fb = 0; fb < g_fB; fb++) {
                const sb = g_foB[fb], lb = g_foB[fb + 1] - sb;
                for (let j = 0; j < lb; j++) {
                    const b0 = g_flB[sb + j] * 3, b1 = g_flB[sb + (j + 1) % lb] * 3;
                    const ebx = wvB[b1] - wvB[b0], eby = wvB[b1 + 1] - wvB[b0 + 1], ebz = wvB[b1 + 2] - wvB[b0 + 2];
                    const cx = eay * ebz - eaz * eby, cy = eaz * ebx - eax * ebz, cz = eax * eby - eay * ebx;
                    if (test_axis(cx, cy, cz, g_nA, g_nB, 2, -1)) return false;
                }
            }
        }
    }

    if (sat.kind === -1) return false;

    const nx = sat.nx, ny = sat.ny, nz = sat.nz;
    out[0] = nx; out[1] = ny; out[2] = nz;

    // Edge-cross winner: single deepest support-pair contact (v1).
    if (sat.kind === 2) {
        emit_single_contact(out, nx, ny, nz, sat.overlap);
        return true;
    }

    // Face winner: reference / incident polygon clipping.
    emit_face_manifold(out, sat.kind === 0, sat.face, nx, ny, nz);
    return true;
}

/**
 * Clip-only entry point — the caller supplies the separating axis `n` (B→A,
 * unit) from a robust GJK+EPA query ({@link gjk_epa_penetration}); we only build
 * the multi-point manifold. This is Bullet's `clipHullAgainstHull(sepNormal,…)`
 * with the normal sourced from GJK rather than SAT. Used for LARGE convex
 * polytopes (authored hulls, a convex mesh as one piece) where SAT's O(Fa·Fb +
 * Ea·Eb) edge term is prohibitive but GJK+EPA is O(support queries). Always
 * emits ≥ 1 contact (the caller already established penetration): if the face
 * clip empties (a vertex/edge feature) it falls back to the deepest support-pair.
 *
 * @param {Float64Array|number[]} out length ≥ CONVEX_CONVEX_OUT_LENGTH
 * @param {{vertices,face_offsets,face_loops}} A @param {{x,y,z}} posA @param {{x,y,z,w}} rotA
 * @param {{vertices,face_offsets,face_loops}} B @param {{x,y,z}} posB @param {{x,y,z,w}} rotB
 * @param {number} nx @param {number} ny @param {number} nz  unit axis, B→A
 * @returns {boolean} always true
 */
export function convex_hull_clip(out, A, posA, rotA, B, posB, rotB, nx, ny, nz) {
    set_geom(A, B);
    load_world(wvA, A.vertices, g_nA, posA, rotA);
    load_world(wvB, B.vertices, g_nB, posB, rotB);

    // Orient the axis B→A using the shapes' GEOMETRIC centres (centroids of the
    // world vertices), the same robust reference the SAT path uses. A body-frame
    // guard (transform positions) degenerates when the two origins nearly
    // coincide — e.g. a tet resting base-to-base on another, both transforms at
    // the contact plane — leaving the normal mis-oriented and the stack tunnels.
    centroid(cenA, wvA, g_nA);
    centroid(cenB, wvB, g_nB);
    if (nx * (cenA[0] - cenB[0]) + ny * (cenA[1] - cenB[1]) + nz * (cenA[2] - cenB[2]) < 0) {
        nx = -nx; ny = -ny; nz = -nz;
    }
    out[0] = nx; out[1] = ny; out[2] = nz;

    // Reference face = the face (A or B) whose outward normal is most aligned
    // with the axis pointing toward the other body. A's contact face points
    // toward B (along −n); B's toward A (along +n). The flatter (better-aligned)
    // one is the reference. Mirrors Bullet's "face most opposing the normal",
    // made symmetric across A and B.
    let bestA = -Infinity, faceA = 0;
    for (let f = 0; f < g_fA; f++) {
        face_normal(tmp_n, wvA, g_flA, g_foA[f], g_foA[f + 1]);
        const l = Math.sqrt(tmp_n[0] * tmp_n[0] + tmp_n[1] * tmp_n[1] + tmp_n[2] * tmp_n[2]) || 1;
        const d = (tmp_n[0] * -nx + tmp_n[1] * -ny + tmp_n[2] * -nz) / l;
        if (d > bestA) { bestA = d; faceA = f; }
    }
    let bestB = -Infinity, faceB = 0;
    for (let f = 0; f < g_fB; f++) {
        face_normal(tmp_n, wvB, g_flB, g_foB[f], g_foB[f + 1]);
        const l = Math.sqrt(tmp_n[0] * tmp_n[0] + tmp_n[1] * tmp_n[1] + tmp_n[2] * tmp_n[2]) || 1;
        const d = (tmp_n[0] * nx + tmp_n[1] * ny + tmp_n[2] * nz) / l;
        if (d > bestB) { bestB = d; faceB = f; }
    }

    const ref_is_A = bestA >= bestB;
    emit_face_manifold(out, ref_is_A, ref_is_A ? faceA : faceB, nx, ny, nz);

    if ((out[3] | 0) === 0) {
        const overlap = projection_overlap(nx, ny, nz);
        emit_single_contact(out, nx, ny, nz, overlap > 0 ? overlap : 0);
    }
    return true;
}

/** Overlap of the two pieces' projections onto axis `n` (B→A); A is on the +n side. */
function projection_overlap(nx, ny, nz) {
    let minA = Infinity, maxB = -Infinity;
    for (let i = 0; i < g_nA; i++) {
        const d = nx * wvA[i * 3] + ny * wvA[i * 3 + 1] + nz * wvA[i * 3 + 2];
        if (d < minA) minA = d;
    }
    for (let i = 0; i < g_nB; i++) {
        const d = nx * wvB[i * 3] + ny * wvB[i * 3 + 1] + nz * wvB[i * 3 + 2];
        if (d > maxB) maxB = d;
    }
    return maxB - minA;
}

/** Single deepest support-pair contact along axis `n` (B→A). */
function emit_single_contact(out, nx, ny, nz, depth) {
    let sa = -Infinity, ai = 0;
    for (let i = 0; i < g_nA; i++) {
        const d = -(nx * wvA[i * 3] + ny * wvA[i * 3 + 1] + nz * wvA[i * 3 + 2]);
        if (d > sa) { sa = d; ai = i; }
    }
    let sb = -Infinity, bi = 0;
    for (let i = 0; i < g_nB; i++) {
        const d = nx * wvB[i * 3] + ny * wvB[i * 3 + 1] + nz * wvB[i * 3 + 2];
        if (d > sb) { sb = d; bi = i; }
    }
    out[3] = 1;
    out[4] = wvA[ai * 3]; out[5] = wvA[ai * 3 + 1]; out[6] = wvA[ai * 3 + 2];
    out[7] = wvB[bi * 3]; out[8] = wvB[bi * 3 + 1]; out[9] = wvB[bi * 3 + 2];
    out[10] = depth;
}

/**
 * Reference/incident polygon clipping given the reference face. Writes the
 * manifold (count + contacts) into `out`; leaves out[0..2] (the normal) as set
 * by the caller. Shared by both the SAT and GJK+EPA entry points.
 *
 * @param {boolean} ref_is_A  reference face belongs to A (else B)
 * @param {number} ref_face   reference face index within its piece
 * @param {number} nx @param {number} ny @param {number} nz  axis (B→A), unit
 */
function emit_face_manifold(out, ref_is_A, ref_face, nx, ny, nz) {
    const ref_wv = ref_is_A ? wvA : wvB;
    const ref_fo = ref_is_A ? g_foA : g_foB;
    const ref_fl = ref_is_A ? g_flA : g_flB;
    const inc_wv = ref_is_A ? wvB : wvA;
    const inc_fo = ref_is_A ? g_foB : g_foA;
    const inc_fl = ref_is_A ? g_flB : g_flA;
    const inc_fcount = ref_is_A ? g_fB : g_fA;

    // Reference outward normal points ref → incident: −n if ref is A, +n if ref is B.
    const rmx = ref_is_A ? -nx : nx, rmy = ref_is_A ? -ny : ny, rmz = ref_is_A ? -nz : nz;

    const ref_s = ref_fo[ref_face], ref_len = ref_fo[ref_face + 1] - ref_s;
    const r0 = ref_fl[ref_s] * 3;
    const r0x = ref_wv[r0], r0y = ref_wv[r0 + 1], r0z = ref_wv[r0 + 2];

    // Incident face = the polygon on the other piece most antiparallel to rm.
    let inc_face = 0, most_anti = Infinity;
    for (let f = 0; f < inc_fcount; f++) {
        face_normal(tmp_n, inc_wv, inc_fl, inc_fo[f], inc_fo[f + 1]);
        const l = Math.sqrt(tmp_n[0] * tmp_n[0] + tmp_n[1] * tmp_n[1] + tmp_n[2] * tmp_n[2]) || 1;
        const dot = (tmp_n[0] * rmx + tmp_n[1] * rmy + tmp_n[2] * rmz) / l;
        if (dot < most_anti) { most_anti = dot; inc_face = f; }
    }

    // Incident polygon (world).
    const inc_s = inc_fo[inc_face], inc_len = inc_fo[inc_face + 1] - inc_s;
    let n_poly = inc_len;
    for (let i = 0; i < inc_len; i++) {
        const vi = inc_fl[inc_s + i] * 3;
        poly_in[i * 3] = inc_wv[vi]; poly_in[i * 3 + 1] = inc_wv[vi + 1]; poly_in[i * 3 + 2] = inc_wv[vi + 2];
    }

    // Reference polygon centroid (orients side planes inward).
    let rcx = 0, rcy = 0, rcz = 0;
    for (let i = 0; i < ref_len; i++) {
        const vi = ref_fl[ref_s + i] * 3;
        rcx += ref_wv[vi]; rcy += ref_wv[vi + 1]; rcz += ref_wv[vi + 2];
    }
    rcx /= ref_len; rcy /= ref_len; rcz /= ref_len;

    // Clip the incident polygon against each reference-face side plane.
    let src = poly_in, dst = poly_out;
    for (let e = 0; e < ref_len && n_poly > 0; e++) {
        const va = ref_fl[ref_s + e] * 3, vb = ref_fl[ref_s + (e + 1) % ref_len] * 3;
        const edx = ref_wv[vb] - ref_wv[va], edy = ref_wv[vb + 1] - ref_wv[va + 1], edz = ref_wv[vb + 2] - ref_wv[va + 2];
        let sx = edy * rmz - edz * rmy, sy = edz * rmx - edx * rmz, sz = edx * rmy - edy * rmx;
        const pax = ref_wv[va], pay = ref_wv[va + 1], paz = ref_wv[va + 2];
        if ((rcx - pax) * sx + (rcy - pay) * sy + (rcz - paz) * sz > 0) { sx = -sx; sy = -sy; sz = -sz; }
        n_poly = clip_poly(src, n_poly, dst, sx, sy, sz, pax, pay, paz);
        const t = src; src = dst; dst = t;
    }
    if (n_poly === 0) { out[3] = 0; return; }

    let nc = 0;
    for (let i = 0; i < n_poly; i++) {
        const px = src[i * 3], py = src[i * 3 + 1], pz = src[i * 3 + 2];
        const sep = (px - r0x) * rmx + (py - r0y) * rmy + (pz - r0z) * rmz;
        const depth = -sep;
        if (depth < 0) continue;
        cand[nc * 4] = px; cand[nc * 4 + 1] = py; cand[nc * 4 + 2] = pz; cand[nc * 4 + 3] = depth;
        nc++;
    }
    if (nc === 0) { out[3] = 0; return; }

    const kept = reduce_manifold_contacts(cand, nc, 4, MAX_CONTACTS);
    out[3] = kept;
    for (let k = 0; k < kept; k++) {
        const px = cand[k * 4], py = cand[k * 4 + 1], pz = cand[k * 4 + 2], depth = cand[k * 4 + 3];
        const d2p = (px - r0x) * rmx + (py - r0y) * rmy + (pz - r0z) * rmz;
        const rpx = px - rmx * d2p, rpy = py - rmy * d2p, rpz = pz - rmz * d2p;
        const base = 4 + k * CONTACT_STRIDE;
        if (ref_is_A) {
            out[base] = rpx; out[base + 1] = rpy; out[base + 2] = rpz;
            out[base + 3] = px; out[base + 4] = py; out[base + 5] = pz;
        } else {
            out[base] = px; out[base + 1] = py; out[base + 2] = pz;
            out[base + 3] = rpx; out[base + 4] = rpy; out[base + 5] = rpz;
        }
        out[base + 6] = depth;
    }
}

/** Sutherland–Hodgman: keep polygon points where (P − a)·s ≤ 0, with crossings. */
function clip_poly(src, n, dst, sx, sy, sz, ax, ay, az) {
    let m = 0;
    for (let i = 0; i < n; i++) {
        const j = (i + 1) % n;
        const px = src[i * 3], py = src[i * 3 + 1], pz = src[i * 3 + 2];
        const qx = src[j * 3], qy = src[j * 3 + 1], qz = src[j * 3 + 2];
        const dp = (px - ax) * sx + (py - ay) * sy + (pz - az) * sz;
        const dq = (qx - ax) * sx + (qy - ay) * sy + (qz - az) * sz;
        const p_in = dp <= 0, q_in = dq <= 0;
        if (p_in) { dst[m * 3] = px; dst[m * 3 + 1] = py; dst[m * 3 + 2] = pz; m++; }
        if (p_in !== q_in) {
            const t = dp / (dp - dq);
            dst[m * 3] = px + (qx - px) * t;
            dst[m * 3 + 1] = py + (qy - py) * t;
            dst[m * 3 + 2] = pz + (qz - pz) * t;
            m++;
            if (m >= MAX_POLY) break;
        }
    }
    return m;
}

// Contact reduction (deepest + farthest-point spread) is shared with
// box_box_manifold via reduce_manifold_contacts (stride 4).
