import { assert } from "../../../core/assert.js";
import { Uint32Heap } from "../../../core/collection/heap/Uint32Heap.js";
import { bt_face_get_centroid } from "../../../core/geom/3d/topology/struct/binary/query/bt_face_get_centroid.js";
import {
    bt_face_get_neighbour_faces
} from "../../../core/geom/3d/topology/struct/binary/query/bt_face_get_neighbour_faces.js";

const open = new Uint32Heap();

/**
 * @type {Set<number>}
 */
const closed = new Set();

/**
 * Least known traversal cost (sum of centroid-to-centroid distances) from start to each face.
 * @type {Map<number, number>}
 */
const g_score = new Map();

/**
 * Reconstruction parent pointers: face -> the face we reached it from on the best path.
 * @type {Map<number, number>}
 */
const came_from = new Map();

/**
 * Note that we limit the supported number of neighbors, a reasonable mesh will fit this criteria
 * @type {Uint32Array}
 */
const neighbors = new Uint32Array(256);

const scratch_array_f32 = new Float32Array(6);

/**
 * Straight-line distance between the centroids of two faces.
 *
 * This is used both as the step cost (between adjacent faces) and as the heuristic (from a face to
 * the goal). Because, by the triangle inequality, the straight-line distance from any face to the
 * goal is never greater than the summed centroid-to-centroid distance of an actual face path between
 * them, the heuristic is admissible AND consistent. A* therefore returns a shortest-distance corridor
 * (under the centroid metric) rather than a fewest-faces one.
 *
 * @param {number} a
 * @param {number} b
 * @param {BinaryTopology} topology
 * @returns {number}
 */
function face_centroid_distance(a, b, topology) {
    bt_face_get_centroid(scratch_array_f32, 0, topology, a);
    bt_face_get_centroid(scratch_array_f32, 3, topology, b);

    const dx = scratch_array_f32[0] - scratch_array_f32[3];
    const dy = scratch_array_f32[1] - scratch_array_f32[4];
    const dz = scratch_array_f32[2] - scratch_array_f32[5];

    return Math.sqrt(dx * dx + dy * dy + dz * dz);
}

/**
 * Walk parent pointers from goal back to start, writing the face sequence into `output` in
 * START -> GOAL order. Never writes more than `max_path_length` entries: if the corridor is longer
 * than that it does not fit the caller's buffer and 0 is returned (no partial/garbage result is used).
 *
 * @param {number[]|Uint32Array} output
 * @param {number} start_face
 * @param {number} goal_face
 * @param {Map<number, number>} came_from
 * @param {number} max_path_length
 * @returns {number} number of faces written, or 0 if the corridor exceeds `max_path_length`
 */
function construct_path(output, start_face, goal_face, came_from, max_path_length) {
    let length = 0;
    let current = goal_face;

    // walk back to the start, writing in reverse (goal -> start)
    while (true) {
        if (length >= max_path_length) {
            // corridor does not fit the output buffer; refuse rather than writing out of bounds
            return 0;
        }

        output[length] = current;
        length++;

        if (current === start_face) {
            break;
        }

        current = came_from.get(current);
    }

    // reverse in place to get START -> GOAL order
    const half_length = length >> 1;

    for (let i = 0; i < half_length; i++) {
        const mirror_i = length - 1 - i;
        const temp = output[i];

        output[i] = output[mirror_i];
        output[mirror_i] = temp;
    }

    return length;
}


/**
 * Find a shortest path through topology faces.
 * If a path is found - the result will contain start and goal faces.
 *
 * NOTE: if either start or goal faces are not part of the topology - an empty path will be produced.
 *
 * @param {number[]|Uint32Array} output path will be written here as a sequence of face IDs
 * @param {number} start_face_id
 * @param {number} goal_face_id
 * @param {BinaryTopology} topology
 * @param {number} [max_path_length=Infinity] cap on the number of faces written to `output`. If the
 *        shortest corridor is longer than this it does not fit the buffer and 0 is returned. Pass the
 *        capacity of `output` to guarantee no out-of-bounds writes.
 * @returns {number} number of faces that make up the path. This will be 0 if no path is found (or the
 *        path does not fit `max_path_length`).
 */
export function bt_mesh_face_find_path(
    output,
    start_face_id,
    goal_face_id,
    topology,
    max_path_length = Infinity,
) {
    assert.isArrayLike(output, 'output');
    assert.isNonNegativeInteger(start_face_id, 'start_face_id');
    assert.isNonNegativeInteger(goal_face_id, 'goal_face_id');
    assert.notNull(topology, 'topology');
    assert.defined(topology, 'topology');
    assert.equal(topology.isBinaryTopology, true, 'topology.isBinaryTopology !== true');

    open.clear();
    closed.clear();
    g_score.clear();
    came_from.clear();

    g_score.set(start_face_id, 0);

    open.insert(start_face_id, face_centroid_distance(start_face_id, goal_face_id, topology));

    while (!open.is_empty()) {

        const current_node = open.pop_min();

        // Lazy deletion: a face may sit in the heap multiple times if its g-score improved after an
        // earlier push. The lowest-f entry is popped first; later popped duplicates land on an
        // already-closed face and must be skipped.
        if (closed.has(current_node)) {
            continue;
        }

        if (current_node === goal_face_id) {
            // Reached the goal
            return construct_path(output, start_face_id, goal_face_id, came_from, max_path_length);
        }

        closed.add(current_node);

        const current_g_score = g_score.get(current_node);

        const neighbor_count = bt_face_get_neighbour_faces(neighbors, 0, topology, current_node);

        for (let i = 0; i < neighbor_count; i++) {

            const neighbor = neighbors[i];

            if (closed.has(neighbor)) {
                // already closed
                continue;
            }

            // step cost is the real geometric distance between adjacent face centroids, so the
            // search minimises corridor length rather than the number of triangles crossed
            const traversal_cost = face_centroid_distance(current_node, neighbor, topology);

            const cost_so_far = current_g_score + traversal_cost;

            if (!g_score.has(neighbor) || cost_so_far < g_score.get(neighbor)) {
                // better path to this neighbour

                g_score.set(neighbor, cost_so_far);
                came_from.set(neighbor, current_node);

                const remaining_heuristic = face_centroid_distance(neighbor, goal_face_id, topology);

                const f_score = cost_so_far + remaining_heuristic;

                // Lazy push: always insert. If neighbor was already on the open list, the older
                // (worse-f) entry will be skipped when popped (see closed-check at the top of the
                // loop). This avoids an O(n) find_index_by_id scan per relaxation.
                open.insert(neighbor, f_score);

            }

        }

    }

    // No result found
    return 0;
}
