poe2-bot/src/Roboto.Navigation/PathFinder.cs

using System.Numerics;
using Roboto.Core;
using Serilog;

namespace Roboto.Navigation;

public static class PathFinder
{
    private static readonly int[] Dx = [-1, 0, 1, 0, -1, -1, 1, 1];
    private static readonly int[] Dy = [0, -1, 0, 1, -1, 1, -1, 1];
    private static readonly float[] Cost = [1f, 1f, 1f, 1f, 1.414f, 1.414f, 1.414f, 1.414f];

    /// <summary>
    /// A* pathfinding on WalkabilitySnapshot. Returns world-coord waypoints or null if no path.
    /// When exploredGrid is provided, explored cells cost 3x more — biasing paths through unexplored territory.
    /// </summary>
    public static List<Vector2>? FindPath(
        WalkabilitySnapshot terrain, Vector2 start, Vector2 goal, float worldToGrid,
        bool[]? exploredGrid = null, int exploredWidth = 0, int exploredHeight = 0)
    {
        var w = terrain.Width;
        var h = terrain.Height;
        var gridToWorld = 1f / worldToGrid;

        var startGx = Math.Clamp((int)(start.X * worldToGrid), 0, w - 1);
        var startGy = Math.Clamp((int)(start.Y * worldToGrid), 0, h - 1);
        var goalGx = Math.Clamp((int)(goal.X * worldToGrid), 0, w - 1);
        var goalGy = Math.Clamp((int)(goal.Y * worldToGrid), 0, h - 1);

        // Snap to nearest walkable if start/goal are in walls
        (startGx, startGy) = SnapToWalkable(terrain, startGx, startGy, w, h);
        (goalGx, goalGy) = SnapToWalkable(terrain, goalGx, goalGy, w, h);

        var startNode = (startGx, startGy);
        var goalNode = (goalGx, goalGy);

        if (startNode == goalNode)
            return [new Vector2(goalGx * gridToWorld, goalGy * gridToWorld)];

        var openSet = new PriorityQueue<(int x, int y), float>();
        var cameFrom = new Dictionary<(int, int), (int, int)>();
        var gScore = new Dictionary<(int, int), float>();
        var closedSet = new HashSet<(int, int)>();

        // Track closest-to-goal node for fallback when goal is unreachable
        (int x, int y) bestNode = startNode;
        var bestDist = Heuristic(startNode, goalNode);

        gScore[startNode] = 0;
        openSet.Enqueue(startNode, Heuristic(startNode, goalNode));

        var iterations = 0;
        const int maxIterations = 200_000;

        while (openSet.Count > 0 && iterations++ < maxIterations)
        {
            var current = openSet.Dequeue();

            if (current == goalNode)
                return ReconstructAndSimplify(cameFrom, current, gridToWorld);

            // Skip already-expanded nodes (duplicate PQ entries)
            if (!closedSet.Add(current))
                continue;

            // Track nearest reachable cell to goal
            var distToGoal = Heuristic(current, goalNode);
            if (distToGoal < bestDist)
            {
                bestDist = distToGoal;
                bestNode = current;
            }

            var currentG = gScore.GetValueOrDefault(current, float.MaxValue);

            for (var i = 0; i < 8; i++)
            {
                var nx = current.x + Dx[i];
                var ny = current.y + Dy[i];

                if (nx < 0 || nx >= w || ny < 0 || ny >= h) continue;
                if (!terrain.IsWalkable(nx, ny)) continue;

                var neighbor = (nx, ny);
                if (closedSet.Contains(neighbor)) continue;

                // Diagonal corner-cut check
                if (i >= 4)
                {
                    if (!terrain.IsWalkable(current.x + Dx[i], current.y) ||
                        !terrain.IsWalkable(current.x, current.y + Dy[i]))
                        continue;
                }

                var stepCost = Cost[i];
                if (exploredGrid is not null && nx < exploredWidth && ny < exploredHeight && exploredGrid[ny * exploredWidth + nx])
                    stepCost *= 1.5f;
                var tentativeG = currentG + stepCost;

                if (tentativeG < gScore.GetValueOrDefault(neighbor, float.MaxValue))
                {
                    cameFrom[neighbor] = current;
                    gScore[neighbor] = tentativeG;
                    openSet.Enqueue(neighbor, tentativeG + Heuristic(neighbor, goalNode));
                }
            }
        }

        // No exact path — check if we exhausted the connected region or hit iteration limit
        var exhausted = openSet.Count == 0;
        Log.Warning("PATH FAIL detail: expanded={Expanded}, {Reason}, bestDist={Best:F0} from goal",
            closedSet.Count, exhausted ? "disconnected regions" : "iteration limit", bestDist);

        // Fallback: path to closest reachable cell (only if meaningfully closer than start)
        if (bestNode != startNode && bestDist < Heuristic(startNode, goalNode) * 0.8f)
        {
            Log.Information("PATH FALLBACK: pathing to nearest reachable cell ({X},{Y}), dist={D:F0} from goal",
                bestNode.x, bestNode.y, bestDist);
            return ReconstructAndSimplify(cameFrom, bestNode, gridToWorld);
        }

        return null;
    }

    private static float Heuristic((int x, int y) a, (int x, int y) b)
    {
        var dx = Math.Abs(a.x - b.x);
        var dy = Math.Abs(a.y - b.y);
        return Math.Max(dx, dy) + 0.414f * Math.Min(dx, dy);
    }

    private static (int, int) SnapToWalkable(WalkabilitySnapshot terrain, int gx, int gy, int w, int h)
    {
        if (terrain.IsWalkable(gx, gy)) return (gx, gy);

        // BFS outward to find nearest walkable cell
        for (var r = 1; r < 20; r++)
        {
            for (var dx = -r; dx <= r; dx++)
            {
                for (var dy = -r; dy <= r; dy++)
                {
                    if (Math.Abs(dx) != r && Math.Abs(dy) != r) continue;
                    var nx = gx + dx;
                    var ny = gy + dy;
                    if (nx >= 0 && nx < w && ny >= 0 && ny < h && terrain.IsWalkable(nx, ny))
                        return (nx, ny);
                }
            }
        }
        return (gx, gy);
    }

    private static List<Vector2> ReconstructAndSimplify(
        Dictionary<(int, int), (int, int)> cameFrom, (int x, int y) current, float gridToWorld)
    {
        var path = new List<(int x, int y)>();
        var node = current;
        while (cameFrom.ContainsKey(node))
        {
            path.Add(node);
            node = cameFrom[node];
        }
        path.Reverse();

        if (path.Count <= 2)
            return path.Select(n => new Vector2(n.x * gridToWorld, n.y * gridToWorld)).ToList();

        // Simplify: skip collinear waypoints
        var simplified = new List<Vector2> { ToWorld(path[0], gridToWorld) };
        for (var i = 2; i < path.Count; i++)
        {
            var prev = path[i - 2];
            var mid = path[i - 1];
            var curr = path[i];

            var d1x = mid.x - prev.x;
            var d1y = mid.y - prev.y;
            var d2x = curr.x - mid.x;
            var d2y = curr.y - mid.y;

            // Direction changed
            if (d1x != d2x || d1y != d2y)
                simplified.Add(ToWorld(mid, gridToWorld));
        }
        simplified.Add(ToWorld(path[^1], gridToWorld));
        return simplified;
    }

    private static Vector2 ToWorld((int x, int y) node, float gridToWorld)
        => new(node.x * gridToWorld, node.y * gridToWorld);
}