/**
 * @file param_example.cpp
 * @brief RL工程参数读取/修改对应sdk示例
 *
 * @copyright Copyright (C) 2024 ROKAE (Beijing) Technology Co., LTD. All Rights Reserved.
 * Information in this file is the intellectual property of Rokae Technology Co., Ltd,
 * And may contains trade secrets that must be stored and viewed confidentially.
 */

#include "rokae/robot.h"
#include "print_helper.hpp"
#include "rokae/data_types.h"
#include <unordered_map>
#include <thread>
#include <iostream>
#include <chrono>
#include <future>

using namespace rokae;
using namespace std;

#define ROKAE_EXE(CMD) \
CMD; \
if(ec) { print(std::cerr, __LINE__, __FUNCTION__, ":", ec.message()); }

namespace {
 xMateRobot robot;
}

// ---------- 辅助 traits: 判断是否是 std::vector ----------
template <typename T, template<typename...> class Template>
struct is_specialization : std::false_type {};

template <template<typename...> class Template, typename... Args>
struct is_specialization<Template<Args...>, Template> : std::true_type {};

/**
 * 获取变量统一函数
**/
template<typename T>
T getProjectVariable_T(const std::string &name) {
  error_code _ec;
  auto var = robot.getProjectVariable(name, _ec);
  if (_ec) {
    print(std::cerr, name, "Read error: ", _ec);
    if constexpr (std::is_default_constructible_v<T>)
      return T{};
    else
      return {}; // 对于非默认构造的类型，可考虑抛异常或特殊处理
  }

  try {
    // 尝试直接取
    return std::get<T>(var);
  } catch (const std::bad_variant_access&) {
    // 类型不匹配，进入特殊处理
  }

  // ====== 标量 <-> 向量 自动转换 ======
  if constexpr (is_specialization<T, std::vector>::value) {
    // 模板参数是 vector<U>
    using U = typename T::value_type;

    if (std::holds_alternative<U>(var)) {
      // 实际是单个元素 U，打包成 vector
      return T{ std::get<U>(var) };
    }
  } else {
    // 模板参数是标量 U
    using U = T;
    if (std::holds_alternative<std::vector<U>>(var)) {
      auto &vec = std::get<std::vector<U>>(var);
      if (!vec.empty()) {
        return vec.front();  // 返回第一个
      } else {
        return U{};  // 空数组时返回默认值
      }
    }
  }

  // 如果仍然不匹配
  print(std::cerr, name, "Read error: type mismatch");
  if constexpr (std::is_default_constructible_v<T>)
    return T{};
  else
    return {};
}

/**
 * 修改笛卡尔点位
**/
bool modifyCartPoint(BaseRobot &robot) {
  error_code ec;
  auto name = "point3";
  bool flag = robot.modifyCartPointByName(name,
                                          name,
                                          {0.644, 0.385, 0.103},
                                          {0, 0, 0},
                                          0,
                                          {2, -1, 4, 7, 1, -1, 5, 0},
                                          {0.2, 10, 0.9, 1.0, 2.0, 0},
                                          ec);
  return flag;
}

/**
 * 修改关节点位
**/
bool modifyJointPoint(BaseRobot &robot) {
  JointPosition joint;
  joint.joints = {-7.8, -21.7, 36.1, 6.1, 91.3, 0, 0};// 轴角
  joint.external = {1, 2, 3, 4, 0, 0};                // 外部轴
  error_code ec;
  auto newName = "test_joint";
  bool flag = robot.modifyJointPointByName(newName, newName, joint.joints, joint.external, ec);
  return flag;
}

/**
 * 修改变量
 **/
bool modifyVarInfo(BaseRobot &robot) {
  error_code ec;
  auto oldName = "string0";
  auto newName = "string1";
  bool flag = robot.modifyVariableByName(newName, "string", "hello", ec);
  return flag;
}

/**
 * 获取变量示例
**/
void getParams() {
  std::cout << "----- Getting parameters -----" << std::endl;
  error_code ec = {};
  // 变量名称，需要提前创建好
  auto cart_name = "test_point_arr";
  auto joint_name = "test_joint";
  auto var_name = "double0";
  auto string_name = "string0";
  auto int_var_name = "int_arr";
  auto bool_var_name = "bool_arr";

  // 读笛卡尔点位数组
  auto cart_array_var = getProjectVariable_T<std::vector<CartesianPosition>>(cart_name);
  int idx = 0;
  print(std::cout, "Cartesian points ", cart_name);
  for (auto &cart_point : cart_array_var) {
    std::cout << "[" << idx++ << "]\n";
    print(std::cout, cart_point);
  }

  // 单个关节点位
  auto joint_point_var = getProjectVariable_T<JointPosition>(joint_name);
  print(std::cout, "Joint point ", joint_name, ": ", joint_point_var.joints);
  // 单个浮点数
  auto double_var = getProjectVariable_T<double>(var_name);
  print(std::cout, "Variable ", var_name, ":", double_var);
  // 单个字符串
  auto string_var = getProjectVariable_T<std::string>(string_name);
  print(std::cout, "Variable ", string_name, ":", string_var);
  // 整型数组
  auto int_var = getProjectVariable_T<std::vector<int>>(int_var_name);
  print(std::cout, "Variable ", int_var_name, ":", int_var);
  // 布尔数组
  auto bool_var = getProjectVariable_T<std::vector<bool>>(bool_var_name);
  print(std::cout, "Variable ", bool_var_name, ":", bool_var);
}

/**
 * @brief 示例 - 修改变量
 */
void editParams(){

  modifyJointPoint(robot);
  modifyCartPoint(robot);
  modifyVarInfo(robot);
}

/**
 * @brief 示例 - 测验点位接口。
 */
void paramsExample() {
  getParams();
  editParams();
}

int main() {
  using namespace rokae;
  try {
    robot.connectToRobot("10.0.2.43");
  } catch (const std::exception& e) {
    std::cerr << e.what() << std::endl;
    return -1;
  }
  paramsExample();
  return 0;
}
