import { assert } from "../../../core/assert.js";
import {
    COLUMN_CHILD_1,
    COLUMN_CHILD_2,
    COLUMN_USER_DATA,
    ELEMENT_WORD_COUNT,
    NULL_NODE
} from "../../../core/bvh2/bvh3/BVH.js";
import { SCRATCH_UINT32_TRAVERSAL_STACK } from "../../../core/collection/SCRATCH_UINT32_TRAVERSAL_STACK.js";
import {
    aabb3_unsigned_distance_sqr_to_point
} from "../../../core/geom/3d/aabb/aabb3_unsigned_distance_sqr_to_point.js";
import { NULL_POINTER } from "../../../core/geom/3d/topology/struct/binary/BinaryTopology.js";
import {
    computeTriangleClosestPointToPointBarycentric
} from "../../../core/geom/3d/triangle/computeTriangleClosestPointToPointBarycentric.js";

const stack = SCRATCH_UINT32_TRAVERSAL_STACK;

// single reusable coordinate scratch; callers unpack into locals between the calls that fill it
const scratch_coords = new Float32Array(3);

/**
 * Find the face of `mesh` whose surface is closest to the given point.
 * Expects `bvh` to have been built by {@link bvh_build_from_bt_mesh}, so each leaf's user data is a face ID.
 * Faces are expected to be triangles (as produced by the navmesh build pipeline).
 *
 * @param {BVH} bvh
 * @param {BinaryTopology} mesh
 * @param {number} x
 * @param {number} y
 * @param {number} z
 * @param {Float32Array|number[]} out_point the closest point ON the winning face's surface (the snapped
 *        contact point) is written here when a face is found. Required, must not be null.
 * @param {number} [out_point_offset=0] offset into `out_point` to write the snapped XYZ triple
 * @param {number} [max_distance=Infinity] optional cutoff, only faces within this distance are considered
 * @returns {number} face ID of the nearest face, or {@link NULL_POINTER} if no face was found within the cutoff
 */
export function bvh_query_nearest_face(
    bvh,
    mesh,
    x, y, z,
    out_point,
    out_point_offset = 0,
    max_distance = Infinity
) {

    assert.defined(bvh, "bvh");
    assert.defined(mesh, "mesh");
    assert.notNull(out_point, "out_point");
    assert.defined(out_point, "out_point");
    assert.equal(mesh.isBinaryTopology, true, "mesh.isBinaryTopology !== true");

    const root = bvh.root;

    if (root === NULL_NODE) {
        return NULL_POINTER;
    }

    const stack_top = stack.pointer++;

    stack[stack_top] = root;

    let best_face = NULL_POINTER;
    let best_distance_sqr = max_distance * max_distance;

    // closest point on the winning face's surface, tracked so callers can snap a query point onto the mesh
    let best_contact_x = 0;
    let best_contact_y = 0;
    let best_contact_z = 0;

    const float32 = bvh.__data_float32;
    const uint32 = bvh.__data_uint32;

    do {
        stack.pointer--;

        const node = stack[stack.pointer];
        const address = node * ELEMENT_WORD_COUNT;

        // lower bound on distance via AABB
        const aabb_distance_sqr = aabb3_unsigned_distance_sqr_to_point(
            float32[address], float32[address + 1], float32[address + 2],
            float32[address + 3], float32[address + 4], float32[address + 5],
            x, y, z
        );

        if (aabb_distance_sqr >= best_distance_sqr) {
            // this subtree cannot contain a closer face
            continue;
        }

        const child_1 = uint32[address + COLUMN_CHILD_1];

        if (child_1 !== NULL_NODE) {
            // internal node, descend into both children
            stack[stack.pointer++] = child_1;
            stack[stack.pointer++] = uint32[address + COLUMN_CHILD_2];
            continue;
        }

        // leaf node: refine with actual triangle distance
        const face_id = uint32[address + COLUMN_USER_DATA];

        const loop_a = mesh.face_read_loop(face_id);

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

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

        // read each vertex into the shared scratch, unpacking into locals before the next read overwrites it
        mesh.vertex_read_coordinate(scratch_coords, 0, mesh.loop_read_vertex(loop_a));
        const ax = scratch_coords[0], ay = scratch_coords[1], az = scratch_coords[2];

        mesh.vertex_read_coordinate(scratch_coords, 0, mesh.loop_read_vertex(loop_b));
        const bx = scratch_coords[0], by = scratch_coords[1], bz = scratch_coords[2];

        mesh.vertex_read_coordinate(scratch_coords, 0, mesh.loop_read_vertex(loop_c));
        const cx = scratch_coords[0], cy = scratch_coords[1], cz = scratch_coords[2];

        // reuse the same scratch for the barycentric output (only the first two slots are written)
        computeTriangleClosestPointToPointBarycentric(
            scratch_coords, 0,
            x, y, z,
            ax, ay, az,
            bx, by, bz,
            cx, cy, cz
        );

        const weight_a = scratch_coords[0];
        const weight_b = scratch_coords[1];
        const weight_c = 1 - weight_a - weight_b;

        const contact_x = ax * weight_a + bx * weight_b + cx * weight_c;
        const contact_y = ay * weight_a + by * weight_b + cy * weight_c;
        const contact_z = az * weight_a + bz * weight_b + cz * weight_c;

        const dx = contact_x - x;
        const dy = contact_y - y;
        const dz = contact_z - z;

        const triangle_distance_sqr = dx * dx + dy * dy + dz * dz;

        if (triangle_distance_sqr < best_distance_sqr) {
            best_distance_sqr = triangle_distance_sqr;
            best_face = face_id;

            best_contact_x = contact_x;
            best_contact_y = contact_y;
            best_contact_z = contact_z;
        }
    } while (stack.pointer > stack_top);

    if (best_face !== NULL_POINTER) {
        out_point[out_point_offset] = best_contact_x;
        out_point[out_point_offset + 1] = best_contact_y;
        out_point[out_point_offset + 2] = best_contact_z;
    }

    return best_face;
}
