import { aabb3_transform_oriented } from "../../../core/geom/3d/aabb/aabb3_transform_oriented.js";
import { v3_quat3_apply } from "../../../core/geom/vec3/v3_quat3_apply.js";
import { aabb_world_to_local } from "./decomposition/aabb_world_to_local.js";
import { decompose_to_triangles } from "./decomposition/decompose_to_triangles.js";
import { TRIANGLE_FLOAT_STRIDE } from "./decomposition/triangle_buffer_layout.js";
import { deepest_pair_penetration } from "./narrowphase_step.js";
import { PosedShape } from "./PosedShape.js";

/**
 * Module-scoped scratch — same single-thread re-entrancy assumption as
 * the other narrowphase utilities. Safe because PhysicsSystem queries
 * (and gameplay code calling this) run on the main thread.
 */
const posed_b = new PosedShape();

/**
 * Scratch normal written by the primary {@link deepest_pair_penetration} query.
 * Copied to the caller's `out_direction` only when the depth clears
 * {@link CONTACT_EPSILON}, preserving the "untouched on no overlap" contract.
 * @type {Float64Array}
 */
const primary_normal = new Float64Array(3);

/**
 * Scratch buffers for the convex-vs-concave recovery path (see
 * {@link concave_recovery_penetration}).
 */
const local_aabb = new Float64Array(6);
const world_aabb = new Float64Array(6);
const concave_query_aabb = new Float64Array(6);
const scratch_v3 = new Float64Array(3);
const scratch_rot = new Float64Array(3);

/**
 * Per-pair triangle decomposition cap for the recovery path. Same rationale as
 * `narrowphase_step.MAX_TRIANGLES_PER_PAIR`: the query AABB is bounded by the
 * convex shape's envelope, so a single pair yields tens of triangles — though
 * a heightmap's count scales O(N²) with its `tessellation` (still inside the
 * buffer for a bounded query at moderate tessellation). Excess is dropped by
 * the enumerator's bounds check.
 * @type {number}
 */
const MAX_TRIANGLES_PER_PAIR = 1024;
const triangle_buffer = new Float64Array(MAX_TRIANGLES_PER_PAIR * TRIANGLE_FLOAT_STRIDE);

/**
 * Penetration depths below this are treated as no contact. The narrowphase
 * dispatch can report sub-micron "overlap" at exact tangent (GJK/EPA noise,
 * or a closed-form solver returning a hair of depth on a kissing contact); the
 * "0 means no overlap" contract is more useful when that noise is filtered out
 * at the source.
 *
 * 1e-4 m (100 µm) is well below any practical world-scale tolerance while
 * still larger than typical convergence residuals at exact tangent.
 *
 * @type {number}
 */
const CONTACT_EPSILON = 1e-4;

/**
 * Compute the penetration depth between two shapes at world poses,
 * returning the depth and writing the separation direction.
 *
 * If the shapes overlap, the return value is the positive distance
 * along `out_direction` you would have to translate `shape_a` to
 * separate it from `shape_b`. `out_direction` is the unit vector
 * pointing from `shape_b` toward `shape_a` — applying
 * `out_direction * return_value` to `position_a` (or equivalently
 * `-out_direction * return_value` to `position_b`) is the minimum
 * translation that produces separation.
 *
 * If the shapes do not overlap, the return value is `0` and
 * `out_direction` is left untouched. Callers should treat 0 as "no
 * penetration".
 *
 * Sign convention matches the narrowphase's stored contact normal:
 * `out_direction` ≡ the "B → A" direction (B's outward surface normal at the
 * contact, pointing toward A).
 *
 * ## How it is computed (hardened)
 *
 * The query routes through the **same narrowphase contact dispatch the solver
 * consumes** ({@link deepest_pair_penetration} → `dispatch_pair`) and reports
 * the deepest contact. That makes it correct — not "correct sometimes" — for
 * every shape pair the engine can build:
 *
 *   - **sphere / box / capsule pairs** → exact closed-form solvers
 *     (`sphere_sphere`, `sphere_box`, `box_box` via SAT, `capsule_*`). Box-box
 *     in particular uses the true minimum-translation axis, so a small body
 *     resting on a large box reports the few-cm overlap through the near face
 *     rather than the metres-deep "exit through the far side" a centroid-seeded
 *     MPR portal used to return.
 *   - **general convex pairs** (anything without a closed form) → GJK + EPA,
 *     which is exact for polytopes and is only ever reached by polytope-like
 *     shapes here, since every curved primitive (sphere, capsule) has a closed
 *     form above.
 *   - **convex vs concave** (one of heightmap / mesh) → triangle decomposition
 *     over the convex AABB + the closed-form per-triangle solvers
 *     (`sphere_triangle`, `box_triangle`, `capsule_triangle`), deepest wins.
 *     These are bounded to each triangle's true 2-D extent, so the historical
 *     over-report on closed-mesh side faces (infinite-plane extrapolation) is
 *     gone.
 *
 * ## Convex-vs-concave recovery (fully-tunnelled bodies)
 *
 * The per-triangle closed-form solvers are intentionally one-sided: a convex
 * shape that has crossed to the *inner* side of a surface produces no
 * from-outside contact (the narrowphase won't shove a body deeper into the
 * solid mid-step). For a standalone penetration / depenetration query that is
 * the wrong answer — the shape *is* overlapping the solid and must be pushed
 * back out. When the primary dispatch finds no contact for a convex-vs-concave
 * pair, this function falls back to a half-space test
 * ({@link concave_recovery_penetration}) that reports the outward push-out
 * vector. This is exact for heightmap terrain and a valid (if not strictly
 * minimal) recovery direction for closed meshes.
 *
 * Concave-vs-concave throws — the M×N triangle-pair cost is out of scope (and
 * is also refused by the narrowphase for dynamic pairs).
 *
 * @param {Float64Array|number[]} out_direction length ≥ 3; receives the unit
 *   separation direction (B → A) on penetration
 * @param {AbstractShape3D} shape_a  in shape_a's local frame; may be concave
 * @param {{x:number,y:number,z:number}} position_a  world position of A
 * @param {{x:number,y:number,z:number,w:number}} rotation_a  world rotation of A
 * @param {AbstractShape3D} shape_b  in shape_b's local frame; may be concave
 * @param {{x:number,y:number,z:number}} position_b  world position of B
 * @param {{x:number,y:number,z:number,w:number}} rotation_b  world rotation of B
 * @returns {number} penetration depth (positive) on overlap, 0 otherwise
 * @throws {Error} if both shapes have `is_convex === false`
 */
