diff --git a/include/config.hpp b/include/config.hpp
index 0a0aa79..ae77731 100644
--- a/include/config.hpp
+++ b/include/config.hpp
@@ -16,8 +16,8 @@ constexpr int MENU_HEIGHT = 200;
 
 // Menu
 constexpr int MENU_PAD = 5;
-constexpr int BUTTON_PAD = 20;
-constexpr int MENU_ROWS = 3;
+constexpr int BUTTON_PAD = 12;
+constexpr int MENU_ROWS = 4;
 constexpr int MENU_COLS = 3;
 
 // Camera Controls
diff --git a/include/input.hpp b/include/input.hpp
index 5f6263e..1832b41 100644
--- a/include/input.hpp
+++ b/include/input.hpp
@@ -17,6 +17,7 @@ public:
   int block_add_x;
   int block_add_y;
 
+  bool mark_path;
   bool mark_solutions;
   bool connect_solutions;
 
@@ -24,7 +25,7 @@ public:
   InputHandler(StateManager &_state)
       : state(_state), hov_x(-1), hov_y(-1), sel_x(0), sel_y(0),
         has_block_add_xy(false), block_add_x(-1), block_add_y(-1),
-        mark_solutions(false), connect_solutions(false) {}
+        mark_path(false), mark_solutions(false), connect_solutions(false) {}
 
   InputHandler(const InputHandler &copy) = delete;
   InputHandler &operator=(const InputHandler &copy) = delete;
diff --git a/include/state.hpp b/include/state.hpp
index a88fa24..b9d0792 100644
--- a/include/state.hpp
+++ b/include/state.hpp
@@ -68,6 +68,8 @@ public:
 
   auto NextPreset() -> void;
 
+  auto NextPath() -> void;
+
   auto FillGraph() -> void;
 
   auto UpdateGraph() -> void;
