import { assert } from "../../../../core/assert.js";
import { NULL_NODE } from "../../../../core/bvh2/bvh3/BVH.js";
import {
    bvh_query_leaves_ray_segment
} from "../../../../core/bvh2/bvh3/query/bvh_query_leaves_ray_segment.js";
import { NULL_POINTER } from "../../../../core/geom/3d/topology/struct/binary/BinaryTopology.js";
import { compute_triangle_area_3d } from "../../../../core/geom/3d/triangle/compute_triangle_area_3d.js";
import {
    computeTriangleRayIntersectionBarycentric
} from "../../../../core/geom/3d/triangle/computeTriangleRayIntersectionBarycentric.js";
import { clamp } from "../../../../core/math/clamp.js";

/**
 * Upper bound on the per-edge subdivision factor, so a single huge triangle cannot explode into an
 * unbounded number of sub-triangles. At this cap a triangle is sampled with up to K_MAX^2 points.
 * @type {number}
 */
const K_MAX = 64;

/**
 * Small offset (in world units) used to lift a ray origin off the surface it sits on, so the upward
 * clearance ray does not immediately re-intersect the floor it starts from.
 * @type {number}
 */
const SURFACE_EPSILON = 1e-4;

// reused scratch (build-time, but there's no reason to churn allocations per triangle)
const ray_leaf_buffer = [];
const intersection_result = new Float32Array(6);
const tri_a = new Float32Array(3);
const tri_b = new Float32Array(3);
const tri_c = new Float32Array(3);

/**
 * True if there is no source geometry within `agent_height` directly above the point, measured along
 * the world up direction. Casts a ray up from the point (nudged off the surface) and checks every
 * source face whose AABB the ray crosses with a precise ray-triangle test.
 *
 * @param {BVH} bvh source-geometry BVH (leaves carry source face IDs)
 * @param {BinaryTopology} source source mesh, used to resolve face IDs to triangle vertices
 * @param {number} px
 * @param {number} py
 * @param {number} pz
 * @param {number} up_x normalized world up
 * @param {number} up_y
 * @param {number} up_z
 * @param {number} agent_height
 * @returns {boolean}
 */
function point_has_clearance(bvh, source, px, py, pz, up_x, up_y, up_z, agent_height) {
    const origin_x = px + up_x * SURFACE_EPSILON;
    const origin_y = py + up_y * SURFACE_EPSILON;
    const origin_z = pz + up_z * SURFACE_EPSILON;

    const leaf_count = bvh_query_leaves_ray_segment(
        bvh, bvh.root,
        ray_leaf_buffer, 0,
        origin_x, origin_y, origin_z,
        up_x, up_y, up_z,
        0, agent_height
    );

    for (let i = 0; i < leaf_count; i++) {
        const node = ray_leaf_buffer[i];
        const face_id = bvh.node_get_user_data(node);

        const loop_a = source.face_read_loop(face_id);

        if (loop_a === NULL_POINTER) {
            continue;
        }

        const loop_b = source.loop_read_next(loop_a);
        const loop_c = source.loop_read_next(loop_b);

        source.vertex_read_coordinate(tri_a, 0, source.loop_read_vertex(loop_a));
        source.vertex_read_coordinate(tri_b, 0, source.loop_read_vertex(loop_b));
        source.vertex_read_coordinate(tri_c, 0, source.loop_read_vertex(loop_c));

        const hit = computeTriangleRayIntersectionBarycentric(
            intersection_result,
            origin_x, origin_y, origin_z,
            up_x, up_y, up_z,
            tri_a[0], tri_a[1], tri_a[2],
            tri_b[0], tri_b[1], tri_b[2],
            tri_c[0], tri_c[1], tri_c[2]
        );

        if (!hit) {
            continue;
        }

        const t = intersection_result[0];

        if (t > 0 && t <= agent_height) {
            // an overhead obstruction sits within the agent's height
            return false;
        }
    }

    return true;
}

/**
 * Append a triangle (9 floats) to `output`.
 */
function emit_triangle(
    output,
    ax, ay, az,
    bx, by, bz,
    cx, cy, cz
) {
    output.push(ax, ay, az, bx, by, bz, cx, cy, cz);
}

