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

const GJK_MAX_ITERATIONS = 64;

/**
 * Search-direction scratch. The Minkowski-support call uses its own
 * pair of scratch buffers from {@link minkowski_support}; this one is
 * just for the per-iteration search direction the simplex algorithm
 * mutates in place.
 *
 * Float64 — see the precision sweep that converted the rest of the
 * solver-path arrays.
 */
const scratch_dir = new Float64Array(3);

/**
 * GJK intersection test for two convex shapes.
 *
 * Determines whether two convex shapes overlap by iteratively building a simplex
 * in the Minkowski difference space. If the origin is enclosed by the simplex,
 * the shapes intersect.
 *
 * Adapted from https://github.com/kevinmoran/GJK/blob/master/GJK.h
 *
 * @param {number[]|Float64Array} simplex Working buffer for simplex vertices (4 vec3s). Must have length >= 12.
 * @param {AbstractShape3D} shape_a
 * @param {AbstractShape3D} shape_b
 * @returns {boolean} true if the shapes intersect
 */
export function gjk(simplex, shape_a, shape_b) {
    // Default initial search direction (1, 0, 0). gjk_core uses the
    // caller's `dir` buffer as both the seed and the working
    // direction, but for this wrapper the seed is fixed and the
    // final direction is discarded.
    scratch_dir[0] = 1;
    scratch_dir[1] = 0;
    scratch_dir[2] = 0;
    return gjk_core(simplex, shape_a, shape_b, scratch_dir);
}

/**
 * Core GJK routine — parameterised on the search-direction buffer.
 * The caller seeds `dir[0..2]` with an initial direction (`(1, 0, 0)`
 * for the {@link gjk} cold-start wrapper), and on return `dir` holds the
 * FINAL search direction the algorithm settled on.
 *
 * @param {number[]|Float64Array} simplex
 * @param {AbstractShape3D} shape_a
 * @param {AbstractShape3D} shape_b
 * @param {Float64Array} dir 3 floats; seed direction on entry, final
 *   direction on exit. The buffer is mutated throughout the function.
 * @returns {boolean}
 */
function gjk_core(simplex, shape_a, shape_b, dir) {

    // Get initial support point (simplex point C). Uses `dir` (the
    // caller's seed) as the search direction.
    minkowski_support(
        simplex, 6,
        shape_a, shape_b,
        dir[0], dir[1], dir[2]
    );

    // Search toward the origin from C
    dir[0] = -simplex[6];
    dir[1] = -simplex[7];
    dir[2] = -simplex[8];

    if (dir[0] === 0 && dir[1] === 0 && dir[2] === 0) {
        // Origin is exactly on the first support point — intersection
        return true;
    }

    // Get second support point (simplex point B)
    minkowski_support(simplex, 3, shape_a, shape_b, dir[0], dir[1], dir[2]);

    // B didn't pass the origin — no intersection possible
    if (v3_dot(simplex[3], simplex[4], simplex[5], dir[0], dir[1], dir[2]) < 0) {
        return false;
    }

    // Line case: compute direction perpendicular to line segment BC toward origin
    // CB = C - B
    const cb_x = simplex[6] - simplex[3];
    const cb_y = simplex[7] - simplex[4];
    const cb_z = simplex[8] - simplex[5];

    // BO = -B (origin - B)
    const bo_x = -simplex[3];
    const bo_y = -simplex[4];
    const bo_z = -simplex[5];

    // dir = CB × BO × CB (triple cross product)
    v3_triple_cross_product(dir, 0, cb_x, cb_y, cb_z, bo_x, bo_y, bo_z);

    // Handle degenerate case where origin is on the line segment
    if (dir[0] === 0 && dir[1] === 0 && dir[2] === 0) {
        // Origin is on the line segment — pick any perpendicular direction
        // Use the cross product of CB with an arbitrary axis
        // CB × (1,0,0)
        dir[0] = cb_y;
        dir[1] = -cb_x;
        dir[2] = 0;

        if (dir[0] === 0 && dir[1] === 0 && dir[2] === 0) {
            // CB is parallel to (1,0,0), use (0,1,0)
            dir[0] = -cb_z;
            dir[1] = 0;
            dir[2] = cb_x;
        }
    }

    let simplex_size = 2; // we have B and C

    for (let iterations = 0; iterations < GJK_MAX_ITERATIONS; iterations++) {
        // Get new support point A
        minkowski_support(simplex, 0, shape_a, shape_b, dir[0], dir[1], dir[2]);

        const a_x = simplex[0];
        const a_y = simplex[1];
        const a_z = simplex[2];

        // Check if A passed the origin
        if (v3_dot(a_x, a_y, a_z, dir[0], dir[1], dir[2]) < 0) {
            return false; // no intersection
        }

        simplex_size++;

        if (simplex_size === 3) {
            // Triangle case — may reduce to line (size 2) or keep as
            // triangle (size 3) before shifting slots for the next iter.
            simplex_size = update_simplex_triangle(dir, simplex);
        } else {
            // Tetrahedron case — 0 if origin enclosed, else new size
            // after recursive face reduction.
            const result = update_simplex_tetrahedron(dir, simplex);
            if (result === 0) {
                return true;
            }
            simplex_size = result;
        }
    }

    // Did not converge — assume no intersection
    return false;
}

