194 lines
6.7 KiB
C++
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) return selected;
|
|
}
|
|
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;
|
|
}
|
|
}
|