import { computeTriangleClosestPointToPointBarycentric } from "../../../core/geom/3d/triangle/computeTriangleClosestPointToPointBarycentric.js";
import { line3_closest_points_segment_segment } from "../../../core/geom/3d/line/line3_closest_points_segment_segment.js";
import { capsule_world_segment } from "./capsule_contacts.js";
import { sphere_triangle_contact } from "./sphere_triangle_contact.js";

/**
 * Multi-point capsule-vs-triangle contact generation.
 *
 * A capsule is the Minkowski sum of a line segment and a sphere of
 * radius `r`, so capsule-vs-triangle reduces to:
 *
 *   1. Find the closest pair of points between the capsule's central
 *      segment and the triangle (segment ↔ triangle distance).
 *   2. If distance ≥ radius, no overlap.
 *   3. Otherwise emit a primary contact at the segment-triangle
 *      closest pair, then additionally treat each cap centre (segment
 *      endpoint) as a sphere of cap-radius and run
 *      {@link sphere_triangle_contact} to produce up to 2 endpoint
 *      contacts. Spatial dedup against the primary so a capsule
 *      touching with one cap doesn't emit two contacts at the same
 *      point.
 *
 * The multi-point manifold (1 primary + 2 endpoints) is what allows a
 * capsule resting flat on a triangle face to be stable — a single
 * contact at the segment's closest point is positionally ambiguous
 * (every interior segment point is equidistant from the face), and
 * the body wobbles around whichever point the iterative closest-point
 * solver happened to converge on. The endpoint contacts pin the
 * capsule's rotation about its long axis.
 *
 * Output layout per contact (matches
 * {@link CAPSULE_BOX_CONTACT_STRIDE} so the same downstream wiring
 * works):
 *   0..2 : world contact on capsule side
 *   3..5 : world contact on triangle side
 *   6..8 : normal from triangle (B) toward capsule (A)
 *   9    : depth (positive = penetration)
 *
 * @author Alex Goldring
 * @copyright Company Named Limited (c) 2026
 */

/**
 * Output stride per contact.
 * @type {number}
 */
export const CAPSULE_TRIANGLE_CONTACT_STRIDE = 10;

/**
 * Maximum contacts emitted: 1 primary + 2 endpoint sphere contacts.
 * @type {number}
 */
export const CAPSULE_TRIANGLE_MAX_CONTACTS = 3;

/**
 * Two world positions are treated as the same contact when their
 * squared distance is below this threshold. Same value as the box
 * variant — looser than the manifold's warm-start match tolerance
 * (≈0.02) because we specifically want to dedupe coincident
 * primary/endpoint contacts; small lateral shifts produce distinct
 * contacts that should both be kept.
 * @type {number}
 */
const DEDUPE_DIST_SQR = 1e-6;

/**
 * Numerical floor for "segment direction roughly parallel to triangle
 * plane". Used to bail out of the case-0 (segment-plane intersection)
 * branch before dividing by `d · n`.
 * @type {number}
 */
const PARALLEL_EPS = 1e-12;

/**
 * Tolerance for "reconstructed point matches input" in the inside-
 * triangle check during case-0.
 * @type {number}
 */
const INSIDE_TRIANGLE_EPS_SQR = 1e-10;

// --- scratch (allocation-free across calls) ---------------------------------

const scratch_seg = new Float64Array(6);
const scratch_bary = new Float64Array(2);
const scratch_pair_st = new Float64Array(2);
const scratch_seg_tri = new Float64Array(6); // closest segment / triangle world points
const scratch_sphere = new Float64Array(10);

/**
 * Compute the closest pair of points between a 3D line segment
 * `(P1, P2)` and a triangle `(A, B, C)`. Writes the closest segment
 * point into `out[0..2]` and the closest triangle point into
 * `out[3..5]`, and returns the (positive) distance between them.
 *
 * Implementation: take the minimum of 6 candidate distances —
 *  - Case 0: segment crosses triangle's plane inside the face
 *    → distance 0; both points coincide on the face.
 *  - Cases 1, 2: each segment endpoint vs its closest point on the
 *    triangle (via {@link computeTriangleClosestPointToPointBarycentric}).
 *    Subsumes the "endpoint vs triangle vertex / edge / face" cases.
 *  - Cases 3, 4, 5: segment vs each of the triangle's 3 edges, via
 *    {@link line3_closest_points_segment_segment}. Covers the
 *    "segment interior closest to triangle edge" case that the
 *    endpoint queries miss.
 *
 * @param {number[]|Float64Array} out length >= 6
 */
