#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"

// 01_切换地图函数测试
void SwitchMapFuncTest(agvc_interface::RpcClientPtr agv)
{
    std::cout << "\n===== 切换地图函数测试 start =====" << std::endl;

    //  释放控制权 
    agv->releasePriority();
    agv->setPriority("user", "");
    
    agvc_interface::Header map_header{};
    // 切换地图
    map_header.id     = "switch_map_test2";
    map_header.map_id = "random_map_id"; // 确保地图id正确
    agv->switchMap(map_header);

    //  等待异步接口执行完成
    while (true)
    {
        agvc_interface::AsyncInterfaceResultStatus mode_status = agv->getAsyncInterfaceResultStatus();

        if (mode_status.switch_map_status.status == agvc_interface::SwitchMapStatus::FINISHED)
        {
            int ret_final = agv->switchMap(map_header);
            std::cout << "异步执行完成，最终查询结果: " << ret_final << std::endl;
            break;
        }

        std::this_thread::sleep_for(std::chrono::seconds(1));
        std::cout << "等待异步接口执行中..." << std::endl;
    }

    std::cout << "===== 切换地图函数测试 end =====" << std::endl;
}

// main 函数（完整可运行）
int main()
{
    std::cout << "AGVC Interface Test Start..." << std::endl;

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

    // 2. 通讯配置
    agv->setRequestTimeout(100);
    agv->connect("192.168.192.100", 30104);
    agv->login("aa", "bb");

    // 3. 执行切换地图测试
    SwitchMapFuncTest(agv);

    return 0;
}
