import { v3_length } from "../../../core/geom/vec3/v3_length.js";
import { minkowski_support } from "./minkowski_support.js";

/**
 * Maximum portal-refinement iterations. The portal must converge to
 * within {@link MPR_TOLERANCE} of the closest face before this many
 * iterations or we accept whatever it has and return it as an
 * approximation — same graceful-degradation strategy as EPA, but
 * generally MPR converges in 5–15 iterations on the shapes it does
 * well on (smooth curves, mixed convex bodies).
 * @type {number}
 */
const MPR_MAX_ITERATIONS = 64;

/**
 * Convergence threshold on the portal-normal direction. When the new
 * support point is within this distance of the current portal plane
 * (measured along the portal normal), we declare convergence and use
 * the portal as the closest-face approximation of the MTV.
 * @type {number}
 */
const MPR_TOLERANCE = 1e-4;

/**
 * Tolerance for collinearity / coincidence degeneracies during portal
 * discovery. Picked to be safely below practical world-distance noise
 * but large enough that real coincidences (shapes sharing a centre)
 * trip it.
 * @type {number}
 */
const MPR_EPSILON = 1e-10;

/**
 * Scale-relative collinearity threshold (sin²θ). A cross product |a×b|² equals
 * |a|²·|b|²·sin²θ, so comparing |a×b|² against this fraction of |a|²·|b|² tests
 * the ANGLE, independent of shape scale — unlike an absolute epsilon on the
 * (length⁴ / length²) cross magnitude, which fired spuriously for sub-millimetre
 * shapes and returned a bogus, inflated MTV. ~ (1e-6 rad)².
 * @type {number}
 */
const MPR_COLLINEAR_REL_SQR = 1e-12;

/**
 * Portal vertices in the Minkowski-difference space.
 * V0 is an interior point of the Mink-diff (the difference of the two
 * bodies' centres); V1, V2, V3 are the portal triangle's vertices; V4
 * is the candidate new vertex during refinement.
 */
const V0 = new Float64Array(3);
const V1 = new Float64Array(3);
const V2 = new Float64Array(3);
const V3 = new Float64Array(3);
const V4 = new Float64Array(3);

/**
 * Minkowski Portal Refinement (XenoCollide, Gary Snethen, GDC 2009).
 *
 * Determines whether two convex shapes overlap and, if so, returns the
 * Minimum Translation Vector (MTV) that would separate them. The
 * algorithm runs in two phases:
 *
 *   1. **Portal discovery.** Starting from an interior point V0 of the
 *      Minkowski difference (the difference of the two body centres),
 *      find three vertices V1, V2, V3 on the Mink-diff boundary that
 *      form a triangle the ray V0→origin pierces. If at any point a
 *      support hyperplane separates V0 from the origin, the shapes
 *      can't overlap and we return false.
 *
 *   2. **Portal refinement.** Iteratively replace one of V1, V2, V3
 *      with a new support V4 found in the direction of the portal
 *      face's outward normal. When V4 doesn't bring the portal any
 *      closer to the origin (within `MPR_TOLERANCE`), the portal face
 *      *is* the closest face on the Mink-diff to the origin; its
 *      outward normal is the MTV direction and the perpendicular
 *      distance from the origin to the face is the penetration depth.
 *
 * Compared to GJK+EPA on the same overlap:
 *   - One algorithm produces both the overlap test and the MTV (EPA is
 *     a separate pass after GJK).
 *   - Better behaviour on smooth or high-vertex-count shapes — EPA's
 *     polytope-expansion can stall on curved surfaces because each new
 *     support produces a face with no flat region to converge on; MPR's
 *     portal stays a single triangle and just slides toward the origin.
 *   - Tends to converge in 5–15 iterations on typical inputs vs. EPA's
 *     32+ on smooth pairs.
 *
 * **Output convention matches the EPA penetration query** ({@link gjk_epa_penetration}'s MTV form):
 *   `result[result_offset .. result_offset+2]` is the MTV vector —
 *   direction is "from A's surface into B" (i.e. the direction you'd
 *   translate B by to separate the shapes), magnitude is the depth.
 *   The caller reads depth as `√(x² + y² + z²)` and normalises to get
 *   the unit normal. This makes MPR a drop-in replacement for EPA at
 *   any narrowphase call site.
 *
 * On failure modes:
 *   - Returns `false` if the shapes don't overlap.
 *   - On a degenerate Mink-diff (shapes touching at a point, perfectly
 *     coincident centres with collinear support points), returns
 *     `true` with a small fallback MTV — same approach as EPA's
 *     non-convergent fallback. The narrowphase consumer already
 *     filters non-positive / non-finite depths.
 *
 * @param {Float64Array} result        destination buffer for the MTV vector
 * @param {number} result_offset
 * @param {PosedShape} shape_a
 * @param {PosedShape} shape_b
 * @returns {boolean} true on overlap
 */