function segment_triangle_closest(
    out,
    p1x, p1y, p1z, p2x, p2y, p2z,
    ax, ay, az, bx, by, bz, cx, cy, cz
) {
    let best_dist_sqr = Infinity;
    let best_sx = 0, best_sy = 0, best_sz = 0;
    let best_tx = 0, best_ty = 0, best_tz = 0;

    // --- Case 0: segment intersects triangle plane INSIDE the face -----------
    const dx_seg = p2x - p1x, dy_seg = p2y - p1y, dz_seg = p2z - p1z;
    const e1x = bx - ax, e1y = by - ay, e1z = bz - az;
    const e2x = cx - ax, e2y = cy - ay, e2z = cz - az;
    const tnx = e1y * e2z - e1z * e2y;
    const tny = e1z * e2x - e1x * e2z;
    const tnz = e1x * e2y - e1y * e2x;
    const d_dot_n = dx_seg * tnx + dy_seg * tny + dz_seg * tnz;
    if (Math.abs(d_dot_n) > PARALLEL_EPS) {
        const t = ((ax - p1x) * tnx + (ay - p1y) * tny + (az - p1z) * tnz) / d_dot_n;
        if (t >= 0 && t <= 1) {
            const ix = p1x + dx_seg * t;
            const iy = p1y + dy_seg * t;
            const iz = p1z + dz_seg * t;
            // Inside-triangle test: feed the intersection through the
            // closest-point algorithm; it returns the BARYCENTRIC of the
            // closest point on the triangle. If the closest point equals
            // the input (within INSIDE_TRIANGLE_EPS_SQR), the intersection
            // is inside the face.
            computeTriangleClosestPointToPointBarycentric(
                scratch_bary, 0,
                ix, iy, iz,
                ax, ay, az,
                bx, by, bz,
                cx, cy, cz
            );
            const alpha = scratch_bary[0];
            const beta = scratch_bary[1];
            const gamma = 1 - alpha - beta;
            const rx = alpha * ax + beta * bx + gamma * cx;
            const ry = alpha * ay + beta * by + gamma * cy;
            const rz = alpha * az + beta * bz + gamma * cz;
            const diff_x = rx - ix, diff_y = ry - iy, diff_z = rz - iz;
            if (diff_x * diff_x + diff_y * diff_y + diff_z * diff_z < INSIDE_TRIANGLE_EPS_SQR) {
                out[0] = ix; out[1] = iy; out[2] = iz;
                out[3] = ix; out[4] = iy; out[5] = iz;
                return 0;
            }
        }
    }

    // --- Case 1: P1 vs closest-on-triangle -----------------------------------
    computeTriangleClosestPointToPointBarycentric(
        scratch_bary, 0,
        p1x, p1y, p1z,
        ax, ay, az,
        bx, by, bz,
        cx, cy, cz
    );
    {
        const alpha = scratch_bary[0];
        const beta = scratch_bary[1];
        const gamma = 1 - alpha - beta;
        const qx = alpha * ax + beta * bx + gamma * cx;
        const qy = alpha * ay + beta * by + gamma * cy;
        const qz = alpha * az + beta * bz + gamma * cz;
        const dx = p1x - qx, dy = p1y - qy, dz = p1z - qz;
        const d_sqr = dx * dx + dy * dy + dz * dz;
        if (d_sqr < best_dist_sqr) {
            best_dist_sqr = d_sqr;
            best_sx = p1x; best_sy = p1y; best_sz = p1z;
            best_tx = qx;  best_ty = qy;  best_tz = qz;
        }
    }

    // --- Case 2: P2 vs closest-on-triangle -----------------------------------
    computeTriangleClosestPointToPointBarycentric(
        scratch_bary, 0,
        p2x, p2y, p2z,
        ax, ay, az,
        bx, by, bz,
        cx, cy, cz
    );
    {
        const alpha = scratch_bary[0];
        const beta = scratch_bary[1];
        const gamma = 1 - alpha - beta;
        const qx = alpha * ax + beta * bx + gamma * cx;
        const qy = alpha * ay + beta * by + gamma * cy;
        const qz = alpha * az + beta * bz + gamma * cz;
        const dx = p2x - qx, dy = p2y - qy, dz = p2z - qz;
        const d_sqr = dx * dx + dy * dy + dz * dz;
        if (d_sqr < best_dist_sqr) {
            best_dist_sqr = d_sqr;
            best_sx = p2x; best_sy = p2y; best_sz = p2z;
            best_tx = qx;  best_ty = qy;  best_tz = qz;
        }
    }

    // --- Cases 3..5: segment vs each triangle edge ---------------------------
    // Helper inline to avoid array allocation.
    function test_edge(e_p1x, e_p1y, e_p1z, e_p2x, e_p2y, e_p2z) {
        line3_closest_points_segment_segment(
            scratch_pair_st,
            p1x, p1y, p1z, p2x, p2y, p2z,
            e_p1x, e_p1y, e_p1z, e_p2x, e_p2y, e_p2z
        );
        const s = scratch_pair_st[0];
        const t = scratch_pair_st[1];
        const sx = p1x + s * (p2x - p1x);
        const sy = p1y + s * (p2y - p1y);
        const sz = p1z + s * (p2z - p1z);
        const tx = e_p1x + t * (e_p2x - e_p1x);
        const ty = e_p1y + t * (e_p2y - e_p1y);
        const tz = e_p1z + t * (e_p2z - e_p1z);
        const dx = sx - tx, dy = sy - ty, dz = sz - tz;
        const d_sqr = dx * dx + dy * dy + dz * dz;
        if (d_sqr < best_dist_sqr) {
            best_dist_sqr = d_sqr;
            best_sx = sx; best_sy = sy; best_sz = sz;
            best_tx = tx; best_ty = ty; best_tz = tz;
        }
    }
    test_edge(ax, ay, az, bx, by, bz);
    test_edge(bx, by, bz, cx, cy, cz);
    test_edge(cx, cy, cz, ax, ay, az);

    out[0] = best_sx; out[1] = best_sy; out[2] = best_sz;
    out[3] = best_tx; out[4] = best_ty; out[5] = best_tz;
    return Math.sqrt(best_dist_sqr);
}

