Brought back the closed_set to avoid visiting nodes already handled.

goap.cpp

@@ -106,6 +106,7 @@ namespace ai {
   }
 
   ActionPlan plan_actions(std::vector<Action>& actions, State start, State goal) {
+    int loop_count = 0;
     std::unordered_map<ActionState, int> open_set;
     std::unordered_map<Action, Action> came_from;
     std::unordered_map<State, int> g_score;
@@ -120,10 +121,12 @@ namespace ai {
     open_set.insert_or_assign(current, h(start, goal));
 
     while(!open_set.empty()) {
+      loop_count++;
       // current := the node in openSet having the lowest fScore[] value
       current = find_lowest(open_set, f_score);
 
       if(is_subset(current.state, goal)) {
+        fmt::println("YES CLOSED COUNT: {}", loop_count);
         return {true,
           reconstruct_path(came_from, current.action)};
       }
@@ -136,7 +139,13 @@ namespace ai {
         if(!neighbor_action.can_effect(current.state)) continue;
 
         auto neighbor = neighbor_action.apply_effect(current.state);
-        // if(closed_set.contains(neighbor)) continue;
+        if(closed_set.contains(neighbor)) {
+          fmt::println("closed_set: {}, open_set: {}, f_score: {}",
+              closed_set.size(), open_set.size(), f_score.size());
+          continue;
+        } else {
+          loop_count++;
+        }
 
         int d_score = d(current.state, neighbor) + neighbor_action.cost;
 
@@ -161,6 +170,7 @@ namespace ai {
       }
     }
 
+    fmt::println("YES CLOSED COUNT: {}", loop_count);
     return {is_subset(current.state, goal), reconstruct_path(came_from, current.action)};
   }
 }