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* algorithm
    /// </summary>
    public List<Vector2Int> FindPath(Vector2Int start, Vector2Int goal)
    {
        var openSet = new List<PathNode>();
        var closedSet = new HashSet<Vector2Int>();

        var startNode = new PathNode(start, null, 0, Heuristic(start, goal));
        openSet.Add(startNode);

        while (openSet.Count > 0)
        {
            // Find node with lowest F cost
            int current = 0;
            for (int i = 1; i < openSet.Count; i++)
            {
                if (openSet[i].FCost < openSet[current].FCost)
                {
                    current = i;
                }
            }

            var currentNode = openSet[current];

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

            openSet.RemoveAt(current);
            closedSet.Add(currentNode.Position);

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

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

                var existingNode = openSet.FirstOrDefault(n => n.Position == neighbor);
                if (existingNode != null)
                {
                    if (tentativeGCost < existingNode.GCost)
                    {
                        existingNode.Parent = currentNode;
                        existingNode.GCost = tentativeGCost;
                    }
                }
                else
                {
                    var hCost = Heuristic(neighbor, goal);
                    openSet.Add(new PathNode(neighbor, currentNode, tentativeGCost, hCost));
                }
            }
        }

        // No path found
        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;
    }
}