/**
 * ROS2 节点：与 EtherCAT 周期线程共享指令/状态，提供文本话题指令解析、
 * JointState 发布、CSP 指令回显与 CSV 记录、超时看门狗等。
 *
 * 主要功能：
 *  - 启动底层 EtherCAT 周期线程（见 ethercat_core::start）
 *  - 订阅字符串命令话题，解析并写入 MotorCommand（g_cmd）
 *  - 周期发布：
 *      * joint_states（实际位置/速度/扭矩）
 *      * joint_states_cmd（CSP 目标位置的回显）
 *      * csp_table（时间戳+每轴的 csp_cmd/real/err）
 *  - 将表格同时写入 CSV 文件
 *  - 看门狗：一段时间未收到上层命令则将速度/扭矩清零
 *
 * 使用示例：
 *  - 切到 CSP 并给 0 号轴位置：
 *      ros2 topic pub /ethercat/set std_msgs/msg/String "data: '0 pos 10000'"
 *  - 切到 CSV 并给 0 号轴速度：
 *      ros2 topic pub /ethercat/set std_msgs/msg/String "data: '0 vel 2000'"
 *  - PVT 模式设置与给值（多条用分号隔开）：
 *      ros2 topic pub --rate 10 /ethercat/set std_msgs/msg/String \
 *        "data: '0 pvt; 0 kp 5; 0 kd 5; 0 pvtpos 10000; 0 pvtvel 2000; 0 pvttor 0'"
 */
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "sensor_msgs/msg/joint_state.hpp"
#include "ethercat_control/ethercat_core.hpp"
#include "ethercat_control/motor_control.hpp"
#include <sstream>
#include <cmath>
#include <fstream>
#include <iomanip>
#include <filesystem>

using std::placeholders::_1;

class EthercatNode : public rclcpp::Node {
public:
  /**
   * 构造函数：初始化 EtherCAT、参数、话题与定时器
   *
   * 过程：
   *  1) 调用 ethercat_core::start() 启动底层主站与 1kHz 周期线程
   *  2) 读取参数 num_joints/csv_path
   *  3) 创建订阅（/ethercat/set）与发布者（joint_states / joint_states_cmd / csp_table）
   *  4) 创建 100Hz 的状态发布定时器
   *  5) 打开 CSV 并写表头
   *  6) 启动看门狗定时器（100ms 检查一次）
   *
   */
  EthercatNode() : Node("ethercat_node") {
    // 启动 EtherCAT 周期线程
    if (!ethercat_core::start()) {
      RCLCPP_FATAL(this->get_logger(), "EtherCAT core start failed.");
      throw std::runtime_error("ethercat start failed");
    }

    // 关节名（给 JointState 用）
    declare_parameter<int>("num_joints", NUM_SLAVES);
    get_parameter("num_joints", num_joints_);
    joint_names_.resize(num_joints_);
    for (int i = 0; i < num_joints_; ++i) {
      joint_names_[i] = "joint_" + std::to_string(i);
    }

    // CSV 文件路径参数，默认存到工作目录下
    declare_parameter<std::string>("csv_path", "csp_log.csv");
    get_parameter("csv_path", csv_path_);

    // 订阅 set 文本命令：例如 "set 0 vel 1000; 1 pos 3000"
    set_sub_ = create_subscription<std_msgs::msg::String>(
        "ethercat/set", 10, std::bind(&EthercatNode::onSet, this, _1));

    // 发布 JointState（100Hz）
    js_pub_ = create_publisher<sensor_msgs::msg::JointState>("ec_joint_states", 10);
    js_cmd_pub_ = create_publisher<sensor_msgs::msg::JointState>("ec_joint_states_cmd", 10);

    // 表格话题，把“轴i csp指令值/真实位置/误差”拼成一行
    csp_table_pub_ = create_publisher<std_msgs::msg::String>("csp_table", 10);

    // 定时器:每 100ms 发布一次状态
    js_timer_ = create_wall_timer(
        std::chrono::milliseconds(100),
        std::bind(&EthercatNode::publishJointState, this));

    // 打开 CSV 并写表头
    open_csv_and_write_header();
    start_time_steady_ = steady_clock_.now();

    RCLCPP_INFO(get_logger(), "ethercat_node ready.");

    // 看门狗，每100ms检测指令是否收到
    last_command_time_ = this->get_clock()->now();
    watchdog_timer_ = this->create_wall_timer(
        std::chrono::milliseconds(100),
        std::bind(&EthercatNode::watchdogCallback, this));
  }

