/**
 * @file arcwelding_example.cpp
 * @brief 焊接工艺包对应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/arcwelding.h"
#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;
 ArcWelding& arcWeld() {
   static ArcWelding v = robot.arcWelding();
   return v;
 }

 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, "路径", _id, ":", _index, "错误:", _ec.message());
       return;
     }
     if(_id == traj_id && _index == index) {
       if(std::any_cast<bool>(info.at(ReachTarget))) {
         print(std::cout, "路径", traj_id, ":", index, "已完成");
       }
       return;
     }
     std::this_thread::sleep_for(std::chrono::milliseconds(200));
   }
 }

 std::ostream &operator<<(std::ostream &os, const ArcWelding::WelderStatus &data) {
   os << "Name: " << data.welding_name << ", Current: " << data.current << ", Voltage: " << data.voltage
   << ", Distance: " << data.welding_distance << ", Path num: " << data.welding_path_num << std::endl;
   return os;
 }
}

// 打印ArcOnData
void printArcOnData(const ArcWelding::ArcOnData &data)
{
  std::cout << "name: " << data.name << std::endl;
  std::cout << "annotation: " << data.annotation << std::endl;
  std::cout << "mode: " << data.mode << std::endl;
  std::cout << "current_mode: " << data.current_mode << std::endl;
  std::cout << "voltage_mode: " << data.voltage_mode << std::endl;
  std::cout << "current: " << data.current << std::endl;
  std::cout << "voltage: " << data.voltage << std::endl;
  std::cout << "hold_time: " << data.hold_time << std::endl;
  std::cout << "detect_time: " << data.detect_time << std::endl;
  std::cout << "confirm_time: " << data.confirm_time << std::endl;
  std::cout << "preflow_time: " << data.preflow_time << std::endl;
  std::cout << "prearc_time: " << data.prearc_time << std::endl;
  std::cout << "slow_wire_feed_speed: " << data.slow_wire_feed_speed << std::endl;
  std::cout << "pre_wirefeed_time: " << data.pre_wirefeed_time << std::endl;
  std::cout << "ramp_time: " << data.ramp_time << std::endl;
  std::cout << "re_arcon.enable: " << data.re_arcon.enable << std::endl;
  std::cout << "re_arcon.retry_time: " << data.re_arcon.retry_time << std::endl;
  std::cout << "re_arcon.wire_retract_delay_time: " << data.re_arcon.wire_retract_delay_time << std::endl;
  std::cout << "re_arcon.wire_retract_time: " << data.re_arcon.wire_retract_time << std::endl;
  std::cout << "re_arcon.current_step: " << data.re_arcon.current_step << std::endl;
  std::cout << "re_arcon.voltage_step: " << data.re_arcon.voltage_step << std::endl;
  std::cout << "scratch_arcon.enable: " << data.scratch_arcon.enable << std::endl;
  std::cout << "scratch_arcon.distance: " << data.scratch_arcon.distance << std::endl;
  std::cout << "scratch_arcon.back_speed: " << data.scratch_arcon.back_speed << std::endl;
  std::cout << "scratch_arcon.step: " << data.scratch_arcon.step << std::endl;
}

// 打印ArcData
void printArcData(const ArcWelding::ArcData &data)
{
  std::cout << "name: " << data.name << std::endl;
  std::cout << "annotation: " << data.annotation << std::endl;
  std::cout << "mode: " << data.mode << std::endl;
  std::cout << "current_mode: " << data.current_mode << std::endl;
  std::cout << "voltage_mode: " << data.voltage_mode << std::endl;
  std::cout << "current: " << data.current << std::endl;
  std::cout << "voltage: " << data.voltage << std::endl;
  std::cout << "weld_speed: " << data.weld_speed << std::endl;
  std::cout << "ramp_time: " << data.ramp_time << std::endl;
  std::cout << "arc_break_param.detect_time: " << data.arc_break_param.detect_time << std::endl;
  std::cout << "arc_break_param.arc_break_option: " << data.arc_break_param.arc_break_option << std::endl;
  std::cout << "arc_break_param.restart_back_distance: " << data.arc_break_param.restart_back_distance << std::endl;
}

// 打印ArcOffData
void printArcOffData(const ArcWelding::ArcOffData &data)
{
  std::cout << "name: " << data.name << std::endl;
  std::cout << "annotation: " << data.annotation << std::endl;
  std::cout << "mode: " << data.mode << std::endl;
  std::cout << "current_mode: " << data.current_mode << std::endl;
  std::cout << "voltage_mode: " << data.voltage_mode << std::endl;
  std::cout << "current: " << data.current << std::endl;
  std::cout << "voltage: " << data.voltage << std::endl;
  std::cout << "hold_time: " << data.hold_time << std::endl;
  std::cout << "delay_gasoff_time: " << data.delay_gasoff_time << std::endl;
  std::cout << "detect_time: " << data.detect_time << std::endl;
  std::cout << "retract_time: " << data.retract_time << std::endl;
  std::cout << "wire_stick_detection_time: " << data.wire_stick_detection_time << std::endl;
  std::cout << "anti_wire_stick_param.enable: " << data.anti_wire_stick_param.enable << std::endl;
  std::cout << "anti_wire_stick_param.current: " << data.anti_wire_stick_param.current << std::endl;
  std::cout << "anti_wire_stick_param.voltage: " << data.anti_wire_stick_param.voltage << std::endl;
  std::cout << "anti_wire_stick_param.time: " << data.anti_wire_stick_param.time << std::endl;
}

// 打印WeaveData
void printWeaveData(const ArcWelding::WeaveData &data)
{
  std::cout << "name: " << data.name << std::endl;
  std::cout << "annotation: " << data.annotation << std::endl;
  std::cout << "weaving_reference: " << data.weaving_reference << std::endl;
  std::cout << "pattern: " << data.pattern << std::endl;
  std::cout << "length or frequency: " << data.weave_length_frequency << std::endl;
  std::cout << "amplitude: " << data.amplitude.left << "," << data.amplitude.right << std::endl;
  std::cout << "dwell_type: " << data.dwell_type << std::endl;
  std::cout << "dwell_time: " << data.dwell_time[0] << "," << data.dwell_time[1] << "," << data.dwell_time[2] << std::endl;
  std::cout << "radius: " << data.radius << std::endl;
  std::cout << "phase_invert: " << data.phase_invert << std::endl;
  std::cout << "elevation_type: " << data.elevation_type << std::endl;
  std::cout << "elevation_angle: " << data.elevation_angle.left << "," << data.elevation_angle.right << std::endl;
  std::cout << "inclination_angle: " << data.inclination_angle << std::endl;
  std::cout << "acc: " << data.acc << std::endl;
  std::cout << "jerk: " << data.jerk << std::endl;
}

// 读写焊接模式
void setWeldMode()
{
  cout << "enter setWeldMode" << endl;
  error_code ec;
  // 切换为空运行模式
  arcWeld().setWeldMode(ArcWelding::WeldMode::TestRun, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  auto mode = arcWeld().getWeldMode(ec); // 读取当前模式
  cout << "mode: " << mode << endl;
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  // 切换为仿真模式
  arcWeld().setWeldMode(ArcWelding::WeldMode::Simu, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  mode = arcWeld().getWeldMode(ec);
  cout << "mode: " << mode << endl;
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  // 切换为实焊模式
  arcWeld().setWeldMode(ArcWelding::WeldMode::Real, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  mode = arcWeld().getWeldMode(ec);
  cout << "mode: " << mode << endl;
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
}

// 读写起弧参数arcondata
void setArcOnData()
{
  cout << "enter setArcOnData" << endl;
  error_code ec;
  // 默认参数
  ArcWelding::ArcOnData arcOnData;
  arcWeld().setArcOnData(arcOnData, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  auto data = arcWeld().getArcOnData("default", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  cout << "default" << endl;
  printArcOnData(data);

  data = arcWeld().getArcOnData("aaa", ec); // 如果获取不存在的参数，提示-272
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  // 自定义参数
  ArcWelding::ArcOnData arcOnData2 = {
    "example_arcondata", "example_annotation", "low_spatter", "wire_feed", "unified", 100, 2, 300, 4000, 500, 600, 0, 2, 300, 100, {true, 10, 100, 200, 15, 5}, // re_arcon
    {true, 50, 100, 20}                                                                                                                           // scaratch_arcon
  };
  arcWeld().setArcOnData(arcOnData2, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  data = arcWeld().getArcOnData(arcOnData2.name, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  cout << "example_arcondata" << endl;
  printArcOnData(data);

  // 删除参数
  arcWeld().removeArcOnData("default",ec);
  data = arcWeld().getArcOnData("default", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272

  // 批量删除
  arcOnData.name = "remove1";
  arcWeld().setArcOnData(arcOnData, ec);
  arcOnData.name = "remove2";
  arcWeld().setArcOnData(arcOnData, ec);
  arcWeld().removeArcOnData(std::vector<std::string>({"remove1", "remove2"}), ec);
  data = arcWeld().getArcOnData("remove1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272
  data = arcWeld().getArcOnData("remove2", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272

  // 可以对hmi工艺包的参数进行修改
  data = arcWeld().getArcOnData("arcondata1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  printArcOnData(data);

  arcOnData.name = "arcondata1";
  arcWeld().setArcOnData(arcOnData, ec);
  data = arcWeld().getArcOnData("arcondata1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  printArcOnData(data);

  arcWeld().removeArcOnData("arcondata1",ec);
  data = arcWeld().getArcOnData("arcondata1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272
}

// 读写焊接参数arcdata
void setArcData()
{
  cout << "enter setArcData" << endl;
  error_code ec;
  // 默认参数
  ArcWelding::ArcData arcData;
  arcWeld().setArcData(arcData, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  auto data = arcWeld().getArcData(arcData.name, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  cout << "default" << endl;
  printArcData(data);

  // 自定义参数
  ArcWelding::ArcData arcData2 = {
    "example_arcdata", "example_annotation", "low_spatter", "wire_feed", "unified", 100, 20, 30, 400, {500, "auto_arc_reignition", 15}};
  arcWeld().setArcData(arcData2, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  data = arcWeld().getArcData(arcData2.name, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  cout << "example_arcdata" << endl;
  printArcData(data);

  // 删除参数
  arcWeld().removeArcData("default", ec);
  data = arcWeld().getArcData("default", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272

  // 批量删除
  arcData.name = "remove1";
  arcWeld().setArcData(arcData, ec);
  arcData.name = "remove2";
  arcWeld().setArcData(arcData, ec);
  arcWeld().removeArcData(std::vector<std::string>({"remove1", "remove2"}), ec);
  data = arcWeld().getArcData("remove1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272
  data = arcWeld().getArcData("remove2", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272

  // 可以对hmi工艺包的参数进行修改
  data = arcWeld().getArcData("arcdata1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  printArcData(data);

  arcData.name = "arcdata1";
  arcWeld().setArcData(arcData, ec);
  data = arcWeld().getArcData("arcdata1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  printArcData(data);

  arcWeld().removeArcData("arcdata1", ec);
  data = arcWeld().getArcData("arcdata1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272
}

// 读写收弧参数arcoffdata
void setArcOffData()
{
  cout << "enter setArcOffData" << endl;
  error_code ec;
  // 默认参数
  ArcWelding::ArcOffData arcOffData;
  arcWeld().setArcOffData(arcOffData, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  auto data = arcWeld().getArcOffData("default", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  cout << "default" << endl;
  printArcOffData(data);

  // 自定义参数
  ArcWelding::ArcOffData arcOffData2 = {
    "example_arcoffdata", "example_annotation", "low_spatter", "wire_feed", "unified", 100, 20, 300, 400, 5500, 600, 700, {true, 20, 15, 100}};
  arcWeld().setArcOffData(arcOffData2, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  data = arcWeld().getArcOffData(arcOffData2.name, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  cout << "example_arcoffdata" << endl;
  printArcOffData(data);

  // 删除参数
  arcWeld().removeArcOffData("default", ec);
  data = arcWeld().getArcOffData("default", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272

  // 批量删除
  arcOffData.name = "remove1";
  arcWeld().setArcOffData(arcOffData, ec);
  arcOffData.name = "remove2";
  arcWeld().setArcOffData(arcOffData, ec);
  arcWeld().removeArcOffData(std::vector<std::string>({"remove1", "remove2"}), ec);
  data = arcWeld().getArcOffData("remove1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272
  data = arcWeld().getArcOffData("remove2", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272

  // 可以对hmi工艺包的参数进行修改
  data = arcWeld().getArcOffData("arcoffdata1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  printArcOffData(data);

  arcOffData.name = "arcoffdata1";
  arcWeld().setArcOffData(arcOffData, ec);
  data = arcWeld().getArcOffData("arcoffdata1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  printArcOffData(data);

  arcWeld().removeArcOffData("arcoffdata1", ec);
  data = arcWeld().getArcOffData("arcoffdata1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272
}

// 读写摆动参数
void setWeaveData()
{
  cout << "enter setWeaveData" << endl;
  error_code ec;
  // 默认参数
  ArcWelding::WeaveData weaveData;
  arcWeld().setWeaveData(weaveData, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  auto data = arcWeld().getWeaveData("default", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  cout << "default" << endl;
  printWeaveData(data);

  // 自定义参数
  ArcWelding::WeaveData weaveData2 = {
    "example_weavedata", "example_annotation", "cycle", "sine", 1, {2, 2}, "weave_stop", {10, 0, 20}, 5, true, "v_pattern", {0, 0}, 0, 1, 10};
  arcWeld().setWeaveData(weaveData2, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  data = arcWeld().getWeaveData(weaveData2.name, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  cout << "example_weavedata" << endl;
  printWeaveData(data);

  // 删除参数
  arcWeld().removeWeaveData("default", ec);
  data = arcWeld().getWeaveData("default", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272

  // 批量删除
  weaveData.name = "remove1";
  arcWeld().setWeaveData(weaveData, ec);
  weaveData.name = "remove2";
  arcWeld().setWeaveData(weaveData, ec);
  arcWeld().removeWeaveData(std::vector<std::string>({"remove1", "remove2"}), ec);
  data = arcWeld().getWeaveData("remove1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272
  data = arcWeld().getWeaveData("remove2", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272

  // 可以对hmi工艺包的参数进行修改
  data = arcWeld().getWeaveData("weavedata1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  printWeaveData(data);

  weaveData.name = "weavedata1";
  arcWeld().setWeaveData(weaveData, ec);
  data = arcWeld().getWeaveData("weavedata1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  printWeaveData(data);

  arcWeld().removeWeaveData("weavedata1", ec);
  data = arcWeld().getWeaveData("weavedata1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272
}

// 读写间断焊参数
void setSegData(){
  cout << "enter setSegData" << endl;
  error_code ec;
  ArcWelding::SegData segdata{"segdata1","segannotation","normal","v10",10,20};
  arcWeld().setSegData(segdata, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  auto data = arcWeld().getSegData("segdata1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  cout << "name: " << data.name << "\n"
       << "annotation: " << data.annotation << "\n"
       << "seg_type: " << data.seg_type << "\n"
       << "non_welded_speed: " << data.non_welded_speed << "\n"
       << "welded_distance: " << data.welded_distance << "\n"
       << "non_welded_distance: " << data.non_welded_distance << endl;

  // 删除参数
  arcWeld().removeSegData("segdata1", ec);
  data = arcWeld().getSegData("segdata1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272

  // 批量删除
  segdata.name = "segdata2";
  arcWeld().setSegData(segdata, ec);
  segdata.name = "segdata3";
  arcWeld().setSegData(segdata, ec);
  arcWeld().removeSegData(std::vector<std::string>({"segdata2","segdata3"}), ec);
  data = arcWeld().getSegData("segdata2", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272
}

// 读写激光跟踪参数
void setLaserTrackData(){
  cout << "enter setLaserTrackData" << endl;
  error_code ec;
  ArcWelding::LaserTrackData laserTrackData{"track1", 2, true, true};
  arcWeld().setLaserTrackData(laserTrackData, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  auto data = arcWeld().getLaserTrackData("track1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  cout << "name: " << data.name << "\n"
       << "job_number:" << data.job_number << "\n"
       << "y_comp: " << data.y_comp << "\n"
       << "z_comp: " << data.z_comp <<  endl;

  // 删除参数
  arcWeld().removeLaserTrackData("track1", ec);
  data = arcWeld().getLaserTrackData("track1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272

  // 批量删除
  laserTrackData.name = "track2";
  arcWeld().setLaserTrackData(laserTrackData, ec);
  laserTrackData.name = "track3";
  arcWeld().setLaserTrackData(laserTrackData, ec);
  arcWeld().removeLaserTrackData(std::vector<std::string>({"track2","track3"}), ec);
  data = arcWeld().getLaserTrackData("track2", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272
}

// 读写激光寻位参数
void setLaserSearchData(){
  cout << "enter setLaserSearchData" << endl;
  error_code ec;
  ArcWelding::LaserSearchData laserSearchData{"search1", 2, "point", "continuous", 3, "lap_joint"};
  arcWeld().setLaserSearchData(laserSearchData, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  auto data = arcWeld().getLaserSearchData("search1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  cout << "name: " << data.name << "\n"
       << "job_number:" << data.job_number << "\n"
       << "search_type: " << data.search_type << "\n"
       << "search_mode: " << data.search_mode << "\n"
       << "step_length: " << data.step_length << "\n"
       << "joint_type: " << data.joint_type << endl;

  // 删除参数
  arcWeld().removeLaserSearchData("search1", ec);
  data = arcWeld().getLaserSearchData("search1", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272

  // 批量删除
  laserSearchData.name = "search2";
  arcWeld().setLaserSearchData(laserSearchData, ec);
  laserSearchData.name = "search3";
  arcWeld().setLaserSearchData(laserSearchData, ec);
  arcWeld().removeLaserSearchData(std::vector<std::string>({"search2","search3"}), ec);
  data = arcWeld().getLaserSearchData("search2", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl; // -272
}

// 设置电弧跟踪参数
void setArcTrackData(){
  cout << "enter setArcTrackData" << endl;
  auto printArcTrackData = [](const ArcWelding::ArcTrackParam &arcTrackData){
    std::cout << "name: " << arcTrackData.name << "\n"
              << "annotation: " << arcTrackData.annotation << "\n"
              << "delay_time: " << arcTrackData.delay_time << "\n"
              << "cycle: " << arcTrackData.cycle << "\n"
              << "lr_enable: " << std::boolalpha << arcTrackData.lr_enable << "\n"
              << "ud_enable: " << std::boolalpha << arcTrackData.ud_enable << "\n"
              << "LrTrackParam:\n"
              << "  pid: " << arcTrackData.lr_param.pid << "\n"
              << "  curr_bias_offset: " << arcTrackData.lr_param.curr_bias_offset << "\n"
              << "  min_compensation: " << arcTrackData.lr_param.min_compensation << "\n"
              << "  max_compensation: " << arcTrackData.lr_param.max_compensation << "\n"
              << "UdTrackParam:\n"
              << "  pid: " << arcTrackData.ud_param.pid << "\n"
              << "  curr_bias_offset: " << arcTrackData.ud_param.curr_bias_offset << "\n"
              << "  min_compensation: " << arcTrackData.ud_param.min_compensation << "\n"
              << "  max_compensation: " << arcTrackData.ud_param.max_compensation << "\n"
              << "  base_current: " << arcTrackData.ud_param.base_current << "\n"
              << "  sampling_start_period: " << arcTrackData.ud_param.sampling_start_period << "\n"
              << "  sampling_duration_period: " << arcTrackData.ud_param.sampling_duration_period << std::endl;
  };
  error_code ec;
  ArcWelding::LrTrackParam lrTrackData{6, 6, 0.02, 0.8};
  ArcWelding::UdTrackParam udTrackData{6, 6, 0.02, 0.8,0.1,6,2};
  ArcWelding::ArcTrackParam arcTrackData{"track1","trackannotation", 2,3, true, true,lrTrackData,udTrackData};
  arcWeld().setArcTrackParam(arcTrackData, ec);

  auto ret = arcWeld().getArcTrackParam("track1", ec);
  printArcTrackData(ret);
}

// 读写IO设置
void setIOSetting()
{
  cout << "enter setIOSetting" << endl;
  ArcWelding::IOSetting ioSetting{
    {"DO0_1", 2, -1, true},      // arc_on
    {"DO0_2", 2, -1, true},      // wire_feed
    {"DO0_0", 2, -1, false},     // wire_feed_back
    {"DO0_0", 2, -1, false},     // gas_test
    {"DO0_0", 2, -1, false},     // positioning_enable
    {"DI0_0", 1, -1, false},     // arc_on_success
    {"DI0_0", 1, -1, false},     // positioning_feedback
    {"register3", 5, 0, true},   // current_set
    {"register4", 5, 0, true},   // voltage_set
    {"register0", 5, 0, true},   // current_get
    {"register2", 5, -1, false}, // voltage_get
    {"DO0_0", 2, -1, true},      // robot_ready
    {"DI1_1", 1, 1, false}       // error
  };
  error_code ec;
  arcWeld().setIOSetting(ioSetting, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  auto data = arcWeld().getIOSetting(ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  auto printIOData = [](const std::string &label, const ArcWelding::IOData &ioData)
  {
    std::cout << label << ":\n"
              << "  Name: " << (ioData.name.empty() ? "null" : ioData.name) << "\n"
              << "  Type: " << ioData.type << "\n"
              << "  Bias: " << ioData.bias << "\n"
              << "  Enable: " << (ioData.enable ? "true" : "false") << "\n";
  };

  printIOData("Arc On", data.arc_on);
  printIOData("Wire Feed", data.wire_feed);
  printIOData("Wire Feed Back", data.wire_feed_back);
  printIOData("Gas Test", data.gas_test);
  printIOData("Positioning Enable", data.positioning_enable);
  printIOData("Arc On Success", data.arc_on_success);
  printIOData("Positioning Feedback", data.positioning_feedback);
  printIOData("Current Set", data.current_set);
  printIOData("Voltage Set", data.voltage_set);
  printIOData("Current Get", data.current_get);
  printIOData("Voltage Get", data.voltage_get);
  printIOData("Robot Ready", data.robot_ready);
  printIOData("Error", data.error);
}

// 读写特性曲线参数
void setCharacteristicCurveData(){
  cout << "enter setCharacteristicCurveData" << endl;
  auto printCharacteristicCurveData = [](const ArcWelding::CharacteristicCurveData &data)
  {
    cout << "name: " << data.name << endl;
    cout << "intercept: " << data.intercept << endl;
    cout << "slope: " << data.slope << endl;
    for (auto &point : data.points)
    {
      cout << "[" << point[0] << "," << point[1] << "]" << endl;
    }
  };

  error_code ec;

  // 读写电流特性曲线参数
  ArcWelding::CharacteristicCurveData currentCharacteristicCurveData;
  currentCharacteristicCurveData.name = "current1";
  currentCharacteristicCurveData.points.push_back({1.0, 200.0});
  currentCharacteristicCurveData.points.push_back({3.0, 300.0});
  currentCharacteristicCurveData.points.push_back({4.0, 350.0});
  arcWeld().setCurrentCharacteristicCurveData(currentCharacteristicCurveData, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  auto data = arcWeld().getCurrentCharacteristicCurveData(currentCharacteristicCurveData.name,ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  printCharacteristicCurveData(data);

  // 读写电压特性曲线参数
  ArcWelding::CharacteristicCurveData voltageCharacteristicCurveData;
  voltageCharacteristicCurveData.name = "voltage1";
  voltageCharacteristicCurveData.points.push_back({1.0,200.0});
  voltageCharacteristicCurveData.points.push_back({3.0,300.0});
  arcWeld().setVoltageCharacteristicCurveData(voltageCharacteristicCurveData, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  data = arcWeld().getVoltageCharacteristicCurveData(voltageCharacteristicCurveData.name,ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  printCharacteristicCurveData(data);

  // 计算电流特性曲线，不保存
  data = arcWeld().calculateCurrentCurve(currentCharacteristicCurveData, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  printCharacteristicCurveData(data);

  // 计算电压特性曲线，不保存
  data = arcWeld().calculateVoltageCurve(voltageCharacteristicCurveData, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  printCharacteristicCurveData(data);
}

// 读写运行参数
void setRunningParam()
{
  cout << "enter setRunningParam" << endl;
  error_code ec;
  ArcWelding::ArcWeldRunningParam runningParam{100, 2, 10};

  arcWeld().setRunningParam(runningParam, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  auto data = arcWeld().getRunningParam(ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  cout << "test_run_speed: " << data.test_run_speed << endl;
  cout << "wire_feed_speed: " << data.wire_feed_speed << endl;
  cout << "gas_detect_time: " << data.gas_detect_time << endl;
}

// 选择焊机，启用焊接功能，断开焊机
void setWelderAndConnect() {
  cout << "enter setWelder" << endl;
  error_code ec;

  auto printWelderSetting = [](const ArcWelding::WelderSetting &welderSetting) {
    cout << "protocol: " << welderSetting.protocol << endl;
    cout << "mfr: " << welderSetting.mfr << endl;
    cout << "model: " << welderSetting.model << endl;
    cout << "current_type: " << welderSetting.current_type << endl;
    cout << "current_file: " << welderSetting.current_file << endl;
    cout << "voltage_file: " << welderSetting.voltage_file << endl;
    cout << "wire_diameter: " << welderSetting.wire_diameter << endl;
    cout << "shielding_gas: " << welderSetting.shielding_gas << endl;
    cout << "program_number: " << welderSetting.program_number << endl;
  };

  auto welderSetting = arcWeld().getWelderSetting(ec);// 获取当前焊机设置，部分参数如果该焊机不涉及可以忽略
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  printWelderSetting(welderSetting);

  welderSetting.protocol = "ethercat";
  welderSetting.mfr = "aotai";
  welderSetting.model = "RL/RPL";
  welderSetting.current_type = 500;
  welderSetting.current_file = "current2";
  welderSetting.voltage_file = "voltage2";
  welderSetting.wire_diameter = 0.8;
  welderSetting.shielding_gas = "CO2";
  welderSetting.program_number = 10;
  arcWeld().setWelder(welderSetting, ec);// 设置焊机（方式一），部分参数如果该焊机不涉及可以忽略
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  welderSetting = arcWeld().getWelderSetting(ec);// 验证设置是否成功
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  printWelderSetting(welderSetting);

  arcWeld().setWelder("ethercat", "aotai", "RL/RPL", 500, ec);// 设置焊机（方式二）
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  arcWeld().connectToWelder(ec);// 启用和连接焊机
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  arcWeld().disconnectFromWelder(ec);// 断开和禁用焊机
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
}

// 读取和设置是否连接物理焊机（切换ENI文件）
void setEniHaveWeld(){
  cout << "enter setEniHaveWeld" << endl;
  error_code ec;
  auto ret = arcWeld().isEniHaveWeld(ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  cout << "isEniHaveWeld: " << ret << endl;

  arcWeld().setIsEniHaveWeld(true, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  ret = arcWeld().isEniHaveWeld(ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  cout << "isEniHaveWeld: " << ret << endl;

  arcWeld().setIsEniHaveWeld(false, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  ret = arcWeld().isEniHaveWeld(ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  cout << "isEniHaveWeld: " << ret << endl;
}

void enableArcData(){
  cout << "enter enableArcData" << endl;
  // 不能为虚拟机并且需要连接焊机
  ArcWelding::ArcData arcData = {
    "example_arcdata", "example_annotation", "low_spatter", "wire_feed", "unified", 100, 20, 30, 400, {500, "auto_arc_reignition", 15}};
  error_code ec;
  arcWeld().enableArcData(arcData, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
}

// 送丝
void feedOnWire()
{
  cout << "enter feedOnWire" << endl;
  error_code ec;

  double time = 0;
  arcWeld().feedOnWire(time, ec); // 时间需要大于0.1，小于0.1错误码报错
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  time = 10;
  arcWeld().feedOnWire(time, ec); // 时间大于0.1，错误码正常
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  std::this_thread::sleep_for(std::chrono::seconds(2));

  arcWeld().feedOnWire(time, ec,false); // 停止送丝
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
}

// 退丝
void feedBackWire()
{
  cout << "enter feedBackWire" << endl;

  error_code ec;
  double time = 0;
  arcWeld().feedBackWire(time, ec); // 时间需要大于0.1，小于0.1错误码报错
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  time = 10;
  arcWeld().feedBackWire(time, ec); // 时间大于0.1，错误码正常
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  std::this_thread::sleep_for(std::chrono::seconds(2));

  arcWeld().feedBackWire(time, ec,false); // 停止退丝
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
}

// 检气
void detectGas()
{
  cout << "enter detectGas" << endl;
  error_code ec;

  double time = 0;
  arcWeld().detectGas(time, ec); // 时间需要大于0.1，小于0.1错误码报错
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  time = 2;
  arcWeld().detectGas(time, ec); // 时间大于0.1，错误码正常
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  std::this_thread::sleep_for(std::chrono::seconds(2));

  arcWeld().detectGas(time, ec,false); // 停止检气
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
}

// 获取焊机状态
void getWelderState()
{
  cout << "enter getWelderState" << endl;
  error_code ec;

  auto welderState = arcWeld().getWelderStatus(ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  cout << "state: " << welderState.state << endl;
  cout << "current: " << welderState.current << endl;
  cout << "voltage: " << welderState.voltage << endl;
  cout << "speed: " << welderState.speed << endl;
  cout << "welding_name: " << welderState.welding_name << endl;
  cout << "arc_welding: " << welderState.arc_welding << endl;
  cerr << "runing_error: " << welderState.running_error.value() << "," << welderState.running_error.message() << endl;
}

// 打印焊接状态
void printWeldState(const EventInfo &info)
{
  using namespace EventInfoKey::ArcWeldState;
  double current = std::any_cast<double>(info.at(Current));
  double voltage = std::any_cast<double>(info.at(Voltage));
  string state = std::any_cast<string>(info.at(State));
  int speed = std::any_cast<int>(info.at(Speed));
  auto error = std::any_cast<error_code>(info.at(RunningError));
  string weldingName = std::any_cast<string>(info.at(WeldingName));
  int arcWelding = std::any_cast<int>(info.at(EventInfoKey::ArcWeldState::ArcWelding));
  double distance = std::any_cast<double>(info.at(WeldingDistance));


  cout << "[焊接状态] current: " << current << ", voltage: " << voltage << ", state: " << state << ", speed: " << speed << endl;
  cout << "weldingName: " << weldingName << ", arcWelding: " << arcWelding << ". ";
  if(info.count(WeldingPathNum)) {
    int weldingPathNum = std::any_cast<int>(info.at(WeldingPathNum));
    std::cout << "Distance: " << distance << " m, PathNum: " << weldingPathNum << "." << std::endl;
  } else {
    std::cout << "Distance: " << distance << " m." << std::endl;
  }
  if(error) {
    std::cerr << "运行错误:" << error << std::endl;
  }
}

// 焊接状态监视
void setArcWeldStateWatcher()
{
  error_code ec;
  robot.setEventWatcher(rokae::Event::arcWeldState, printWeldState, ec);
  if(ec) {
    print(std::cerr, "设置焊接状态监视失败:", ec);
  }
}

// 主动获取焊接状态
void queryWeldState()
{
  error_code ec;
  auto info = robot.queryEventInfo(Event::arcWeldState, ec);
  printWeldState(info);
}

// 打印move状态
void printMoveState(const EventInfo &info)
{
  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)),
        "已完成: ", std::any_cast<bool>(info.at(ReachTarget)) ? "YES": "NO", std::any_cast<error_code>(info.at(Error)),
        std::any_cast<std::string>(info.at(Remark)));
}

// move状态监视
void setMoveStateWatcher()
{
  error_code ec;
  robot.setEventWatcher(rokae::Event::moveExecution, printMoveState, ec);
  if(ec) {
    print(std::cerr, "设置运动事件监视失败:", ec);
  }
}

// 在线修改和保存电流电压
void setAndSaveCurrentVoltage(){
  error_code ec;
  cout << "enter setAndSaveCurrentVoltage" << endl;
  arcWeld().setCurrentAndVoltage(100,20,ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  arcWeld().saveCurrentAndVoltage(100,20,ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
}

// 铆焊
void exampleWelding(){
  error_code ec;
  cout << "enter exampleWelding" << endl;
  arcWeld().startWelding(100,0,ec); // 默认一元化
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  arcWeld().startWelding(100,10,ec,"separate"); // 分别模式
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  std::this_thread::sleep_for(std::chrono::seconds(2));
  arcWeld().stopWelding(ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
}

// 指令示例
void exampleCommand()
{
  cout << "enter exampleCommand" << endl;
  error_code ec;
  // 需要先启用焊机
  arcWeld().setWelder("ethercat", "aotai", "RL/RPL", 500, ec); // 设置焊机
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  arcWeld().connectToWelder(ec); // 启用和连接焊机
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  ArcWelding::ArcOnCommand arcon;
  arcon.setArcOnData("default", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  arcon.setArcData("default", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  ArcWelding::ArcOffCommand arcoff;
  arcoff.setArcOffData("default", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  ArcWelding::WMoveLCommand wml({0.556769, -0.15, 0.4872, -M_PI, 0, M_PI}, 10, 1); // 直线摆动
  wml.setWeaveData("default", ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  cout << "wml: " << wml.getWeaveData() << endl;

  MoveLCommand ml({0.556769, -0.15, 0.4872, -M_PI, 0, M_PI}, 10, 1); // 直线

  ArcWelding::WMoveCCommand wmc({0.786, 0, 0.431, 3, 0.98, 3}, {0.786, 0, 0.431, 3, 0.98, 3}, 10, 1); // 圆弧摆动
  wmc.setWeaveData("default", ec);

  ArcWelding::WeaveOnCommand weaveon;
  weaveon.setWeaveData("aaa", ec);
 
  ArcWelding::WeaveOffCommand weaveoff;

  robot.setMotionControlMode(MotionControlMode::NrtCommand, ec);
  string id;

  robot.moveAppend(arcon, id, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  robot.moveAppend(weaveon, id, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  // robot->moveAppend(wmc,id,ec);
  // cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  // robot->moveAppend(ml,id,ec);
  // cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  robot.moveAppend(wml, id, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  robot.moveAppend(weaveoff, id, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  robot.moveAppend(arcoff, id, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  robot.setPowerState(true,ec);

  robot.moveStart(ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  std::this_thread::sleep_for(std::chrono::seconds(1));
  robot.stop(ec); // 暂停
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  robot.moveStart(ec);// 继续
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
}

// 整圆焊接示例
void wmovecfExample() {
  // 点位机型 XMC7-R850-W7G3B4C
  cout << "enter exampleCommand" << endl;
  auto rlPoint2sdkPoint = [](const std::vector<double> &rlPoint) -> std::array<double, 6> {
    std::array<double, 6> sdkPoint;
    for (int i = 0; i < 3; i++) {
      sdkPoint[i] = rlPoint[i] / 1000.0;
    }
    for (int i = 3; i < 6; i++) {
      sdkPoint[i] = rlPoint[i] / 180.0 * M_PI;
    }
    return sdkPoint;
  };
  error_code ec;
  ArcWelding::ArcOnCommand arcon("arcondata1", "arcdata1");
  ArcWelding::ArcOffCommand arcoff("arcoffdata1");
  ArcWelding::SegOnCommand segon("segdata1");
  ArcWelding::SegOffCommand segooff;
  ArcWelding::WeaveOnCommand weaveon("weavedata1");
  ArcWelding::WeaveOffCommand weaveoff;
  std::vector<double> p1point({556.769126, 101.641184, 358.852436, 158.661084, -17.184105, -148.826026});
  std::vector<double> p2point({590.049781, 101.641177, 358.852421, 158.66108, -17.184102, -148.826024});
  std::vector<double> p3point({556.769146, 17.242862, 358.852448, 179.999964, -0.000009, -179.999957});
  std::vector<double> p4point({556.769126, 101.641184, 358.852436, 179.999964, -0.000009, -179.999957});
  std::vector<double> p5point({590.049781, 101.641177, 358.852421, 179.999964, -0.000009, -179.999957});
  CartesianPosition p1(rlPoint2sdkPoint(p1point));
  CartesianPosition p2(rlPoint2sdkPoint(p2point));
  CartesianPosition p3(rlPoint2sdkPoint(p3point));
  CartesianPosition p4(rlPoint2sdkPoint(p4point));
  CartesianPosition p5(rlPoint2sdkPoint(p5point));
  ArcWelding::WMoveLCommand wml(p3, 200, 1);
  ArcWelding::WMoveCFCommand wmcf(p2, p1, 300 / 180.0 * M_PI);
  ArcWelding::WMoveCFCommand wmcf2(p4, p5, 300 / 180.0 * M_PI);

  // 设置旋转类型
  // wmcf.rotType = MoveCFCommand::constPose;
  wmcf.rotType = MoveCFCommand::constPose;
  // wmcf.rotType = MoveCFCommand::rotAxis;

  string id;

  // 单独整圆运动
  // robot.moveAppend(arcon, id, ec);
  // robot.moveAppend(wmcf, id, ec);
  // robot.moveAppend(arcoff, id, ec);
  // robot.moveStart(ec);

  // 搭配摆动运动
  // wmcf.setWeaveData("weavedata1", ec);
  // robot.moveAppend(arcon, id, ec);
  // robot.moveAppend(weaveon, id, ec);
  // robot.moveAppend(wmcf, id, ec);
  // robot.moveAppend(weaveoff, id, ec);
  // robot.moveAppend(arcoff, id, ec);
  // robot.moveStart(ec);

  // 搭配间断焊运动
  // robot.moveAppend(arcon, id, ec);
  // robot.moveAppend(segon, id, ec);
  // robot.moveAppend(wmcf, id, ec);
  // robot.moveAppend(segooff, id, ec);
  // robot.moveAppend(arcoff, id, ec);
  // robot.moveStart(ec);

  // 搭配电弧跟踪，需要搭配摆动
  // wmcf.setWeaveData("weavedata1", ec);
  // wmcf.setTrackData("arctrackdata1", ec);
  // robot.moveAppend(arcon, id, ec);
  // robot.moveAppend(weaveon, id, ec);
  // robot.moveAppend(wmcf, id, ec);
  // robot.moveAppend(weaveoff, id, ec);
  // robot.moveAppend(arcoff, id, ec);
  // robot.moveStart(ec);

  // arcdata中设置了断弧回退距离，暂停后继续运动会回退，需要有前置轨迹
  // robot.setDefaultConfOpt(false,ec);
  // robot.moveAppend(arcon, id, ec);
  // robot.moveAppend(wml, id, ec);
  // robot.moveAppend(wmcf, id, ec);
  // robot.moveAppend(arcoff, id, ec);
  // robot.moveStart(ec);
  // std::this_thread::sleep_for(std::chrono::milliseconds(4000));
  // robot.stop(ec); // 暂停
  // std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  // robot.moveStart(ec);// 继续
}

//打印激光跟踪器状态
void printLaserSensorState(const EventInfo& info)
{
  using namespace rokae::EventInfoKey::LaserTrackState;
  print(std::cout, "[激光跟踪器状态信息] 设备名称: ", std::any_cast<std::string>(info.at(DeviceName)),
        " 连接:", std::any_cast<bool>(info.at(Connect)) ? "YES " : "NO ",
        "激光开启:", std::any_cast<bool>(info.at(LaserOn)) ? "YES " : "NO ",
        "使能开启:", std::any_cast<bool>(info.at(PowerOn)) ? "YES " : "NO ");//注意：明图传感器没有使能状态，这里的使能状态无效
}

// 激光跟踪器状态监视
void setLaserSensorStateWatcher()
{
  error_code ec;
  robot.setEventWatcher(rokae::Event::lasertrackState, printLaserSensorState, ec);
  if(ec) {
    print(std::cerr, "Set laser state watcher failed:", ec);
  }
}

/**
 * @brief 示例 - 激光传感器相关设置
 */
