import { TRIANGLE_FEATURE_ID_OFFSET, TRIANGLE_FLOAT_STRIDE } from "./triangle_buffer_layout.js";

/**
 * Write the heightmap's triangles that may overlap the given body-local
 * query AABB into `output`, starting at float-index `offset`.
 *
 * Each triangle occupies {@link TRIANGLE_FLOAT_STRIDE} consecutive floats:
 *   [+0..+2]  vA.xyz   (body-local frame)
 *   [+3..+5]  vB.xyz
 *   [+6..+8]  vC.xyz
 *   [+9]      feature_id   (stable across frames; warm-start key)
 *
 * ## Tessellation
 *
 * Each sampler cell is split into `N × N` sub-cells, where `N =
 * shape.tessellation` (a non-negative integer, validated at construction).
 * The sub-cell corners are sampled with the same Catmull-Rom filter the
 * surface uses everywhere else, so finer `N` makes the faceted collision
 * surface converge toward the smooth surface the renderer draws (which
 * samples the identical cubic, just at its own `size × resolution` density).
 * `N = 1` emits exactly one quad per sampler cell — the legacy behaviour;
 * `N = 0` emits nothing. Cost is O(N²) per cell, so the caller owns the
 * fidelity/cost trade-off.
 *
 * The collision grid therefore has `seg_u = (W − 1)·N` segments along u and
 * `seg_v = (H − 1)·N` along v. The feature_id encodes
 * `(sub_y · seg_u + sub_x) · 2 + tri_idx`, where (sub_x, sub_y) is the
 * sub-cell and tri_idx ∈ {0, 1}: a unique, frame-stable key per sub-cell
 * triangle (`seg_u · seg_v · 2` stays well under 2^53 for any realistic
 * sampler × N). Two triangles per sub-cell, lower-left-diagonal split
 * (matches the existing {@link build_height_field_geometry} winding).
 *
 * The query AABB is filtered against the footprint *and* against the
 * cell grid; triangles outside the query AABB along the height axis are
 * NOT filtered out — that test is left to the per-triangle narrowphase
 * (where the precise GJK distance is computed anyway). This keeps the
 * enumerator branch-free over the height field and aligned with what
 * Bullet's btHeightfieldTerrainShape does.
 *
 * The caller is responsible for ensuring
 *   `output.length - offset >= sub_cells_in_query * 2 * TRIANGLE_FLOAT_STRIDE`
 * — sizing the scratch buffer for the *worst-case* query AABB is the
 * right pattern. The broadphase already bounded the AABB to one body's
 * world envelope, so the sampler-cell count is bounded by the footprint
 * resolution; note the sub-cell count (and thus the buffer requirement)
 * scales by N² with {@link HeightMapShape3D#tessellation}. Writes past the
 * end of `output` are silently dropped by the typed array, so an
 * undersized buffer degrades to a missed contact on a far cell (recovered
 * next broadphase), never a crash.
 *
 * @param {Float64Array} output
 * @param {number} offset float-index into output
 * @param {HeightMapShape3D} shape
 * @param {number} aabb_min_x  query AABB in shape's body-local frame
 * @param {number} aabb_min_y
 * @param {number} aabb_min_z
 * @param {number} aabb_max_x
 * @param {number} aabb_max_y
 * @param {number} aabb_max_z
 * @returns {number} number of triangles written
 */
