raycaster/goap.cpp

#include "dbc.hpp"
#include "goap.hpp"
#include "ai_debug.hpp"
#include "stats.hpp"
#include <queue>

namespace ai {

  using namespace nlohmann;
  using namespace dbc;

  bool is_subset(State& source, State& target) {
    State result = source & target;
    return result == target;
  }

  void Action::needs(int name, bool val) {
    if(val) {
      $positive_preconds[name] = true;
      $negative_preconds[name] = false;
    } else {
      $negative_preconds[name] = true;
      $positive_preconds[name] = false;
    }
  }

  void Action::effect(int name, bool val) {
    if(val) {
      $positive_effects[name] = true;
      $negative_effects[name] = false;
    } else {
      $negative_effects[name] = true;
      $positive_effects[name] = false;
    }
  }

  void Action::ignore(int name) {
    $positive_preconds[name] = false;
    $negative_preconds[name] = false;
  }

  bool Action::can_effect(State& state) {
    return ((state & $positive_preconds) == $positive_preconds) &&
      ((state & $negative_preconds) == ALL_ZERO);
  }

  State Action::apply_effect(State& state) {
    return (state | $positive_effects) & ~$negative_effects;
  }

  int distance_to_goal(State from, State to) {
    auto result = from ^ to;
    int count = result.count();
    return count;
  }

  inline void dump_came_from(std::string msg, std::unordered_map<Action, Action>& came_from, Action& current) {
    fmt::println("{}: {}", msg, current.name);

    for(auto& [from, to] : came_from) {
      fmt::println("from={}; to={}", from.name, to.name);
    }
  }

  inline void path_invariant(std::unordered_map<Action, Action>& came_from, Action& current) {
#if defined(NDEBUG)
    (void)came_from; // disable errors about unused
    (void)current;
#else
    bool final_found = current == FINAL_ACTION;

    for(size_t i = 0; i <= came_from.size() && came_from.contains(current); i++) {
      current = came_from.at(current);
      final_found = current == FINAL_ACTION;
    }

    if(!final_found) {
      dump_came_from("CYCLE DETECTED!", came_from, current);
      dbc::sentinel("AI CYCLE FOUND!");
    }
#endif
  }

  Script reconstruct_path(std::unordered_map<Action, Action>& came_from, Action& current) {
    Script total_path{current};

    path_invariant(came_from, current);

    for(size_t i = 0; i <= came_from.size() && came_from.contains(current); i++) {
      auto next = came_from.at(current);

      if(next != FINAL_ACTION) {
        // remove the previous node to avoid cycles and repeated actions
        total_path.push_front(next);
        came_from.erase(current);
        current = next;
      } else {
        // found the terminator, done
        break;
      }
    }

    return total_path;
  }

  inline int h(State start, State goal) {
    return distance_to_goal(start, goal);
  }

  inline int d(State start, State goal) {
    return distance_to_goal(start, goal);
  }

  using FScorePair = std::pair<int, ActionState>;
  auto FScorePair_cmp = [](const FScorePair& l, const FScorePair& r) {
      return l.first < r.first;
  };
  using FScoreQueue = std::vector<FScorePair>;

  ActionState find_lowest(std::unordered_map<ActionState, int>& open_set,
      FScoreQueue& f_scores)
  {
    check(!open_set.empty(), "open set can't be empty in find_lowest");

    for(auto& [score, astate] : f_scores) {
      if(open_set.contains(astate)) {
        return astate;
      }
    }

    dbc::sentinel("lowest not found!");
  }

  ActionPlan plan_actions(std::vector<Action>& actions, State start, State goal) {
    std::unordered_map<ActionState, int> open_set;
    std::unordered_map<Action, Action> came_from;
    std::unordered_map<State, int> g_score;
    FScoreQueue f_score;
    std::unordered_map<State, bool> closed_set;
    ActionState current{FINAL_ACTION, start};

    g_score.insert_or_assign(start, 0);
    f_score.emplace_back(h(start, goal), current);
    std::push_heap(f_score.begin(), f_score.end(), FScorePair_cmp);

    open_set.insert_or_assign(current, h(start, goal));

    while(!open_set.empty()) {
      // current := the node in openSet having the lowest fScore[] value
      current = find_lowest(open_set, f_score);

      if(is_subset(current.state, goal)) {
        return {true,
          reconstruct_path(came_from, current.action)};
      }

      open_set.erase(current);
      closed_set.insert_or_assign(current.state, true);

      for(auto& neighbor_action : actions) {
        // calculate the State being current/neighbor
        if(!neighbor_action.can_effect(current.state)) continue;

        auto neighbor = neighbor_action.apply_effect(current.state);
        if(closed_set.contains(neighbor)) continue;

        int d_score = d(current.state, neighbor) + neighbor_action.cost;

        int tentative_g_score = g_score[current.state] + d_score;
        int neighbor_g_score = g_score.contains(neighbor) ? g_score[neighbor] : SCORE_MAX;

        if(tentative_g_score < neighbor_g_score) {
          came_from.insert_or_assign(neighbor_action, current.action);

          g_score.insert_or_assign(neighbor, tentative_g_score);
          ActionState neighbor_as{neighbor_action, neighbor};

          int score = tentative_g_score + h(neighbor, goal);
          f_score.emplace_back(score, neighbor_as);
          std::push_heap(f_score.begin(), f_score.end(), FScorePair_cmp);

          // this maybe doesn't need score
          open_set.insert_or_assign(neighbor_as, score);
        }
      }
    }

    return {is_subset(current.state, goal), reconstruct_path(came_from, current.action)};
  }
}