/**
 * Knock holes in the walkable triangle soup wherever an agent of the given height would not fit under
 * overhead geometry.
 *
 * Approach: subdivide each candidate triangle to a resolution tied to the agent's footprint, cast an
 * upward clearance ray from each sub-triangle centroid against the source geometry, and keep only the
 * sub-triangles that have clearance. Fully-clear triangles are emitted unchanged (no subdivision), so
 * the common case adds no triangles; fully-blocked triangles are dropped; partially-blocked triangles
 * are replaced by their clear sub-triangles. Accuracy is bounded by the sampling resolution.
 *
 * The `triangles` array is rewritten in place with the surviving triangles.
 *
 * @param {BVH} bvh source-geometry BVH (leaves carry source face IDs)
 * @param {BinaryTopology} source source mesh, used to resolve face IDs to triangle vertices
 * @param {number} agent_height
 * @param {number} agent_radius
 * @param {number} triangle_count number of triangles currently in `triangles`
 * @param {number[]} triangles flat XYZ soup, 9 floats per triangle
 * @param {Vector3} up world up direction
 * @returns {number} new triangle count
 */
export function enforce_agent_height_clearance({
                                                   bvh,
                                                   source,
                                                   agent_height,
                                                   agent_radius,
                                                   triangle_count,
                                                   triangles,
                                                   up,
                                               }) {

    assert.defined(bvh, 'bvh');
    assert.defined(source, 'source');
    assert.greaterThan(agent_height, 0, 'agent_height');

    if (bvh.root === NULL_NODE) {
        // no source geometry to obstruct anything
        return triangle_count;
    }

    // normalize up so ray-intersection distances are world distances
    let up_x = up.x;
    let up_y = up.y;
    let up_z = up.z;

    const up_length = Math.sqrt(up_x * up_x + up_y * up_y + up_z * up_z);

    assert.greaterThan(up_length, 0, 'up vector length');

    up_x /= up_length;
    up_y /= up_length;
    up_z /= up_length;

    /**
     * Resolution at which clearance is sampled. Tied to the agent footprint so the sampling grid is
     * fine enough to resolve gaps the agent could (or could not) fit through.
     * @type {number}
     */
    const sampling_resolution = Math.min(
        agent_height / 4,
        Math.max(agent_radius / 2, 0.001)
    );

    assert.isFinite(sampling_resolution, 'sampling_resolution');
    assert.greaterThan(sampling_resolution, 0, 'sampling_resolution');

    /**
     * Surviving triangles. Built separately because the count can grow (subdivision), then copied back.
     * @type {number[]}
     */
    const output = [];

    for (let i = 0; i < triangle_count; i++) {
        const address = i * 9;

        const ax = triangles[address];
        const ay = triangles[address + 1];
        const az = triangles[address + 2];

        const bx = triangles[address + 3];
        const by = triangles[address + 4];
        const bz = triangles[address + 5];

        const cx = triangles[address + 6];
        const cy = triangles[address + 7];
        const cz = triangles[address + 8];

        const area = compute_triangle_area_3d(ax, ay, az, bx, by, bz, cx, cy, cz);

        if (area === 0) {
            // degenerate, nothing meaningful to sample - keep as-is
            emit_triangle(output, ax, ay, az, bx, by, bz, cx, cy, cz);
            continue;
        }

        // edges, used both to size the subdivision and to place sub-triangle vertices
        const e0x = bx - ax, e0y = by - ay, e0z = bz - az; // a -> b
        const e1x = cx - ax, e1y = cy - ay, e1z = cz - az; // a -> c

        const len_ab = Math.sqrt(e0x * e0x + e0y * e0y + e0z * e0z);
        const len_ac = Math.sqrt(e1x * e1x + e1y * e1y + e1z * e1z);
        const dbcx = cx - bx, dbcy = cy - by, dbcz = cz - bz;
        const len_bc = Math.sqrt(dbcx * dbcx + dbcy * dbcy + dbcz * dbcz);

        const longest_edge = Math.max(len_ab, len_ac, len_bc);

        const k = clamp(Math.ceil(longest_edge / sampling_resolution), 1, K_MAX);

        const inv_k = 1 / k;

        // Sub-triangle vertices are P(i,j) = a + (i/k)*(b-a) + (j/k)*(c-a), for i+j <= k.
        // Walk the k^2 sub-triangles (k*(k+... )/...) testing each centroid for clearance.
        let any_blocked = false;
        let any_clear = false;

        // reuse a per-triangle scratch list of clear sub-triangles only when we actually subdivide
        const clear_subtriangles = [];

        for (let row = 0; row < k; row++) {
            for (let col = 0; col < k - row; col++) {
                // "upward" sub-triangle: P(col,row), P(col+1,row), P(col,row+1)
                emit_subtriangle_if_clear(
                    bvh, source, agent_height, up_x, up_y, up_z,
                    ax, ay, az, e0x, e0y, e0z, e1x, e1y, e1z, inv_k,
                    col, row, col + 1, row, col, row + 1,
                    clear_subtriangles
                ) ? (any_clear = true) : (any_blocked = true);

                // "downward" sub-triangle: P(col+1,row), P(col+1,row+1), P(col,row+1)
                if (col < k - row - 1) {
                    emit_subtriangle_if_clear(
                        bvh, source, agent_height, up_x, up_y, up_z,
                        ax, ay, az, e0x, e0y, e0z, e1x, e1y, e1z, inv_k,
                        col + 1, row, col + 1, row + 1, col, row + 1,
                        clear_subtriangles
                    ) ? (any_clear = true) : (any_blocked = true);
                }
            }
        }

        if (!any_blocked) {
            // fully clear - keep the original triangle, no subdivision, no bloat
            emit_triangle(output, ax, ay, az, bx, by, bz, cx, cy, cz);
        } else if (any_clear) {
            // partially blocked - replace with the surviving sub-triangles
            for (let s = 0; s < clear_subtriangles.length; s++) {
                output.push(clear_subtriangles[s]);
            }
        }
        // else fully blocked - drop the triangle entirely
    }

    // rewrite the soup in place with the survivors
    for (let i = 0; i < output.length; i++) {
        triangles[i] = output[i];
    }
    triangles.length = output.length;

    return (output.length / 9) | 0;
}

