import { line3_closest_points_segment_segment } from "../../../core/geom/3d/line/line3_closest_points_segment_segment.js";
import { line3_compute_segment_nearest_point_to_point_t } from "../../../core/geom/3d/line/line3_compute_segment_nearest_point_to_point_t.js";
import { v3_quat3_apply } from "../../../core/geom/vec3/v3_quat3_apply.js";
import { v3_quat3_apply_inverse } from "../../../core/geom/vec3/v3_quat3_apply_inverse.js";
import { sphere_box_contact } from "./sphere_box_contact.js";

/**
 * Closed-form narrowphase helpers for the capsule family. A capsule is the
 * Minkowski sum of a line segment and a sphere of radius `r`, so contact
 * resolution reduces to:
 *   - capsule-sphere      → point-on-segment distance, then sphere-style normal
 *   - capsule-capsule     → segment-segment closest pair
 *   - capsule-box         → segment-vs-OBB closest point (iterative)
 *
 * Output convention (matches the rest of the narrowphase family):
 *   out[0..2] : normal pointing from B toward A
 *   out[3]    : penetration depth (positive)
 *   out[4..6] : world contact on A's surface
 *   out[7..9] : world contact on B's surface
 *
 * @author Alex Goldring
 * @copyright Company Named Limited (c) 2026
 */

const scratch_st = new Float64Array(2);

const scratch_seg = new Float64Array(6);

/**
 * Compute the two world endpoints of a capsule's central segment given its
 * pose. The capsule is Y-aligned in body frame; the segment runs from
 * `(0, -h/2, 0)` to `(0, +h/2, 0)`. Writes endpoints into `out` as
 * `[ax, ay, az, bx, by, bz]`.
 *
 * @param {number[]|Float64Array} out length >= 6
 * @param {number} cx capsule centre x
 * @param {number} cy
 * @param {number} cz
 * @param {number} qx quaternion x
 * @param {number} qy
 * @param {number} qz
 * @param {number} qw
 * @param {number} half_height
 */
export function capsule_world_segment(out, cx, cy, cz, qx, qy, qz, qw, half_height) {
    v3_quat3_apply(out, 0, 0, -half_height, 0, qx, qy, qz, qw);
    v3_quat3_apply(out, 3, 0,  half_height, 0, qx, qy, qz, qw);
    out[0] += cx; out[1] += cy; out[2] += cz;
    out[3] += cx; out[4] += cy; out[5] += cz;
}

// --- capsule vs sphere ------------------------------------------------------

/**
 * Capsule (A) vs sphere (B) contact, closed-form via point-segment distance.
 *
 * @param {number[]|Float64Array} out length >= 10
 * @param {number} a_cx  capsule centre x
 * @param {number} a_cy
 * @param {number} a_cz
 * @param {number} a_qx  capsule rotation
 * @param {number} a_qy
 * @param {number} a_qz
 * @param {number} a_qw
 * @param {number} a_radius
 * @param {number} a_half_h capsule half-height (not including caps)
 * @param {number} b_cx  sphere centre x
 * @param {number} b_cy
 * @param {number} b_cz
 * @param {number} b_radius
 * @returns {boolean} true on overlap
 */
export function capsule_sphere_contact(
    out,
    a_cx, a_cy, a_cz, a_qx, a_qy, a_qz, a_qw, a_radius, a_half_h,
    b_cx, b_cy, b_cz, b_radius
) {
    capsule_world_segment(scratch_seg, a_cx, a_cy, a_cz, a_qx, a_qy, a_qz, a_qw, a_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];

    const t = line3_compute_segment_nearest_point_to_point_t(sax, say, saz, sbx, sby, sbz, b_cx, b_cy, b_cz);

    const px = sax + (sbx - sax) * t;
    const py = say + (sby - say) * t;
    const pz = saz + (sbz - saz) * t;

    const dx = px - b_cx;
    const dy = py - b_cy;
    const dz = pz - b_cz;
    const dist_sqr = dx * dx + dy * dy + dz * dz;
    const sum = a_radius + b_radius;
    if (dist_sqr >= sum * sum) return false;

    const dist = Math.sqrt(dist_sqr);
    let nx, ny, nz;
    if (dist > 0) {
        const inv = 1 / dist;
        nx = dx * inv; ny = dy * inv; nz = dz * inv;
    } else {
        // Sphere centre exactly on the capsule segment — pick a deterministic tie-break.
        nx = 1; ny = 0; nz = 0;
    }

    out[0] = nx; out[1] = ny; out[2] = nz; // normal from B to A
    out[3] = sum - dist;
    // World contact on A (capsule): segment point pushed back by capsule radius along -normal
    out[4] = px - nx * a_radius;
    out[5] = py - ny * a_radius;
    out[6] = pz - nz * a_radius;
    // World contact on B (sphere): sphere centre pushed along +normal by sphere radius
    out[7] = b_cx + nx * b_radius;
    out[8] = b_cy + ny * b_radius;
    out[9] = b_cz + nz * b_radius;
    return true;
}

