194 lines
6.7 KiB
C++

#include "hierarchical_fsm.hpp"
#include <algorithm>
#include <cstddef>
#include <utility>
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) continue;
}
return selected;
}
std::vector<StateId> ancestry(const Chart &chart, StateId id)
{
std::vector<StateId> 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<Memory> &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<Memory> &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<Record> &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<Record> &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<StateId> from = ancestry(chart, source);
const std::vector<StateId> 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() && source != target ? 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.back());
}
output.trace.push_back(Record{Phase::leave, leaving, description.leave_tag});
}
next.active.assign(old.active.begin(), old.active.begin() + static_cast<std::ptrdiff_t>(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<StateId> 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<Record> &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<Record> 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;
}
}