export function mpr(result, result_offset, shape_a, shape_b) {
    // ── Step 1: interior point V0 = centerA − centerB ───────────────────
    //
    // PosedShape carries the body's world-space position; the centre of
    // its underlying shape (in body-local space) is the origin, so the
    // world centre is just (px, py, pz). The difference of two body
    // centres lies inside the Mink-diff for any convex pair whose shape
    // origins are interior to their geometry — true for spheres,
    // boxes, capsules, and any closed mesh whose bounding box contains
    // its centroid.
    V0[0] = shape_a.px - shape_b.px;
    V0[1] = shape_a.py - shape_b.py;
    V0[2] = shape_a.pz - shape_b.pz;

    // If the two centres coincide exactly, V0 is at the origin and we
    // can't direction-find from it. Perturb to break the symmetry —
    // any direction works; the algorithm will converge to the same
    // answer regardless of which we pick.
    if (V0[0] === 0 && V0[1] === 0 && V0[2] === 0) {
        V0[0] = MPR_EPSILON;
    }

    // ── Step 2: first support V1 in direction −V0 (toward the origin) ──
    minkowski_support(V1, 0, shape_a, shape_b, -V0[0], -V0[1], -V0[2]);

    // If V1 isn't past the origin along −V0, the origin lies outside
    // the Mink-diff and the shapes are separated.
    if (V1[0] * -V0[0] + V1[1] * -V0[1] + V1[2] * -V0[2] < 0) {
        return false;
    }

    // ── Step 3: portal discovery ────────────────────────────────────────
    //
    // Find V2 in a direction perpendicular to the V0–V1 axis. n = V1 × V0
    // is perpendicular to both; if it degenerates to zero, V0 and V1
    // are collinear with the origin, which means the origin lies on
    // the segment from V0 to V1 (since V0 is interior and V1 is past
    // the origin) — definite overlap, emit a fallback MTV along V1.
    let nx = V1[1] * V0[2] - V1[2] * V0[1];
    let ny = V1[2] * V0[0] - V1[0] * V0[2];
    let nz = V1[0] * V0[1] - V1[1] * V0[0];

    // Scale-relative collinearity test: |V1×V0|² vs sin²θ·|V1|²·|V0|².
    const v0_len_sqr = V0[0] * V0[0] + V0[1] * V0[1] + V0[2] * V0[2];
    const v1_len_sqr = V1[0] * V1[0] + V1[1] * V1[1] + V1[2] * V1[2];
    if (nx * nx + ny * ny + nz * nz < MPR_COLLINEAR_REL_SQR * v0_len_sqr * v1_len_sqr) {
        // Collinear degeneracy: project the answer along V1.
        const v1_len = v3_length(V1[0], V1[1], V1[2]);
        if (v1_len > MPR_EPSILON) {
            result[result_offset]     = V1[0];
            result[result_offset + 1] = V1[1];
            result[result_offset + 2] = V1[2];
        } else {
            // V1 at origin — shapes touching exactly. Emit a tiny MTV.
            result[result_offset]     = MPR_EPSILON;
            result[result_offset + 1] = 0;
            result[result_offset + 2] = 0;
        }
        return true;
    }

    minkowski_support(V2, 0, shape_a, shape_b, nx, ny, nz);

    if (V2[0] * nx + V2[1] * ny + V2[2] * nz < 0) {
        return false;
    }

    // Compute the portal-triangle normal (V1 − V0) × (V2 − V0) and
    // orient it away from V0 (so it points roughly toward the origin).
    let v1mv0_x = V1[0] - V0[0], v1mv0_y = V1[1] - V0[1], v1mv0_z = V1[2] - V0[2];
    let v2mv0_x = V2[0] - V0[0], v2mv0_y = V2[1] - V0[1], v2mv0_z = V2[2] - V0[2];
    nx = v1mv0_y * v2mv0_z - v1mv0_z * v2mv0_y;
    ny = v1mv0_z * v2mv0_x - v1mv0_x * v2mv0_z;
    nz = v1mv0_x * v2mv0_y - v1mv0_y * v2mv0_x;

    // If the normal points along +V0 (back toward V0's side), swap V1/V2
    // and flip — we want the portal facing away from V0 toward the
    // origin.
    if (nx * V0[0] + ny * V0[1] + nz * V0[2] > 0) {
        const tx = V1[0], ty = V1[1], tz = V1[2];
        V1[0] = V2[0]; V1[1] = V2[1]; V1[2] = V2[2];
        V2[0] = tx;    V2[1] = ty;    V2[2] = tz;
        nx = -nx; ny = -ny; nz = -nz;
    }

    // Portal-discovery iteration. We keep refining V3 until the
    // tetrahedron V0–V1–V2–V3 actually contains the origin.
    let discovered = false;
    for (let i = 0; i < MPR_MAX_ITERATIONS; i++) {
        minkowski_support(V3, 0, shape_a, shape_b, nx, ny, nz);

        if (V3[0] * nx + V3[1] * ny + V3[2] * nz < 0) {
            return false;
        }

        // Test if origin lies outside plane V0–V1–V3 (V2's side).
        // n_013 = (V1 − V0) × (V3 − V0) — points away from V2 if portal
        // is consistently oriented.
        const v3mv0_x = V3[0] - V0[0], v3mv0_y = V3[1] - V0[1], v3mv0_z = V3[2] - V0[2];
        const n013_x = v1mv0_y * v3mv0_z - v1mv0_z * v3mv0_y;
        const n013_y = v1mv0_z * v3mv0_x - v1mv0_x * v3mv0_z;
        const n013_z = v1mv0_x * v3mv0_y - v1mv0_y * v3mv0_x;

        // origin's signed distance from plane V0-V1-V3 along n013 is
        // dot(−V0, n013), since the plane passes through V0.
        if (-V0[0] * n013_x - V0[1] * n013_y - V0[2] * n013_z > 0) {
            // origin outside V0-V1-V3 plane on the V2 side — replace V2
            V2[0] = V3[0]; V2[1] = V3[1]; V2[2] = V3[2];
            v2mv0_x = v3mv0_x; v2mv0_y = v3mv0_y; v2mv0_z = v3mv0_z;
            nx = n013_x; ny = n013_y; nz = n013_z;
            continue;
        }

        // Test if origin lies outside plane V0-V2-V3 (V1's side).
        const n023_x = v2mv0_y * v3mv0_z - v2mv0_z * v3mv0_y;
        const n023_y = v2mv0_z * v3mv0_x - v2mv0_x * v3mv0_z;
        const n023_z = v2mv0_x * v3mv0_y - v2mv0_y * v3mv0_x;

        if (-V0[0] * n023_x - V0[1] * n023_y - V0[2] * n023_z < 0) {
            // origin outside V0-V2-V3 plane on V1's side — replace V1
            V1[0] = V3[0]; V1[1] = V3[1]; V1[2] = V3[2];
            v1mv0_x = v3mv0_x; v1mv0_y = v3mv0_y; v1mv0_z = v3mv0_z;
            // New portal normal: (V1' - V0) × (V2 - V0) where V1' is the new V1 (= old V3)
            nx = v3mv0_y * v2mv0_z - v3mv0_z * v2mv0_y;
            ny = v3mv0_z * v2mv0_x - v3mv0_x * v2mv0_z;
            nz = v3mv0_x * v2mv0_y - v3mv0_y * v2mv0_x;
            continue;
        }

        // Origin is inside the tetrahedron V0-V1-V2-V3 — portal found.
        discovered = true;
        break;
    }

    if (!discovered) {
        // Ran out of portal-discovery iterations — treat as a near-miss.
        return false;
    }

    // ── Step 4: portal refinement ───────────────────────────────────────
    //
    // Now we know V0–V1–V2–V3 is a valid tetrahedron containing the
    // origin. Iteratively replace one of V1/V2/V3 with a new support
    // V4 (found in the direction of the portal triangle's outward
    // normal). When V4 doesn't extend the portal further, the portal
    // face is the closest Mink-diff face to the origin and its normal
    // is the MTV.
    for (let i = 0; i < MPR_MAX_ITERATIONS; i++) {
        // Portal face normal = (V2 - V1) × (V3 - V1)
        const e1_x = V2[0] - V1[0], e1_y = V2[1] - V1[1], e1_z = V2[2] - V1[2];
        const e2_x = V3[0] - V1[0], e2_y = V3[1] - V1[1], e2_z = V3[2] - V1[2];
        let pn_x = e1_y * e2_z - e1_z * e2_y;
        let pn_y = e1_z * e2_x - e1_x * e2_z;
        let pn_z = e1_x * e2_y - e1_y * e2_x;
        // Scale-relative degeneracy: |e1×e2|² vs sin²θ·|e1|²·|e2|².
        const pn_len_sqr = pn_x * pn_x + pn_y * pn_y + pn_z * pn_z;
        const e1_len_sqr = e1_x * e1_x + e1_y * e1_y + e1_z * e1_z;
        const e2_len_sqr = e2_x * e2_x + e2_y * e2_y + e2_z * e2_z;
        if (pn_len_sqr < MPR_COLLINEAR_REL_SQR * e1_len_sqr * e2_len_sqr) {
            // Degenerate portal (collinear vertices) — return the
            // current state as a fallback approximation.
            result[result_offset]     = pn_x;
            result[result_offset + 1] = pn_y;
            result[result_offset + 2] = pn_z;
            return true;
        }
        const inv_pn = 1 / Math.sqrt(pn_len_sqr);
        pn_x *= inv_pn; pn_y *= inv_pn; pn_z *= inv_pn;

        // Perpendicular distance from the origin to the portal plane,
        // = dot(V1, portal_normal). This is the candidate depth on
        // every iteration; we accept it once V4 fails to push the
        // portal further.
        const portal_dist = V1[0] * pn_x + V1[1] * pn_y + V1[2] * pn_z;

        // New support V4 in the portal normal direction.
        minkowski_support(V4, 0, shape_a, shape_b, pn_x, pn_y, pn_z);

        const v4_dist = V4[0] * pn_x + V4[1] * pn_y + V4[2] * pn_z;

        // Convergence: V4 doesn't extend the portal beyond the current
        // face (within tolerance). Emit MTV = normal × depth.
        if (v4_dist - portal_dist < MPR_TOLERANCE) {
            result[result_offset]     = pn_x * portal_dist;
            result[result_offset + 1] = pn_y * portal_dist;
            result[result_offset + 2] = pn_z * portal_dist;
            return true;
        }

        // V4 didn't pass the origin along the portal normal — the
        // shapes aren't actually overlapping (shouldn't happen after
        // successful portal discovery, but defensive).
        if (v4_dist < 0) {
            return false;
        }

        // Replace one of V1, V2, V3 with V4 such that the new portal
        // still contains the V0→origin ray. The split is determined by
        // three cross-product / dot tests against V4 × V0: signs of
        // (Vi · (V4 × V0)) place V0 in one of the three sub-portals.
        //
        // The branch structure here mirrors libccd / bullet's MPR
        // (`btMprExpandPortal`).
        const c_x = V4[1] * V0[2] - V4[2] * V0[1];
        const c_y = V4[2] * V0[0] - V4[0] * V0[2];
        const c_z = V4[0] * V0[1] - V4[1] * V0[0];

        const d1 = V1[0] * c_x + V1[1] * c_y + V1[2] * c_z;

        if (d1 >= 0) {
            const d2 = V2[0] * c_x + V2[1] * c_y + V2[2] * c_z;
            if (d2 >= 0) {
                // origin sits in sub-portal between V4 and V1 — drop V1
                V1[0] = V4[0]; V1[1] = V4[1]; V1[2] = V4[2];
            } else {
                // sub-portal between V4 and V3 — drop V3
                V3[0] = V4[0]; V3[1] = V4[1]; V3[2] = V4[2];
            }
        } else {
            const d3 = V3[0] * c_x + V3[1] * c_y + V3[2] * c_z;
            if (d3 >= 0) {
                // sub-portal between V4 and V2 — drop V2
                V2[0] = V4[0]; V2[1] = V4[1]; V2[2] = V4[2];
            } else {
                // sub-portal between V4 and V1 (other side) — drop V1
                V1[0] = V4[0]; V1[1] = V4[1]; V1[2] = V4[2];
            }
        }
    }

    // Refinement ran out of iterations — emit the current portal face
    // as the best-known MTV, same graceful-degradation as EPA.
    const e1_x = V2[0] - V1[0], e1_y = V2[1] - V1[1], e1_z = V2[2] - V1[2];
    const e2_x = V3[0] - V1[0], e2_y = V3[1] - V1[1], e2_z = V3[2] - V1[2];
    let pn_x = e1_y * e2_z - e1_z * e2_y;
    let pn_y = e1_z * e2_x - e1_x * e2_z;
    let pn_z = e1_x * e2_y - e1_y * e2_x;
    const pn_len_sqr = pn_x * pn_x + pn_y * pn_y + pn_z * pn_z;
    const e1_len_sqr = e1_x * e1_x + e1_y * e1_y + e1_z * e1_z;
    const e2_len_sqr = e2_x * e2_x + e2_y * e2_y + e2_z * e2_z;
    if (pn_len_sqr < MPR_COLLINEAR_REL_SQR * e1_len_sqr * e2_len_sqr) {
        result[result_offset]     = 0;
        result[result_offset + 1] = 0;
        result[result_offset + 2] = 0;
        return true;
    }
    const inv_pn = 1 / Math.sqrt(pn_len_sqr);
    pn_x *= inv_pn; pn_y *= inv_pn; pn_z *= inv_pn;
    const portal_dist = V1[0] * pn_x + V1[1] * pn_y + V1[2] * pn_z;
    result[result_offset]     = pn_x * portal_dist;
    result[result_offset + 1] = pn_y * portal_dist;
    result[result_offset + 2] = pn_z * portal_dist;
    return true;
}