// --- main --------------------------------------------------------------------

/**
 * @param {number[]|Float64Array} out length >= CAPSULE_TRIANGLE_MAX_CONTACTS *
 *   CAPSULE_TRIANGLE_CONTACT_STRIDE
 * @param {number} c_cx capsule centre x
 * @param {number} c_cy
 * @param {number} c_cz
 * @param {number} c_qx capsule rotation
 * @param {number} c_qy
 * @param {number} c_qz
 * @param {number} c_qw
 * @param {number} c_radius
 * @param {number} c_half_h capsule half-height (not including caps)
 * @param {number} ax triangle vertex A x (world)
 * @param {number} ay
 * @param {number} az
 * @param {number} bx triangle vertex B x (world)
 * @param {number} by
 * @param {number} bz
 * @param {number} cx triangle vertex C x (world)
 * @param {number} cy
 * @param {number} cz
 * @returns {number} number of contacts emitted, in `[0, CAPSULE_TRIANGLE_MAX_CONTACTS]`
 */
export function capsule_triangle_contact(
    out,
    c_cx, c_cy, c_cz, c_qx, c_qy, c_qz, c_qw, c_radius, c_half_h,
    ax, ay, az, bx, by, bz, cx, cy, cz
) {
    // Capsule's world-space segment endpoints.
    capsule_world_segment(scratch_seg, c_cx, c_cy, c_cz, c_qx, c_qy, c_qz, c_qw, c_half_h);
    const sax = scratch_seg[0], say = scratch_seg[1], saz = scratch_seg[2];
    const sbx = scratch_seg[3], sby = scratch_seg[4], sbz = scratch_seg[5];

    // Primary contact: closest segment point ↔ closest triangle point.
    const dist = segment_triangle_closest(
        scratch_seg_tri,
        sax, say, saz, sbx, sby, sbz,
        ax, ay, az, bx, by, bz, cx, cy, cz
    );
    if (dist >= c_radius) return 0;

    const seg_pt_x = scratch_seg_tri[0];
    const seg_pt_y = scratch_seg_tri[1];
    const seg_pt_z = scratch_seg_tri[2];
    const tri_pt_x = scratch_seg_tri[3];
    const tri_pt_y = scratch_seg_tri[4];
    const tri_pt_z = scratch_seg_tri[5];

    // Normal points from the triangle closest point toward the segment.
    let nx, ny, nz;
    if (dist > 0) {
        const inv = 1 / dist;
        nx = (seg_pt_x - tri_pt_x) * inv;
        ny = (seg_pt_y - tri_pt_y) * inv;
        nz = (seg_pt_z - tri_pt_z) * inv;
    } else {
        // Segment crosses or touches the triangle plane on the face —
        // pick the triangle's outward face normal as a deterministic
        // tie-break, same convention as sphere_triangle_contact.
        const e1x = bx - ax, e1y = by - ay, e1z = bz - az;
        const e2x = cx - ax, e2y = cy - ay, e2z = cz - az;
        const fnx = e1y * e2z - e1z * e2y;
        const fny = e1z * e2x - e1x * e2z;
        const fnz = e1x * e2y - e1y * e2x;
        const fn_mag = Math.sqrt(fnx * fnx + fny * fny + fnz * fnz);
        if (fn_mag === 0) return 0; // degenerate triangle
        nx = fnx / fn_mag;
        ny = fny / fn_mag;
        nz = fnz / fn_mag;
    }

    const depth = c_radius - dist;

    // Primary contact emission.
    // Capsule surface point: segment point pushed -radius along the normal
    // (back toward the triangle).
    out[0] = seg_pt_x - nx * c_radius;
    out[1] = seg_pt_y - ny * c_radius;
    out[2] = seg_pt_z - nz * c_radius;
    out[3] = tri_pt_x; out[4] = tri_pt_y; out[5] = tri_pt_z;
    out[6] = nx;       out[7] = ny;       out[8] = nz;
    out[9] = depth;
    let count = 1;

    // Endpoint sphere contacts. Inline the 2 endpoint queries to keep
    // allocation-free.
    count = try_endpoint_contact(out, count, sax, say, saz, c_radius, ax, ay, az, bx, by, bz, cx, cy, cz);
    count = try_endpoint_contact(out, count, sbx, sby, sbz, c_radius, ax, ay, az, bx, by, bz, cx, cy, cz);

    return count;
}

