The autowalker now uses the GOAP AI system to walk the map and do its thing. The code needs a big cleanup, so I might just do a full rewrite based on what I know now.

scratchpad/a_star.cpp

@@ -0,0 +1,82 @@
+
+constexpr const float SCORE_MAX = std::numeric_limits<float>::max()
+
+using AStarPath = std::deque<Point>;
+
+void update_map(Matrix& map, std::deque<Point>& total_path) {
+  for(auto &point : total_path) {
+    map[point.y][point.x] = 10;
+  }
+}
+
+AStarPath reconstruct_path(std::unordered_map<Point, Point>& came_from, Point current) {
+  std::deque<Point> total_path{current};
+
+  while(came_from.contains(current)) {
+    current = came_from[current];
+    total_path.push_front(current);
+  }
+
+  return total_path;
+}
+
+inline float h(Point from, Point to) {
+  return std::hypot(float(from.x) - float(to.x),
+      float(from.y) - float(to.y));
+}
+
+inline float d(Point current, Point neighbor) {
+  return std::hypot(float(current.x) - float(neighbor.x),
+      float(current.y) - float(neighbor.y));
+}
+
+inline Point find_lowest(std::unordered_map<Point, float>& open_set) {
+  dbc::check(!open_set.empty(), "open set can't be empty in find_lowest");
+  Point result;
+  float lowest_score = SCORE_MAX;
+
+  for(auto [point, score] : open_set) {
+    if(score < lowest_score) {
+      lowest_score = score;
+      result = point;
+    }
+  }
+
+  return result;
+}
+
+
+std::optional<AStarPath> path_to_player(Matrix& map, Point start, Point goal) {
+  std::unordered_map<Point, float> open_set;
+  std::unordered_map<Point, Point> came_from;
+  std::unordered_map<Point, float> g_score;
+  g_score[start] = 0;
+
+  open_set[start] = g_score[start] + h(start, goal);
+
+  while(!open_set.empty()) {
+    auto current = find_lowest(open_set);
+
+    if(current == goal) {
+      return std::make_optional<AStarPath>(reconstruct_path(came_from, current));
+    }
+
+    open_set.erase(current);
+
+    for(matrix::compass it{map, current.x, current.y}; it.next();) {
+      Point neighbor{it.x, it.y};
+
+      float d_score = d(current, neighbor) + map[it.y][it.x] * SCORE_MAX;
+      float tentative_g_score = g_score[current] + d_score;
+      float neighbor_g_score = g_score.contains(neighbor) ? g_score[neighbor] : SCORE_MAX;
+      if(tentative_g_score < neighbor_g_score) {
+        came_from[neighbor] = current;
+        g_score[neighbor] = tentative_g_score;
+        // open_set gets the fScore
+        open_set[neighbor] = tentative_g_score + h(neighbor, goal);
+      }
+    }
+  }
+
+  return std::nullopt;
+}