#pragma once
#include <array>
#include <atomic>
#include <cstdint>
#include <algorithm>

constexpr int NUM_SLAVES = 2; //定义电机数量

//运行模式
enum class OpMode : int8_t {
  CSP = 8,    // Cyclic Synchronous Position
  CSV = 9,    // Cyclic Synchronous Velocity
  CST = 10,   // Cyclic Synchronous Torque
  PVT = 5     // PVT模式
};

// 周期线程与上层通讯的共享结构
// 电机命令
struct MotorCommand {
  std::array<std::atomic<int8_t>,  NUM_SLAVES> mode;             // 运行模式
  std::array<std::atomic<int32_t>, NUM_SLAVES> target_vel;       // 目标速度	
  std::array<std::atomic<int32_t>, NUM_SLAVES> target_pos;       // 目标位置
  std::array<std::atomic<int16_t>, NUM_SLAVES> target_torque;    // 目标扭矩
  std::atomic<double>  pvt_kp[NUM_SLAVES];                       // PVT kp增益
  std::atomic<double>  pvt_kd[NUM_SLAVES];                       // PVT kd增益

  // 运行控制意图（周期线程据此决定是否写 0x001F）
  std::array<std::atomic<bool>,    NUM_SLAVES> run_enable;

  // 故障复位/急停意图（待后续添加）
  std::array<std::atomic<bool>,    NUM_SLAVES> fault_reset;
  std::array<std::atomic<bool>,    NUM_SLAVES> quick_stop;
};
// 电机状态
struct MotorState {
  std::array<std::atomic<uint16_t>,NUM_SLAVES> status_word;      // 从站状态字
  std::array<std::atomic<int32_t>, NUM_SLAVES> pos_act;          // 实际位置
  std::array<std::atomic<int32_t>, NUM_SLAVES> vel_act;          // 实际速度
  std::array<std::atomic<int16_t>, NUM_SLAVES> torque_act;       // 实际扭矩
  std::array<std::atomic<int32_t>, NUM_SLAVES> pos_cmd;          // 每个周期CSP的目标位置（因速度限制，由target_pos拆分而来）
};

// 全局共享实例（由 .cpp 定义）
extern MotorCommand g_cmd;
extern MotorState   g_state;

// ---------- 控制接口（供上层/ROS2 调用） ----------

// 初始化缺省值（上电后调用一次）
void mc_init_defaults(OpMode default_mode = OpMode::CSV);

// 设定单轴模式/目标值/运行意图
void mc_set_mode(int idx, OpMode mode);
void mc_set_target_velocity(int idx, int32_t vel);
void mc_set_target_position(int idx, int32_t pos);
void mc_set_target_torque(int idx, int16_t tq);
void mc_enable_run(int idx, bool enable);

// 批量设置电机指令
void mc_set_all_mode(OpMode mode);
void mc_set_all_target_velocity(int32_t vel);
void mc_enable_all(bool enable);

// 设置PVT参数
void mc_set_all_pvt_gains(double kp, double kd);
void mc_set_pvt_gains(int axis, double kp, double kd);

// 故障/急停意图（待后续添加）
void mc_fault_reset(int idx);
void mc_quick_stop(int idx);

// 状态读取（从共享状态快照中取，非直接读 PDO）
void mc_get_state(int idx, uint16_t &status, int32_t &pos, int32_t &vel, int16_t &torque);

// 检查电机轴号是否合法
inline bool mc_valid_index(int idx) { return idx >= 0 && idx < NUM_SLAVES; }

// void mc_get_pos_cmd_and_act(int idx, int32_t &pos_cmd, int32_t &pos_act);
// void mc_print_pos_cmd_and_act(int idx);              // 打印单轴
// void mc_print_all_pos_cmd_and_act();                 // 打印全部