export function heightmap_enumerate_triangles(
    output, offset, shape,
    aabb_min_x, aabb_min_y, aabb_min_z,
    aabb_max_x, aabb_max_y, aabb_max_z
) {
    const sampler = shape.sampler;

    if (sampler === null) {
        return 0;
    }

    const W = sampler.width;
    const H = sampler.height;

    if (W < 2 || H < 2) {
        // need at least one cell (one cell = 2 vertices per axis)
        return 0;
    }

    const size_x = shape.size[0];
    const size_z = shape.size[2];

    if (size_x <= 0 || size_z <= 0) {
        return 0;
    }

    // Collision tessellation: split each sampler cell into N×N sub-cells so a
    // coarse sampler's contact surface can approach render density. N is a
    // non-negative integer (validated by HeightMapShape3D.from); N=1
    // reproduces the legacy one-quad-per-sampler-cell grid exactly (seg_u =
    // W-1, seg_v = H-1), and N=0 collapses the grid so the cell-range check
    // below returns 0 — the degenerate empty case, handled without a branch.
    const N = shape.tessellation;

    const seg_u = (W - 1) * N;
    const seg_v = (H - 1) * N;

    // ── Project body-local AABB into heightmap-local (u, v, h) frame ─────
    // Basis rows = (u_axis, v_axis, n_axis) in body-local frame. The
    // body→heightmap rotation is therefore B (rows of basis), and the
    // Arvo trick gives us the projected AABB extents.

    shape._ensure_basis();
    const b = shape._basis;

    const c_x = 0.5 * (aabb_min_x + aabb_max_x);
    const c_y = 0.5 * (aabb_min_y + aabb_max_y);
    const c_z = 0.5 * (aabb_min_z + aabb_max_z);

    const h_x = 0.5 * (aabb_max_x - aabb_min_x);
    const h_y = 0.5 * (aabb_max_y - aabb_min_y);
    const h_z = 0.5 * (aabb_max_z - aabb_min_z);

    const u_c = b[0] * c_x + b[1] * c_y + b[2] * c_z;
    const v_c = b[3] * c_x + b[4] * c_y + b[5] * c_z;
    // height axis (b[6..8]) is intentionally not projected — see fn docstring

    const u_e = Math.abs(b[0]) * h_x + Math.abs(b[1]) * h_y + Math.abs(b[2]) * h_z;
    const v_e = Math.abs(b[3]) * h_x + Math.abs(b[4]) * h_y + Math.abs(b[5]) * h_z;

    const half_u = size_x * 0.5;
    const half_v = size_z * 0.5;

    // ── Intersect with footprint, derive cell range ──────────────────────

    const u_lo_q = u_c - u_e;
    const u_hi_q = u_c + u_e;
    const v_lo_q = v_c - v_e;
    const v_hi_q = v_c + v_e;

    if (u_lo_q > half_u || u_hi_q < -half_u) return 0;
    if (v_lo_q > half_v || v_hi_q < -half_v) return 0;

    const u_lo = u_lo_q > -half_u ? u_lo_q : -half_u;
    const u_hi = u_hi_q < +half_u ? u_hi_q : +half_u;
    const v_lo = v_lo_q > -half_v ? v_lo_q : -half_v;
    const v_hi = v_hi_q < +half_v ? v_hi_q : +half_v;

    const inv_cell_u = seg_u / size_x;
    const inv_cell_v = seg_v / size_z;

    let i_first = Math.floor((u_lo + half_u) * inv_cell_u);
    let i_last  = Math.ceil ((u_hi + half_u) * inv_cell_u) - 1;
    let j_first = Math.floor((v_lo + half_v) * inv_cell_v);
    let j_last  = Math.ceil ((v_hi + half_v) * inv_cell_v) - 1;

    if (i_first < 0) i_first = 0;
    if (j_first < 0) j_first = 0;
    if (i_last  > seg_u - 1) i_last  = seg_u - 1;
    if (j_last  > seg_v - 1) j_last  = seg_v - 1;

    if (i_first > i_last || j_first > j_last) return 0;

    // ── Emit triangles ───────────────────────────────────────────────────
    //
    // For each sub-cell (i, j) on the seg_u × seg_v grid we sample 4 corner
    // heights (Catmull-Rom, at the sub-cell UVs) and emit 2 triangles
    // matching the build_height_field_geometry winding:
    //   A = (i,   j  ),  B = (i+1, j  )
    //   D = (i,   j+1),  C = (i+1, j+1)
    //   tri 0 = (A, D, B)
    //   tri 1 = (D, C, B)
    //
    // body_point = u_axis * u_coord + v_axis * v_coord + n_axis * h_coord
    // where columns of the body-frame projection matrix are the basis rows
    // of B (since B is orthonormal, body→heightmap and heightmap→body
    // matrices are mutual transposes).

    const ux = b[0]; const uy = b[1]; const uz = b[2];
    const vx = b[3]; const vy = b[4]; const vz = b[5];
    const nx = b[6]; const ny = b[7]; const nz = b[8];

    const inv_segU = 1 / seg_u;
    const inv_segV = 1 / seg_v;

    let count = 0;
    let cursor = offset;

    for (let j = j_first; j <= j_last; j++) {
        const v01_lo = j * inv_segV;
        const v01_hi = (j + 1) * inv_segV;
        const v_lo_coord = -half_v + size_z * v01_lo;
        const v_hi_coord = -half_v + size_z * v01_hi;

        for (let i = i_first; i <= i_last; i++) {
            const u01_lo = i * inv_segU;
            const u01_hi = (i + 1) * inv_segU;
            const u_lo_coord = -half_u + size_x * u01_lo;
            const u_hi_coord = -half_u + size_x * u01_hi;

            const hA = sampler.sampleChannelCatmullRomUV(u01_lo, v01_lo, 0);
            const hB = sampler.sampleChannelCatmullRomUV(u01_hi, v01_lo, 0);
            const hC = sampler.sampleChannelCatmullRomUV(u01_hi, v01_hi, 0);
            const hD = sampler.sampleChannelCatmullRomUV(u01_lo, v01_hi, 0);

            const Ax = ux * u_lo_coord + vx * v_lo_coord + nx * hA;
            const Ay = uy * u_lo_coord + vy * v_lo_coord + ny * hA;
            const Az = uz * u_lo_coord + vz * v_lo_coord + nz * hA;

            const Bx = ux * u_hi_coord + vx * v_lo_coord + nx * hB;
            const By = uy * u_hi_coord + vy * v_lo_coord + ny * hB;
            const Bz = uz * u_hi_coord + vz * v_lo_coord + nz * hB;

            const Cx = ux * u_hi_coord + vx * v_hi_coord + nx * hC;
            const Cy = uy * u_hi_coord + vy * v_hi_coord + ny * hC;
            const Cz = uz * u_hi_coord + vz * v_hi_coord + nz * hC;

            const Dx = ux * u_lo_coord + vx * v_hi_coord + nx * hD;
            const Dy = uy * u_lo_coord + vy * v_hi_coord + ny * hD;
            const Dz = uz * u_lo_coord + vz * v_hi_coord + nz * hD;

            const cell_idx = j * seg_u + i;
            const fid_base = cell_idx * 2;

            // triangle 0: (A, D, B)
            output[cursor    ] = Ax; output[cursor + 1] = Ay; output[cursor + 2] = Az;
            output[cursor + 3] = Dx; output[cursor + 4] = Dy; output[cursor + 5] = Dz;
            output[cursor + 6] = Bx; output[cursor + 7] = By; output[cursor + 8] = Bz;
            output[cursor + TRIANGLE_FEATURE_ID_OFFSET] = fid_base;
            cursor += TRIANGLE_FLOAT_STRIDE;
            count++;

            // triangle 1: (D, C, B)
            output[cursor    ] = Dx; output[cursor + 1] = Dy; output[cursor + 2] = Dz;
            output[cursor + 3] = Cx; output[cursor + 4] = Cy; output[cursor + 5] = Cz;
            output[cursor + 6] = Bx; output[cursor + 7] = By; output[cursor + 8] = Bz;
            output[cursor + TRIANGLE_FEATURE_ID_OFFSET] = fid_base + 1;
            cursor += TRIANGLE_FLOAT_STRIDE;
            count++;
        }
    }

    return count;
}
