import { v3_quat3_apply } from "../../../core/geom/vec3/v3_quat3_apply.js";
import { TRIANGLE_FLOAT_STRIDE } from "./decomposition/triangle_buffer_layout.js";
import { mesh_enumerate_triangles } from "./decomposition/mesh_enumerate_triangles.js";
import { heightmap_enumerate_triangles } from "./decomposition/heightmap_enumerate_triangles.js";

/**
 * # Ray vs a concave shape (mesh / heightmap)
 *
 * Refines a ray hit against a non-convex collider by triangle decomposition,
 * mirroring the contact narrowphase's concave path. Working in the shape's
 * local frame:
 *   1. clip the ray to the shape's local bounding box (a quick reject, and a
 *      finite query segment even when `tMax` is unbounded);
 *   2. enumerate the triangles overlapping that segment's AABB
 *      (`mesh_enumerate_triangles` / `heightmap_enumerate_triangles`);
 *   3. two-sided Möller–Trumbore each, keeping the nearest crossing;
 *   4. return its distance and the world surface normal (the triangle's
 *      geometric normal, oriented to face the ray).
 *
 * The triangle scratch grows on saturation and is bounded by {@link MAX_TRIS}
 * (a ray crossing more triangles than that in one mesh is not a realistic
 * raycast — it would mean skewering a 65k-triangle mesh lengthwise).
 *
 * @author Alex Goldring
 * @copyright Company Named Limited (c) 2026
 */

const lo = new Float64Array(3); // ray origin, shape-local
const ld = new Float64Array(3); // ray direction, shape-local
const bounds = new Float64Array(6); // shape local AABB

const INITIAL_TRIS = 1024;
const MAX_TRIS = 1 << 16;
let tri_buf = new Float64Array(INITIAL_TRIS * TRIANGLE_FLOAT_STRIDE);

const EPS = 1e-12;
const AABB_FATTEN = 1e-5; // guards triangles whose AABB just abuts the segment

/**
 * @param {AbstractShape3D} shape  a concave shape (`is_convex === false`)
 * @param {{x:number,y:number,z:number}} position
 * @param {ArrayLike<number>} rotation  body rotation quaternion (x,y,z,w)
 * @param {number} ox @param {number} oy @param {number} oz  ray origin (world)
 * @param {number} dx @param {number} dy @param {number} dz  ray dir (world, unit)
 * @param {number} tMax
 * @param {Float64Array|Vector3} outNormal  world surface normal, written on hit
 * @returns {number} hit distance, or `Infinity` on miss
 */
