Using the A* algorithm to search a graph generated from a grid

Following on from my previous question, I finally got round to actually doing the A* search for the Graph<MapTile>. The changes to the code from the previous question were pretty minor (and no API change) so I won't include the code again.

First, I defined an interface for a hueristic.

internal interface ICartesianHeuristic
{
    int Calculate(Coordinate start, Coordinate end);
}

I then decided to use Manhattan distance because I'm only allowing up, down, right and left (i.e. no diagonal) which seemed pretty standard:

internal sealed class ManhattanDistanceHueristic : ICartesianHeuristic
{
    public int Calculate(Coordinate start, Coordinate end)
    {
        return Math.Abs(start.X - end.X) + Math.Abs(start.Y - end.Y);
    }
}

I needed a priority queue so I used the first one that came up in Nuget for that search (OptimizedPriorityQueue) which provides the SimplePriorityQueue class that I use in the implementation:

internal sealed class AStarMapTileSearch
{
    private ICartesianHeuristic hueristic;

    internal AStarMapTileSearch(ICartesianHeuristic hueristic)
    {
        if (hueristic == null)
        {
            throw new ArgumentNullException(nameof(hueristic));
        }

        this.hueristic = hueristic;
    }

    internal IEnumerable<MapTile> GetShortestPath(Graph<MapTile> graph, Coordinate start, Coordinate goal)
    {
        var startingTile = GetStartTile(graph, start);
        var frontier = new SimplePriorityQueue<MapTile>();
        frontier.Enqueue(startingTile, 0);

        MapTile foundGoal;
        var paths = GetPathsToGoal(graph, goal, startingTile, frontier, out foundGoal);

        if (foundGoal == null)
        {
            throw new InvalidOperationException($"No path between { start } and { goal } was found.");
        }

        return BuildPathFromPaths(start, foundGoal, paths);
    }

    private static IEnumerable<MapTile> BuildPathFromPaths(Coordinate start, MapTile foundGoal, Dictionary<MapTile, Tuple<MapTile, int>> paths)
    {
        var path = new Stack<MapTile>();
        path.Push(foundGoal);
        var current = foundGoal;
        while (current.Location != start)
        {
            current = paths[current].Item1;
            path.Push(current);
        }
        return path;
    }

    private Dictionary<MapTile, Tuple<MapTile, int>> GetPathsToGoal(Graph<MapTile> graph, Coordinate goalLocation, MapTile startingTile, SimplePriorityQueue<MapTile> boundaryTiles, out MapTile foundGoal)
    {
        var paths = new Dictionary<MapTile, Tuple<MapTile, int>>();
        paths[startingTile] = Tuple.Create<MapTile, int>(null, 0);
        foundGoal = null;
        while (boundaryTiles.Any())
        {
            var currentTile = boundaryTiles.Dequeue();

            if (currentTile.Location == goalLocation)
            {
                foundGoal = currentTile;
                break;
            }

            foreach (var neighbour in graph.Neighbours(currentTile))
            {
                int newCost = CalculateCostToMoveTo(paths, currentTile);
                if (!paths.ContainsKey(neighbour) || newCost < paths[neighbour].Item2)
                {
                    paths[neighbour] = Tuple.Create(currentTile, newCost);
                    boundaryTiles.Enqueue(neighbour, newCost + hueristic.Calculate(currentTile.Location, goalLocation));
                }
            }
        }
        return paths;
    }

    private static int CalculateCostToMoveTo(Dictionary<MapTile, Tuple<MapTile, int>> paths, MapTile currentTile)
    {
        return paths[currentTile].Item2 + currentTile.Cost.GetValueOrDefault();
    }


    private static MapTile GetStartTile(Graph<MapTile> graph, Coordinate start)
    {
        var node = graph.AllNodes.FirstOrDefault(n => n.Location == start);
        if (node == null)
        {
            throw new InvalidOperationException($"{start} is not within the given graph");
        }
        return node;
    }
}

I ended up having to implement the search specifically for a graph of MapTiles rather than working against any graph because I wanted to use a square grid for verification. Introducing some interfaces like ICostedNode and ICostedGraph would let me extend it to any graph with costs/weightings but I didn't really want to :)

Here's an example of it working (not really wanting a review of this bit!):

class Program
{
    static void Main(string[] args)
    {
        var mapBuilder = new RectangularMapGenerator(10, 10);

        for (var x = 1; x < 2; x++)
        {
            for (var y = 0; y < 9; y++)
            {
                mapBuilder.AddWall(new Coordinate(x, y));
            }
        }
        for (var x = 2; x < 3; x++)
        {
            for (var y = 8; y < 10; y++)
            {
                mapBuilder.AddWater(new Coordinate(x, y));
            }
        }
        mapBuilder.AddWater(new Coordinate(4, 9));
        var graph = mapBuilder.Build();

        foreach (var row in graph.AllNodes.GroupBy(n => n.Location.Y))
        {
            Console.WriteLine(
                string.Join(" ",
                    row.OrderBy(a => a.Location.X)
                     .Select(a => a.Cost.HasValue ? a.Cost.Value.ToString() : "-")));
        }
        Console.WriteLine();
        Console.WriteLine("Shortest path:" );
        var path = new AStarMapTileSearch(new ManhattanDistanceHueristic()).GetShortestPath(graph, new Coordinate(0,0), new Coordinate(8, 7));
        var pathAsString = string.Join(" -> ", path.Select(p => p.Location));
        Console.WriteLine(pathAsString);
        Console.ReadKey();
    }
}

I basically followed this blog: http://www.redblobgames.com/pathfinding/a-star/introduction.html (which is awesome and really well explained).