/**
 * Update simplex for the triangle case (3 points: A, B, C).
 *
 * Determines the Voronoi region of the origin relative to triangle ABC
 * and updates the search direction accordingly. When the origin is
 * closest to a triangle edge the simplex is reduced to a line (return
 * 2). When the origin is above or below the triangle, the existing
 * vertices are shifted (A → B, B → C, C → D for "above";
 * B → D, A → B for "below") so the next iteration's new support
 * point can be written into slot 0 without losing simplex history,
 * and the function returns 3 — the main loop's `simplex_size++` then
 * promotes it to 4 (tetrahedron case) on the next pass.
 *
 * simplex layout: [A(0-2), B(3-5), C(6-8), D(9-11)]
 *
 * @param {number[]|Float64Array} search_dir output: next search direction
 * @param {Float64Array} simplex
 * @returns {number} new simplex size — 2 if reduced to a line,
 *   3 if a triangle was retained (with vertices shifted toward higher
 *   slots so the next iteration grows into a tetrahedron)
 */
function update_simplex_triangle(search_dir, simplex) {
    const a_x = simplex[0], a_y = simplex[1], a_z = simplex[2];
    const b_x = simplex[3], b_y = simplex[4], b_z = simplex[5];
    const c_x = simplex[6], c_y = simplex[7], c_z = simplex[8];

    // AB = B - A
    const ab_x = b_x - a_x;
    const ab_y = b_y - a_y;
    const ab_z = b_z - a_z;

    // AC = C - A
    const ac_x = c_x - a_x;
    const ac_y = c_y - a_y;
    const ac_z = c_z - a_z;

    // AO = -A (origin - A)
    const ao_x = -a_x;
    const ao_y = -a_y;
    const ao_z = -a_z;

    // Normal of triangle ABC
    const abc_x = ab_y * ac_z - ab_z * ac_y;
    const abc_y = ab_z * ac_x - ab_x * ac_z;
    const abc_z = ab_x * ac_y - ab_y * ac_x;

    // Test edge AC
    // ABC × AC
    const abc_cross_ac_x = abc_y * ac_z - abc_z * ac_y;
    const abc_cross_ac_y = abc_z * ac_x - abc_x * ac_z;
    const abc_cross_ac_z = abc_x * ac_y - abc_y * ac_x;

    if (v3_dot(abc_cross_ac_x, abc_cross_ac_y, abc_cross_ac_z, ao_x, ao_y, ao_z) > 0) {
        // Origin is on the AC side — reduce to line {A, C}, drop B.
        //
        // Slot management note: every main-loop iteration writes the
        // newest support point into simplex[0..2] (the A slot),
        // OVERWRITING the previous A. For "old A" to survive into the
        // next iteration's triangle, it must be moved into another
        // slot here. Kevin Moran's reference does `b = a;` — A's value
        // is copied into the B slot. C stays where it is. Next iter's
        // new A lands at slot 0; the simplex is then {new A, old A
        // (in B slot), C} — a valid 3-vertex triangle.
        //
        // The previous implementation here wrote `simplex[3..5] = C`,
        // which both duplicated C and discarded old A. Combined with
        // the missing simplex-size accounting that kept the main loop
        // calling `update_simplex_tetrahedron` with a stale D slot,
        // this produced false-positive overlap for sphere-vs-sphere
        // configurations placed off-axis. See `gjk.spec.js`'s
        // documented regression block.
        simplex[3] = a_x;
        simplex[4] = a_y;
        simplex[5] = a_z;

        // New direction: AC × AO × AC
        v3_triple_cross_product(search_dir, 0, ac_x, ac_y, ac_z, ao_x, ao_y, ao_z);
        return 2;
    }

    // Test edge AB
    // AB × ABC
    const ab_cross_abc_x = ab_y * abc_z - ab_z * abc_y;
    const ab_cross_abc_y = ab_z * abc_x - ab_x * abc_z;
    const ab_cross_abc_z = ab_x * abc_y - ab_y * abc_x;

    if (v3_dot(ab_cross_abc_x, ab_cross_abc_y, ab_cross_abc_z, ao_x, ao_y, ao_z) > 0) {
        // Origin is on the AB side — reduce to line {A, B}, drop C.
        // Kevin Moran: `c = a;` — A's value into C slot. B stays.
        simplex[6] = a_x;
        simplex[7] = a_y;
        simplex[8] = a_z;

        // New direction: AB × AO × AB
        v3_triple_cross_product(search_dir, 0, ab_x, ab_y, ab_z, ao_x, ao_y, ao_z);
        return 2;
    }

    // Origin is above or below the triangle. We're growing to a
    // tetrahedron next iteration, so shift the existing vertices out
    // of slot 0 to make room for the next minkowski_support.
    if (v3_dot(abc_x, abc_y, abc_z, ao_x, ao_y, ao_z) > 0) {
        // Origin above — `d = c; c = b; b = a;` (Kevin Moran). The
        // next iteration writes new A at slot 0; the tetrahedron then
        // has all four vertices populated with valid simplex history.
        simplex[9]  = simplex[6]; simplex[10] = simplex[7]; simplex[11] = simplex[8];
        simplex[6]  = simplex[3]; simplex[7]  = simplex[4]; simplex[8]  = simplex[5];
        simplex[3]  = simplex[0]; simplex[4]  = simplex[1]; simplex[5]  = simplex[2];
        search_dir[0] = abc_x;
        search_dir[1] = abc_y;
        search_dir[2] = abc_z;
    } else {
        // Origin below — `d = b; b = a;` (C stays). Inverted-winding
        // tetrahedron; the face tests in `update_simplex_tetrahedron`
        // multiply face_test by face_d_side and compare against 0, so
        // they're winding-agnostic.
        simplex[9]  = simplex[3]; simplex[10] = simplex[4]; simplex[11] = simplex[5];
        simplex[3]  = simplex[0]; simplex[4]  = simplex[1]; simplex[5]  = simplex[2];
        search_dir[0] = -abc_x;
        search_dir[1] = -abc_y;
        search_dir[2] = -abc_z;
    }
    return 3;
}

