/**
 * @file welding_application_example.cpp
 * @brief 焊接应用示例
 *
 * @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 <thread>
#include <atomic>
#include "rokae/robot.h"
#include "print_helper.hpp"
#include "rokae/welding.h"

using namespace rokae;

/**
 * @brief 等待运动结束 - 通过查询路径ID及路点序号是否已完成的方式
 */
void waitForFinish(BaseRobot &robot, const std::string &traj_id, int index){
  using namespace rokae::EventInfoKey::MoveExecution;
  error_code ec;
  while(true) {
    auto info = robot.queryEventInfo(Event::moveExecution, ec);
    auto _id = std::any_cast<std::string>(info.at(ID));
    auto _index = std::any_cast<int>(info.at(WaypointIndex));
    if(auto _ec = std::any_cast<error_code>(info.at(Error))) {
      //print(std::cout, "Path", _id, ":", _index, "err:", _ec.message());
      return;
    }
    if(_id == traj_id && _index == index) {
      if(std::any_cast<bool>(info.at(ReachTarget))) {
        //print(std::cout, "Path", traj_id, ":", index, "finshed");
      }
      return;
    }
    std::this_thread::sleep_for(std::chrono::milliseconds(200));
  }
}

void printInfo(const rokae::EventInfo &info) {
  using namespace rokae;
  using namespace rokae::EventInfoKey::MoveExecution;
  print(std::cout, "ID:", std::any_cast<std::string>(info.at(ID)), "Index:", std::any_cast<int>(info.at(WaypointIndex)),
        "Reach target: ", std::any_cast<bool>(info.at(ReachTarget)), std::any_cast<error_code>(info.at(Error)));
}

/**
 * @brief 阻塞等待运动指令完成
 */
template<class Command>
void moveBlocked(xMateRobot &robot, const std::vector<Command> &cmds) {
  error_code ec;
  std::string id;

  robot.moveAppend(cmds, id, ec);
  robot.moveStart(ec);
  waitForFinish(robot, id, (int)cmds.size() - 1);
}

/**
 * @brief 示例 - 其它接口
 */
void example_otherInterfaces(xMateRobot &robot) {
  error_code ec;
  auto welding = robot.welding();

  // 计算坐标系偏移
  Frame start_point({0.563, -0.2, 0.4324, M_PI, 0, M_PI}),
    target_point({0.563, 0.0, 0.4324,M_PI, 0, M_PI}), start_offs, target_offs;
  start_offs.trans[0] = 0.003;
  target_offs.trans[1] = 0.008;
  // 偏移方式一: 焊道坐标系Z轴方向同末端工具坐标系Z轴
  welding.calcWeldOffset(start_point, start_offs, target_point, target_offs, ec);

  print(std::cout, "[偏移方式一] 偏移后起始点:", start_point, "\n目标点:", target_point);

  // 偏移方式二: 焊道坐标系Z轴由XY叉乘得到
  // 辅助接口: 计算焊道坐标系
  Frame line_start {{0.553,0.107,0.309, M_PI, 0,-M_PI}},
  line_target {{0.68,0.03543,0.309, M_PI, 0,-M_PI}},
  teach_point_1 {{0.5473,-0.044443,0.309, -2.50422633,-0.02798354,3.12087822082}},
  teach_point_2 {{0.515890,0.0220580,0.309,2.803391761,-0.2970729056392,-2.9592301950}};
  double offset_1 { 0.05 }, offset_2 { 0.04 };
  std::array<double, 3> offset_start {}, offset_target{};
  welding.transformOffsetByTeaching(line_start, line_target, teach_point_1, offset_1, teach_point_2, offset_2, offset_start, offset_target, ec);
  print(std::cout, "坐标系转换结果 - 起点:", offset_start, ", 终点:", offset_target);
  welding.calcWeldOffsetCalculatedZ(start_point, start_offs, target_point, target_offs, ec);
  print(std::cout, "[偏移方式二] 偏移后起始点:", start_point, "\n目标点:", target_point);

  // 调整摆动偏移量 X+0.001m
  // 支持摆动中调整
  welding.adjustTrackOffset({1e-3, 0, 0}, ec);

  // 多段轨迹多层多道偏移计算
  Frame way_point0 {{0.98873878627071521,0.32799716615705909,0.50667443610447238,-2.7495292945202761,-1.8806033765539331e-06,-3.1415917428588926}},
  way_point1 {{0.70435025687185759,0.41419272120934003,0.31575354435938097,-2.5732143763667032,-0.17399606099405901,-3.1415922388504662}},
  way_point2 {{0.86204854682289622,0.57179479424978619,0.35782254260870483,-2.8272425864960207,-0.16851548275263212,-3.0980736037376517}};
  std::vector<Frame> way_points({start_point, way_point0, way_point1, way_point2, target_point});
  Frame teach_point_3 {{0.90,0.4,0.5,-2.7495292945202761,-1.8806033765539331e-06,-3.1415917428588926}},
  teach_point_4 {{0.72,0.42,0.32,-2.5732143763667032,-0.17399606099405901,-3.1415922388504662}},
  teach_point_5 {{0.86,0.59,0.34,-2.5,-0.16851548275263212,-3.0980736037376517}};

  std::vector<Frame> teach_points({teach_point_3, teach_point_4, teach_point_5, teach_point_2});
  std::vector<std::array<double, 7>> offsets({{0.01, 0, 0.01, 0, 0, 0, 0}, {0, 0, 0.015, 0, 0, 0, 0}, {0.02, 0, 0.02, 0, 0, 0, 0}});
  int error_index = 0;
  auto result = welding.calcWeldOffset_Multi(way_points, teach_points, offsets, error_index, ec);
  if(ec) {
    print(std::cerr, "偏移计算失败", ec.message(), ", 错误点位:", error_index);
  } else {
    print(std::cout, "多轨迹偏移结果:", error_index);
    for(int i = 0; i < result.size(); ++i) {
      print(std::cout, " --- 第", i+1, "道 ---");
      for(int j = 0; j < result[i].size(); ++j) {
        print(std::cout, "轨迹点", j, ":", result[i][j]);
      }
    }
  }
}

