#include "StateMachine.h"
#include <Arduino.h>

void StateMachine::update(bool armed, bool stand_request, bool walk_active, float dt) {
  if (!armed) {
    state_ = RobotState::DISARMED;
    blend_ = 0;
    return;
  }
  if (state_ == RobotState::DISARMED) {
    state_ = RobotState::SIT;   // arming always lands in sit first, never straight to stand
  }

  float target_blend = stand_request ? 1.0f : 0.0f;
  float step = BLEND_RATE * dt;
  if (blend_ < target_blend)      blend_ = min(blend_ + step, target_blend);
  else if (blend_ > target_blend) blend_ = max(blend_ - step, target_blend);

  if (blend_ <= 0.001f)      state_ = RobotState::SIT;
  else if (blend_ >= 0.999f) state_ = walk_active ? RobotState::WALKING : RobotState::STAND;
  else                        state_ = RobotState::STAND;   // mid-transition
}