/**
 * Update simplex for the tetrahedron case (4 points: A, B, C, D).
 *
 * Tests which face of the tetrahedron is closest to the origin and
 * reduces the simplex accordingly. Returns 0 if the origin is inside
 * the tetrahedron, otherwise returns the new simplex size (2 = line,
 * 3 = triangle) so the main loop can dispatch the next iteration
 * correctly. The size after reduction is determined by the recursive
 * `update_simplex_triangle` call on the surviving face.
 *
 * simplex layout: [A(0-2), B(3-5), C(6-8), D(9-11)]
 *
 * @param {number[]|Float64Array} search_dir output: next search direction (only written on non-enclosure)
 * @param {Float64Array} simplex
 * @returns {number} 0 if origin is inside, else new simplex size (2 or 3)
 */
function update_simplex_tetrahedron(search_dir, simplex) {
    const a_x = simplex[0], a_y = simplex[1], a_z = simplex[2];
    const b_x = simplex[3], b_y = simplex[4], b_z = simplex[5];
    const c_x = simplex[6], c_y = simplex[7], c_z = simplex[8];
    const d_x = simplex[9], d_y = simplex[10], d_z = simplex[11];

    // AB, AC, AD vectors
    const ab_x = b_x - a_x, ab_y = b_y - a_y, ab_z = b_z - a_z;
    const ac_x = c_x - a_x, ac_y = c_y - a_y, ac_z = c_z - a_z;
    const ad_x = d_x - a_x, ad_y = d_y - a_y, ad_z = d_z - a_z;

    // AO = origin - A = -A
    const ao_x = -a_x, ao_y = -a_y, ao_z = -a_z;

    // Face normals (outward-facing from A's perspective)
    // ABC normal
    const abc_x = ab_y * ac_z - ab_z * ac_y;
    const abc_y = ab_z * ac_x - ab_x * ac_z;
    const abc_z = ab_x * ac_y - ab_y * ac_x;

    // ACD normal
    const acd_x = ac_y * ad_z - ac_z * ad_y;
    const acd_y = ac_z * ad_x - ac_x * ad_z;
    const acd_z = ac_x * ad_y - ac_y * ad_x;

    // ADB normal
    const adb_x = ad_y * ab_z - ad_z * ab_y;
    const adb_y = ad_z * ab_x - ad_x * ab_z;
    const adb_z = ad_x * ab_y - ad_y * ab_x;

    // Test each face to see if the origin is on the outside

    // Check face ABC (opposite to D)
    const abc_test = v3_dot(abc_x, abc_y, abc_z, ao_x, ao_y, ao_z);
    // Make sure normal points away from D
    const abc_d_side = v3_dot(abc_x, abc_y, abc_z, ad_x, ad_y, ad_z);

    if (abc_test * abc_d_side < 0) {
        // Origin is on the opposite side of ABC from D
        // Reduce to triangle ABC: D is dropped
        // simplex = [A, B, C] — A(0), B(3), C(6) already in place
        return update_simplex_triangle(search_dir, simplex);
    }

    // Check face ACD (opposite to B)
    const acd_test = v3_dot(acd_x, acd_y, acd_z, ao_x, ao_y, ao_z);
    const acd_b_side = v3_dot(acd_x, acd_y, acd_z, ab_x, ab_y, ab_z);

    if (acd_test * acd_b_side < 0) {
        // Origin is outside face ACD
        // Reduce to triangle ACD: replace B with D
        simplex[3] = c_x;
        simplex[4] = c_y;
        simplex[5] = c_z;
        simplex[6] = d_x;
        simplex[7] = d_y;
        simplex[8] = d_z;

        return update_simplex_triangle(search_dir, simplex);
    }

    // Check face ADB (opposite to C)
    const adb_test = v3_dot(adb_x, adb_y, adb_z, ao_x, ao_y, ao_z);
    const adb_c_side = v3_dot(adb_x, adb_y, adb_z, ac_x, ac_y, ac_z);

    if (adb_test * adb_c_side < 0) {
        // Origin is outside face ADB
        // Reduce to triangle ADB: replace C with B, B with D
        simplex[6] = b_x;
        simplex[7] = b_y;
        simplex[8] = b_z;
        simplex[3] = d_x;
        simplex[4] = d_y;
        simplex[5] = d_z;

        return update_simplex_triangle(search_dir, simplex);
    }

    // Origin is inside the tetrahedron
    return 0;
}
