Actually I can just use the ai::distance_to_goal function on the fit_sort to sort by cost and distance.

ai.cpp

@@ -167,8 +167,8 @@ namespace ai {
     if(active()) {
       std::sort(plan.script.begin(), plan.script.end(),
           [&](auto& l, auto& r) {
-            int l_cost = l.cost + (!l.can_effect(start) * ai::SCORE_MAX);
-            int r_cost = r.cost + (!r.can_effect(start) * ai::SCORE_MAX);
+            int l_cost = l.cost + ai::distance_to_goal(start, goal);
+            int r_cost = r.cost + ai::distance_to_goal(start, goal);
             return l_cost < r_cost;
           });
     }