void LaserSensorSettingExample() {
  //设置激光跟踪器配置信息
  ArcWelding::LaserSensorCfg cfg;
  cfg.name = "sensor1";
  cfg.ip = "192.168.2.3";
  cfg.port = 502;
  cfg.overtime = 800;
  cfg.communication_cycle = 60;
  cfg.type = ArcWelding::LaserSensorType::CRNT;//创想传感器
  //cfg.type = ArcWelding::LaserSensorType::SMART_IMAGE;//明图传感器
  //cfg.type = ArcWelding::LaserSensorType::INTELLIGENT;//英莱传感器

  error_code ec;
  //发送指令设置AddLaserSensorCfg
  arcWeld().setLaserSensorCfg(cfg, ec);
  if (ec) cout << ec.message()<<std::endl;

  //查询设备配置信息
  ArcWelding::LaserSensorCfg cfg_get = arcWeld().getLaserSensorCfg(cfg.name, ec);
  cout << "name:" << cfg_get.name << "  ip:" << cfg_get.ip << endl;
  if (ec) cout << ec.message() << std::endl;

  //设置传感器状态监控watch
  setLaserSensorStateWatcher();

  //连接激光跟踪器ConnLaserTrack
  arcWeld().connLaserSensorDev(cfg.name, ec);
  if (ec) cout << ec.message() << std::endl;

  //断开连接DisConnLaserTrack
  arcWeld().disconnLaserSensorDev(cfg.name,ec);
  if (ec) cout << ec.message() << std::endl;

  //删除设备配置信息RemoveLaserSensorCfg
  arcWeld().removeLaserSensorCfg(cfg.name, ec);
  if (ec) cout << ec.message() << std::endl;

  //设置手眼标定结果
  ArcWelding::HandeyeData handeyedata0;
  handeyedata0.mode = true; // 使用XYZ_ABC的形式
  handeyedata0.xyz_abc = { 29.456,-14.151,70.047,4.5006 / 180.0 * M_PI,4.0438 / 180.0 * M_PI,-97.358 / 180.0 * M_PI };
  handeyedata0.name = "handeyedata0";
  arcWeld().setHandeyeData(handeyedata0, ec);
  if (ec) cout << ec.message() << std::endl;

  //查询手眼标定结果
  std::string name = "handeyedata0";
  auto handeyedata0_get = arcWeld().getHandeyeData(name, ec);
  if (ec) cout << ec.message() << std::endl;
  cout << "name:" << handeyedata0_get.name << "  xyz_abc:{" << handeyedata0_get.xyz_abc[0] << "," << handeyedata0_get.xyz_abc[2] << ","
       << handeyedata0_get.xyz_abc[2] << "," << handeyedata0_get.xyz_abc[3] << "," << handeyedata0_get.xyz_abc[4] << ","
       << handeyedata0_get.xyz_abc[5] << "}"<< endl;

  //删除手眼标定结果
  arcWeld().removeHandeyeData(name,ec);
  if (ec) cout << ec.message() << std::endl;
}

