/**
 * @file read_robot_state.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 <fstream>
#include "rokae/utility.h"
#include "rokae/robot.h"
#include "print_helper.hpp"

using namespace std;
using namespace rokae;

void WaitRobot();
void readStateData_Nrt();
void readStateData_Rt();

namespace {
 xMateRobot g_robot {};
}
/**
 * @brief main program
 */
int main() {
  try {
    using namespace RtSupportedFields;
    g_robot.connectToRobot("10.0.2.43", "10.0.2.121");
    error_code ec;
    readStateData_Rt();


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

/**
 * @brief 等待机器人停止
 */
void WaitRobot() {
  bool checking = true;
  while (checking) {
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    error_code ec;
    auto st = g_robot.operationState(ec);
    if(st == OperationState::idle || st == OperationState::unknown){
      checking = false;
    }
  }
}

/**
 * @brief 非实时数据发送功能示例
 */
void readStateData_Nrt() {
  error_code ec;
  // 打开状态数据发送功能
  // 发送间隔100ms；不设置扩展数据；端口是默认的16666（暂不支持修改端口）
  g_robot.startReceiveRobotState_Nrt(chrono::milliseconds(100), ec, {"dio_board4_di", "dio_board4_do"}, 16666);
  if(ec) {
    print(std::cerr, "startReceiveRobotState_Nrt failed:", ec.message());
    if(ec.value() == 264);
    else return;
  }
  // 自行打开一个TCP客户端（支持多连接），连接到机器人16666 端口，连接后开始发送数据
  print(std::cout, "Press q continue");
  while(getchar() != 'q');
  std::vector<std::string> clients_list;
  auto server_st = g_robot.queryStateDataPublisherServerInfo(clients_list, ec);
  switch (server_st) {
    case 0:print(std::cout, "服务端监听中"); break;
    case 2:print(std::cout, "服务端已连接:", clients_list);
      break;
    case 3:print(std::cout, "服务端已关闭"); break;
    default:print(std::cout, "发生错误", ec);
  }
  print(std::cout, "Press q stop");
  while(getchar() != 'q');
  g_robot.stopReceiveRobotState_Nrt(ec);
  print(std::cout, "状态数据发送已关闭", ec);

}

void readStateData_Rt() {
  using namespace RtSupportedFields;
  error_code ec;

  g_robot.setMotionControlMode(rokae::MotionControlMode::NrtCommand, ec);

  // 设置数据发送间隔为1s, 接收机器人末端位姿、关节力矩和关节角度
  g_robot.startReceiveRobotState(chrono::seconds(1), {tcpPoseAbc_m, tau_m, jointPos_m});
  std::array<double, 6> tcpPose{};
  std::array<double, 6> arr6{};

  std::atomic_bool running{true};

  // 接收状态数据的队列不会自动覆盖旧数据，可以通过循环读取的方法清除旧数据
  while (g_robot.updateRobotState(chrono::steady_clock::duration::zero()));
  // 输出到文件
  std::ofstream ofs;
  ofs.open(("read_" + std::to_string(std::chrono::duration_cast<std::chrono::seconds>(std::chrono::steady_clock::now().time_since_epoch()).count()) + ".csv"), std::ios::out);

  // 打印末端位姿和关节角度到控制台
  std::thread readState([&] {
    while (running) {
      // 周期性获取当前状态数据，参数timeout最好和设置的数据发送间隔保持一致
      // 或者按照发送频率读取
      g_robot.updateRobotState(chrono::seconds(1));
      g_robot.getStateData(tcpPoseAbc_m, tcpPose);
      g_robot.getStateData(jointPos_m, arr6);
      ofs << tcpPose[0] << "," << tcpPose[1] << "," << tcpPose[2] << ","
          << tcpPose[3] << "," << tcpPose[4] << "," << tcpPose[5] << ",,"
          << arr6[0] << ","<< arr6[1] << ","<< arr6[2] << ","
          << arr6[3] << ","<< arr6[4] << ","<< arr6[5] <<std::endl;
//        print(os, "Ts:", ts, "TCP pose:", pose, "\nJoint:", Utils::radToDeg(jnt));
    }
  });

  // 开始一个运动线程
  std::thread moveThread([&]{
    g_robot.setOperateMode(rokae::OperateMode::automatic, ec);
    g_robot.setPowerState(true, ec);
    MoveAbsJCommand p1({0,0,0,0,0,0}), p2({0, M_PI/6, M_PI/3, 0, M_PI_2, 0});
    std::string id;
    g_robot.moveAppend({p1, p2}, id, ec);
    g_robot.moveStart(ec);
    WaitRobot();
  });

  // 等待运动结束
  moveThread.join();
  running = false;
  readState.join();

  // 控制器停止发送
  g_robot.stopReceiveRobotState();
  // 打开状态数据发送功能
  // 发送间隔100ms；不设置扩展数据；端口是默认
}