/**
 * @file external_example.cpp
 * @brief 外部轴sdk示例,点位基于CR7
 *
 * @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 <iostream>

using namespace rokae;
using namespace std;
xMateRobot robot;

void waitRobot(BaseRobot *robot);
std::array<double, 6> rlPoint2sdkPoint(const std::vector<double> &rlPoint);
void toDragPos();
void pringExternalPos(const CartesianPosition &cart);
void printFrame(const Frame &frame);

// 获取外部轴位置
void getExternalPos() {
  std::error_code ec;
  // 法一
  cout << "carPosture: " << endl;
  auto pos = robot.cartPosture(CoordinateType::endInRef, ec);// 查看外部轴位置
  pringExternalPos(pos);
  // 法二
  auto jointpos = robot.BaseRobot::jointPos(ec);// 需要用基类的方法
  cout << "jointpos: ";
  for (auto &i : jointpos) {
    cout << i << " ";
  }
  cout << endl;
}

// moveabsj控制外部轴
void moveAbsJ_external() {
  toDragPos();
  JointPosition drag({M_PI / 2, M_PI / 6, -M_PI / 2, 0, -M_PI / 3, 0});
  drag.external = {0.1};// 外部轴运动到0.1m
  MoveAbsJCommand absj(drag);
  string id;
  std::error_code ec;
  robot.moveAppend(absj, id, ec);
  robot.moveStart(ec);
  waitRobot(&robot);
  getExternalPos();
}

// movej控制外部轴
void moveJ_external() {
  toDragPos();
  std::vector<double> p1point({557.362, -150, 359.691, 156.799, -19.970, -149.947});
  CartesianPosition p1(rlPoint2sdkPoint(p1point));
  p1.external = {0.2};// 外部轴运动到0.2m
  MoveJCommand movej(p1);
  string id;
  std::error_code ec;
  robot.moveAppend(movej, id, ec);
  robot.moveStart(ec);
  waitRobot(&robot);
  auto pos = robot.cartPosture(CoordinateType::endInRef, ec);// 查看外部轴位置
  getExternalPos();
}

// moveL控制外部轴
void moveL_external() {
  toDragPos();
  waitRobot(&robot);
  CartesianPosition target(rlPoint2sdkPoint(vector<double>({556.769, 41.781, 358.852, 180, 0, 180})));
  target.external = {0.3};// 外部轴运动到0.2m
  MoveLCommand movel(target);
  string id;
  std::error_code ec;
  robot.moveAppend(movel, id, ec);
  robot.moveStart(ec);
  waitRobot(&robot);
  getExternalPos();
}

// moveC控制外部轴
void moveC_external() {
  toDragPos();
  waitRobot(&robot);
  CartesianPosition target(rlPoint2sdkPoint(vector<double>({634.14606911717374, 7.82724588007801, 414.04438636209187, 179.99992690843217, -0.00011203662060, -179.99995973570532})));
  target.external = {0.2};// 外部轴运动到0.2m
  CartesianPosition aux(rlPoint2sdkPoint(vector<double>({657.35246661046972, 7.82724833169642, 414.04438988603999, 179.9999676367, -0.00001071222744, -179.99997003696470})));
  aux.external = {0.1};// 外部轴运动到0.1m
  MoveCCommand movec(target, aux);
  string id;
  std::error_code ec;
  robot.moveAppend(movec, id, ec);
  robot.moveStart(ec);
  waitRobot(&robot);
  getExternalPos();
}

// moveCF控制外部轴
void moveCF_external() {
  toDragPos();
  waitRobot(&robot);
  CartesianPosition target(rlPoint2sdkPoint(vector<double>({657.35246661046972, 7.82724833169642, 414.04438988603999, 657.35246661046972, 7.82724833169642, 414.04438988603999})));
  target.external = {0.2};// 外部轴运动到0.1m
  CartesianPosition aux(rlPoint2sdkPoint(vector<double>({600.14606911717374, 7.82724588007801, 414.04438636209187, 179.99992690843217, -0.00011203662060, -179.99995973570532})));
  MoveCFCommand movecf(target, aux, 200 / 180.0 * M_PI);
  movecf.rotType = MoveCFCommand::RotType::constPose;
  string id;
  std::error_code ec;
  robot.moveAppend(movecf, id, ec);
  robot.moveStart(ec);
  waitRobot(&robot);
  getExternalPos();
}

// jog外部轴
void jogExternal() {
  std::error_code ec;
  cout << "start pos" << endl;
  getExternalPos();
  auto space = JogOpt::Space::jointSpace;// 轴空间
  auto rate = 1;                         // 速率
  auto step = 100;                       // 步长，mm或度
  auto index = 6;                        // 轴号,外部轴为轴数向后加1，笛卡尔空间同理
  auto direction = true;                 // 轴方向，true为正向，false为反向
  robot.startJog(space, rate, step, index, direction, ec);
  cout << "ec: " << ec.value() << "," << ec.message() << endl;
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  cout << "end pos" << endl;
  getExternalPos();
  robot.stop(ec);
}

// 外部轴设置，同导轨工艺包
void externalSetting() {
  std::error_code ec;
  // 名称
  std::string name = "rail_custom";
  robot.setRailParameter("name", name, ec);
  robot.getRailParameter("name", name, ec);
  cout << "name:" << name << endl;

  // 软限位范围
  std::vector<double> softLimit({-0.7, 0.7});
  robot.setRailParameter("softLimit", softLimit, ec);
  robot.getRailParameter("softLimit", softLimit, ec);
  cout << "softLimit:" << softLimit[0] << "," << softLimit[1] << endl;

  // 开、关
  bool enable = true;
  robot.setRailParameter("enable", enable, ec);
  robot.getRailParameter("enable", enable, ec);
  cout << "enable:" << enable << endl;

  // 基坐标系
  Frame base({0.1, 0, 0, 0, 0, 0});
  robot.setRailParameter("baseFrame", 0, ec);
  robot.getRailParameter("baseFrame", base, ec);
  cout << "baseFrame:" << base.trans[0] << endl;
}

void waitRobot(BaseRobot *robot) {
  // 等待机器人停止
  bool running = true;
  while (running) {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    error_code ec;
    auto st = robot->operationState(ec);
    if (st == OperationState::idle || st == OperationState::unknown) {
      running = false;
    }
  }
}

std::array<double, 6> rlPoint2sdkPoint(const std::vector<double> &rlPoint) {
  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;
};

void toDragPos() {
  // 运动到拖拽位姿
  JointPosition drag({0, M_PI / 6, -M_PI / 2, 0, -M_PI / 3, 0});
  MoveAbsJCommand absj(drag);
  string id;
  std::error_code ec;
  robot.moveAppend(absj, id, ec);
  robot.moveStart(ec);
  waitRobot(&robot);
}

void pringExternalPos(const CartesianPosition &cart) {
  cout << "trans:[";
  for (auto &i : cart.trans) {
    cout << i << ",";
  }
  cout << "], rpy:[";
  for (auto &i : cart.rpy) {
    cout << i << ",";
  }
  cout << "], external:" << cart.external[0] << endl;
}

void printFrame(const Frame &frame) {
  cout << "trans:[";
  for (auto &i : frame.trans) {
    cout << i << ",";
  }
  cout << "], rpy:[";
  for (auto &i : frame.rpy) {
    cout << i << ",";
  }
  cout << "]" << endl;
}

int main() {
  try {
    std::string ip = "192.168.0.160";
    std::error_code ec;
    robot.connectToRobot(ip);
    robot.setPowerState(true, ec);
    moveAbsJ_external();
    moveJ_external();
    moveL_external();
    moveC_external();
    moveCF_external();
    jogExternal();
    robot.setPowerState(false, ec);
  } catch (const rokae::Exception &e) {
    std::cerr << e.what();
  }
  return 0;
}