#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <thread>
#include <pthread.h>
#include <sched.h>
#include <optional>          // ✅ 新增
#include "motor_control.hpp"

// 由你现有源码提供：
void cyclic_task();        // 实时循环（已接入 g_cmd / g_state）
void ethercat_after_init();// 可选：若你想在这里放 mc_init_defaults 等

// ---------- 模式解析（安全版）：失败返回 nullopt，不做任何电机操作 ----------
static std::optional<OpMode> parse_mode(const std::string &s){
  if (s=="csv" || s=="CSV" || s=="vel" || s=="VEL" || s=="speed" || s=="SPEED")
    return OpMode::CSV; // 9
  if (s=="csp" || s=="CSP" || s=="pos" || s=="POS" || s=="position" || s=="POSITION")
    return OpMode::CSP; // 8
  if (s=="cst" || s=="CST" || s=="tor" || s=="TOR" || s=="torque" || s=="TORQUE")
    return OpMode::CST; // 10
  return std::nullopt;  // ❗未知输入 -> 调用者处理，绝不默认 CSV
}

void start_rt_cyclic_thread(){
  std::thread([]{
    // 把实时优先级设置到线程上（不占用主线程）
    sched_param param{};
    param.sched_priority = sched_get_priority_max(SCHED_FIFO);
    pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);
    cyclic_task(); // 阻塞
  }).detach();
}

static void print_help(){
  std::cout <<
R"(命令格式：
  help                      显示帮助
  quit / exit               退出程序
  enable on|off [i|-1]      使能/禁止运行；省略或 -1 表示所有电机
  state [i]                 打印第 i 台状态（省略打印 0..19 摘要）
  set <i> csp pos <val>; <j> csv vel <val>; <k> cst tor <val>   同时设置多个轴
  （别名：csv|vel|speed，csp|pos|position，cst|tor|torque）
)"
  << std::endl;
}

