#pragma once
#include <cstdint>
#include <vector>
#include <string>

/* 舵机编号枚举 */
enum class EServo : uint8_t {
    SERVO_0 = 0,
    SERVO_1,
    SERVO_2,
};

/* 电机运行模式 */
enum class EMotorOperatingMode : uint8_t {
    Position,   // 位置模式
    Velocity,   // 速度模式
    Current,    // 电流 / 力矩模式
};

/* 电机报警类型（可以根据 Dynamixel 详细 bitmask 扩展） */
enum class EMotorAlarm : uint16_t {
    InputVoltageError   = 0x01,
    OverTemperature     = 0x02,
    MotorEncoderError   = 0x04,
    Overload            = 0x08,
    DriverFault         = 0x10,
    // 更多可按需要添加
};

/* 电机运动命令结构体 */
struct ServoCommand {
    EServo servo;                   // 哪个电机
    EMotorOperatingMode mode;       // 运行模式
    int32_t target;                 // 目标值
    uint32_t velocity_limit = 0;    // 最大速度（0 = 不修改）
    uint32_t acceleration   = 0;    // 加速度（0 = 不修改）
    uint16_t current_limit  = 0;    // 电流限制（0 = 不修改）
};

/* 电机控制接口 */
class ServoControl
{
public:
    ServoControl();
    ~ServoControl();

    void servoInit(EServo servo);
    bool servoExecute(const ServoCommand& cmd);

    int32_t getMotorPosition(EServo servo);
    int32_t getMotorVelocity(EServo servo);
    int32_t getMotorCurrent(EServo servo);

    /* ======================
       新增报警查询接口
       返回 bitmask，0 = 无报警
    ====================== */
    uint16_t getMotorAlarmStatus(EServo servo);
    bool hasMotorAlarm(EServo servo, EMotorAlarm alarm);

private:
    bool enableTorque(EServo servo);
    bool disableTorque(EServo servo);
    bool setOperatingMode(EServo servo, EMotorOperatingMode mode);
    bool setVelocityLimit(EServo servo, uint32_t vel);
    bool setAcceleration(EServo servo, uint32_t acc);
    bool setCurrentLimit(EServo servo, uint16_t cur);

    struct ServoState {
        EMotorOperatingMode mode;
        int32_t position;
        int32_t velocity;
        int32_t current;
        bool torque_on;
        uint16_t alarm_status;   // 新增：报警状态
    };

    ServoState servo_states_[3];
};