#include "hierarchical_fsm.hpp" #include #include #include namespace hierarchical_fsm { namespace { const State &node(const Chart &chart, StateId id) { const auto found = std::lower_bound(chart.states.begin(), chart.states.end(), id, [](const State &state, StateId key) { return state.id < key; }); return *found; } bool valid_event(const RawEvent &event) { return event.event <= 255 && event.flags <= 4294967295ULL; } bool enabled(const Transition &rule, const RawEvent &event) { return rule.event == event.event && (event.flags & rule.required) == rule.required && (event.flags & rule.forbidden) == 0; } bool better(const Transition &left, const Transition &right) { return left.priority > right.priority || (left.priority == right.priority && left.id < right.id); } Selection choose(const Chart &chart, const Snapshot &state, const RawEvent &event) { Selection selected; for (auto active = state.active.rbegin(); active != state.active.rend(); ++active) { auto rule = std::lower_bound(chart.transitions.begin(), chart.transitions.end(), *active, [](const Transition &candidate, StateId source) { return candidate.source < source; }); while (rule != chart.transitions.end() && rule->source == *active) { if (enabled(*rule, event) && (!selected.found || better(*rule, selected.transition))) { selected.found = true; selected.transition = *rule; } ++rule; } if (selected.found) return selected; } return selected; } std::vector ancestry(const Chart &chart, StateId id) { std::vector result; while (id != 0) { result.push_back(id); id = node(chart, id).parent; } std::reverse(result.begin(), result.end()); return result; } void remember(std::vector &history, StateId composite, StateId child) { const auto position = std::lower_bound(history.begin(), history.end(), composite, [](const Memory &memory, StateId id) { return memory.composite < id; }); if (position != history.end() && position->composite == composite) { position->child = child; } else { history.insert(position, Memory{composite, child}); } } StateId recalled(const std::vector &history, StateId composite) { const auto position = std::lower_bound(history.begin(), history.end(), composite, [](const Memory &memory, StateId id) { return memory.composite < id; }); if (position == history.end() || position->composite != composite) return 0; return position->child; } void enter(const Chart &chart, StateId id, Snapshot &state, std::vector &trace) { state.active.push_back(id); trace.push_back(Record{Phase::enter, id, node(chart, id).enter_tag}); } void initial_descent(const Chart &chart, StateId from, Snapshot &state, std::vector &trace) { StateId child = node(chart, from).initial; while (child != 0) { enter(chart, child, state, trace); child = node(chart, child).initial; } } void external(const Chart &chart, const Snapshot &old, const Transition &rule, Snapshot &next, Step &output) { const StateId source = rule.source; const StateId target = rule.target; const std::vector from = ancestry(chart, source); const std::vector to = ancestry(chart, target); std::size_t common = 0; while (common < from.size() && common < to.size() && from[common] == to[common]) { ++common; } const std::size_t keep = common == from.size() ? common - 1 : common; output.boundary = keep == 0 ? 0 : old.active[keep - 1]; for (std::size_t reverse = old.active.size(); reverse > keep; --reverse) { const std::size_t position = reverse - 1; const StateId leaving = old.active[position]; const State &description = node(chart, leaving); if (description.initial != 0) { remember(next.history, leaving, old.active[position + 1]); } output.trace.push_back(Record{Phase::leave, leaving, description.leave_tag}); } next.active.assign(old.active.begin(), old.active.begin() + static_cast(keep)); output.trace.push_back(Record{Phase::action, source, rule.action_tag}); for (std::size_t position = keep; position < to.size(); ++position) { enter(chart, to[position], next, output.trace); } StateId child = node(chart, target).initial; if (rule.target_mode == 1) { const StateId saved = recalled(next.history, target); if (saved != 0) child = saved; } if (child != 0) { const std::vector restored = ancestry(chart, child); for (std::size_t position = to.size(); position < restored.size(); ++position) { enter(chart, restored[position], next, output.trace); } initial_descent(chart, child, next, output.trace); } } } Snapshot Machine::snapshot() const { return state_; } Status Machine::reset(const Chart &chart, std::vector &trace) { trace.clear(); Analysis analyzed; if (analyze_chart(chart, analyzed) != Status::ok) return Status::invalid; Chart normalized; normalized.root = chart.root; normalized.transitions = std::move(analyzed.transitions); for (const StateInfo &info : analyzed.states) normalized.states.push_back(info.state); Snapshot replacement; replacement.configured = true; replacement.root = chart.root; std::vector entered; enter(normalized, chart.root, replacement, entered); initial_descent(normalized, chart.root, replacement, entered); chart_ = std::move(normalized); state_ = std::move(replacement); trace = std::move(entered); return Status::ok; } Status Machine::select(const RawEvent &event, Selection &output) const { output = Selection{}; if (!state_.configured) return Status::unconfigured; if (!valid_event(event)) return Status::invalid; output = choose(chart_, state_, event); return Status::ok; } Status Machine::dispatch(const RawEvent &event, Step &output) { output = Step{}; Selection selected; const Status status = select(event, selected); if (status != Status::ok) return status; if (!selected.found) return Status::ok; Snapshot replacement = state_; Step result; result.selected = selected; if (selected.transition.kind == 0) { result.trace.push_back(Record{Phase::action, selected.transition.source, selected.transition.action_tag}); } else { external(chart_, state_, selected.transition, replacement, result); } state_ = std::move(replacement); output = std::move(result); return Status::ok; } }