// --- capsule vs capsule -----------------------------------------------------

const scratch_seg_b = new Float64Array(6);

/**
 * Two capsules (A and B). Reduces to segment-segment closest pair + radius.
 *
 * @param {number[]|Float64Array} out length >= 10
 * @returns {boolean} true on overlap
 */
export function capsule_capsule_contact(
    out,
    a_cx, a_cy, a_cz, a_qx, a_qy, a_qz, a_qw, a_radius, a_half_h,
    b_cx, b_cy, b_cz, b_qx, b_qy, b_qz, b_qw, b_radius, b_half_h
) {
    capsule_world_segment(scratch_seg,   a_cx, a_cy, a_cz, a_qx, a_qy, a_qz, a_qw, a_half_h);
    capsule_world_segment(scratch_seg_b, b_cx, b_cy, b_cz, b_qx, b_qy, b_qz, b_qw, b_half_h);

    const a0x = scratch_seg[0], a0y = scratch_seg[1], a0z = scratch_seg[2];
    const a1x = scratch_seg[3], a1y = scratch_seg[4], a1z = scratch_seg[5];
    const b0x = scratch_seg_b[0], b0y = scratch_seg_b[1], b0z = scratch_seg_b[2];
    const b1x = scratch_seg_b[3], b1y = scratch_seg_b[4], b1z = scratch_seg_b[5];

    line3_closest_points_segment_segment(
        scratch_st,
        a0x, a0y, a0z, a1x, a1y, a1z,
        b0x, b0y, b0z, b1x, b1y, b1z
    );

    const s = scratch_st[0];
    const t = scratch_st[1];

    const pax = a0x + (a1x - a0x) * s;
    const pay = a0y + (a1y - a0y) * s;
    const paz = a0z + (a1z - a0z) * s;
    const pbx = b0x + (b1x - b0x) * t;
    const pby = b0y + (b1y - b0y) * t;
    const pbz = b0z + (b1z - b0z) * t;

    const dx = pax - pbx;
    const dy = pay - pby;
    const dz = paz - pbz;
    const dist_sqr = dx * dx + dy * dy + dz * dz;
    const sum = a_radius + b_radius;
    if (dist_sqr >= sum * sum) return false;

    const dist = Math.sqrt(dist_sqr);
    let nx, ny, nz;
    if (dist > 0) {
        const inv = 1 / dist;
        nx = dx * inv; ny = dy * inv; nz = dz * inv;
    } else {
        // Segments touch — fall back to the world-frame direction from B's
        // segment centre toward A's segment centre as a tie-break.
        const acx = (a0x + a1x) * 0.5;
        const acy = (a0y + a1y) * 0.5;
        const acz = (a0z + a1z) * 0.5;
        const bcx = (b0x + b1x) * 0.5;
        const bcy = (b0y + b1y) * 0.5;
        const bcz = (b0z + b1z) * 0.5;
        const ddx = acx - bcx, ddy = acy - bcy, ddz = acz - bcz;
        const dd_sqr = ddx * ddx + ddy * ddy + ddz * ddz;
        if (dd_sqr > 0) {
            const inv2 = 1 / Math.sqrt(dd_sqr);
            nx = ddx * inv2; ny = ddy * inv2; nz = ddz * inv2;
        } else {
            nx = 1; ny = 0; nz = 0;
        }
    }

    out[0] = nx; out[1] = ny; out[2] = nz;
    out[3] = sum - dist;
    out[4] = pax - nx * a_radius;
    out[5] = pay - ny * a_radius;
    out[6] = paz - nz * a_radius;
    out[7] = pbx + nx * b_radius;
    out[8] = pby + ny * b_radius;
    out[9] = pbz + nz * b_radius;
    return true;
}

// --- capsule vs oriented box ------------------------------------------------