diff --git a/src/distance.cpp b/src/distance.cpp
index 977b586..a727ca8 100644
--- a/src/distance.cpp
+++ b/src/distance.cpp
@@ -56,17 +56,12 @@ auto CalculateDistances(
     queue.pop();
 
     for (std::size_t neighbor : adjacency[current]) {
-      // std::cout << "Visiting edge (" << current << "->" << neighbor << ")."
-      //           << std::endl;
       if (distances[neighbor] == -1) {
         // If distance is -1 we haven't visited the node yet
         distances[neighbor] = distances[current] + 1;
         parents[neighbor] = current;
         nearest_targets[neighbor] = nearest_targets[current];
-        // std::cout << "- Distance: " << distances[neighbor]
-        //           << ", Parent: " << parents[neighbor]
-        //           << ", Nearest Target: " << nearest_targets[neighbor] << "."
-        //           << std::endl;
+
         queue.push(neighbor);
       }
     }
diff --git a/src/input.cpp b/src/input.cpp
index 35b0fd0..5421d16 100644
--- a/src/input.cpp
+++ b/src/input.cpp
@@ -103,19 +103,19 @@ auto InputHandler::HandleMouse() -> void {
 }
 
 auto InputHandler::HandleKeys() -> void {
-  if (IsKeyPressed(KEY_W) || IsKeyPressed(KEY_K)) {
+  if (IsKeyPressed(KEY_W)) {
     if (state.current_state.MoveBlockAt(sel_x, sel_y, Direction::NOR)) {
       sel_y--;
     }
-  } else if (IsKeyPressed(KEY_A) || IsKeyPressed(KEY_H)) {
+  } else if (IsKeyPressed(KEY_A)) {
     if (state.current_state.MoveBlockAt(sel_x, sel_y, Direction::WES)) {
       sel_x--;
     }
-  } else if (IsKeyPressed(KEY_S) || IsKeyPressed(KEY_J)) {
+  } else if (IsKeyPressed(KEY_S)) {
     if (state.current_state.MoveBlockAt(sel_x, sel_y, Direction::SOU)) {
       sel_y++;
     }
-  } else if (IsKeyPressed(KEY_D) || IsKeyPressed(KEY_L)) {
+  } else if (IsKeyPressed(KEY_D)) {
     if (state.current_state.MoveBlockAt(sel_x, sel_y, Direction::EAS)) {
       sel_x++;
     }
@@ -150,6 +150,10 @@ auto InputHandler::HandleKeys() -> void {
     mark_solutions = !mark_solutions;
   } else if (IsKeyPressed(KEY_O)) {
     connect_solutions = !connect_solutions;
+  } else if (IsKeyPressed(KEY_U)) {
+    mark_path = !mark_path;
+  } else if (IsKeyPressed(KEY_SPACE)) {
+    state.NextPath();
   } else if (IsKeyPressed(KEY_F)) {
     state.current_state.ToggleRestricted();
     state.ClearGraph();
diff --git a/src/renderer.cpp b/src/renderer.cpp
index dcf97b7..e947862 100644
--- a/src/renderer.cpp
+++ b/src/renderer.cpp
@@ -153,6 +153,17 @@ auto Renderer::DrawMassSprings(const std::vector<Vector3> &masses) -> void {
     }
   }
 
+  // Mark winning path
+  if (input.mark_path) {
+    for (const std::size_t &_state : state.winning_path) {
+      if (masses.size() > _state) {
+        const Vector3 &path_mass = masses.at(_state);
+        DrawCube(path_mass, VERTEX_SIZE * 1.75, VERTEX_SIZE * 1.75,
+                 VERTEX_SIZE * 1.75, YELLOW);
+      }
+    }
+  }
+
   // Mark starting state
   std::size_t starting_index = state.states.at(state.starting_state);
   if (masses.size() > starting_index) {
@@ -308,7 +319,7 @@ auto Renderer::DrawMenu(const std::vector<Vector3> &masses) -> void {
     DrawRectangle(posx, posy, btn_width, btn_height, Fade(color, 0.6));
     DrawRectangleLines(posx, posy, btn_width, btn_height, color);
     DrawText(text.data(), posx + BUTTON_PAD, posy + BUTTON_PAD,
-             btn_height - 2 * BUTTON_PAD, WHITE);
+             btn_height - 2.0 * BUTTON_PAD, WHITE);
   };
 
   draw_btn(0, 0,
@@ -336,9 +347,11 @@ auto Renderer::DrawMenu(const std::vector<Vector3> &masses) -> void {
   draw_btn(2, 1, std::format("Populate Graph (G), Clear Graph (C)"),
            DARKPURPLE);
   draw_btn(2, 2,
-           std::format("Mark (I): {} / Connect (O): {}", input.mark_solutions,
+           std::format("Path (U): {} / Mark (I): {} / Connect (O): {}",
+                       input.mark_path, input.mark_solutions,
                        input.connect_solutions),
            DARKPURPLE);
+  draw_btn(2, 3, std::format("Move along Path (Space)"), DARKPURPLE);
 
   DrawLine(0, MENU_HEIGHT - 1, GetScreenWidth(), MENU_HEIGHT - 1, BLACK);
   EndTextureMode();
diff --git a/src/state.cpp b/src/state.cpp
index f037a2d..1d217d6 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -50,6 +50,7 @@ auto StateManager::LoadPreset(int preset) -> void {
 auto StateManager::ResetState() -> void {
   current_state = presets.at(current_preset);
   previous_state = current_state;
+  FindTargetPath();
   if (edited) {
     // We also need to clear the graph in case the state has been edited
     // because the graph could contain states that are impossible to reach
@@ -67,6 +68,23 @@ auto StateManager::NextPreset() -> void {
   LoadPreset((current_preset + 1) % presets.size());
 }
 
+auto StateManager::NextPath() -> void {
+  if (target_distances.Empty()) {
+    return;
+  }
+
+  // Already there
+  if (target_distances.distances[CurrentMassIndex()] == 0) {
+    return;
+  }
+
+  std::size_t parent = target_distances.parents[CurrentMassIndex()];
+  // std::cout << "Parent of node " << CurrentMassIndex() << " is " << parent
+  //           << std::endl;
+  current_state = masses.at(parent);
+  FindTargetPath();
+}
+
 auto StateManager::FillGraph() -> void {
   ZoneScoped;
 
@@ -80,7 +98,7 @@ auto StateManager::FillGraph() -> void {
   physics.AddMassSpringsCmd(closure.first.size(), closure.second);
   for (const State &state : closure.first) {
     states.insert(std::make_pair(state, states.size()));
-    masses.insert(std::make_pair(masses.size(), state));
+    masses.insert(std::make_pair(states.size() - 1, state));
   }
   for (const auto &[from, to] : closure.second) {
     springs.emplace_back(from, to);
@@ -88,6 +106,12 @@ auto StateManager::FillGraph() -> void {
   FindWinningStates();
   FindTargetDistances();
   FindTargetPath();
+
+  // Sanity check. Both values need to be equal
+  // for (const auto &[mass, state] : masses) {
+  //   std::cout << "Masses: " << mass << ", States: " << states.at(state)
+  //             << std::endl;
+  // }
 }
 
 auto StateManager::UpdateGraph() -> void {
@@ -97,7 +121,7 @@ auto StateManager::UpdateGraph() -> void {
 
   if (!states.contains(current_state)) {
     states.insert(std::make_pair(current_state, states.size()));
-    masses.insert(std::make_pair(masses.size(), current_state));
+    masses.insert(std::make_pair(states.size() - 1, current_state));
     springs.emplace_back(states.at(current_state), states.at(previous_state));
     physics.AddMassCmd();
     physics.AddSpringCmd(states.at(current_state), states.at(previous_state));
@@ -124,7 +148,7 @@ auto StateManager::ClearGraph() -> void {
 
   // Re-add the default stuff to the graph
   states.insert(std::make_pair(current_state, states.size()));
-  masses.insert(std::make_pair(masses.size(), current_state));
+  masses.insert(std::make_pair(states.size() - 1, current_state));
   visited_states.insert(current_state);
   physics.AddMassCmd();
 
@@ -158,8 +182,9 @@ auto StateManager::FindTargetDistances() -> void {
 
   target_distances = CalculateDistances(states.size(), springs, targets);
 
-  std::cout << "Calculated " << target_distances.distances.size()
-            << " distances to " << targets.size() << " targets." << std::endl;
+  // std::cout << "Calculated " << target_distances.distances.size()
+  //           << " distances to " << targets.size() << " targets." <<
+  //           std::endl;
 }
 
 auto StateManager::FindTargetPath() -> void {
@@ -168,9 +193,8 @@ auto StateManager::FindTargetPath() -> void {
   }
 
   winning_path = GetPath(target_distances, CurrentMassIndex());
-
-  std::cout << "Nearest target is " << winning_path.size() << " moves away."
-            << std::endl;
+  // std::cout << "Nearest target is " << winning_path.size() << " moves away."
+  //           << std::endl;
 }
 
 auto StateManager::CurrentMassIndex() const -> std::size_t {