eis/eqpalg/utility/fb_state_machine.cpp

// eqpalg/utility/fb_state_machine.cpp
#include <eqpalg/utility/fb_state_machine.h>
#include <cmath>
#include <cfloat>
#include <string>

// ========== 配置 ==========

void FbStateMachine::configure(bool keepMode, TimeDur timeout) {
  keep_mode_ = keepMode;
  timeout_ = timeout;
  state_ = FbState::Idle;
}

// ========== 每周期状态更新 ==========

FbUpdateResult FbStateMachine::update(bool actTriggered, TimePoint now,
                                       std::map<std::string, double>& mm_vars,
                                       size_t tag_count) {
  FbUpdateResult result;
  result.funVarsNeedReset = false;

  switch (state_) {

  case FbState::Idle:
    if (actTriggered) {
      // → Started：快照变量
      snapshotActionStart(now, mm_vars, tag_count);
      state_ = FbState::Started;
      start_time_ = now;
      result.state = FbState::Started;
    } else {
      result.state = FbState::Idle;
      result.funVarsNeedReset = true;  // 空闲时重置 fun_vars
    }
    break;

  case FbState::Started:
    // 更新累积变量（首周期也需要更新）
    updateActionVars(now, mm_vars, tag_count);

    if (!actTriggered && keep_mode_) {
      state_ = FbState::Idle;
      result.state = FbState::NotHold;
      result.funVarsNeedReset = true;
    } else if (isAccumulatorNearOverflow(mm_vars, tag_count)) {
      state_ = FbState::Idle;
      result.state = FbState::Timeout;
      result.funVarsNeedReset = true;
    } else {
      state_ = FbState::InProgress;
      result.state = FbState::Started;  // 返回 Started，调用方跳过反馈检查
    }
    break;

  case FbState::InProgress:
    // 先更新累积变量
    updateActionVars(now, mm_vars, tag_count);

    if (!actTriggered && keep_mode_) {
      state_ = FbState::Idle;
      result.state = FbState::NotHold;
      result.funVarsNeedReset = true;
    } else if (isAccumulatorNearOverflow(mm_vars, tag_count)) {
      clearActionAccumulators(mm_vars, tag_count);
      state_ = FbState::Idle;
      result.state = FbState::Timeout;
      result.funVarsNeedReset = true;
    } else if (timeout_ != TimeDur(-32768) &&
               (now - start_time_) > timeout_) {
      clearActionAccumulators(mm_vars, tag_count);
      state_ = FbState::Idle;
      result.state = FbState::Timeout;
      result.funVarsNeedReset = true;
    } else {
      result.state = FbState::InProgress;
    }
    break;

  // 终端状态 → 下周期自动回 Idle
  default:
    state_ = FbState::Idle;
    result.state = FbState::Idle;
    result.funVarsNeedReset = true;
    break;
  }

  return result;
}

// ========== 反馈条件检查 ==========

bool FbStateMachine::checkFeedback(bool fbCondition, TimePoint now,
                                    std::map<std::string, double>& mm_vars) {
  if (state_ != FbState::InProgress) {
    return false;
  }

  if (fbCondition) {
    recordActionEnd(now, mm_vars);
    state_ = FbState::Idle;  // 下周期回到 Idle
    return true;
  }
  return false;
}

// ========== 查询 ==========

bool FbStateMachine::isActive() const {
  return state_ == FbState::Started || state_ == FbState::InProgress;
}

// ========== 强制重置 ==========

void FbStateMachine::forceReset() {
  state_ = FbState::Idle;
  start_time_ = TimePoint{};
}

// ========== 内部变量操作 ==========

void FbStateMachine::snapshotActionStart(
    TimePoint now,
    std::map<std::string, double>& mm_vars,
    size_t tag_count) {

  mm_vars["stime"] = std::chrono::duration_cast<std::chrono::milliseconds>(
                         now.time_since_epoch())
                         .count();
  mm_vars["time"] = 0;

  for (size_t i = 0; i < tag_count; i++) {
    std::string idx = std::to_string(i + 1);
    double tag_val = mm_vars["tag" + idx];
    double p_val = mm_vars["p" + idx];

    // s[n] = tag[n] 在动作开始时刻的快照值
    mm_vars["s" + idx] = tag_val;

    // 极值初始化为当前值
    mm_vars["mx_tag" + idx] = tag_val;
    mm_vars["mi_tag" + idx] = tag_val;

    // 累积器初始化为 0
    mm_vars["mv2_tag" + idx] = 0;
    mm_vars["mv2_p" + idx] = 0;

    // 上升沿/下降沿初值
    mm_vars["up_tag" + idx] = (p_val == 0 && tag_val == 1) ? 1.0 : 0.0;
    mm_vars["dw_tag" + idx] = (p_val == 1 && tag_val == 0) ? 1.0 : 0.0;

    // 若当前已为 1，初始计数为 1
    if (tag_val == 1) {
      mm_vars["mv2_tag" + idx] = 1;
    }
    if (p_val == 1) {
      mm_vars["mv2_p" + idx] = 1;
    }
  }
}

void FbStateMachine::updateActionVars(
    TimePoint now,
    std::map<std::string, double>& mm_vars,
    size_t tag_count) {

  mm_vars["time"] = mm_vars["now"] - mm_vars["stime"];

  for (size_t i = 0; i < tag_count; i++) {
    std::string idx = std::to_string(i + 1);
    double tag_val = mm_vars["tag" + idx];
    double p_val = mm_vars["p" + idx];

    // 更新极值
    mm_vars["mx_tag" + idx] = std::max(tag_val, mm_vars["mx_tag" + idx]);
    mm_vars["mi_tag" + idx] = std::min(tag_val, mm_vars["mi_tag" + idx]);

    // 累积 1 的计数
    if (tag_val == 1) {
      mm_vars["mv2_tag" + idx] += 1;
    }
    if (p_val == 1) {
      mm_vars["mv2_p" + idx] += 1;
    }

    // 上升沿/下降沿计数
    mm_vars["up_tag" + idx] += (p_val == 0 && tag_val == 1) ? 1.0 : 0.0;
    mm_vars["dw_tag" + idx] += (p_val == 1 && tag_val == 0) ? 1.0 : 0.0;
  }
}

void FbStateMachine::recordActionEnd(
    TimePoint now,
    std::map<std::string, double>& mm_vars) {

  mm_vars["etime"] = mm_vars["now"];
}

void FbStateMachine::clearActionAccumulators(
    std::map<std::string, double>& mm_vars,
    size_t tag_count) {

  for (size_t i = 0; i < tag_count; i++) {
    std::string idx = std::to_string(i + 1);
    mm_vars["mv2_tag" + idx] = 0;
    mm_vars["mv2_p" + idx] = 0;
    mm_vars["up_tag" + idx] = 0;
    mm_vars["dw_tag" + idx] = 0;
  }
}

bool FbStateMachine::isAccumulatorNearOverflow(
    const std::map<std::string, double>& mm_vars,
    size_t tag_count) const {

  for (size_t i = 0; i < tag_count; i++) {
    std::string key = "mv2_tag" + std::to_string(i + 1);
    auto it = mm_vars.find(key);
    if (it != mm_vars.end()) {
      if (std::abs(it->second - DBL_MAX) < 2.0) {
        return true;
      }
    }
  }
  return false;
}