const scratch_a0_local = new Float64Array(3);
const scratch_a1_local = new Float64Array(3);

/**
 * Capsule (A) vs oriented box (B). Brings the capsule segment into box-local
 * space, then refines via 2 fixed-point iterations between:
 *   - closest point on the segment to the current box-side candidate
 *   - clamped point on the box closest to the current segment-side candidate
 *
 * Two iterations suffice for the cases this engine targets (sphere-like cap
 * geometry against axis-aligned-in-local box). When the segment passes
 * through the box, the "inside" branch picks the smallest-overlap face
 * deterministically, mirroring {@link sphere_box_contact}.
 *
 * @param {number[]|Float64Array} out length >= 10
 * @returns {boolean}
 */
export function capsule_box_contact(
    out,
    a_cx, a_cy, a_cz, a_qx, a_qy, a_qz, a_qw, a_radius, a_half_h,
    b_cx, b_cy, b_cz, b_qx, b_qy, b_qz, b_qw, b_hx, b_hy, b_hz
) {
    // Bring the capsule's segment endpoints into box-local space: build them in
    // world first (from the capsule centre + rotated half-height axis), then map
    // world → box-local below.
    capsule_world_segment(scratch_seg, a_cx, a_cy, a_cz, a_qx, a_qy, a_qz, a_qw, a_half_h);
    // World → box-local: subtract box centre, then rotate by conjugate of box quat.
    const w_a0x = scratch_seg[0] - b_cx, w_a0y = scratch_seg[1] - b_cy, w_a0z = scratch_seg[2] - b_cz;
    const w_a1x = scratch_seg[3] - b_cx, w_a1y = scratch_seg[4] - b_cy, w_a1z = scratch_seg[5] - b_cz;
    v3_quat3_apply_inverse(scratch_a0_local, 0, w_a0x, w_a0y, w_a0z, b_qx, b_qy, b_qz, b_qw);
    v3_quat3_apply_inverse(scratch_a1_local, 0, w_a1x, w_a1y, w_a1z, b_qx, b_qy, b_qz, b_qw);

    const a0lx = scratch_a0_local[0], a0ly = scratch_a0_local[1], a0lz = scratch_a0_local[2];
    const a1lx = scratch_a1_local[0], a1ly = scratch_a1_local[1], a1lz = scratch_a1_local[2];

    // Fixed-point iteration: start with t at the closest point to the box centre.
    let t = line3_compute_segment_nearest_point_to_point_t(a0lx, a0ly, a0lz, a1lx, a1ly, a1lz, 0, 0, 0);
    let plx = a0lx + (a1lx - a0lx) * t;
    let ply = a0ly + (a1ly - a0ly) * t;
    let plz = a0lz + (a1lz - a0lz) * t;

    for (let iter = 0; iter < 2; iter++) {
        // Clamp current segment point to the box → closest box-surface point.
        const qlx = plx < -b_hx ? -b_hx : (plx > b_hx ? b_hx : plx);
        const qly = ply < -b_hy ? -b_hy : (ply > b_hy ? b_hy : ply);
        const qlz = plz < -b_hz ? -b_hz : (plz > b_hz ? b_hz : plz);

        // Re-find closest segment point to that box point.
        t = line3_compute_segment_nearest_point_to_point_t(
            a0lx, a0ly, a0lz, a1lx, a1ly, a1lz,
            qlx, qly, qlz
        );
        plx = a0lx + (a1lx - a0lx) * t;
        ply = a0ly + (a1ly - a0ly) * t;
        plz = a0lz + (a1lz - a0lz) * t;
    }

    // Final box-side point.
    const qlx = plx < -b_hx ? -b_hx : (plx > b_hx ? b_hx : plx);
    const qly = ply < -b_hy ? -b_hy : (ply > b_hy ? b_hy : ply);
    const qlz = plz < -b_hz ? -b_hz : (plz > b_hz ? b_hz : plz);
    const inside = qlx === plx && qly === ply && qlz === plz;

    let nlx, nly, nlz, dist;
    if (!inside) {
        const ex = plx - qlx, ey = ply - qly, ez = plz - qlz;
        const d_sqr = ex * ex + ey * ey + ez * ez;
        if (d_sqr >= a_radius * a_radius) return false;
        dist = Math.sqrt(d_sqr);
        if (dist > 0) {
            const inv = 1 / dist;
            nlx = ex * inv; nly = ey * inv; nlz = ez * inv;
        } else {
            nlx = 1; nly = 0; nlz = 0;
        }
    } else {
        // Segment point is inside the box. Pick smallest-overlap face.
        const dxf = b_hx - Math.abs(plx);
        const dyf = b_hy - Math.abs(ply);
        const dzf = b_hz - Math.abs(plz);
        if (dxf <= dyf && dxf <= dzf) {
            nlx = plx >= 0 ? 1 : -1; nly = 0; nlz = 0; dist = -dxf;
        } else if (dyf <= dzf) {
            nlx = 0; nly = ply >= 0 ? 1 : -1; nlz = 0; dist = -dyf;
        } else {
            nlx = 0; nly = 0; nlz = plz >= 0 ? 1 : -1; dist = -dzf;
        }
    }

    // Normal (box → capsule axis point) into world.
    v3_quat3_apply(out, 0, nlx, nly, nlz, b_qx, b_qy, b_qz, b_qw);
    const nx = out[0], ny = out[1], nz = out[2];
    out[3] = a_radius - dist;

    // Capsule-side contact: world segment point pushed back by radius.
    v3_quat3_apply(out, 4, plx, ply, plz, b_qx, b_qy, b_qz, b_qw);
    out[4] += b_cx - nx * a_radius;
    out[5] += b_cy - ny * a_radius;
    out[6] += b_cz - nz * a_radius;

    // Box-side contact: world projection of the box-surface point.
    v3_quat3_apply(out, 7, qlx, qly, qlz, b_qx, b_qy, b_qz, b_qw);
    out[7] += b_cx;
    out[8] += b_cy;
    out[9] += b_cz;

    return true;
}