export function refine_ray_concave(shape, position, rotation, ox, oy, oz, dx, dy, dz, tMax, outNormal) {
    const qx = rotation[0], qy = rotation[1], qz = rotation[2], qw = rotation[3];
    // World → local (inverse body rotation; unit dir stays unit → t preserved).
    v3_quat3_apply(lo, 0, ox - position.x, oy - position.y, oz - position.z, -qx, -qy, -qz, qw);
    v3_quat3_apply(ld, 0, dx, dy, dz, -qx, -qy, -qz, qw);
    const lox = lo[0], loy = lo[1], loz = lo[2];
    const ldx = ld[0], ldy = ld[1], ldz = ld[2];

    // Clip the local ray to the shape's local AABB → segment [tEnter, tExit].
    shape.compute_bounding_box(bounds);
    let tEnter = 0, tExit = tMax;
    if (ldx > EPS || ldx < -EPS) {
        const inv = 1 / ldx;
        let t1 = (bounds[0] - lox) * inv, t2 = (bounds[3] - lox) * inv;
        if (t1 > t2) { const s = t1; t1 = t2; t2 = s; }
        if (t1 > tEnter) tEnter = t1;
        if (t2 < tExit) tExit = t2;
    } else if (lox < bounds[0] || lox > bounds[3]) return Infinity;
    if (ldy > EPS || ldy < -EPS) {
        const inv = 1 / ldy;
        let t1 = (bounds[1] - loy) * inv, t2 = (bounds[4] - loy) * inv;
        if (t1 > t2) { const s = t1; t1 = t2; t2 = s; }
        if (t1 > tEnter) tEnter = t1;
        if (t2 < tExit) tExit = t2;
    } else if (loy < bounds[1] || loy > bounds[4]) return Infinity;
    if (ldz > EPS || ldz < -EPS) {
        const inv = 1 / ldz;
        let t1 = (bounds[2] - loz) * inv, t2 = (bounds[5] - loz) * inv;
        if (t1 > t2) { const s = t1; t1 = t2; t2 = s; }
        if (t1 > tEnter) tEnter = t1;
        if (t2 < tExit) tExit = t2;
    } else if (loz < bounds[2] || loz > bounds[5]) return Infinity;
    if (tExit < tEnter) return Infinity; // ray misses the shape's AABB entirely

    // Query AABB = bbox of the clipped local segment.
    const sax = lox + ldx * tEnter, say = loy + ldy * tEnter, saz = loz + ldz * tEnter;
    const sbx = lox + ldx * tExit, sby = loy + ldy * tExit, sbz = loz + ldz * tExit;
    const qminx = (sax < sbx ? sax : sbx) - AABB_FATTEN;
    const qminy = (say < sby ? say : sby) - AABB_FATTEN;
    const qminz = (saz < sbz ? saz : sbz) - AABB_FATTEN;
    const qmaxx = (sax > sbx ? sax : sbx) + AABB_FATTEN;
    const qmaxy = (say > sby ? say : sby) + AABB_FATTEN;
    const qmaxz = (saz > sbz ? saz : sbz) + AABB_FATTEN;

    const enumerate = shape.isHeightMapShape3D === true ? heightmap_enumerate_triangles : mesh_enumerate_triangles;
    let cap = (tri_buf.length / TRIANGLE_FLOAT_STRIDE) | 0;
    // The enumerators return the TRUE overlap count, which can EXCEED the buffer
    // capacity (writes past the end are silently dropped). So the grow trigger is
    // `count > cap` (not `=== cap`): regrow whenever the true count overflowed.
    let count = enumerate(tri_buf, 0, shape, qminx, qminy, qminz, qmaxx, qmaxy, qmaxz);
    while (count > cap && cap < MAX_TRIS) {
        cap = cap * 2 < MAX_TRIS ? cap * 2 : MAX_TRIS;
        tri_buf = new Float64Array(cap * TRIANGLE_FLOAT_STRIDE);
        count = enumerate(tri_buf, 0, shape, qminx, qminy, qminz, qmaxx, qmaxy, qmaxz);
    }
    // If the true count still exceeds MAX_TRIS, only `cap` triangles were written;
    // clamp so the consume loop never reads undefined past the buffer (which would
    // poison `best` with NaN). Dropping the overflow is the documented MAX_TRIS bound.
    const n_tris = count < cap ? count : cap;

    // Two-sided Möller–Trumbore over the candidates; keep the nearest.
    let best = tMax;
    let found = false;
    let nx = 0, ny = 0, nz = 0;
    for (let i = 0; i < n_tris; i++) {
        const o = i * TRIANGLE_FLOAT_STRIDE;
        const ax = tri_buf[o], ay = tri_buf[o + 1], az = tri_buf[o + 2];
        const e1x = tri_buf[o + 3] - ax, e1y = tri_buf[o + 4] - ay, e1z = tri_buf[o + 5] - az;
        const e2x = tri_buf[o + 6] - ax, e2y = tri_buf[o + 7] - ay, e2z = tri_buf[o + 8] - az;

        const px = ldy * e2z - ldz * e2y, py = ldz * e2x - ldx * e2z, pz = ldx * e2y - ldy * e2x;
        const det = e1x * px + e1y * py + e1z * pz;
        if (det < EPS && det > -EPS) continue; // ray parallel to the triangle
        const invDet = 1 / det;

        const tvx = lox - ax, tvy = loy - ay, tvz = loz - az;
        const u = (tvx * px + tvy * py + tvz * pz) * invDet;
        if (u < 0 || u > 1) continue;

        const wx = tvy * e1z - tvz * e1y, wy = tvz * e1x - tvx * e1z, wz = tvx * e1y - tvy * e1x;
        const v = (ldx * wx + ldy * wy + ldz * wz) * invDet;
        if (v < 0 || u + v > 1) continue;

        const t = (e2x * wx + e2y * wy + e2z * wz) * invDet;
        if (t < 0 || t >= best) continue;

        best = t;
        found = true;
        // Geometric normal e1 × e2 (local, unnormalised).
        nx = e1y * e2z - e1z * e2y;
        ny = e1z * e2x - e1x * e2z;
        nz = e1x * e2y - e1y * e2x;
    }

    if (!found) return Infinity;

    // Normalise, orient to face the ray, then rotate the local normal to world.
    let nl = Math.sqrt(nx * nx + ny * ny + nz * nz);
    if (nl === 0) nl = 1;
    nx /= nl; ny /= nl; nz /= nl;
    if (nx * ldx + ny * ldy + nz * ldz > 0) { nx = -nx; ny = -ny; nz = -nz; }
    v3_quat3_apply(outNormal, 0, nx, ny, nz, qx, qy, qz, qw);
    return best;
}
