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 {
    computeTriangleClosestPointToPointBarycentric
} from "../../../../../core/geom/3d/triangle/computeTriangleClosestPointToPointBarycentric.js";
import { v3_compute_triangle_normal } from "../../../../../core/geom/3d/triangle/v3_compute_triangle_normal.js";
import { v3_distance_sqr } from "../../../../../core/geom/vec3/v3_distance_sqr.js";

/**
 *
 * @type {number[]}
 */
const v3_scratch_0 = [];
const stack = SCRATCH_UINT32_TRAVERSAL_STACK;

/**
 *
 * @param {SurfacePoint3} result
 * @param {BVH} bvh
 * @param {number[]|Float32Array} positions
 * @param {number[]|Uint32Array|Uint16Array} indices
 * @param {number} x
 * @param {number} y
 * @param {number} z
 * @param {number} max_distance
 * @return {boolean}
 */
export function query_bvh_geometry_nearest(
    result,
    bvh,
    positions,
    indices,
    x, y, z,
    max_distance
) {

    const root = bvh.root;

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


    /**
     * Move stack pointer to local variable scope to avoid de-referencing inside the loop
     * @type {number}
     */
    let pointer = stack.pointer;

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

    stack[pointer++] = root;

    /*
     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;

    let nearest_distance_sqr = max_distance * max_distance;

    let got_a_hit = false;

    do {
        --pointer;

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

        const address = node * ELEMENT_WORD_COUNT;

        // test node against the ray


        // 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
            const child_2 = uint32[address + COLUMN_CHILD_2];

            // to achieve faster convergence, we sort children before adding them to the stack by their proximity to the reference point
            const child_1_address = child_1 * ELEMENT_WORD_COUNT;

            const distance_sqr_to_child1 = aabb3_unsigned_distance_sqr_to_point(
                float32[child_1_address], float32[child_1_address + 1], float32[child_1_address + 2],
                float32[child_1_address + 3], float32[child_1_address + 4], float32[child_1_address + 5],
                x, y, z
            );

            const child_2_address = child_2 * ELEMENT_WORD_COUNT;

            const distance_sqr_to_child2 = aabb3_unsigned_distance_sqr_to_point(
                float32[child_2_address], float32[child_2_address + 1], float32[child_2_address + 2],
                float32[child_2_address + 3], float32[child_2_address + 4], float32[child_2_address + 5],
                x, y, z
            );

            if (distance_sqr_to_child1 < distance_sqr_to_child2) {

                if (distance_sqr_to_child2 < nearest_distance_sqr) {
                    stack[pointer++] = child_2;
                }

                if (distance_sqr_to_child1 < nearest_distance_sqr) {
                    stack[pointer++] = child_1;
                }

            } else {

                if (distance_sqr_to_child1 < nearest_distance_sqr) {
                    stack[pointer++] = child_1;
                }

                if (distance_sqr_to_child2 < nearest_distance_sqr) {
                    stack[pointer++] = child_2;
                }

            }


        } else {
            // leaf node
            // read triangle data
            const triangle_index = uint32[address + COLUMN_USER_DATA];
            const index_offset = triangle_index*3;

            const a = indices[index_offset];
            const b = indices[index_offset + 1];
            const c = indices[index_offset + 2];


            const a_address = a * 3;
            const b_address = b * 3;
            const c_address = c * 3;

            const ax = positions[a_address];
            const ay = positions[a_address + 1];
            const az = positions[a_address + 2];

            const bx = positions[b_address];
            const by = positions[b_address + 1];
            const bz = positions[b_address + 2];

            const cx = positions[c_address];
            const cy = positions[c_address + 1];
            const cz = positions[c_address + 2];

            computeTriangleClosestPointToPointBarycentric(
                v3_scratch_0, 0,
                x, y, z,
                ax, ay, az,
                bx, by, bz,
                cx, cy, cz
            );

            const u = v3_scratch_0[0];
            const v = v3_scratch_0[1];
            const w = 1 - u - v;

            // construct edge

            const contact_x = ax * u + bx * v + cx * w;
            const contact_y = ay * u + by * v + cy * w;
            const contact_z = az * u + bz * v + cz * w;

            const distance_to_triangle_sqr = v3_distance_sqr(
                contact_x, contact_y, contact_z,
                x, y, z
            );

            if (distance_to_triangle_sqr >= nearest_distance_sqr) {
                continue;
            }

            nearest_distance_sqr = distance_to_triangle_sqr;

            result.position.set(contact_x, contact_y, contact_z);

            v3_compute_triangle_normal(result.normal, 0,
                ax, ay, az,
                bx, by, bz,
                cx, cy, cz
            );

            result.index = triangle_index;

            got_a_hit = true;
        }
    } while (pointer > stack_top);

    return got_a_hit;
}