  // 析构时收尾
  ~EthercatNode() override {
    if (csv_.is_open()) {
      csv_.flush();
      csv_.close();
    }
  }

private:
  void onSet(const std_msgs::msg::String::SharedPtr msg) {
  last_command_time_ = this->get_clock()->now();  //刷新看门狗时间
  // 对读到的控制指令进行处理
  // 简易去空白工具
  auto ltrim=[](std::string& s){ auto p=s.find_first_not_of(" \t"); if(p!=std::string::npos) s.erase(0,p); };
  auto rtrim=[](std::string& s){ auto p=s.find_last_not_of(" \t");  if(p!=std::string::npos) s.erase(p+1); };

  std::stringstream ss(msg->data);       //整条命令
  std::string chunk;                     //单条命令（整条命令切割而来）

  while (std::getline(ss, chunk, ';')) { //将指令用；分割
    ltrim(chunk); rtrim(chunk);
    if (chunk.empty()) continue;         //空段跳过

    std::istringstream si(chunk);
    int idx = -1;                        // 轴号
    std::string kind;                    // 操作类型
    std::string vstr;                    // 操作值

    si >> idx >> kind;  //第一个token为轴号，第二个token为操作类型
    if (!si || !mc_valid_index(idx)) {
      // RCLCPP_WARN(get_logger(), "[set] bad: '%s'", chunk.c_str());
      continue;
    }
    // 第三个Token为参数值
    if (!(si >> vstr)) vstr.clear();

    // 转换函数 字符串转换为long long和double形式
    auto to_ll = [](const std::string& s)->long long {
      try { size_t pos=0; long long v = std::stoll(s, &pos, 0); (void)pos; return v; }
      catch (...) { return 0LL; }
    };
    auto to_d = [](const std::string& s)->double {
      try { size_t pos=0; double v = std::stod(s, &pos); (void)pos; return v; }
      catch (...) { return 0.0; }
    };

    // ======= PVT 指令解析处理 =======
    if (kind == "pvt") {
      mc_set_mode(idx, OpMode::PVT);
      RCLCPP_INFO(get_logger(), "[S%d] switch to PVT mode", idx);
      continue;
    }
    else if (kind == "kp" || kind == "pvt_kp") {
      if (vstr.empty()) { RCLCPP_WARN(get_logger(), "[S%d] kp needs a value", idx); continue; }
      const double kp = to_d(vstr);
      // 取当前 kd，避免把 kd 覆盖没了
      const double kd_cur = g_cmd.pvt_kd[idx].load(std::memory_order_relaxed);
      mc_set_pvt_gains(idx, kp, kd_cur);
      RCLCPP_INFO(get_logger(), "[S%d] PVT_KP=%.6f (KD=%.6f keep)", idx, kp, kd_cur);
      continue;
    }
    else if (kind == "kd" || kind == "pvt_kd") {
      if (vstr.empty()) { RCLCPP_WARN(get_logger(), "[S%d] kd needs a value", idx); continue; }
      const double kd = to_d(vstr);
      // 取当前 kp，避免把 kp 覆盖没了
      const double kp_cur = g_cmd.pvt_kp[idx].load(std::memory_order_relaxed);
      mc_set_pvt_gains(idx, kp_cur, kd);
      RCLCPP_INFO(get_logger(), "[S%d] PVT_KD=%.6f (KP=%.6f keep)", idx, kd, kp_cur);
      continue;
    }
    // 在 PVT 模式下下发目标（为了简单，提供带前缀的别名）
    else if (kind == "pvtpos" || kind == "pvt_pos") {
      if (vstr.empty()) { RCLCPP_WARN(get_logger(), "[S%d] pvtpos needs a value", idx); continue; }
      mc_set_mode(idx, OpMode::PVT);
      mc_set_target_position(idx, static_cast<int32_t>(to_ll(vstr)));
      RCLCPP_INFO(get_logger(), "[S%d] PVT pos=%s", idx, vstr.c_str());
      continue;
    }
    else if (kind == "pvtvel" || kind == "pvt_vel") {
      if (vstr.empty()) { RCLCPP_WARN(get_logger(), "[S%d] pvtvel needs a value", idx); continue; }
      mc_set_mode(idx, OpMode::PVT);
      mc_set_target_velocity(idx, static_cast<int32_t>(to_ll(vstr)));
      RCLCPP_INFO(get_logger(), "[S%d] PVT vel=%s", idx, vstr.c_str());
      continue;
    }
    else if (kind == "pvttor" || kind == "pvt_tor") {
      if (vstr.empty()) { RCLCPP_WARN(get_logger(), "[S%d] pvttor needs a value", idx); continue; }
      mc_set_mode(idx, OpMode::PVT);
      mc_set_target_torque(idx, static_cast<int16_t>(to_ll(vstr)));
      RCLCPP_INFO(get_logger(), "[S%d] PVT tor=%s", idx, vstr.c_str());
      continue;
    }

    // ======= CSP / CSV / CST 指令解析处理 =======
    if (kind=="pos" || kind=="position") {
      if (vstr.empty()) { 
        RCLCPP_WARN(get_logger(), "[S%d] pos needs a value; ignored", idx);
        continue;
      }

      // 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;
      // }

      mc_set_mode(idx, OpMode::CSP);
      mc_set_target_position(idx, static_cast<int32_t>(to_ll(vstr)));
      // RCLCPP_INFO(get_logger(), "[S%d] CSP pos=%s", idx, vstr.c_str());
    } else if (kind=="vel" || kind=="velocity" || kind=="speed") {
      if (vstr.empty()) { 
        RCLCPP_WARN(get_logger(), "[S%d] vel needs a value; ignored", idx);
        continue;
      }
      mc_set_mode(idx, OpMode::CSV);
      mc_set_target_velocity(idx, static_cast<int32_t>(to_ll(vstr)));
      RCLCPP_INFO(get_logger(), "[S%d] CSV vel=%s", idx, vstr.c_str());
    } else if (kind=="tor" || kind=="torque") {
      if (vstr.empty()) { 
        RCLCPP_WARN(get_logger(), "[S%d] tor needs a value; ignored", idx);
        continue;
      }
      mc_set_mode(idx, OpMode::CST);
      mc_set_target_torque(idx, static_cast<int16_t>(to_ll(vstr)));
      RCLCPP_INFO(get_logger(), "[S%d] CST tor=%s", idx, vstr.c_str());
    } else if (kind=="ena" || kind=="enable") {
      if (vstr.empty()) { 
        RCLCPP_WARN(get_logger(), "[S%d] tor needs a value; ignored", idx);
        continue;
      }
      mc_enable_run(idx, to_ll(vstr));
      RCLCPP_INFO(get_logger(), "[S%d] enable=%s", idx, vstr.c_str());
    }else {
      RCLCPP_WARN(get_logger(), "[set] unknown field: '%s'", kind.c_str());
    }
  }
}

