MazeWalkerTv/Assets/Scripts/MazePathfinder.cs

using UnityEngine;
using System.Collections.Generic;
using System.Linq;

/// <summary>
/// Handles pathfinding through the maze using A* algorithm
/// Takes into account terrain movement costs
/// </summary>
public class MazePathfinder
{
    private MazeData mazeData;

    public class PathNode
    {
        public Vector2Int Position { get; set; }
        public PathNode Parent { get; set; }
        public float GCost { get; set; }  // Cost from start
        public float HCost { get; set; }  // Heuristic cost to goal
        public float FCost => GCost + HCost;

        public PathNode(Vector2Int position, PathNode parent, float gCost, float hCost)
        {
            Position = position;
            Parent = parent;
            GCost = gCost;
            HCost = hCost;
        }
    }

    public MazePathfinder(MazeData mazeData)
    {
        this.mazeData = mazeData;
    }

    /// <summary>
    /// Finds a path from start to goal using A* with a min-heap open set (O(n log n)).
    /// </summary>
    public List<Vector2Int> FindPath(Vector2Int start, Vector2Int goal)
    {
        // Min-heap keyed by FCost
        var openHeap = new MinHeap<PathNode>();
        var closedSet = new HashSet<Vector2Int>();
        // Fast lookup: position → best node in open set
        var openLookup = new Dictionary<Vector2Int, PathNode>();

        var startNode = new PathNode(start, null, 0, Heuristic(start, goal));
        openHeap.Enqueue(startNode, startNode.FCost);
        openLookup[start] = startNode;

        while (openHeap.Count > 0)
        {
            var currentNode = openHeap.Dequeue();
            openLookup.Remove(currentNode.Position);

            if (currentNode.Position == goal)
                return ReconstructPath(currentNode);

            closedSet.Add(currentNode.Position);

            foreach (var neighbor in GetNeighbors(currentNode.Position))
            {
                if (closedSet.Contains(neighbor)) continue;

                float movementCost = GetMovementCost(neighbor);
                float tentativeG = currentNode.GCost + movementCost;

                if (openLookup.TryGetValue(neighbor, out var existingNode))
                {
                    if (tentativeG < existingNode.GCost)
                    {
                        existingNode.Parent = currentNode;
                        existingNode.GCost = tentativeG;
                        openHeap.UpdatePriority(existingNode, existingNode.FCost);
                    }
                }
                else
                {
                    float hCost = Heuristic(neighbor, goal);
                    var newNode = new PathNode(neighbor, currentNode, tentativeG, hCost);
                    openHeap.Enqueue(newNode, newNode.FCost);
                    openLookup[neighbor] = newNode;
                }
            }
        }

        return new List<Vector2Int>();
    }

    /// <summary>
    /// Gets the movable neighbors of a position
    /// </summary>
    private List<Vector2Int> GetNeighbors(Vector2Int position)
    {
        var neighbors = new List<Vector2Int>();
        Vector2Int[] directions = new[]
        {
            new Vector2Int(position.x + 1, position.y),
            new Vector2Int(position.x - 1, position.y),
            new Vector2Int(position.x, position.y + 1),
            new Vector2Int(position.x, position.y - 1),
            // Optionally add diagonals:
            // new Vector2Int(position.x + 1, position.y + 1),
            // new Vector2Int(position.x - 1, position.y + 1),
            // new Vector2Int(position.x + 1, position.y - 1),
            // new Vector2Int(position.x - 1, position.y - 1),
        };

        foreach (var dir in directions)
        {
            if (mazeData.IsWalkable(dir.x, dir.y))
            {
                neighbors.Add(dir);
            }
        }

        return neighbors;
    }

    /// <summary>
    /// Gets the movement cost for a tile based on terrain
    /// </summary>
    private float GetMovementCost(Vector2Int position)
    {
        var tile = mazeData.GetTile(position.x, position.y);
        if (tile == null) return float.MaxValue;
        return tile.GetMovementCost();
    }

    /// <summary>
    /// Heuristic function (Manhattan distance)
    /// </summary>
    private float Heuristic(Vector2Int from, Vector2Int to)
    {
        return Mathf.Abs(from.x - to.x) + Mathf.Abs(from.y - to.y);
    }

    /// <summary>
    /// Reconstructs the path from goal to start
    /// </summary>
    private List<Vector2Int> ReconstructPath(PathNode node)
    {
        var path = new List<Vector2Int>();
        while (node != null)
        {
            path.Add(node.Position);
            node = node.Parent;
        }
        path.Reverse();
        return path;
    }

    /// <summary>
    /// Finds all reachable tiles from a starting position within a certain distance
    /// </summary>
    public HashSet<Vector2Int> FindReachableTiles(Vector2Int start, float maxDistance)
    {
        var reachable = new HashSet<Vector2Int>();
        var queue = new Queue<(Vector2Int pos, float distance)>();
        queue.Enqueue((start, 0));
        reachable.Add(start);

        while (queue.Count > 0)
        {
            var (current, distance) = queue.Dequeue();
            var neighbors = GetNeighbors(current);

            foreach (var neighbor in neighbors)
            {
                if (!reachable.Contains(neighbor))
                {
                    float cost = GetMovementCost(neighbor);
                    float newDistance = distance + cost;

                    if (newDistance <= maxDistance)
                    {
                        reachable.Add(neighbor);
                        queue.Enqueue((neighbor, newDistance));
                    }
                }
            }
        }

        return reachable;
    }
}