/**
 * @brief 机器人运动 - 摆动轨迹
 */
void weaveMove(xMateRobot &robot) {
  error_code ec;
  Toolset default_toolset{};
  robot.setToolset(default_toolset, ec);
  robot.setDefaultSpeed(100, ec);
  robot.setDefaultZone(10, ec);

  auto welding = robot.welding();

  MoveAbsJCommand absj0({0, M_PI/6, -M_PI_2, 0, -M_PI/3, 0});
  MoveLCommand l0({0.440, -0.15, 0.413, M_PI, 0, M_PI}, 1000, 10),
    l1({0.440, 0.04, 0.413, M_PI, 0, M_PI}, 1000, 10),
    l2({0.582, 0.04, 0.413, M_PI, 0, M_PI}, 1000, 10),
    l3({0.582, -0.15, 0.413, M_PI, 0, M_PI}, 1000, 10),
    l4({0.556, -0.15, 0.413,M_PI, 0, M_PI}, 1000, 10);
  std::vector<MoveAbsJCommand> regular_points({absj0});
  std::vector<MoveLCommand> swing_points({l1, l2, l3, l4});

  // 先MoveAbsJ到焊接起始点，非摆动
  moveBlocked(robot, regular_points);

  double frequency_initial = 2;
  double amplitude_initial = 0.001;

  // 设置加速度，加加速度
  welding.setSwingMotion(0.4, 4, ec);

  // 切换到摆动轨迹, 并设置初始摆动频率、幅值和两侧停留时间
  welding.setWeave(true, ec, frequency_initial, amplitude_initial, {0, 0, 0});
  // 支持摆动过程中调整摆动参数
  welding.setWeaveParameters( frequency_initial, amplitude_initial, ec);

  // 开始执行摆动轨迹
  moveBlocked(robot, swing_points);

  // 关闭摆动
  welding.setWeave(false, ec);

}

/**
 * @brief 实时读取激光器数据并设置偏移值
 */
void readLaserDataAndAddOffset(xMateRobot& robot,rokae::BaseWelding& welding,double& x,double& z,bool& valid,error_code& ec) {
  //int offset_x, offset_z, line_width, line_depth;
  std::vector<int> weld_offset;
  //int laser_enable, weld_line_valid, tracker_enable;
  std::vector<int> weld_valid;

  // 这个频率测试的时候根据跟踪效果调整
  for (int i = 0; i < 100000; i++)
  {
    robot.readRegister("weld_offset", 0, weld_offset, ec);
    robot.readRegister("weld_valid", 0, weld_valid, ec);

    if (weld_valid[1] == 255) {
      valid = true;
    }
    x = weld_offset[0]/ 10000.0;
    z = weld_offset[1]/ 10000.0;
    welding.addLaserTrackOffset(x, z, valid, ec);

    // 激光器有Y方向数据时
//    double y = weld_offset[1]/ 10000.0;
//    welding.addLaserTrackOffset(x, y, z, true, ec);

    // 频率按照30ms时，可满足大部分跟踪要求，不建议大幅度调节该参数
    std::this_thread::sleep_for(std::chrono::milliseconds(30));
  }
}

/**
 * @brief 激光跟踪运动任务
 */