void LaserCali() {
  error_code ec;
  // 开始自动标定，需要已经连接激光器
  arcWeld().startHandeyeCalibration("handeyedata0", 6, ec);
  cout << "startHandeyeCalibration: " << ec.value() << ", " << ec.message() << endl;

  auto printCaliResult = [](const ArcWelding::CalibrationResult &result) {
    std::cout << "ID: " << result.id << std::endl;
    std::cout << "Robot Coordinates: ";
    for (const auto &coord : result.p_robot) {
      std::cout << coord << " ";
    }
    std::cout << std::endl;
    std::cout << "Laser Coordinates: ";
    for (const auto &coord : result.p_laser) {
      std::cout << coord << " ";
    }
    std::cout << std::endl;
  };

  // 每个点位采集配合jog进行

  // 采集尖端坐标
  auto ret = arcWeld().calibratePoint(0, ec);
  cout << "calibratePoint: " << ec.value() << ", " << ec.message() << endl;
  printCaliResult(ret);

  // 采集1-6点位坐标
  for (int i = 1; i <= 6; ++i) {
    ret = arcWeld().calibratePoint(i, ec);
    cout << "calibratePoint: " << ec.value() << ", " << ec.message() << endl;
    printCaliResult(ret);
  }

  ArcWelding::HandeyeData handeyedata;
  // 结束标定，计算矩阵
  auto loss = arcWeld().calibrateEnd(handeyedata, ec);
  cout << "calibrateEnd: " << ec.value() << ", " << ec.message() << endl;
  cout << "loss: " << loss[0] << "," << loss[1] << "," << loss[2] << endl;
  cout << "name:" << handeyedata.name << endl
       << "mode:" << handeyedata.mode << endl
       << "xyz_abc:{" << handeyedata.xyz_abc[0] << "," << handeyedata.xyz_abc[2] << ","
       << handeyedata.xyz_abc[2] << "," << handeyedata.xyz_abc[3] << "," << handeyedata.xyz_abc[4] << ","
       << handeyedata.xyz_abc[5] << "}" << endl
       << "trans:{" << handeyedata.transVector[0] << "," << handeyedata.transVector[1] << "," << handeyedata.transVector[2] << "}" << endl
       << "rot:{" << handeyedata.rotMatrix[0] << "," << handeyedata.rotMatrix[1] << "," << handeyedata.rotMatrix[2] << "," << handeyedata.rotMatrix[3] << "," << handeyedata.rotMatrix[4] << "," << handeyedata.rotMatrix[5] << "}" << endl;
}