  // 计时：以节点启动时为零，使用稳态时钟
  rclcpp::Clock steady_clock_{RCL_STEADY_TIME};     
  rclcpp::Time  start_time_steady_{0, 0, RCL_STEADY_TIME}; 

  // 关节状态发布（实际值和命令值）
  void publishJointState() {
    // 实际值
    sensor_msgs::msg::JointState js;
    js.header.stamp = now();
    js.name = joint_names_;
    js.position.resize(num_joints_);
    js.velocity.resize(num_joints_);
    js.effort.resize(num_joints_);
    for (int i = 0; i < num_joints_; ++i) {
      uint16_t sw; int32_t pos, vel; int16_t tq;
      mc_get_state(i, sw, pos, vel, tq);
      js.position[i] = static_cast<double>(pos);
      js.velocity[i] = static_cast<double>(vel);
      js.effort[i]   = static_cast<double>(tq);
    }
    js_pub_->publish(js);

    // 命令位置（CSP 回显），与实际值用同一时间戳
    sensor_msgs::msg::JointState js_cmd;
    js_cmd.header.stamp = js.header.stamp;
    js_cmd.name = joint_names_;
    js_cmd.position.resize(num_joints_);
    for (int i = 0; i < num_joints_; ++i) {
      const int32_t pc = g_state.pos_cmd[i].load(std::memory_order_relaxed); // 读取csp值
      js_cmd.position[i] = static_cast<double>(pc);
    }
    js_cmd_pub_->publish(js_cmd);

    // 计算相对启动的时间（ms），并量化到 0.1 ms
    const rclcpp::Duration dt = steady_clock_.now() - start_time_steady_;
    double t_ms = dt.seconds() * 1000.0;
    t_ms = std::round(t_ms * 10.0) / 10.0;  // 四舍五入到 0.1ms

    // 生成“表格行”：timestamp,轴1csp指令值,轴1真实位置,误差,轴2csp指令值,轴2真实位置,误差,...
    std::ostringstream row;
    row.setf(std::ios::fixed);
    // 先写时间戳
    row << std::setprecision(1) << t_ms;

    if (num_joints_ > 0) row << ",";       
    for (int i = 0; i < num_joints_; ++i) {
      const long long csp = static_cast<long long>(js_cmd.position[i]);
      const long long act = static_cast<long long>(js.position[i]);
      const long long err = csp - act;
      row << csp << ',' << act << ',' << err;
      if (i != num_joints_ - 1) row << ','; 
    }
    const std::string line = row.str();

    // 发布到 csp_table 话题
    std_msgs::msg::String msg;
    msg.data = line;
    csp_table_pub_->publish(msg);

    // 写入 CSV（首行已写表头）
    if (csv_.is_open()) {
      csv_ << line << '\n';
      if ((++csv_lines_ % 100) == 0) csv_.flush(); // 每100行刷一次盘，减轻开销
    }
  }