export function compute_penetration(
    out_direction,
    shape_a, position_a, rotation_a,
    shape_b, position_b, rotation_b
) {
    const isConcaveA = shape_a.is_convex === false;
    const isConcaveB = shape_b.is_convex === false;

    if (isConcaveA && isConcaveB) {
        throw new Error("compute_penetration: at most one shape may be non-convex (concave-vs-concave triangle-pair cost is out of scope)");
    }

    // ── Primary: the exact narrowphase dispatch, deepest contact = MTV. ──
    const depth = deepest_pair_penetration(
        primary_normal,
        shape_a, position_a, rotation_a,
        shape_b, position_b, rotation_b
    );

    if (depth > CONTACT_EPSILON && Number.isFinite(depth)) {
        out_direction[0] = primary_normal[0];
        out_direction[1] = primary_normal[1];
        out_direction[2] = primary_normal[2];
        return depth;
    }

    // ── Recovery: a convex shape fully crossed to the inner side of a concave
    //    surface produces no from-outside contact above. Push it back out. ──
    if (isConcaveA !== isConcaveB) {
        return concave_recovery_penetration(
            out_direction, isConcaveA,
            shape_a, position_a, rotation_a,
            shape_b, position_b, rotation_b
        );
    }

    return 0;
}

/**
 * Recovery fallback for a convex shape that has tunnelled to the inner side of
 * a concave surface (heightmap / mesh), where the one-sided per-triangle
 * solvers report no contact.
 *
 * Per-triangle half-space test, deepest-wins:
 *   1. Face normal of the triangle in world frame (outward by winding).
 *   2. `convex.support(-face_normal)` — the convex's deepest point along the
 *      inward face axis.
 *   3. Signed distance of that point to the triangle's plane. If positive the
 *      convex is fully outside this face → skip. If negative, its magnitude is
 *      the depth and the face normal is the contact axis.
 *
 * Exact for heightmaps (adjacent triangles cover the boundary); for closed
 * meshes the infinite-plane extrapolation can over-report on side faces, but
 * deepest-wins gives a valid outward push that resolves over iterations — and
 * this path only runs once a body is already inside the solid, where any
 * outward direction is progress.
 *
 * @private
 */
