public static void NeoPathfinding(Point P, Point Goal) { Util.PriorityQueue <Point> frontier = new Util.PriorityQueue <Point>(); frontier.Enqueue(P, 0); Dictionary <Point, Point> came_from = new Dictionary <Point, Point>(); Dictionary <Point, int> cost_so_far = new Dictionary <Point, int>(); Dictionary <Point, Tool> toolset = new Dictionary <Point, Tool>(); //came_from.Add(new Point(P.X, P.Y), ); cost_so_far.Add(new Point(P.X, P.Y), 0); toolset.Add(new Point(P.X, P.Y), Tool.Torch); while (frontier.Count > 0) { Point current = frontier.Dequeue(); if (current == Goal) { if (toolset[current] != Tool.Torch) { Console.WriteLine("PART 2 T: " + (cost_so_far[current] + 11)); } else { Console.WriteLine("PART 2: " + (cost_so_far[current])); } break; } List <Point> chkPoints = new List <Point>(); if (doesExist(onMyTop(current))) { chkPoints.Add(onMyTop(current)); } if (doesExist(onMyLeft(current))) { chkPoints.Add(onMyLeft(current)); } if (doesExist(onMyRight(current))) { chkPoints.Add(onMyRight(current)); } if (doesExist(onMyBottom(current))) { chkPoints.Add(onMyBottom(current)); } foreach (var next in chkPoints) { int new_cost = cost_so_far[current] + costCalc(toolset[current], next); if (cost_so_far.ContainsKey(next) == false || new_cost < cost_so_far[next]) { cost_so_far[next] = new_cost; int priority = new_cost + Util.ManhattanDist(Goal, next); frontier.Enqueue(next, priority); came_from[next] = current; if (isObstructed(next, toolset[current])) { toolset[next] = switchTool(toolset[current], next); } else { toolset[next] = toolset[current]; } } } } }