/**
 * ROS2上位机和实时循环的通信桥梁
 *
 * 模块功能：
 *  - 读取实时循环状态 g_state
 *  - 给实时循环发送指令 g_cmd 
 *
 * 线程模型与并发：
 *  - 多生产者（ROS2 回调/控制线程） --> 写 g_cmd / 读 g_state；
 *  - 单消费者（1kHz 实时循环）      --> 读 g_cmd / 写 g_state。
 *  - 全部原子变量使用 memory_order_relaxed，低延迟、低开销；
 *
 * 数据与单位约定（与从站字典保持一致）：
 *  - 位置：编码器计数（counts）
 *  - 速度：counts/s
 *  - 扭矩：
 *  - 模式：DS402 0x6060（CSP=8, CSV=9, CST=10, PVT=5）
 *  - PVT 增益：这里以 double 表示的无量纲系数
 *
 */
#include "ethercat_control/motor_control.hpp"

MotorCommand g_cmd;   //全局命令缓冲
MotorState   g_state; //全局状态缓冲

static inline int clamp_index(int idx) { return std::clamp(idx, 0, NUM_SLAVES - 1); }

// 初始化所有轴的缺省命令与状态
void mc_init_defaults(OpMode default_mode) {
  for (int i = 0; i < NUM_SLAVES; ++i) {
    g_cmd.target_vel[i].store(0,          std::memory_order_relaxed);
    g_cmd.target_pos[i].store(0,          std::memory_order_relaxed);
    g_cmd.target_torque[i].store(0,       std::memory_order_relaxed);
    g_cmd.mode[i].store(static_cast<int8_t>(default_mode), std::memory_order_relaxed);
    g_cmd.run_enable[i].store(false,      std::memory_order_relaxed); 
    g_cmd.fault_reset[i].store(false,     std::memory_order_relaxed);
    g_cmd.quick_stop[i].store(false,      std::memory_order_relaxed);

    g_state.status_word[i].store(0,       std::memory_order_relaxed);
    g_state.pos_act[i].store(0,           std::memory_order_relaxed);
    g_state.vel_act[i].store(0,           std::memory_order_relaxed);
    g_state.torque_act[i].store(0,        std::memory_order_relaxed);
    g_state.pos_cmd[i].store(0,           std::memory_order_relaxed);
    // 新增：PVT 缺省增益（可按需改）
    g_cmd.pvt_kp[i].store(0.0,            std::memory_order_relaxed);
    g_cmd.pvt_kd[i].store(0.0,            std::memory_order_relaxed);

  }
}
//设置工作模式（轴号，操作模式）
void mc_set_mode(int idx, OpMode mode) {
  if (!mc_valid_index(idx)) return;
  g_cmd.mode[idx].store(static_cast<int8_t>(mode), std::memory_order_relaxed);
}
//设置速度（轴号，速度）
void mc_set_target_velocity(int idx, int32_t vel) {
  if (!mc_valid_index(idx)) return;
  g_cmd.target_vel[idx].store(vel, std::memory_order_relaxed);
}
//设置位置（轴号，位置）
void mc_set_target_position(int idx, int32_t pos) {
  if (!mc_valid_index(idx)) return;
  g_cmd.target_pos[idx].store(pos, std::memory_order_relaxed);
}
//设置扭矩（轴号，扭矩）
void mc_set_target_torque(int idx, int16_t tq) {
  if (!mc_valid_index(idx)) return;
  g_cmd.target_torque[idx].store(tq, std::memory_order_relaxed);
}
// 设置所有电机PVT kp kd
void mc_set_all_pvt_gains(double kp, double kd) {
  for (int i = 0; i < NUM_SLAVES; ++i) {
    g_cmd.pvt_kp[i].store(kp, std::memory_order_relaxed);
    g_cmd.pvt_kd[i].store(kd, std::memory_order_relaxed);
  }
}
// 设置PVT kp kd （轴号，kp，kd）
void mc_set_pvt_gains(int axis, double kp, double kd) {
  if (axis < 0 || axis >= NUM_SLAVES) return;
  g_cmd.pvt_kp[axis].store(kp, std::memory_order_relaxed);
  g_cmd.pvt_kd[axis].store(kd, std::memory_order_relaxed);
}
//使能/禁止运行（轴号）
void mc_enable_run(int idx, bool enable) {
  if (!mc_valid_index(idx)) return;
  g_cmd.run_enable[idx].store(enable, std::memory_order_relaxed);
}
//设置所有电机运行模式（模式）
void mc_set_all_mode(OpMode mode) {
  for (int i = 0; i < NUM_SLAVES; ++i) mc_set_mode(i, mode);
}
//设置所有电机速度（速度）
void mc_set_all_target_velocity(int32_t vel) {
  for (int i = 0; i < NUM_SLAVES; ++i) mc_set_target_velocity(i, vel);
}
//批量使能/禁止运行
void mc_enable_all(bool enable) {
  for (int i = 0; i < NUM_SLAVES; ++i) mc_enable_run(i, enable);
}
//故障复位，待开发
void mc_fault_reset(int idx) {
  if (!mc_valid_index(idx)) return;
  g_cmd.fault_reset[idx].store(true, std::memory_order_relaxed);
}
//急停（轴号）
void mc_quick_stop(int idx) {
  if (!mc_valid_index(idx)) return;
  g_cmd.quick_stop[idx].store(true, std::memory_order_relaxed);
}



//读取轴状态（轴号，状态字，位置，速度，扭矩）
void mc_get_state(int idx, uint16_t &status, int32_t &pos, int32_t &vel, int16_t &torque) {
  idx = clamp_index(idx);

  // This should be done in HAL
  // slave index transport into joint index.
  // switch (idx)
  //   {
  //   case 0:
  //     idx = 2;
  //     break;
  //   case 1:
  //     idx = 3;
  //     break;
  //   case 2:
  //     idx = 0;
  //     break;
  //   case 3:
  //     idx = 1;
  //     break;
  //   case 4:
  //     idx = 5;
  //     break;
  //   case 5:
  //     idx = 4;
  //     break;
  //   default:
  //     break;
  //   }

  status = g_state.status_word[idx].load(std::memory_order_relaxed);
  pos    = g_state.pos_act[idx].load(std::memory_order_relaxed);
  vel    = g_state.vel_act[idx].load(std::memory_order_relaxed);
  torque = g_state.torque_act[idx].load(std::memory_order_relaxed);
}