function concave_recovery_penetration(
    out_direction, isConcaveA,
    shape_a, position_a, rotation_a,
    shape_b, position_b, rotation_b
) {
    const concave_shape = isConcaveA ? shape_a : shape_b;
    const concave_pos   = isConcaveA ? position_a : position_b;
    const concave_rot   = isConcaveA ? rotation_a : rotation_b;
    const convex_shape  = isConcaveA ? shape_b : shape_a;
    const convex_pos    = isConcaveA ? position_b : position_a;
    const convex_rot    = isConcaveA ? rotation_b : rotation_a;

    // ── 1. Convex shape's world AABB ───────────────────────────────
    convex_shape.compute_bounding_box(local_aabb);
    aabb3_transform_oriented(
        world_aabb, 0,
        local_aabb[0], local_aabb[1], local_aabb[2],
        local_aabb[3], local_aabb[4], local_aabb[5],
        convex_pos.x, convex_pos.y, convex_pos.z,
        convex_rot.x, convex_rot.y, convex_rot.z, convex_rot.w
    );

    // ── 2. Project into concave's body-local frame ─────────────────
    aabb_world_to_local(
        concave_query_aabb, 0,
        world_aabb,
        concave_pos.x, concave_pos.y, concave_pos.z,
        concave_rot.x, concave_rot.y, concave_rot.z, concave_rot.w
    );

    // ── 3. Decompose to triangles ──────────────────────────────────
    const tri_count = decompose_to_triangles(
        triangle_buffer, 0, concave_shape,
        concave_query_aabb[0], concave_query_aabb[1], concave_query_aabb[2],
        concave_query_aabb[3], concave_query_aabb[4], concave_query_aabb[5]
    );
    if (tri_count === 0) return 0;

    // ── 4. Setup convex PosedShape (we'll call support on it per tri) ─
    posed_b.setup(convex_shape, convex_pos, convex_rot);

    const cqx = concave_rot.x;
    const cqy = concave_rot.y;
    const cqz = concave_rot.z;
    const cqw = concave_rot.w;
    const c_pos_x = concave_pos.x;
    const c_pos_y = concave_pos.y;
    const c_pos_z = concave_pos.z;

    // ── 5. Per-triangle half-space test, deepest-wins ──────────────
    let best_depth = 0;
    let best_fnx_w = 0, best_fny_w = 0, best_fnz_w = 0;

    for (let i = 0; i < tri_count; i++) {
        const tri_offset = i * TRIANGLE_FLOAT_STRIDE;

        const ax = triangle_buffer[tri_offset    ];
        const ay = triangle_buffer[tri_offset + 1];
        const az = triangle_buffer[tri_offset + 2];
        const bx = triangle_buffer[tri_offset + 3];
        const by = triangle_buffer[tri_offset + 4];
        const bz = triangle_buffer[tri_offset + 5];
        const cx_v = triangle_buffer[tri_offset + 6];
        const cy_v = triangle_buffer[tri_offset + 7];
        const cz_v = triangle_buffer[tri_offset + 8];

        // Face normal in body-local: (B − A) × (C − A), then normalise.
        const e1x_l = bx - ax, e1y_l = by - ay, e1z_l = bz - az;
        const e2x_l = cx_v - ax, e2y_l = cy_v - ay, e2z_l = cz_v - az;
        let fnx_l = e1y_l * e2z_l - e1z_l * e2y_l;
        let fny_l = e1z_l * e2x_l - e1x_l * e2z_l;
        let fnz_l = e1x_l * e2y_l - e1y_l * e2x_l;
        const fn_mag = Math.sqrt(fnx_l * fnx_l + fny_l * fny_l + fnz_l * fnz_l);
        if (fn_mag < 1e-12) continue;  // degenerate triangle
        const fn_inv = 1 / fn_mag;
        fnx_l *= fn_inv; fny_l *= fn_inv; fnz_l *= fn_inv;

        // Rotate face normal to world via the concave body's quaternion.
        v3_quat3_apply(scratch_rot, 0, fnx_l, fny_l, fnz_l, cqx, cqy, cqz, cqw);
        const fnx_w = scratch_rot[0];
        const fny_w = scratch_rot[1];
        const fnz_w = scratch_rot[2];

        // Centroid in body-local → world (one point on the triangle's plane).
        const cent_lx = (ax + bx + cx_v) / 3;
        const cent_ly = (ay + by + cy_v) / 3;
        const cent_lz = (az + bz + cz_v) / 3;
        v3_quat3_apply(scratch_rot, 0, cent_lx, cent_ly, cent_lz, cqx, cqy, cqz, cqw);
        const cent_wx = scratch_rot[0] + c_pos_x;
        const cent_wy = scratch_rot[1] + c_pos_y;
        const cent_wz = scratch_rot[2] + c_pos_z;

        // Deepest inward point of convex along -face_normal.
        posed_b.support(scratch_v3, 0, -fnx_w, -fny_w, -fnz_w);

        // Signed distance from that point to the triangle plane along
        // the face normal: > 0 ⇒ convex is fully on outward side,
        // ≤ 0 ⇒ convex extends |dist| into the solid through this face.
        const signed_dist =
              (scratch_v3[0] - cent_wx) * fnx_w
            + (scratch_v3[1] - cent_wy) * fny_w
            + (scratch_v3[2] - cent_wz) * fnz_w;

        if (signed_dist >= 0) continue;

        const depth = -signed_dist;
        if (depth < CONTACT_EPSILON) continue;

        if (depth > best_depth) {
            best_depth = depth;
            best_fnx_w = fnx_w;
            best_fny_w = fny_w;
            best_fnz_w = fnz_w;
        }
    }

    if (best_depth === 0) return 0;

    // ── 6. Write out_direction in the user's "B → A" convention ────
    //   - isConcaveA: original A = concave. "B → A" = convex → concave
    //     = INTO the solid = −face_normal.
    //   - isConcaveB: original A = convex. "B → A" = concave → convex
    //     = AWAY from the solid = +face_normal.
    if (isConcaveA) {
        out_direction[0] = -best_fnx_w;
        out_direction[1] = -best_fny_w;
        out_direction[2] = -best_fnz_w;
    } else {
        out_direction[0] = best_fnx_w;
        out_direction[1] = best_fny_w;
        out_direction[2] = best_fnz_w;
    }

    return best_depth;
}
