static void Main(string[] args) { var endGoalFound = false; var costs = new[] { //1, 1, 1, 1, 0, 1, 0, 1, 0, 1, 1, 1 0, 0, 1, 0, 1, 1, 1, 0, 1 }; var map = new Map(3, 3, costs); var start = new Node(2, 0); var end = new Node(0, 2); var cameFrom = new Dictionary <Node, Node>(); cameFrom.Add(start, null); var costSoFar = new Dictionary <Node, int>(); costSoFar.Add(start, 0); var frontier = new List <Node>(); frontier.Add(start); while (frontier.Count > 0) { var current = frontier.OrderBy(x => x.Priority).First(); frontier.Remove(current); if (Equals(current, end)) { endGoalFound = true; break; } var neighbours = map.GetNeighbours(current); foreach (var neighbour in neighbours) { var newCost = costSoFar[current] + map.Cost(current, neighbour); if (costSoFar.ContainsKey(neighbour) == false || costSoFar[neighbour] > newCost) { costSoFar[neighbour] = newCost; neighbour.Priority = newCost + EstimateDistance(end, neighbour); frontier.Add(neighbour); cameFrom[neighbour] = current; } } } if (endGoalFound == false) { Console.WriteLine("No path to end"); return; } var path = new List <Node>(); var pathNode = end; while (!Equals(pathNode, start)) { path.Add(pathNode); pathNode = cameFrom[pathNode]; } path.Reverse(); foreach (var node in path) { Console.WriteLine($"{node.X}, {node.Y}"); //Console.WriteLine($"{map.Nodes.IndexOf(node)}"); } Console.ReadKey(); }