/**
 * @file checkpath_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 "print_helper.hpp"
#include "rokae/data_types.h"
#include "rokae/model.h"
#include "rokae/robot.h"
#include "rokae/utility.h"
#include <iostream>
#include <thread>
using namespace rokae;
using namespace std;

// 检验单点位直线运动
void checkpathLineSingle(xMateRobot &robot, std::error_code &ec) {
  // 点位基于ER7
  // 起始位置
  CartesianPosition start;
  start.trans = {0.631250, 0.0, 0.507386};
  start.rpy = {180.0 * M_PI / 180, 0.0, 180.0 * M_PI / 180};

  // 起始关节角度
  std::vector<double> start_joint = {
    0.000, 30.0 * M_PI / 180, 60.0 * M_PI / 180, 0.0,
    90.0 * M_PI / 180, 0.0};

  // 目标位置
  CartesianPosition target;
  target.trans = {0.615167, 0.141585, 0.507386};
  target.rpy = {180.000 * M_PI / 180, 0.0, -167.039 * M_PI / 180};

  auto ret = robot.checkPath(start, start_joint, target, ec);
  cout << "checkpathLineSingle: " << ret << endl;
  cout << "ec: " << ec.value() << "," << ec.message() << endl;
}

// 检验多点位直线运动
void checkpathLineMulti(xMateRobot &robot, std::error_code &ec) {
  // 点位基于ER7
  // 起始位置
  CartesianPosition start;
  start.trans = {0.631250, 0.0, 0.507386};
  start.rpy = {180.0 * M_PI / 180, 0.0, 180.0 * M_PI / 180};

  // 起始关节角度
  std::vector<double> start_joint = {
    0.000, 30.0 * M_PI / 180, 60.0 * M_PI / 180, 0.0,
    90.0 * M_PI / 180, 0.0};

  // 目标位置
  CartesianPosition target;
  target.trans = {0.615167, 0.141585, 0.507386};
  target.rpy = {180.000 * M_PI / 180, 0.0, -167.039 * M_PI / 180};

  // 第二个目标位置
  CartesianPosition target2;
  target2.trans = {0.615167, 0.141585, 0.517386};
  target2.rpy = {180.000 * M_PI / 180, 0.0, -167.039 * M_PI / 180};

  std::vector<double> target_joint;
  std::vector<CartesianPosition> waypoints = {start, target, target2};
  auto ret = robot.checkPath(start_joint, waypoints, target_joint, ec);
  cout << "checkpathLineMulti: " << ret << endl;
  cout << "ec: " << ec.value() << "," << ec.message() << endl;
}

// 校验弧线运动
void checkpathArc(xMateRobot &robot, std::error_code &ec) {
  // 点位基于ER7
  // 起始位置
  CartesianPosition start;
  start.trans = {0.631250, 0.0, 0.507386};
  start.rpy = {180.0 * M_PI / 180, 0.0, 180.0 * M_PI / 180};

  // 起始关节角度
  std::vector<double> start_joint = {
    0.000, 30.0 * M_PI / 180, 60.0 * M_PI / 180, 0.0,
    90.0 * M_PI / 180, 0.0};

  // 辅助点位置
  CartesianPosition aux;
  aux.trans = {0.583553, 0.134309, 0.628928};
  aux.rpy = {180.0 * M_PI / 180, 11.286 * M_PI / 180, -167.039 * M_PI / 180};

  // 目标位置
  CartesianPosition target;
  target.trans = {0.615167, 0.141585, 0.507386};
  target.rpy = {180.000 * M_PI / 180, 0.0, -167.039 * M_PI / 180};

  // 调用checkPath函数
  auto ret = robot.checkPath(start, start_joint, aux, target, ec);
  cout << "checkpathArc: " << ret << endl;
  cout << "ec: " << ec.value() << "," << ec.message() << endl;
}

// 校验圆弧运动
void checkpathCircle(xMateRobot &robot, std::error_code &ec) {
  // 点位基于ER7
  // 起始位置
  CartesianPosition start;
  start.trans = {0.631250, 0.0, 0.507386};
  start.rpy = {180.0 * M_PI / 180, 0.0, 180.0 * M_PI / 180};

  // 起始关节角度
  std::vector<double> start_joint = {
    0.000, 30.0 * M_PI / 180, 60.0 * M_PI / 180, 0.0,
    90.0 * M_PI / 180, 0.0};

  // 辅助点位置
  CartesianPosition aux;
  aux.trans = {0.583553, 0.134309, 0.628928};
  aux.rpy = {180.0 * M_PI / 180, 11.286 * M_PI / 180, -167.039 * M_PI / 180};

  // 目标位置
  CartesianPosition target;
  target.trans = {0.615167, 0.141585, 0.507386};
  target.rpy = {180.000 * M_PI / 180, 0.0, -167.039 * M_PI / 180};

  // 旋转角度
  double angle = 360 * M_PI / 180;

  // 旋转类型
  MoveCFCommand::RotType rot_type = MoveCFCommand::RotType::constPose;

  auto ret = robot.checkPath(start, start_joint, aux, target, angle, rot_type, ec);
  cout << "checkpathCircle: " << ret << endl;
  cout << "ec: " << ec.value() << "," << ec.message() << endl;
}

int main() {
  try {
    std::string ip = "192.168.0.160";
    std::error_code ec;
    xMateRobot robot(ip);
    checkpathLineSingle(robot, ec);
    checkpathLineMulti(robot, ec);
    checkpathArc(robot, ec);
    checkpathCircle(robot, ec);

  } catch (const rokae::Exception &e) {
    std::cerr << e.what();
  }
  return 0;
}