src/SilkroadBot.Navigation/Algorithms/AStarPathfinder.cs

using SilkroadBot.Domain.Models;
using SilkroadBot.Navigation.Interfaces;
using SilkroadBot.Navigation.Models;

namespace SilkroadBot.Navigation.Algorithms;

/// <summary>
/// A* pathfinding implementation for Silkroad map navigation.
/// Uses a grid-based approach with support for diagonal movement.
/// </summary>
public class AStarPathfinder : IPathfindingAlgorithm
{
    private readonly IMapDataProvider _mapProvider;
    private readonly Dictionary<short, RegionNavData> _loadedRegions = new();

    public string Name => "A* Pathfinder";

    public AStarPathfinder(IMapDataProvider mapProvider)
    {
        _mapProvider = mapProvider;
    }

    public async Task<PathResult> FindPathAsync(WorldPosition start, WorldPosition end, PathfindingOptions? options = null)
    {
        options ??= new PathfindingOptions();
        var sw = System.Diagnostics.Stopwatch.StartNew();

        // Handle cross-region paths
        if (start.RegionId != end.RegionId)
        {
            return PathResult.Failed("Cross-region pathfinding not yet implemented. Use waypoint-based navigation.");
        }

        // Load region data
        var region = await EnsureRegionLoadedAsync(start.RegionId);
        if (region == null)
            return PathResult.Failed($"No navigation data for region {start.RegionId}");

        // Run A*
        var path = ComputeAStar(start, end, region, options);
        
        sw.Stop();

        if (path == null || path.Count == 0)
            return PathResult.Failed("No path found between start and end positions");

        // Optionally smooth the path
        var waypoints = options.SmoothPath ? SmoothPath(path, region) : path;
        
        // Calculate total distance
        double totalDist = 0;
        for (int i = 1; i < waypoints.Count; i++)
            totalDist += waypoints[i - 1].DistanceTo(waypoints[i]);

        return new PathResult
        {
            Success = true,
            Waypoints = waypoints,
            TotalDistance = totalDist,
            ComputeTime = sw.Elapsed
        };
    }

    public bool IsPositionWalkable(WorldPosition position)
    {
        if (!_loadedRegions.TryGetValue(position.RegionId, out var region))
            return true; // Assume walkable if no data loaded
        
        int gridX = (int)((position.X + region.Width / 2f) / region.Width * region.WalkabilityGrid.GetLength(0));
        int gridY = (int)((position.Y + region.Height / 2f) / region.Height * region.WalkabilityGrid.GetLength(1));
        
        if (gridX < 0 || gridX >= region.WalkabilityGrid.GetLength(0) ||
            gridY < 0 || gridY >= region.WalkabilityGrid.GetLength(1))
            return false;
        
        return region.WalkabilityGrid[gridX, gridY] > 0;
    }

    public WorldPosition GetNearestWalkablePosition(WorldPosition position)
    {
        if (IsPositionWalkable(position))
            return position;

        // Spiral search for nearest walkable
        for (float radius = 1; radius <= 50; radius += 1)
        {
            for (float angle = 0; angle < 360; angle += 15)
            {
                var rad = angle * Math.PI / 180;
                var candidate = position with
                {
                    X = position.X + (float)(radius * Math.Cos(rad)),
                    Y = position.Y + (float)(radius * Math.Sin(rad))
                };
                if (IsPositionWalkable(candidate))
                    return candidate;
            }
        }

        return position; // Fallback to original
    }

    private List<WorldPosition>? ComputeAStar(WorldPosition start, WorldPosition end, RegionNavData region, PathfindingOptions options)
    {
        var openSet = new PriorityQueue<GridNode, double>();
        var closedSet = new HashSet<(int, int)>();
        var cameFrom = new Dictionary<(int, int), (int, int)>();
        var gScore = new Dictionary<(int, int), double>();

        var startGrid = WorldToGrid(start, region);
        var endGrid = WorldToGrid(end, region);

        gScore[startGrid] = 0;
        openSet.Enqueue(new GridNode(startGrid.Item1, startGrid.Item2), Heuristic(startGrid, endGrid));

        int iterations = 0;

        while (openSet.Count > 0 && iterations < options.MaxIterations)
        {
            iterations++;
            var current = openSet.Dequeue();
            var currentPos = (current.X, current.Y);

            if (currentPos == endGrid)
            {
                // Reconstruct path
                return ReconstructPath(cameFrom, currentPos, start.RegionId, region);
            }

            closedSet.Add(currentPos);

            foreach (var neighbor in GetNeighbors(current.X, current.Y, region, options.AllowDiagonal))
            {
                var neighborPos = (neighbor.X, neighbor.Y);
                if (closedSet.Contains(neighborPos))
                    continue;

                double tentativeG = gScore[currentPos] + MoveCost(currentPos, neighborPos);

                if (!gScore.ContainsKey(neighborPos) || tentativeG < gScore[neighborPos])
                {
                    cameFrom[neighborPos] = currentPos;
                    gScore[neighborPos] = tentativeG;
                    double fScore = tentativeG + Heuristic(neighborPos, endGrid);
                    openSet.Enqueue(new GridNode(neighbor.X, neighbor.Y), fScore);
                }
            }
        }

        return null; // No path found
    }