// static void print_help(){
//   std::cout <<
// R"(命令格式：
//   help                      显示帮助
//   quit / exit               退出程序
//   enable on|off [i|-1]      使能/禁止运行；省略或 -1 表示所有电机
//   mode i csv|csp|cst        设置第 i 台的模式（速度/位置/扭矩）
//   vel  i value              设置第 i 台目标速度（int32）
//   pos  i value              设置第 i 台目标位置（int32）
//   tor  i value              设置第 i 台目标扭矩（int16）
//   all vel value             所有电机目标速度
//   state [i]                 打印第 i 台状态（省略打印 0..19 摘要）
//   set <i> csp pos <val>; <j> csv vel <val>; <k> cst tor <val>   同时设置多个轴
//   （别名：csv|vel|speed，csp|pos|position，cst|tor|torque）
// )"
//   << std::endl;
// }
void cli_command_loop(){
  print_help();
  std::string line;
  while (std::cout << "> " && std::getline(std::cin, line)){
    std::istringstream iss(line);
    std::string cmd; iss >> cmd;
    if (cmd.empty()) continue;

    if (cmd=="help") { print_help(); continue; }
    if (cmd=="quit" || cmd=="exit") { break; }

    if (cmd=="enable"){
      std::string onoff; int idx = -1;
      iss >> onoff >> idx;
      bool en = (onoff=="on" || onoff=="1" || onoff=="true");
      if (!iss || idx==-1){
        mc_enable_all(en);
        std::cout << "[ALL] enable=" << en << "\n";
      }else{
        if (mc_valid_index(idx)){ mc_enable_run(idx,en); std::cout << "[S"<<idx<<"] enable="<<en<<"\n";}
        else std::cout << "无效索引\n";
      }
      continue;
    }

    if (cmd=="mode"){
      int idx; std::string m; iss >> idx >> m;
      if (iss && mc_valid_index(idx)){
        auto pm = parse_mode(m);
        if (!pm){
          std::cout << "无效模式: " << m << "（允许: csv/csp/cst 或 vel/pos/tor）\n";
          continue; // ❗不做任何电机操作
        }
        mc_set_mode(idx, *pm);
        std::cout << "[S"<<idx<<"] mode="<<m<<"\n";
      }else {
        std::cout << "用法: mode i csv|csp|cst\n";
      }
      continue;
    }

    if (cmd=="vel"){
      int idx; long v; iss >> idx >> v;
      if (iss && mc_valid_index(idx)){
        mc_set_target_velocity(idx, (int32_t)v);
        std::cout << "[S"<<idx<<"] vel="<<v<<"\n";
      }else std::cout << "用法: vel i value\n";
      continue;
    }

    if (cmd=="pos"){
      int idx; long p; iss >> idx >> p;
      if (iss && mc_valid_index(idx)){
        mc_set_target_position(idx, (int32_t)p);
        std::cout << "[S"<<idx<<"] pos="<<p<<"\n";
      }else std::cout << "用法: pos i value\n";
      continue;
    }

    if (cmd=="tor"){
      int idx; long t; iss >> idx >> t;
      if (iss && mc_valid_index(idx)){
        mc_set_target_torque(idx, (int16_t)t);
        std::cout << "[S"<<idx<<"] tor="<<t<<"\n";
      }else std::cout << "用法: tor i value\n";
      continue;
    }

    if (cmd=="all"){
      std::string what; long v;
      iss >> what >> v;
      if (what=="vel" && iss){
        mc_set_all_target_velocity((int32_t)v);
        std::cout << "[ALL] vel="<<v<<"\n";
      }else{
        std::cout << "用法: all vel value\n";
      }
      continue;
    }

    if (cmd == "set") { // 批量控制函数（先校验，后执行）
      // 把这一行剩余内容整体取出来，然后按 ';' 切成多段
      std::string rest;
      std::getline(iss, rest);
      if (rest.empty()) {
          std::cout << "用法: set <i> csp pos <val>; <j> csv vel <val>; <k> cst tor <val>\n";
          continue;
      }

      // 去掉可能的开头空格/结尾空格
      auto ltrim = [](std::string& s){
          size_t p = s.find_first_not_of(" \t");
          if (p != std::string::npos) s.erase(0, p);
      };
      auto rtrim = [](std::string& s){
          size_t p = s.find_last_not_of(" \t");
          if (p != std::string::npos) s.erase(p+1);
      };

      // 按 ';' 分割子命令
      std::vector<std::string> chunks;
      {
          std::stringstream ss(rest);
          std::string piece;
          while (std::getline(ss, piece, ';')) {
              ltrim(piece); rtrim(piece);
              if (!piece.empty()) chunks.push_back(piece);
          }
      }
      if (chunks.empty()) {
          std::cout << "用法: set <i> csp pos <val>; <j> csv vel <val>; <k> cst tor <val>\n";
          continue;
      }

      // 逐个子命令解析与执行：⚠️ 先校验，再动作；出错则跳过该子命令
      for (auto& one : chunks) {
          std::istringstream si(one);
          int idx = -1;
          std::string mode_s, field;
          long long val = 0;

          si >> idx >> mode_s >> field >> val;
          if (!si || !mc_valid_index(idx)) {
              std::cout << "[set] 子命令格式错误或轴号越界: \"" << one << "\"\n";
              continue; // ❗不做任何电机操作
          }

          // 解析模式
          auto pm = parse_mode(mode_s);
          if (!pm) {
              std::cout << "[set] 未知模式: " << mode_s << " in \"" << one << "\"\n";
              continue; // ❗不做任何电机操作
          }
          OpMode mode = *pm;

          // 校验字段是否与模式匹配
          bool ok_field = false;
          if (mode == OpMode::CSP) {
              ok_field = (field=="pos" || field=="position" || field=="POS" || field=="POSITION");
              if (!ok_field) {
                  std::cout << "[set] CSP 需要字段: pos <val> ；Got \"" << field << "\"\n";
                  continue; // ❗不做任何电机操作
              }
          } else if (mode == OpMode::CSV) {
              ok_field = (field=="vel" || field=="velocity" || field=="speed" ||
                          field=="VEL" || field=="VELOCITY" || field=="SPEED");
              if (!ok_field) {
                  std::cout << "[set] CSV 需要字段: vel <val> ；Got \"" << field << "\"\n";
                  continue; // ❗不做任何电机操作
              }
          } else if (mode == OpMode::CST) {
              ok_field = (field=="tor" || field=="torque" || field=="TOR" || field=="TORQUE");
              if (!ok_field) {
                  std::cout << "[set] CST 需要字段: tor <val> ；Got \"" << field << "\"\n";
                  continue; // ❗不做任何电机操作
              }
          }

          // —— 所有校验通过，才真正执行 —— //
          mc_set_mode(idx, mode);
          if (mode == OpMode::CSP) {
              mc_set_target_position(idx, static_cast<int32_t>(val));
              std::cout << "[S" << idx << "] mode=CSP pos=" << val << "\n";
          } else if (mode == OpMode::CSV) {
              mc_set_target_velocity(idx, static_cast<int32_t>(val));
              std::cout << "[S" << idx << "] mode=CSV vel=" << val << "\n";
          } else if (mode == OpMode::CST) {
              mc_set_target_torque(idx, static_cast<int16_t>(val));
              std::cout << "[S" << idx << "] mode=CST tor=" << val << "\n";
          }
          // 是否自动 enable 不在此处改变，保持你当前逻辑；如需可追加：
          // mc_enable_run(idx, true);
      }
      continue;
    }

    if (cmd=="state"){
      int idx=-1; iss >> idx;
      if (!iss){ // 打印摘要
        for (int i=0;i<NUM_SLAVES;++i){
          uint16_t sw; int32_t pos,vel; int16_t tq;
          mc_get_state(i, sw, pos, vel, tq);
          std::cout << "[S"<<i<<"] SW=0x"<<std::hex<<sw<<std::dec
                    <<" pos="<<pos<<" vel="<<vel<<" tq="<<tq<<"\n";
        }
      }else if (mc_valid_index(idx)){
        uint16_t sw; int32_t pos,vel; int16_t tq;
        mc_get_state(idx, sw, pos, vel, tq);
        std::cout << "[S"<<idx<<"] SW=0x"<<std::hex<<sw<<std::dec
                  <<" pos="<<pos<<" vel="<<vel<<" tq="<<tq<<"\n";
      }else{
        std::cout << "索引无效\n";
      }
      continue;
    }

    std::cout << "未知命令，输入 help 查看帮助\n";
  }
  std::cout << "退出命令行控制。\n";
}
