diff --git a/main.cpp b/main.cpp
index 57ee257..ed962e6 100644
--- a/main.cpp
+++ b/main.cpp
@@ -9,11 +9,9 @@ struct Maze {
 };
 
 struct MazeNode {
-    MazeNode(uint x, uint y, uint cost, uint travelled) : x(x), y(y), cost(cost), travelled(travelled) {}
-    uint x;
-    uint y;
-    uint cost;
-    uint travelled;
+    MazeNode(std::vector<uint> v, uint priority) : v(v), priority(priority) {}
+    std::vector<uint> v;
+    uint priority;
 };
 
 Maze solve_maze(const Maze &maze) {
@@ -22,49 +20,66 @@ Maze solve_maze(const Maze &maze) {
     result = maze;
     
     std::vector<MazeNode> nodes;
-    std::make_heap(nodes.begin(), nodes.end(), [](MazeNode n1, MazeNode n2){return n1.cost > n2.cost;});
+    std::make_heap(nodes.begin(), nodes.end(), [](MazeNode n1, MazeNode n2){return n1.priority > n2.priority;});
 
-    nodes.push_back(MazeNode(1, 1));
-    std::push_heap(nodes.begin(), nodes.end(), [](MazeNode n1, MazeNode n2){return n1.cost > n2.cost;});
+    nodes.push_back(MazeNode(std::vector<uint>{1, 1}, 0));
+    std::push_heap(nodes.begin(), nodes.end(), [](MazeNode n1, MazeNode n2){return n1.priority > n2.priority;});
 
-    std::map<MazeNode, uint> costs;
-    std::map<MazeNode, MazeNode> parents;
+    std::map<std::vector<uint>, uint> costs;
+    std::map<std::vector<uint>, std::vector<uint>> parents;
 
-    for (int i = 0; i < 5000; i++) {
+    costs[std::vector<uint>{1, 1}] = 1;
+
+    for (int i = 0; i < 270; i++) {
         // Get the best node
         MazeNode best = nodes.front();
-        std::pop_heap(nodes.begin(), nodes.end(), [](MazeNode n1, MazeNode n2){return n1.cost > n2.cost;});
+        std::pop_heap(nodes.begin(), nodes.end(), [](MazeNode n1, MazeNode n2){return n1.priority > n2.priority;});
         nodes.pop_back();
 
-        int current_x = best.x;
-        int current_y = best.y;
+        int current_x = best.v[0];
+        int current_y = best.v[1];
+        std::vector<uint> here = std::vector<uint>{current_x, current_y};
 
         result.data[current_y][current_x] = 'P';
 
-        std::cout << i << ": " << current_x << ", " << current_y << " with " << best.travelled << " travelled" << " and cost " << best.cost << std::endl;
+        std::cout << i << ": " << current_x << ", " << current_y << " with " << " and priority " << best.priority << std::endl;
+
+        // The cost is the travelled distance to the next node
+        uint cost = costs[here] + 1;
 
         for (int kernel_y = -1; kernel_y <= 1; kernel_y++) {
             for (int kernel_x = -1; kernel_x <= 1; kernel_x++) {
                 // Ignore the current position
                 if (kernel_x == 0 && kernel_y == 0) continue;
 
-                int x = current_x + kernel_x;
-                int y = current_y + kernel_y;
+                uint x = current_x + kernel_x;
+                uint y = current_y + kernel_y;
 
                 // Can we go here?
                 if (maze.data[y][x] == '#') continue;
 
-                // The cost is the already travelled distance together with the optimal
-                //   distance from here to the target
-                uint cost = best.travelled + 1 + (39 - x) + (39 - y);
+                // Are we done?
+                if (x == 39 && y == 39) {
+                    return result;
+                }
 
-                // Add this node
-                nodes.push_back(MazeNode(x, y, cost, best.travelled + 1));
-                std::push_heap(nodes.begin(), nodes.end(), [](MazeNode n1, MazeNode n2){return n1.cost > n2.cost;});
+                std::vector<uint> next = std::vector<uint>{x, y};
+
+                // If this node hasn't been added yet or its costs are better, add it!
+                if (costs.find(next) == costs.end() || cost < costs[next]) {
+                    costs[next] = cost;
+                    uint priority = cost + (39 - x) + (39 - y);
+
+                    std::cout << x << ", " << y << ": Adding node with priority " << priority << std::endl;
+
+                    // Add this node
+                    nodes.push_back(MazeNode(next, priority));
+                    std::push_heap(nodes.begin(), nodes.end(), [](MazeNode n1, MazeNode n2){return n1.priority > n2.priority;});
+
+                    parents[next] = here;
+                }
             }
         }
-
-        std::cout << nodes.size() << std::endl;
     }
     
     return result;