import { SCRATCH_UINT32_TRAVERSAL_STACK } from "../../../collection/SCRATCH_UINT32_TRAVERSAL_STACK.js";
import { aabb3_unsigned_distance_sqr_to_point } from "../../../geom/3d/aabb/aabb3_unsigned_distance_sqr_to_point.js";
import { COLUMN_CHILD_1, COLUMN_CHILD_2, ELEMENT_WORD_COUNT, NULL_NODE } from "../BVH.js";

const stack = SCRATCH_UINT32_TRAVERSAL_STACK;

/**
 *
 * @param {BVH} bvh
 * @param {number} x
 * @param {number} y
 * @param {number} z
 * @param {number} [max_distance=Infinity] how far to search
 * @returns {number} user data of the leaf, or -1 if not found
 */
export function bvh_query_user_data_nearest_to_point(bvh, x, y, z, max_distance = Infinity) {
    const root = bvh.root;

    if (root === NULL_NODE) {
        return -1;
    }

    /**
     *
     * @type {number}
     */
    const stack_top = stack.pointer++;

    stack[stack_top] = root;

    let best_leaf = -1;
    let best_distance_sqr = max_distance * max_distance;


    /*
     For performance, we bind data directly to avoid extra copies required to read out AABB
     */
    const float32 = bvh.__data_float32;
    const uint32 = bvh.__data_uint32;

    do {
        stack.pointer--;

        /**
         *
         * @type {number}
         */
        const node = stack[stack.pointer];

        const address = node * ELEMENT_WORD_COUNT;

        // compute distance to box
        const 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 (distance_sqr >= best_distance_sqr) {
            // node is too far
            continue;
        }

        // get fist child to check if this is a leaf node or not
        const child_1 = uint32[address + COLUMN_CHILD_1];

        if (child_1 !== NULL_NODE) {

            // this is not a leaf node, push children onto traversal stack
            stack[stack.pointer++] = child_1;
            stack[stack.pointer++] = uint32[address + COLUMN_CHILD_2];

        } else {

            // leaf node
            best_distance_sqr = distance_sqr;
            best_leaf = node;

        }
    } while (stack.pointer > stack_top)

    if (best_leaf === -1) {
        return -1;
    }

    return bvh.node_get_user_data(best_leaf);
}
