TheriapolisV3/Theriapolis.Core/Util/AStarPathfinder.cs

namespace Theriapolis.Core.Util;

/// <summary>
/// 8-directional A* pathfinder on the 1024×1024 world tile grid.
/// Uses pre-allocated arrays and a generation counter to avoid clearing between queries.
/// The cost function receives (fromX, fromY, toX, toY, entryDir) and returns the cost
/// of moving to the 'to' tile, or float.PositiveInfinity if impassable.
/// </summary>
public sealed class AStarPathfinder
{
    private const int W = C.WORLD_WIDTH_TILES;
    private const int H = C.WORLD_HEIGHT_TILES;
    private const int TileCount = W * H;

    // Persistent arrays reused across queries (generation counter avoids clearing)
    private readonly float[] _gScore     = new float[TileCount];
    private readonly int[]   _cameFrom   = new int[TileCount];  // -1 = none
    private readonly byte[]  _generation = new byte[TileCount]; // current run's gen tag
    private byte _currentGen;

    private readonly BinaryHeap<int> _open = new(4096);

    // 8-directional deltas: N, NE, E, SE, S, SW, W, NW
    private static readonly (int dx, int dy)[] Dirs =
    {
        ( 0,-1), ( 1,-1), ( 1, 0), ( 1, 1),
        ( 0, 1), (-1, 1), (-1, 0), (-1,-1),
    };

    private static float Diagonal(int dx, int dy)
        => (dx != 0 && dy != 0) ? 1.4142f : 1f; // sqrt(2) for diagonals

    private static float OctileHeuristic(int x0, int y0, int x1, int y1)
    {
        int dx = Math.Abs(x1 - x0);
        int dy = Math.Abs(y1 - y0);
        return Math.Max(dx, dy) + (1.4142f - 1f) * Math.Min(dx, dy);
    }

    /// <summary>
    /// Find the lowest-cost path from (sx,sy) to (gx,gy).
    /// <paramref name="costFn"/> receives (fromX, fromY, toX, toY, entryDir) and returns
    /// the cost of entering the 'to' tile from this direction, or PositiveInfinity if impassable.
    /// Returns null if no path exists within the tile grid or if maxExpansions is exceeded.
    /// </summary>
    public List<(int X, int Y)>? FindPath(
        int sx, int sy,
        int gx, int gy,
        Func<int, int, int, int, byte, float> costFn,
        int maxExpansions = 600_000)
    {
        if (sx == gx && sy == gy) return new List<(int, int)> { (sx, sy) };
        int expansions = 0;

        // Increment generation (wrap around)
        _currentGen = (byte)((_currentGen + 1) == 0 ? 1 : _currentGen + 1);
        _open.Clear();

        int startIdx = sy * W + sx;
        _gScore[startIdx]     = 0f;
        _cameFrom[startIdx]   = -1;
        _generation[startIdx] = _currentGen;

        float h0 = OctileHeuristic(sx, sy, gx, gy);
        _open.Insert(startIdx, h0);

        while (!_open.IsEmpty)
        {
            if (++expansions > maxExpansions) return null; // safety cap

            int curIdx = _open.ExtractMin();
            int curX = curIdx % W;
            int curY = curIdx / W;

            if (curX == gx && curY == gy)
                return ReconstructPath(curIdx);

            float curG = _gScore[curIdx];

            for (int d = 0; d < 8; d++)
            {
                int nx = curX + Dirs[d].dx;
                int ny = curY + Dirs[d].dy;
                if ((uint)nx >= W || (uint)ny >= H) continue;

                byte entryDir = (byte)d;
                float moveCost = costFn(curX, curY, nx, ny, entryDir);
                if (float.IsPositiveInfinity(moveCost)) continue;

                float newG = curG + Diagonal(Dirs[d].dx, Dirs[d].dy) + moveCost;
                int nIdx = ny * W + nx;

                if (_generation[nIdx] == _currentGen && newG >= _gScore[nIdx])
                    continue; // already found a better path

                _gScore[nIdx]     = newG;
                _cameFrom[nIdx]   = curIdx;
                _generation[nIdx] = _currentGen;

                float f = newG + OctileHeuristic(nx, ny, gx, gy);
                _open.Insert(nIdx, f);
            }
        }

        return null; // no path found
    }

    private List<(int X, int Y)> ReconstructPath(int goalIdx)
    {
        var path = new List<(int, int)>();
        int cur = goalIdx;
        while (cur != -1)
        {
            path.Add((cur % W, cur / W));
            cur = _cameFrom[cur];
        }
        path.Reverse();
        return path;
    }
}