/**
 * Run sphere_triangle_contact at one cap centre and append the
 * resulting contact to `out` (with spatial dedup against the
 * already-emitted entries). Returns the new contact count.
 *
 * @returns {number}
 */
function try_endpoint_contact(
    out, count,
    cap_x, cap_y, cap_z, radius,
    ax, ay, az, bx, by, bz, cx, cy, cz
) {
    const ok = sphere_triangle_contact(
        scratch_sphere,
        cap_x, cap_y, cap_z, radius,
        ax, ay, az, bx, by, bz, cx, cy, cz
    );
    if (!ok) return count;

    const e_nx = scratch_sphere[0], e_ny = scratch_sphere[1], e_nz = scratch_sphere[2];
    const e_depth = scratch_sphere[3];
    const e_cap_x = scratch_sphere[4], e_cap_y = scratch_sphere[5], e_cap_z = scratch_sphere[6];
    const e_tri_x = scratch_sphere[7], e_tri_y = scratch_sphere[8], e_tri_z = scratch_sphere[9];

    // Dedup: compare capsule-side world position against every prior
    // contact. Two distinct triangle-surface points always have
    // distinct capsule-side projections along the cap-radius vector,
    // so capsule-side position uniquely identifies a contact.
    for (let k = 0; k < count; k++) {
        const ko = k * CAPSULE_TRIANGLE_CONTACT_STRIDE;
        const dx = e_cap_x - out[ko];
        const dy = e_cap_y - out[ko + 1];
        const dz = e_cap_z - out[ko + 2];
        if (dx * dx + dy * dy + dz * dz < DEDUPE_DIST_SQR) return count;
    }

    const o = count * CAPSULE_TRIANGLE_CONTACT_STRIDE;
    out[o]     = e_cap_x; out[o + 1] = e_cap_y; out[o + 2] = e_cap_z;
    out[o + 3] = e_tri_x; out[o + 4] = e_tri_y; out[o + 5] = e_tri_z;
    out[o + 6] = e_nx;    out[o + 7] = e_ny;    out[o + 8] = e_nz;
    out[o + 9] = e_depth;
    return count + 1;
}
