/**
 * @file hmi_disconnection_behavior_example.cpp
 * @brief HMI断连行为示例
 *
 * @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/robot.h"
#include <chrono>
#include <future>
#include <iostream>
#include <thread>
#include <unordered_map>

using namespace rokae;
using namespace std;

xMateRobot robot;
error_code ec;

// 断连设置回调
void printDisconnectionBehaviorState(EventInfo info) {
  using namespace EventInfoKey::DisconnectionBehaviour;
  auto behavior = std::any_cast<int>(info.at(Behaviour));
  auto detect_time = std::any_cast<int>(info.at(DetectTime));
  cout << "callback Disconnection Behavior : " << behavior << " Detect Time : " << detect_time << endl;
}

void waitRobot(BaseRobot *robot, bool &running) {
  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;
    }
  }
}

void move() {
  string id;
  bool running;
  JointPosition j1{1, 1, 1, 1, 1, 1};
  MoveAbsJCommand abs1(j1);
  robot.moveAppend(abs1, id, ec);
  robot.moveStart(ec);
  waitRobot(&robot, running);
}

void behaviorExample() {
  robot.setEventWatcher(Event::disconnectionBehaviour, printDisconnectionBehaviorState, ec);// 设置回调

  int set_behavior = 1;
  int set_detect_time = 200;
  robot.setHmiDisconnectBehaviour(set_behavior, set_detect_time, ec);

  int behavior;
  int detect_time;
  robot.getHmiDisconnectBehaviour(behavior, detect_time, ec);
  cout << "getHmiDisconnectBehaviour: Disconnection Behavior : " << behavior << " Detect Time : " << detect_time << endl;

  move();
  cout << "move finished" << endl;// 当hmi断连机器人停止运动
}

int main() {
  using namespace rokae;
  try {
    string ip = "192.168.0.160";
    robot.connectToRobot(ip);
    behaviorExample();
  } catch (const std::exception &e) {
    std::cerr << e.what() << std::endl;
    return -1;
  }
}