void LaserSearchExample() {
  // 保存手眼标定参数
  ArcWelding::HandeyeData handeyedata0;
  handeyedata0.mode = true;
  handeyedata0.xyz_abc = {29.456, -14.151, 70.047, 4.5006 / 180.0 * M_PI, 4.0438 / 180.0 * M_PI, -97.358 / 180.0 * M_PI};
  handeyedata0.name = "laserhandeyedata1";
  error_code ec;
  arcWeld().setHandeyeData(handeyedata0, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  // 保存寻位参数
  ArcWelding::LaserSearchData laserSearchData{"lasersearchdata1", 2, "point", "continuous", 3, "lap_joint"};
  arcWeld().setLaserSearchData(laserSearchData, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;

  CartesianPosition laserSearchPos;
  laserSearchPos.rpy = {3.14, 0, 3.14};
  laserSearchPos.trans ={0.563,0.3,0.43};
  ArcWelding::LaserSearchCommand laserSearchCmd("laserhandeyedata1","lasersearchdata1",laserSearchPos, 10, 0);
  bool isFound;
  CartesianPosition pos;
  std::tuple<bool, rokae::CartesianPosition> ret;
  // 示例中选择同步等待寻位结果，设置超时时间为 30 秒。可根据轨迹速度和长度来设置超时时间。
  std::chrono::seconds timeout(30);
  // 一、单独运动
  // 使用 std::async 启动一个线程执行 executeLaserSearch
  auto future = std::async(std::launch::async,
                           [laserSearchCmd, timeout]() {
                             error_code _ec;
                             auto r = arcWeld().executeLaserSearch(laserSearchCmd, false, timeout, _ec);
                             cout << "executeLaserSearch ec: " << _ec.value() << ", " << _ec.message() << endl;
                             return r;
                           });
  // 主线程执行 moveStart
  std::this_thread::sleep_for(std::chrono::seconds(1)); // 注意处理异步之间的顺序，这里用延时代替
  robot.moveStart(ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  // 等待异步线程完成
  ret = future.get();
  isFound = std::get<0>(ret);
  pos = std::get<1>(ret);
  std::cout << "isFound: " << isFound << ", pos: ";
  std::copy(pos.trans.begin(), pos.trans.end(), std::ostream_iterator<double>(std::cout, ", "));
  std::cout << "rpy: ";
  std::copy(pos.rpy.begin(), pos.rpy.end(), std::ostream_iterator<double>(std::cout, ", "));
  std::cout << std::endl;

  // 二、寻位并运动
  robot.moveReset(ec);
  ret = arcWeld().executeLaserSearch(laserSearchCmd, true,timeout, ec);
  cout << "ec: " << ec.value() << ", " << ec.message() << endl;
  isFound = std::get<0>(ret);
  pos = std::get<1>(ret);
  std::cout << "isFound: " << isFound << ", pos: ";
  std::copy(pos.trans.begin(), pos.trans.end(), std::ostream_iterator<double>(std::cout, ", "));
  std::cout << "rpy: ";
  std::copy(pos.rpy.begin(), pos.rpy.end(), std::ostream_iterator<double>(std::cout, ", "));
  std::cout << std::endl;
}

/**
 * @brief 示例 - 电弧跟踪，点位适配机型NB4h-R580-XX
 */
void arcTrackExample() {
  error_code ec;
  //设置电弧跟踪参数
  ArcWelding::ArcTrackParam param;
  param.name = "arctrackdata1";
  param.annotation = "test1";
  param.delay_time = 5;//其他的值类似设置就行，这里直接使用默认值
  arcWeld().setArcTrackParam(param,ec);

  //获取对应的电弧跟踪参数
  auto param_get = arcWeld().getArcTrackParam(param.name, ec);
  std::cout << "name: " << param_get.name << " annotation: " << param_get.annotation << " delay_time: " << param_get.delay_time << std::endl;

  //本例验证时运行于NB4h-R580-3B(注意点位匹配)
  auto p0 = { 0.357487, 0.000633967, 0.222628, 3.0, 0.0685018, 3.12448 };
  auto p1 = { 0.357487, 0.143835, 0.222628, 3.13197, 0.0685018, 3.12448 };
  auto p2 = { 0.456268, 0.000634313, 0.222628, 3.13197, 0.0685026, 3.12448 };
  auto p3 = { 0.456268, 0.143835, 0.222628, 3.13197, 0.0685024, 3.12448 };
  auto p4 = { 0.456268, -0.103198, 0.222628, 3.13197, 0.068502, 3.12448 };

  ArcWelding::ArcOnCommand arcon("arcondata1", "arcdata1");
  ArcWelding::ArcOffCommand arcoff("arcoffdata1");
  MoveJCommand mj(p0, 100, 1);
  // 第一条，开启电弧跟踪，用跟踪参数arctrackdata1
  ArcWelding::WMoveLCommand wml1(p1, 20, 1, "weavedata1", "arctrackdata1");
  // 第二条，开启电弧跟踪，用跟踪参数arctrackdata2
  ArcWelding::WMoveLCommand wml2(p2, 20, 1, "weavedata1", "arctrackdata2");
  // 第三条，电弧跟踪参数为空，不跟踪
  ArcWelding::WMoveLCommand wml3(p3, 20, 1, "weavedata1");
  // 第四条和第五条，开启电弧跟踪，用跟踪参数arctrackdata2
  ArcWelding::WMoveLCommand wml4(p4, 20, 1, "weavedata1", "arctrackdata2");
  ArcWelding::WMoveLCommand wml5(p3, 20, 1, "weavedata1", "arctrackdata2");
  // 第六条，不跟踪
  ArcWelding::WMoveLCommand wml6(p2, 20, 1, "weavedata1"); // 直线摆动
  ArcWelding::WeaveOnCommand weaveon("weavedata1");
  ArcWelding::WeaveOffCommand weaveoff;
  auto wmove_list = {wml1, wml2, wml3, wml4, wml5, wml6};

  string id, move_id;
  robot.setPowerState(true, ec);
  robot.moveAppend(mj, move_id, ec);
  robot.moveAppend(arcon, id, ec); if(ec) std::cout << __LINE__ << ":" << ec.message() << std::endl;
  robot.moveAppend(weaveon, id, ec); if(ec) std::cout << __LINE__ << ":" << ec.message() << std::endl;
  robot.moveAppend(wmove_list, move_id, ec); if(ec) std::cout << __LINE__ << ":" << ec.message() << std::endl;
  robot.moveAppend(weaveoff, id, ec); if(ec) std::cout << __LINE__ << ":" << ec.message() << std::endl;
  robot.moveAppend(arcoff, id, ec); if(ec) std::cout << __LINE__ << ":" << ec.message() << std::endl;
  robot.moveStart(ec); if(ec) std::cout << __LINE__ << ":" << ec.message() << std::endl;

  waitForFinish(robot, move_id, (int)wmove_list.size() - 1);
  while(-1 != std::any_cast<int>(robot.queryEventInfo(Event::arcWeldState, ec).at(EventInfoKey::ArcWeldState::ArcWelding))) {
    // waiting for arc off
    std::this_thread::sleep_for(std::chrono::milliseconds (10));
  }
  print(std::cout, "Finish running");
}

/**
 * @brief 示例 - 焊接过程中设置焊接参数。点位适用机型XMS5
 */
void arcSetExample() {
  std::string cmd_id;
  error_code ec;
  ArcWelding::ArcOnCommand arc_on_command("arcondata1", "arcdata1");
  ArcWelding::ArcOffCommand arc_off_command("arcoffdata1");
  MoveAbsJCommand absj {{0, M_PI / 6, -M_PI_2, 0, -M_PI / 3, 0}, 1000};
  MoveLCommand l {{0.614, 0.136, 0.389, -M_PI, 0, M_PI }, 500};
  ArcWelding::WMoveLCommand wmovel1 {{0.553, -0.107, 0.309, -M_PI, 0, M_PI}, 20};
  ArcWelding::WMoveLCommand wmovel2 {{0.553, 0.107, 0.309, -M_PI, 0, M_PI}, 20};
  wmovel2.arc_set_opt = { "arcdata2", true, 80 };

  robot.setPowerState(true, ec);

  robot.moveAppend(absj, cmd_id, ec);
  robot.moveAppend(l, cmd_id, ec);
  robot.moveAppend(arc_on_command, cmd_id, ec);
  robot.moveAppend({wmovel1, wmovel2}, cmd_id, ec);
  robot.moveAppend(arc_off_command, cmd_id, ec);
  robot.moveStart(ec);
  while(getchar() != '\n');

  robot.setPowerState(false, ec);
}

/**
 * @brief 示例 - 间断焊。适用CR机型，SR机型需要调整偏移量
 */
void segOnExample(){
  error_code ec;

  // 根据起始轴角度，计算笛卡尔位姿
  auto model = robot.model();
  std::array<double, 6> drag_posture = {0, M_PI / 6, -M_PI_2, 0, -M_PI / 3, 0};
  auto p_start = model.calcFk(drag_posture, ec);
  print(std::cout, "start pose: ", p_start);

  // 设置间断焊参数
  ArcWelding::SegData segdata {"segdata1", "segannotation", "normal","v5",15,30};
  arcWeld().setSegData(segdata, ec);
  // 开始间断焊指令，设置间断焊参数为segdata1
  ArcWelding::SegOnCommand segOnCmd("segdata1");
  ArcWelding::SegOffCommand segOffCmd;
  // 几条焊接运动指令
  ArcWelding::WMoveLCommand non_seg_wmovel0(p_start, 20, 0);
  non_seg_wmovel0.offset.type = CartesianPosition::Offset::Type::offs;
  non_seg_wmovel0.offset.frame.trans = {0.01, 0.03, -0.02};
  ArcWelding::WMoveLCommand seg_wmovel0(p_start, 20, 0);
  seg_wmovel0.offset.type = CartesianPosition::Offset::Type::offs;
  seg_wmovel0.offset.frame.trans = {0, -0.01, -0.02};
  seg_wmovel0.arc_set_opt = { "arcdata2", true, 0 };
  ArcWelding::WMoveLCommand seg_wmovel1(p_start, 20, 0);
  seg_wmovel1.offset.type = CartesianPosition::Offset::Type::offs;
  seg_wmovel1.offset.frame.trans = {0, 0.08, -0.02};
  seg_wmovel1.arc_set_opt = { "arcdata3", true, 0 };
  ArcWelding::WMoveCCommand seg_wmc(p_start, p_start, 10, 0);
  seg_wmc.targetOffset.type = CartesianPosition::Offset::Type::offs;
  seg_wmc.targetOffset.frame.trans = {0, -0.05, 0.01};
  seg_wmc.auxOffset.type = CartesianPosition::Offset::Type::offs;
  seg_wmc.auxOffset.frame.trans = {0, 0.070, 0};

  ArcWelding::WMoveLCommand non_seg_wmovel1(p_start, 20, 1);
  non_seg_wmovel1.offset.type = CartesianPosition::Offset::Type::offs;
  non_seg_wmovel1.offset.frame.trans = {-0.05, 0, 0};
  non_seg_wmovel1.arc_set_opt = { "arcdata1", true, 0 };

  ArcWelding::ArcOnCommand arcOnCmd("arcondata1", "arcdata1");
  ArcWelding::ArcOffCommand arcOffCmd("arcoffdata1");
  MoveAbsJCommand absj {{0, M_PI / 6, -M_PI_2, 0, -M_PI / 3, 0}, 500};
  ArcWelding::WeaveOnCommand weave_on_command("weavedata1");
  ArcWelding::WeaveOffCommand weaveOffCmd;

  string id;

  robot.setPowerState(true, ec);
  robot.moveAppend(absj, id, ec);
  robot.moveAppend(arcOnCmd, id, ec);
  robot.moveAppend(weave_on_command, id, ec);
  robot.moveAppend({non_seg_wmovel0}, id, ec);
  robot.moveAppend(segOnCmd, id, ec);
  robot.moveAppend({seg_wmovel0, seg_wmovel1}, id, ec);
  robot.moveAppend(seg_wmc, id, ec);
  robot.moveAppend(segOffCmd, id, ec);
  robot.moveAppend(non_seg_wmovel1, id, ec);
  robot.moveAppend(weaveOffCmd, id, ec);
  robot.moveAppend(arcOffCmd, id, ec);
  robot.moveStart(ec);

  while(getchar() != '\n');
}

/**
 * @brief 示例 - 焊接钟摆过程中设置焊接参数。
 */
void arcSetPendulumExample() {
    std::string cmd_id;
    error_code ec;
    // 设置起弧/收弧
    ArcWelding::ArcData arcData;
    arcData.name = "arcdata1";
    arcWeld().setArcData(arcData, ec);

    arcData.name = "arcdata2";
    arcData.current = 20;
    arcData.voltage = 15;
    arcWeld().setArcData(arcData, ec);

    arcData.name = "arcdata3";
    arcData.current = 100;
    arcData.voltage = 20;
    arcData.weld_speed = 15;
    arcWeld().setArcData(arcData, ec);

    ArcWelding::ArcOnData arconData;
    arconData.name = "arcondata1";
    arcWeld().setArcOnData(arconData, ec);

    ArcWelding::ArcOffData arcOffData;
    arcOffData.name = "arcoffdata1";
    arcWeld().setArcOffData(arcOffData, ec);

    // 设置摆动
    ArcWelding::WeaveData weaveData;
    weaveData.weave_length_frequency = 3.5;
    weaveData.amplitude.left = 6;
    weaveData.amplitude.right = 4.5;
    arcWeld().setWeaveData(weaveData, ec);

    ArcWelding::ArcOnCommand arc_on_command("arcondata1", "arcdata1");
    ArcWelding::ArcOffCommand arc_off_command("arcoffdata1");
    ArcWelding::WeaveOnCommand weave_on;
    ArcWelding::WeaveOffCommand weave_off;
    MoveAbsJCommand absj{ {0.168,0.369, -0.883,-0.210,-0.637,-0.137}, 1000 };
    // 使用栈上对象而不是堆上对象，避免内存泄漏
    CartesianPosition start1{ 0.68015, -0.077702, 0.777555, -2.319, 1.112, -2.228 };
    CartesianPosition startAux1{ 0.672849, -0.0777043, 0.777555,   -2.319, 1.112, -2.228 };
    CartesianPosition targetAux1{ 0.647849, -0.0943, 0.750882,   -2.319, 1.112, -2.228 };
    CartesianPosition target1{ 0.647849, -0.09473, 0.747882,   -2.319, 1.112, -2.228 };

    CartesianPosition targetAux2{ 0.437346, -0.2597, 0.793403,   -2.319, 1.112, -2.228 };
    CartesianPosition target2{ 0.435346, -0.2597, 0.796403,   -2.319, 1.112, -2.228 };

    CartesianPosition targetAux3{ 0.6117849, -0.14534, 0.666882,   -2.319, 1.112, -2.228 };
    CartesianPosition target3{ 0.599849, -0.14574, 0.667882,   -2.319, 1.112, -2.228 };

    MoveLCommand l{ start1, 500 };
    // 钟摆
    ArcWelding::WMoveLPendulumCommand wp1{ startAux1, target1, targetAux1, 20, 0.5, 100, 2 };

    ArcWelding::WMoveLPendulumCommand wp2{ NULL, target2, targetAux2, 20, 3, 234, 1 };
    wp2.arc_set_opt = { "arcdata2", true, 0 };

    ArcWelding::WMoveLPendulumCommand wp3{ NULL, target3, targetAux3, USE_DEFAULT, 3.1, 4, 5 };
    wp3.arc_set_opt = { "arcdata3", true, 0 };

    robot.setPowerState(true, ec);
    robot.moveAppend(absj, cmd_id, ec);
    robot.moveAppend(l, cmd_id, ec);
    robot.moveAppend(arc_on_command, cmd_id, ec);
    robot.moveAppend(wp1, cmd_id, ec);
    robot.moveAppend(wp2, cmd_id, ec);
    robot.moveAppend(wp3, cmd_id, ec);
    robot.moveAppend(arc_off_command, cmd_id, ec);
    robot.moveStart(ec);
    while (getchar() != '\n');

    robot.setPowerState(false, ec);
}

/**
 * @brief 示例 - 激光跟踪。点位适配机型XMS5
 */
void LaserTrackCommandExample(){
  error_code ec;
  // 设置传感器状态监控
  setLaserSensorStateWatcher();
  // 连接激光器
  arcWeld().connLaserSensorDev("sensor1", ec);
  if (ec) {
    print(std::cerr, "Connected laser sensor failed", ec);
    return;
  }

  ArcWelding::ArcOnCommand  arcOnCmd("arcondata1", "arcdata1");
  ArcWelding::ArcOffCommand arcOffCmd("arcoffdata1");
  ArcWelding::LaserTrackOnCommand laserTrackOnCmd; // 默认手眼标定参数和激光跟踪参数名为"default",参考设置这两个参数的方式设置
  ArcWelding::LaserTrackOffCommand laserTrackOffCmd;
  MoveJCommand movej {{0.614, 0.136, 0.389, -M_PI, 0, M_PI }, 500};
  ArcWelding::WMoveLCommand wmovel1 {{0.553, -0.107, 0.309, -M_PI, 0, M_PI}, 20};
  ArcWelding::WMoveLCommand wmovel2 {{0.553, 0.107, 0.309, -M_PI, 0, M_PI}, 20};
  string id, move_id;
  robot.setOperateMode(rokae::OperateMode::automatic, ec);
  robot.setPowerState(true, ec);
  if(ec) {
    print(std::cerr, "Set power state failed", ec);
    return;
  }

  // 打开激光传感器
  arcWeld().openLaserTrack("sensor1", ec);
  if(ec) {
    print(std::cerr, "Open laser sensor failed", ec);
    return;
  }

  ROKAE_EXE(robot.moveAppend(movej, move_id, ec));
  ROKAE_EXE(robot.moveAppend(arcOnCmd, id, ec));
  ROKAE_EXE(robot.moveAppend(laserTrackOnCmd, id, ec));
  ROKAE_EXE(robot.moveAppend({wmovel1, wmovel2}, move_id, ec));
  ROKAE_EXE(robot.moveAppend(laserTrackOffCmd,id, ec));
  ROKAE_EXE(robot.moveAppend(arcOffCmd, id, ec));
  ROKAE_EXE(robot.moveStart(ec));
  if(ec) return;

  waitForFinish(robot, move_id, 1);
  while(-1 != std::any_cast<int>(robot.queryEventInfo(Event::arcWeldState, ec).at(EventInfoKey::ArcWeldState::ArcWelding))) {
    // waiting for arc off
    std::this_thread::sleep_for(std::chrono::milliseconds (10));
  }

  // 关闭激光传感器
  ROKAE_EXE(arcWeld().closeLaserTrack("sensor1", ec));

  robot.setPowerState(false, ec);
  robot.setOperateMode(rokae::OperateMode::manual, ec);
}

void printAnticollisionState(const EventInfo& info) {
  using namespace rokae::EventInfoKey::AnticollisionState;
  cout << "anticollision: \n"
        << "signale: " << std::any_cast<std::string>(info.at(Signal))
        << "\nenable: " << std::any_cast<bool>(info.at(Enable))
        << "\nblock: " << std::any_cast<bool>(info.at(Block))
        << "\ncountdown: " << std::any_cast<int>(info.at(Countdown)) << std::endl;
}

void SetAnticollision() {
  error_code ec;
  robot.setEventWatcher(Event::anticollisionState, printAnticollisionState, ec);
  if (ec)
      cout << "setEventWatcher" << ec.message() << std::endl;

  arcWeld().setAnticollision("DI0_0", true, false, 30, ec);
  if (ec)
      cout << "setAnticollision" << ec.message() << std::endl;

  auto ret = arcWeld().getAnticollisionState(ec);
  if (ec)
      cout << "getAnticollisionState" << ec.message() << std::endl;
  cout << "anticollision: \n"
       << "signale: " << ret.signal << "\nenable: " << ret.enable << "\nblock: " << ret.block << "\ncountdown: " << ret.countdown << std::endl;
}

// 设置摆动自适应参数-自适应，须开启摆动+激光跟踪
void setWeaveAdaptiveData() {
    cout << "enter setWeaveAdaptiveData" << endl;

    error_code ec;
    ArcWelding::WeaveAdaptiveData weaveAdaptiveData;
    weaveAdaptiveData.isAdaptive = true;
    weaveAdaptiveData.groove_width_standard = 10;
    weaveAdaptiveData.groove_width_ratio = 4;
    weaveAdaptiveData.max_groove_width = 15;
    weaveAdaptiveData.max_electric = 300;
    weaveAdaptiveData.max_speed = 6;

    arcWeld().setWeaveAdaptiveData(weaveAdaptiveData, ec);
    if (ec) cout << "ec: " << ec.value() << " " << ec.message() << endl;
}

// 获取摆动自适应参数
void getWeaveAdaptiveData() {
    cout << "enter getWeaveAdaptiveData" << endl;

    error_code ec;
    auto _weaveAdaptiveData = arcWeld().getWeaveAdaptiveData(ec);
    if (ec) cout << "ec: " << ec.value() << " " << ec.message() << endl;

    cout << "isAdaptive: " << _weaveAdaptiveData.isAdaptive << endl;
    cout << "groove_width_standard: " << _weaveAdaptiveData.groove_width_standard << endl;
    cout << "groove_width_ratio: " << _weaveAdaptiveData.groove_width_ratio << endl;
    cout << "max_groove_width: " << _weaveAdaptiveData.max_groove_width << endl;
    cout << "max_speed: " << _weaveAdaptiveData.max_speed << endl;
    cout << "max_electric: " << _weaveAdaptiveData.max_electric << endl;
}

/**
 * @brief 示例 - 摆动自适应。点位适用机型xMateCR12-P
 */
void WeaveAdaptiveExample() {
    error_code ec;

    // 切换为实焊模式
    arcWeld().setWeldMode(ArcWelding::WeldMode::Real, ec);
    cout << "ec: " << ec.value() << ", " << ec.message() << endl;

    //设置激光跟踪器配置信息
    ArcWelding::LaserSensorCfg cfg;
    cfg.name = "sensor1";
    cfg.ip = "192.168.2.3";
    cfg.port = 502;
    cfg.overtime = 1000;
    cfg.communication_cycle = 10;
    //cfg.type = ArcWelding::LaserSensorType::INTELLIGENT;//英莱传感器
    cfg.type = ArcWelding::LaserSensorType::SMART_IMAGE;
    arcWeld().setLaserSensorCfg(cfg, ec);

    // 设置控制模式
    robot.setMotionControlMode(MotionControlMode::NrtCommand, ec);
    //设置传感器状态监控watch
    setLaserSensorStateWatcher();
    robot.setPowerState(true, ec);

    //连接激光跟踪器ConnLaserTrack
    arcWeld().connLaserSensorDev(cfg.name, ec);
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    //开启焊缝跟踪器，开始获取激光传感器数据OpenLaserTrack
    arcWeld().openLaserTrack(cfg.name, ec);
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));

    ArcWelding::LaserTrackOnCommand laserTrackOnCmd("laserhandeyedata1", "lasertrackdata2");
    string id, move_id;
    ArcWelding::WeaveOnCommand weave_on_command("weavedata1");

    // 设置摆动自适应参数
    ArcWelding::WeaveAdaptiveData weaveAdaptiveData; 
    weaveAdaptiveData.isAdaptive = true;
    weaveAdaptiveData.groove_width_standard = 10;
    weaveAdaptiveData.groove_width_ratio = 4;
    weaveAdaptiveData.max_groove_width = 15;
    weaveAdaptiveData.max_electric = 300;
    weaveAdaptiveData.max_speed = 6;

    arcWeld().setWeaveAdaptiveData(weaveAdaptiveData, ec);

    MoveLCommand ml0({ 779.3484337462 / 1000.0, 413.67018305079 / 1000.0, 49.8918122309 / 1000.0,
          -177.08512657858 / 180 * M_PI, 3.5808044275 / 180 * M_PI, -157.11238586592 / 180 * M_PI }, 10, 1);
    ml0.target.confData = { 12, 50, -84, 48, -63, -42 };

    ArcWelding::ArcOnCommand arcOnCmd("arcondata1", "arcdata1");
    ArcWelding::WMoveLCommand wmL({ 740.22573950576 / 1000.0, 540.09560221072 / 1000.0, 47.03271791844 / 1000.0,
        -177.0852401317 / 180 * M_PI, 3.58072487015 / 180 * M_PI, -157.1121845564 / 180 * M_PI }, 10, 1, "weavedata1"); // 圆弧摆动
    wmL.target.confData = { 20, 49, -86, 52, -58, -39 };
    ArcWelding::ArcOffCommand arcOffCmd("arcoffdata1");
    ArcWelding::WeaveOffCommand weaveoff;

    ArcWelding::LaserTrackOffCommand laserTrackOffCmd;
    cout << "start: " << endl;

    robot.moveAppend(ml0, move_id, ec);
    robot.moveAppend(laserTrackOnCmd, id, ec);
    robot.moveAppend(weave_on_command, id, ec);
    robot.moveAppend(arcOnCmd, id, ec);
    robot.moveAppend(wmL, move_id, ec);
    robot.moveAppend(arcOffCmd, id, ec);
    robot.moveAppend(weaveoff, id, ec);
    robot.moveAppend(laserTrackOffCmd, id, ec);
    robot.moveStart(ec);

    waitForFinish(robot, move_id, 0);
    robot.setPowerState(false, ec);

    //关闭焊缝跟踪器CloseLaserTrack
    arcWeld().closeLaserTrack(cfg.name, ec);
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));
    //断开连接DisConnLaserTrack
    arcWeld().disconnLaserSensorDev(cfg.name, ec);
    std::this_thread::sleep_for(std::chrono::milliseconds(2000));
}

