#pragma once

enum class RobotState { DISARMED, SIT, STAND, WALKING };

class StateMachine {
public:
  void begin() { state_ = RobotState::DISARMED; blend_ = 0; }
  void update(bool armed_request, bool stand_request, bool walk_active_request, float dt);

  RobotState state() const { return state_; }
  float sitStandBlend() const { return blend_; }   // 0 = sit pose, 1 = stand pose

private:
  RobotState state_ = RobotState::DISARMED;
  float blend_ = 0;
  static constexpr float BLEND_RATE = 1.0f;   // per second - full sit->stand in ~1s
};