// --- capsule vs oriented box, multi-point manifold --------------------------

/**
 * Output stride per contact in {@link capsule_box_multi_contacts}.
 * Matches the candidate-buffer layout used by `narrowphase_step`.
 *
 *   0..2  : world contact on capsule side
 *   3..5  : world contact on box side
 *   6..8  : normal from box (B) toward capsule (A)
 *   9     : depth (positive = penetration)
 *
 * @type {number}
 */
export const CAPSULE_BOX_CONTACT_STRIDE = 10;

/**
 * Maximum contact points {@link capsule_box_multi_contacts} can emit.
 *  - 1 from the closest-point-on-segment vs OBB query (primary)
 *  - up to 2 from the cap-centre sphere-vs-OBB queries (one per endpoint)
 * @type {number}
 */
export const CAPSULE_BOX_MAX_CONTACTS = 3;

const scratch_primary = new Float64Array(10);
const scratch_endpoint = new Float64Array(10);

/**
 * Two world positions are treated as the same contact when their squared
 * distance is below this threshold. Picked so that a capsule resting flat
 * doesn't double up the primary closest-segment-point contact with whichever
 * endpoint it happens to coincide with.
 *
 * Looser than the manifold's warm-start match tolerance (≈0.02) because we
 * specifically want to dedupe contacts at the *same* point; small lateral
 * shifts of the capsule produce distinct contacts that should both be kept.
 * @type {number}
 */
const DEDUPE_DIST_SQR = 1e-6;

/**
 * Multi-point capsule-vs-OBB contact generation. Emits up to
 * {@link CAPSULE_BOX_MAX_CONTACTS} contacts into `out` in the
 * `(wax, way, waz, wbx, wby, wbz, nx, ny, nz, depth)` stride layout.
 *
 * The motivation: a capsule lying flat on a face needs more than one
 * contact to rest stably — 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. Two contacts at
 * the cap centres pin the capsule's rotation about its long axis.
 *
 * Algorithm:
 *  1. Closest-point-on-segment vs OBB → primary contact (same as
 *     {@link capsule_box_contact}).
 *  2. Treat each cap centre as a sphere of cap-radius and run
 *     {@link sphere_box_contact} → up to two additional contacts.
 *  3. Dedupe spatially against the primary so a capsule touching with one
 *     cap doesn't emit two contacts at the same point.
 *
 * @param {Float64Array} out length >= CAPSULE_BOX_MAX_CONTACTS * CAPSULE_BOX_CONTACT_STRIDE
 * @param {number} a_cx capsule centre x
 * @param {number} a_cy
 * @param {number} a_cz
 * @param {number} a_qx capsule rotation
 * @param {number} a_qy
 * @param {number} a_qz
 * @param {number} a_qw
 * @param {number} a_radius
 * @param {number} a_half_h
 * @param {number} b_cx box centre x
 * @param {number} b_cy
 * @param {number} b_cz
 * @param {number} b_qx
 * @param {number} b_qy
 * @param {number} b_qz
 * @param {number} b_qw
 * @param {number} b_hx
 * @param {number} b_hy
 * @param {number} b_hz
 * @returns {number} number of contacts emitted, in `[0, CAPSULE_BOX_MAX_CONTACTS]`
 */