void laserTrackMotion(xMateRobot& robot, rokae::BaseWelding& welding) {
  error_code ec;
  //设置工具工件坐标
  std::array<double, 6> tool_array = { 10.52 / 1000.0, 25.31 / 1000.0, 355.49 / 1000.0, 0.0, -45.0 / 180.0 * M_PI, 45.0 / 180.0 * M_PI };
  auto tool_weld_frame = Frame(tool_array);
  Toolset tool_weld;
  tool_weld.end = tool_weld_frame;
  robot.setToolset(tool_weld, ec);
  // 设置运动速度10mm/s, 无转弯区
  robot.setDefaultSpeed(10, ec);
  robot.setDefaultZone(0, ec);

  // 设置手眼标定结果
  std::array<double, 6> frame_array = { 29.456 / 1000.0, -14.151 / 1000.0, 70.047 / 1000.0, 4.5006 / 180.0 * 3.1415, 4.0438 / 180.0 * 3.1415, -97.358 / 180.0 * 3.1415 };
  Frame sensor_frame, tar_tcp;
  sensor_frame = Frame(frame_array);
  welding.setSensorFrame(sensor_frame, ec);

  // 点位适用XMS5
  MoveAbsJCommand absj0({ 26.78566360473633,23.09151077270508, -120.26367187500000, -42.13174438476563,-62.16889801025390,51.46855773925781, 0.00000000000000 });
  MoveLCommand l0({ 344.72134700576515,-317.88785844172963, -9.14529501966743,-179.99640216568793, -0.00354844880624,-175.86072256085905 }, 10, 0);
  std::vector<MoveAbsJCommand> regular_points({ absj0 });
  std::vector<MoveLCommand> movel_points({ l0 });

  // 先MoveAbsJ到焊接起始点
  moveBlocked(robot, regular_points);
  std::cout << "已移动到焊接起始点";
  std::cout << "开始直线运动";

  // 开始跟踪前, 设置激光跟踪参数
  LaserTrackParams params;
  welding.setLaserTrackParameters(params, ec);

  // 开始跟踪，需要放到阻塞的运动指令前，否则导致提前结束跟踪
  welding.startTracking(ec);
  moveBlocked(robot, movel_points);
  welding.stopTracking(ec);
}

/**
 * @brief 示例 - 读取摆动方向数据
 */
void readWeavingData(xMateRobot& robot) {
  using namespace RtSupportedFields;

  // 设置数据发送间隔为1ms, 接收焊机电流和摆动方向
  robot.startReceiveRobotState(std::chrono::milliseconds(1), {current_weld, weaving_state});
  double current_d;
  std::array<bool, 3> weaving_b {};

  // 接收状态数据的队列不会自动覆盖旧数据，可以通过循环读取的方法清除旧数据
  while (robot.updateRobotState(std::chrono::steady_clock::duration::zero()));

  // 打印末端位姿和关节角度到控制台
  for (int i = 0; i < 100; ++i) {
    // 周期性获取当前状态数据，参数timeout最好和设置的数据发送间隔保持一致
    // 或者按照发送频率读取
    robot.updateRobotState(std::chrono::milliseconds(1));
    robot.getStateData(current_weld, current_d);
    robot.getStateData(weaving_state, weaving_b);
    print(std::cout, "当前弧焊电流为：", current_d);
    print(std::cout, "当前摆动状态为：", weaving_b[0], "-", weaving_b[1], "-", weaving_b[2]);
  }
  // 控制器停止发送
  robot.stopReceiveRobotState();
}

/**
 * @brief 绑定摆动方向到DO
 * @param robot
 */
void example_BindIO(BaseWelding &weld) {
  error_code ec;
  // 设置DO0_0和DO0_1为摆动输出DO
  weld.bindWeaveDo("DO0_0", "pos", true, ec);
  weld.bindWeaveDo("DO0_1", "vel", false, ec);

  // 取消设置DO输出
  weld.unbindWeaveDo("DO0_0", ec);
  weld.unbindWeaveDo("DO0_1", ec);
}

/**
 * @brief 示例：焊接+激光跟踪器
 */
void example_LaserTracking(xMateRobot& robot) {
  auto welding = robot.welding();
  error_code ec;
  double x, z;
  bool valid = false;

  // 开启静态任务 开始跟踪
  std::thread static_thread(readLaserDataAndAddOffset,std::ref(robot),std::ref(welding), std::ref(x), std::ref(z), std::ref(valid), std::ref(ec));

  // 开启运动任务
  std::thread motion_thread(laserTrackMotion, std::ref(robot), std::ref(welding));

  // 获取跟踪偏移量
  // welding.getLaserOffset(x, z, tar_tcp, ec);

  //等待运动进程结束
  motion_thread.join();
  static_thread.join();
}

int main() {
  using namespace rokae;

  try {
    xMateRobot robot("192.168.0.160", "192.168.0.100");
    error_code ec;
    robot.setOperateMode(rokae::OperateMode::automatic, ec);
    robot.setPowerState(true, ec);
    robot.setMotionControlMode(rokae::MotionControlMode::NrtCommand, ec);
    robot.setEventWatcher(Event::moveExecution, printInfo, ec);

    //开启摆动
    auto _weld = robot.welding();

    example_otherInterfaces(robot);
    // 摆动运动示例
//    weaveMove(robot);

    // 调用激光跟踪功能
//    example_LaserTracking(robot);

  } catch(const std::exception &e){
    print(std::cerr, e.what());
  }
  return 0;
}