#include <iostream>
#include <chrono>
#include <functional>
#include <memory>
#include <thread>
#include <unistd.h>
#include "agvc_interface/rpc.h"
#include "agvc_interface/rtde.h"
#include "agvc_interface/agvc_interface.h"

// 切换建图模式
void changeRunningMode_test(agvc_interface::RpcClientPtr rpc)
{
    std::cout << "===== 切换建图模式测试 start =====" << std::endl;

    // 抢占控制权
    rpc->setPriority("aa", "");

    // 切换建图模式
    agvc_interface::Header header;
    header.id = "123sdaw-asdafas-zff";
    agvc_interface::RunningMode mode = agvc_interface::RunningMode::MAPPING;
    auto ret = rpc->changeRunningMode(mode, header);

    // 查询异步接口(changeRunningMode)执行状态
    agvc_interface::AsyncInterfaceResultStatus async_result = rpc->getAsyncInterfaceResultStatus();
    agvc_interface::ChangeRunningModeStatus change_running_mode_status = async_result.change_running_mode_status.status;

    // 如果异步接口运行状态为Running, 则重复调用，直到异步接口运行完成
    while (change_running_mode_status == agvc_interface::ChangeRunningModeStatus::RUNNING)
    {
        change_running_mode_status = rpc->getAsyncInterfaceResultStatus().change_running_mode_status.status;
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        std::cout << "切换中..." << std::endl;
    }

    // 异步接口运行结束，重新调用接口查询执行结果
    ret = rpc->changeRunningMode(mode, header);

    // 判断异步接口运行结果
    if(ret == 10100000){
        std::cout << "切换建图模式成功" << std::endl;
    }else{
        std::cout << "切换建图模式失败，错误码：" << ret << std::endl;
    }

    std::cout << "===== 切换建图模式测试 end =====" << std::endl;
}

int main()
{
    std::cout << "AGVC Interface Test..." << std::endl;

    // 1. agv sdk初始化
    auto rpc = std::make_shared<agvc_interface::RpcClient>();

    // 2. agv通讯设置
    // 2.1 设置通讯超时时间
    rpc->setRequestTimeout(100);
    // 2.2 连接agv
    rpc->connect("192.168.192.100", 30104);
    // 2.3 登录
    rpc->login("aa", "bb");

    // 3. 执行切换建图模式测试
    changeRunningMode_test(rpc);

    return 0;
}