/**
 * Build sub-triangle vertices P(i,j) = a + (i/k)*e0 + (j/k)*e1, test its centroid for clearance, and
 * push its 9 coordinates into `out` when clear. Returns whether it was clear.
 */
function emit_subtriangle_if_clear(
    bvh, source, agent_height, up_x, up_y, up_z,
    ax, ay, az, e0x, e0y, e0z, e1x, e1y, e1z, inv_k,
    i0, j0, i1, j1, i2, j2,
    out
) {
    const s0 = i0 * inv_k, t0 = j0 * inv_k;
    const s1 = i1 * inv_k, t1 = j1 * inv_k;
    const s2 = i2 * inv_k, t2 = j2 * inv_k;

    const p0x = ax + s0 * e0x + t0 * e1x;
    const p0y = ay + s0 * e0y + t0 * e1y;
    const p0z = az + s0 * e0z + t0 * e1z;

    const p1x = ax + s1 * e0x + t1 * e1x;
    const p1y = ay + s1 * e0y + t1 * e1y;
    const p1z = az + s1 * e0z + t1 * e1z;

    const p2x = ax + s2 * e0x + t2 * e1x;
    const p2y = ay + s2 * e0y + t2 * e1y;
    const p2z = az + s2 * e0z + t2 * e1z;

    const centroid_x = (p0x + p1x + p2x) / 3;
    const centroid_y = (p0y + p1y + p2y) / 3;
    const centroid_z = (p0z + p1z + p2z) / 3;

    const clear = point_has_clearance(
        bvh, source,
        centroid_x, centroid_y, centroid_z,
        up_x, up_y, up_z,
        agent_height
    );

    if (clear) {
        out.push(p0x, p0y, p0z, p1x, p1y, p1z, p2x, p2y, p2z);
    }

    return clear;
}