/**
 * @brief 示例 - 退丝/送丝指令。
 */
void FeedOnOrBackWireExample() {
    error_code ec;

    robot.setPowerState(true, ec);

    double time = 0.0, speed = 0.0;
    bool enable = false;

    ArcWelding::FeedOnWireCommand feedon(0.1, 1, true);     // 送丝指令 时间：秒，速度：m/min
    feedon.setFeedOnWireData(0.2, 1.2, true, ec);           // 设置送丝参数
    feedon.getFeedOnWireData(time, speed, enable);          // 获取送丝参数
    std::cout << "Feed On Wire time: " << time << ", speed: " << speed << ", enable: " << enable << std::endl;

    ArcWelding::FeedBackWireCommand feedback(0.1, 1, true); // 退丝指令 时间：秒，速度：m/min
    feedback.setFeedBackWireData(0.3, 1.3, true, ec);       // 设置退丝参数
    feedback.getFeedBackWireData(time, speed, enable);      // 获取退丝参数
    std::cout << "Feed Back Wire time: " << time << ", speed: " << speed << ", enable: " << enable << std::endl;

    string id = "";
    robot.moveAppend(feedon, id, ec);
    robot.moveAppend(feedback, id, ec);
    robot.moveStart(ec);

    robot.setPowerState(false, ec);

}

int main()
{
  using namespace rokae;
  try {
    robot.connectToRobot("192.168.0.160");
  } catch (const std::exception& e) {
    std::cerr << e.what() << std::endl;
    return -1;
  }
  // 启用焊接工艺包
  error_code ec;
  arcWeld().connectToWelder(ec);
  if(ec) {
    std::cerr << "Failed enable arcweld function: " << ec.message() << std::endl;
    return 0;
  }

  // 接收焊接状态反馈和机器人运动指令反馈
  setArcWeldStateWatcher();
  setMoveStateWatcher();
  // 运行其它示例
//  exampleCommand();

}