  //  打开 CSV 并写表头
  void open_csv_and_write_header() {
    try {
      std::filesystem::path p(csv_path_);
      if (p.has_parent_path()) {
        std::error_code ec;
        std::filesystem::create_directories(p.parent_path(), ec);
        if (ec) {
          RCLCPP_WARN(get_logger(), "Failed to create dir for CSV: %s", ec.message().c_str());
        }
      }
      csv_.open(csv_path_, std::ios::out | std::ios::trunc);
      if (!csv_) {
        RCLCPP_ERROR(get_logger(), "Cannot open CSV file: %s", csv_path_.c_str());
        return;
      }
      // 表头：轴1csp指令值,轴1真实位置,误差,轴2csp指令值,...
      std::ostringstream header;
      header << "time(ms)";
      if (num_joints_ > 0) header << ",";  // 后面还有列再加逗号
      for (int i = 0; i < num_joints_; ++i) {
        header << "axis" << (i+1) << "csp_command"
               << "," << "axis" << (i+1) << "real_position"
               << "," << "error";
        if (i != num_joints_ - 1) header << ",";
      }
      csv_ << header.str() << '\n';
      csv_.flush();

      // 同步把表头也发到 topic
      std_msgs::msg::String head_msg;
      head_msg.data = header.str();
      csp_table_pub_->publish(head_msg);

      RCLCPP_INFO(get_logger(), "Logging CSP table to: %s", csv_path_.c_str());
    } catch (const std::exception& e) {
      RCLCPP_ERROR(get_logger(), "open_csv_and_write_header exception: %s", e.what());
    }
  }

  int num_joints_{0};
  std::vector<std::string> joint_names_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr set_sub_;
  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr js_pub_;
  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr js_cmd_pub_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr csp_table_pub_; // NEW
  rclcpp::TimerBase::SharedPtr js_timer_;
  rclcpp::TimerBase::SharedPtr watchdog_timer_;
  rclcpp::Time last_command_time_;

  // CSV
  std::string csv_path_;
  std::ofstream csv_;
  size_t csv_lines_{0};
  
  //看门狗
  void watchdogCallback() {
    const rclcpp::Time now = this->get_clock()->now();
    const rclcpp::Duration dt = now - last_command_time_;
    if (dt.seconds() > 0.2) {
      //RCLCPP_WARN(get_logger(), "Command timeout! Stopping all motors."); //看门狗超时信息
      for (int i = 0; i < num_joints_; ++i) {
        mc_set_target_velocity(i, 0);
        mc_set_target_torque(i, 0);
      }
      last_command_time_ = now;
    }
  }
};

int main(int argc, char** argv){
  rclcpp::init(argc, argv);
  rclcpp::on_shutdown([]{
    ethercat_core::stop_soft();
    std::this_thread::sleep_for(std::chrono::milliseconds(50));
    ethercat_core::request_exit();
  });
  rclcpp::spin(std::make_shared<EthercatNode>());
  rclcpp::shutdown();
  return 0;
}