export function capsule_box_multi_contacts(
    out,
    a_cx, a_cy, a_cz, a_qx, a_qy, a_qz, a_qw, a_radius, a_half_h,
    b_cx, b_cy, b_cz, b_qx, b_qy, b_qz, b_qw, b_hx, b_hy, b_hz
) {
    // 1. Primary contact: closest segment point vs OBB.
    const primary_ok = capsule_box_contact(
        scratch_primary,
        a_cx, a_cy, a_cz, a_qx, a_qy, a_qz, a_qw, a_radius, a_half_h,
        b_cx, b_cy, b_cz, b_qx, b_qy, b_qz, b_qw, b_hx, b_hy, b_hz
    );
    if (!primary_ok) return 0;

    // Repack scratch_primary (normal, depth, A, B) into the candidate stride
    // (A, B, normal, depth) at out[0..10).
    const p_nx = scratch_primary[0], p_ny = scratch_primary[1], p_nz = scratch_primary[2];
    const p_depth = scratch_primary[3];
    const p_ax = scratch_primary[4], p_ay = scratch_primary[5], p_az = scratch_primary[6];
    const p_bx = scratch_primary[7], p_by = scratch_primary[8], p_bz = scratch_primary[9];
    out[0] = p_ax; out[1] = p_ay; out[2] = p_az;
    out[3] = p_bx; out[4] = p_by; out[5] = p_bz;
    out[6] = p_nx; out[7] = p_ny; out[8] = p_nz;
    out[9] = p_depth;
    let count = 1;

    // 2. Endpoint contacts: treat each cap centre as a sphere of cap-radius.
    capsule_world_segment(scratch_seg, a_cx, a_cy, a_cz, a_qx, a_qy, a_qz, a_qw, a_half_h);

    for (let i = 0; i < 2; i++) {
        // Endpoint i lives at scratch_seg[i*3 .. i*3+2] — index directly rather
        // than allocating three throwaway arrays per call (hot narrowphase path).
        const ok = sphere_box_contact(
            scratch_endpoint,
            scratch_seg[i * 3], scratch_seg[i * 3 + 1], scratch_seg[i * 3 + 2], a_radius,
            b_cx, b_cy, b_cz, b_qx, b_qy, b_qz, b_qw,
            b_hx, b_hy, b_hz
        );
        if (!ok) continue;

        const e_nx = scratch_endpoint[0], e_ny = scratch_endpoint[1], e_nz = scratch_endpoint[2];
        const e_depth = scratch_endpoint[3];
        const e_ax = scratch_endpoint[4], e_ay = scratch_endpoint[5], e_az = scratch_endpoint[6];
        const e_bx = scratch_endpoint[7], e_by = scratch_endpoint[8], e_bz = scratch_endpoint[9];

        // Dedupe against every already-emitted contact (primary, then any
        // prior endpoint we've already accepted). The capsule-side world
        // point uniquely identifies a contact for this purpose — two distinct
        // box-surface points always have distinct capsule-side projections
        // along the local-A axis.
        let duplicate = false;
        for (let k = 0; k < count; k++) {
            const ko = k * CAPSULE_BOX_CONTACT_STRIDE;
            const dx = e_ax - out[ko];
            const dy = e_ay - out[ko + 1];
            const dz = e_az - out[ko + 2];
            if (dx * dx + dy * dy + dz * dz < DEDUPE_DIST_SQR) { duplicate = true; break; }
        }
        if (duplicate) continue;

        const o = count * CAPSULE_BOX_CONTACT_STRIDE;
        out[o]     = e_ax; out[o + 1] = e_ay; out[o + 2] = e_az;
        out[o + 3] = e_bx; out[o + 4] = e_by; out[o + 5] = e_bz;
        out[o + 6] = e_nx; out[o + 7] = e_ny; out[o + 8] = e_nz;
        out[o + 9] = e_depth;
        count++;
    }

    return count;
}
