/**
 * A* pathfinding on an arbitrary graph.
 *
 * The graph is defined entirely through two callbacks you provide:
 *   - neighbours(node)  → { node, cost }[]   edge traversal
 *   - heuristic(a, b)   → number             admissible estimate of remaining cost
 *
 * Nodes are identified by a key function (default: String(node)).
 * Works for any domain: grid cells, navmesh polygons, waypoint graphs,
 * hex tiles, 3-D voxels, abstract state machines — anything.
 *
 * Guarantees:
 *   - Optimal path when heuristic is admissible (never over-estimates).
 *   - Complete: always finds a path if one exists.
 *   - O((V + E) log V) time via a binary min-heap open set.
 *
 * Usage:
 *   const nav = new AStar<Vec2>({
 *     neighbours: (n) => grid.passableNeighbours(n).map(nb => ({ node: nb, cost: 1 })),
 *     heuristic:  (a, b) => Math.abs(a.x - b.x) + Math.abs(a.y - b.y),
 *     key:        (n) => `${n.x},${n.y}`,
 *   });
 *
 *   const result = nav.find(start, goal);
 *   if (result) console.log(result.path, result.cost);
 */

import type { Point } from "@common/geometry";
import {
    buildAStarOptionsFrom2D,
    MinHeap,
    type AStar2DOptions,
    type AStarOptions,
    type AStarResult,
    type Edge,
} from "@common/navigation/utils";


// ---------------------------------------------------------------------------
// A* class
// ---------------------------------------------------------------------------

export class AStar<N> {
    private readonly neighbours: (node: N) => Iterable<Edge<N>>;
    private readonly heuristic: (from: N, to: N) => number;
    private readonly key: (node: N) => string;
    private readonly maxExpansions: number;

    constructor(options: AStarOptions<N>) {
        this.neighbours = options.neighbours;
        this.heuristic = options.heuristic;
        this.key = options.key ?? ((n) => String(n));
        this.maxExpansions = options.maxExpansions ?? Infinity;
    }

    /**
     * Find the shortest path from `start` to `goal`.
     *
     * @returns AStarResult if a path exists, null if unreachable or
     *          maxExpansions was exceeded.
     */
    find(start: N, goal: N): AStarResult<N> | null {
        const startKey = this.key(start);
        const goalKey = this.key(goal);

        // Early exit — already there.
        if (startKey === goalKey) {
            return { path: [start], cost: 0, expansions: 0 };
        }

        // g[k]      = best known cost from start to node k
        // cameFrom  = for path reconstruction
        const g = new Map<string, number>();
        const cameFrom = new Map<string, { node: N; key: string }>();

        g.set(startKey, 0);

        const open = new MinHeap<N>();
        open.push({
            node: start,
            nodeKey: startKey,
            g: 0,
            f: this.heuristic(start, goal),
        });

        // Nodes fully expanded (closed set).
        const closed = new Set<string>();

        let expansions = 0;

        while (open.size > 0) {
            if (expansions >= this.maxExpansions) return null;

            const current = open.pop()!;
            const { node, nodeKey, g: gCurrent } = current;

            // Stale entry — a shorter path to this node was already processed.
            if (closed.has(nodeKey)) continue;
            closed.add(nodeKey);
            expansions++;

            if (nodeKey === goalKey) {
                return {
                    path: this._reconstructPath(cameFrom, node, nodeKey, start, startKey),
                    cost: gCurrent,
                    expansions,
                };
            }

            for (const edge of this.neighbours(node)) {
                if (edge.cost <= 0) {
                    throw new RangeError(
                        `A*: edge cost must be > 0, got ${edge.cost} from node "${nodeKey}"`,
                    );
                }

                const neighbourKey = this.key(edge.node);
                if (closed.has(neighbourKey)) continue;

                const gNext = gCurrent + edge.cost;
                const gBest = g.get(neighbourKey);

                if (gBest === undefined || gNext < gBest) {
                    g.set(neighbourKey, gNext);
                    cameFrom.set(neighbourKey, { node, key: nodeKey });
                    open.push({
                        node: edge.node,
                        nodeKey: neighbourKey,
                        g: gNext,
                        f: gNext + this.heuristic(edge.node, goal),
                    });
                }
            }
        }

        return null; // No path found.
    }

    /**
     * Reconstruct the path by walking cameFrom back from goal to start.
     */
    private _reconstructPath(
        cameFrom: Map<string, { node: N; key: string }>,
        goalNode: N,
        goalKey: string,
        startNode: N,
        startKey: string,
    ): N[] {
        const path: N[] = [];
        let currentKey = goalKey;
        let currentNode = goalNode;

        while (currentKey !== startKey) {
            path.push(currentNode);
            const prev = cameFrom.get(currentKey)!;
            currentKey = prev.key;
            currentNode = prev.node;
        }

        path.push(startNode);
        path.reverse();
        return path;
    }

    /**
     * Convenience: check reachability without allocating a full path.
     */
    isReachable(start: N, goal: N): boolean {
        return this.find(start, goal) !== null;
    }

    /**
     * Multi-goal search — find the nearest reachable goal from a set.
     *
     * Runs a single A* from `start` using a combined heuristic:
     * min h(node, goal) over all goals. Stops at the first goal expanded.
     *
     * @param goals  Non-empty iterable of candidate goals.
     */
    findNearest(start: N, goals: Iterable<N>): AStarResult<N> | null {
        const goalList = [...goals];
        if (goalList.length === 0) throw new RangeError('findNearest: goals is empty');

        const goalKeys = new Set(goalList.map(g => this.key(g)));
        const goalNodes = new Map(goalList.map(g => [this.key(g), g]));

        const startKey = this.key(start);

        if (goalKeys.has(startKey)) {
            return { path: [start], cost: 0, expansions: 0 };
        }

        const heuristic = (node: N) =>
            Math.min(...goalList.map(g => this.heuristic(node, g)));

        const g = new Map<string, number>();
        const cameFrom = new Map<string, { node: N; key: string }>();

        g.set(startKey, 0);

        const open = new MinHeap<N>();
        open.push({ node: start, nodeKey: startKey, g: 0, f: heuristic(start) });

        const closed = new Set<string>();
        let expansions = 0;

        while (open.size > 0) {
            if (expansions >= this.maxExpansions) return null;

            const current = open.pop()!;
            const { node, nodeKey, g: gCurrent } = current;

            if (closed.has(nodeKey)) continue;
            closed.add(nodeKey);
            expansions++;

            if (goalKeys.has(nodeKey)) {
                const goalNode = goalNodes.get(nodeKey)!;
                return {
                    path: this._reconstructPath(cameFrom, goalNode, nodeKey, start, startKey),
                    cost: gCurrent,
                    expansions,
                };
            }

            for (const edge of this.neighbours(node)) {
                const neighbourKey = this.key(edge.node);
                if (closed.has(neighbourKey)) continue;

                const gNext = gCurrent + edge.cost;
                if (!g.has(neighbourKey) || gNext < g.get(neighbourKey)!) {
                    g.set(neighbourKey, gNext);
                    cameFrom.set(neighbourKey, { node, key: nodeKey });
                    open.push({
                        node: edge.node,
                        nodeKey: neighbourKey,
                        g: gNext,
                        f: gNext + heuristic(edge.node),
                    });
                }
            }
        }

        return null;
    }
}

// ---------------------------------------------------------------------------
// AStar2D — convenience subclass for 2-D grids
// ---------------------------------------------------------------------------

export class AStar2D extends AStar<Point> {
    constructor(options: AStar2DOptions) {
        super(buildAStarOptionsFrom2D(options));
    }
}