    private List<WorldPosition> ReconstructPath(
        Dictionary<(int, int), (int, int)> cameFrom, 
        (int, int) current, 
        short regionId, 
        RegionNavData region)
    {
        var path = new List<WorldPosition>();
        while (cameFrom.ContainsKey(current))
        {
            path.Add(GridToWorld(current, regionId, region));
            current = cameFrom[current];
        }
        path.Add(GridToWorld(current, regionId, region));
        path.Reverse();
        return path;
    }

    private List<WorldPosition> SmoothPath(List<WorldPosition> path, RegionNavData region)
    {
        if (path.Count <= 2) return path;
        
        var smoothed = new List<WorldPosition> { path[0] };
        int current = 0;

        while (current < path.Count - 1)
        {
            int farthest = current + 1;
            for (int i = path.Count - 1; i > current + 1; i--)
            {
                if (HasLineOfSight(path[current], path[i], region))
                {
                    farthest = i;
                    break;
                }
            }
            smoothed.Add(path[farthest]);
            current = farthest;
        }

        return smoothed;
    }

    private bool HasLineOfSight(WorldPosition a, WorldPosition b, RegionNavData region)
    {
        int steps = (int)(a.DistanceTo(b) / 2) + 1;
        for (int i = 0; i <= steps; i++)
        {
            float t = (float)i / steps;
            var point = new WorldPosition(
                a.RegionId,
                a.X + (b.X - a.X) * t,
                a.Y + (b.Y - a.Y) * t,
                a.Z + (b.Z - a.Z) * t
            );
            if (!IsPositionWalkable(point))
                return false;
        }
        return true;
    }

    private static IEnumerable<(int X, int Y)> GetNeighbors(int x, int y, RegionNavData region, bool diagonal)
    {
        int w = region.WalkabilityGrid.GetLength(0);
        int h = region.WalkabilityGrid.GetLength(1);
        
        var dirs = new (int, int)[] { (0, 1), (1, 0), (0, -1), (-1, 0) };
        if (diagonal)
            dirs = dirs.Concat(new (int, int)[] { (1, 1), (-1, 1), (1, -1), (-1, -1) }).ToArray();

        foreach (var (dx, dy) in dirs)
        {
            int nx = x + dx, ny = y + dy;
            if (nx >= 0 && nx < w && ny >= 0 && ny < h && region.WalkabilityGrid[nx, ny] > 0)
                yield return (nx, ny);
        }
    }

    private static double Heuristic((int, int) a, (int, int) b)
    {
        return Math.Sqrt(Math.Pow(a.Item1 - b.Item1, 2) + Math.Pow(a.Item2 - b.Item2, 2));
    }

    private static double MoveCost((int, int) from, (int, int) to)
    {
        return (from.Item1 != to.Item1 && from.Item2 != to.Item2) ? 1.414 : 1.0;
    }

    private static (int, int) WorldToGrid(WorldPosition pos, RegionNavData region)
    {
        int gx = (int)((pos.X + region.Width / 2f) / region.Width * region.WalkabilityGrid.GetLength(0));
        int gy = (int)((pos.Y + region.Height / 2f) / region.Height * region.WalkabilityGrid.GetLength(1));
        gx = Math.Clamp(gx, 0, region.WalkabilityGrid.GetLength(0) - 1);
        gy = Math.Clamp(gy, 0, region.WalkabilityGrid.GetLength(1) - 1);
        return (gx, gy);
    }

    private static WorldPosition GridToWorld((int, int) grid, short regionId, RegionNavData region)
    {
        float x = (float)grid.Item1 / region.WalkabilityGrid.GetLength(0) * region.Width - region.Width / 2f;
        float y = (float)grid.Item2 / region.WalkabilityGrid.GetLength(1) * region.Height - region.Height / 2f;
        return new WorldPosition(regionId, x, y, 0);
    }

    private async Task<RegionNavData?> EnsureRegionLoadedAsync(short regionId)
    {
        if (_loadedRegions.TryGetValue(regionId, out var data))
            return data;
        
        data = await _mapProvider.LoadRegionAsync(regionId);
        if (data != null)
            _loadedRegions[regionId] = data;
        return data;
    }

    private record GridNode(int X, int Y);
}