#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 saveMapFuncTest(agvc_interface::RpcClientPtr rpc)
{
    std::cout << "\n===== 保存地图功能测试 start =====" << std::endl;

    // ====================== 1. 释放控制权 ======================
    int ret_release = rpc->releasePriority();
    std::cout << "释放控制权返回值: " << ret_release << std::endl;

    // ====================== 2. 获取控制权 ======================
    int ret_get_prio = rpc->setPriority("aa", "");
    std::cout << "获取控制权返回值: " << ret_get_prio << std::endl;

    // ====================== 3. 调用保存地图接口 ======================
    agvc_interface::Header map_header{};
    map_header.id     = "save_map_test";
    map_header.map_id = "map";
    map_header.name   = "test_map";

    // 调用保存地图接口
    int ret_save = rpc->saveMap(map_header);
    std::cout << "保存地图接口调用返回值: " << ret_save << std::endl;

    // 等待异步接口执行完成
    while (true)
    {
        agvc_interface::AsyncInterfaceResultStatus async_result = rpc->getAsyncInterfaceResultStatus();
        auto status = async_result.save_map_status.status;

        if (status == agvc_interface::SaveMapStatus::FINISHED)
        {
            std::cout << "保存地图异步执行完成！" << std::endl;
            break;
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }

    // 最终查询结果
    ret_save = rpc->saveMap(map_header);
    if (ret_save == 10100000){
        std::cout << "保存地图成功" << std::endl;
    }
    else{
        std::cout << "保存地图失败，错误码：" << ret_save << std::endl;
    }

    std::cout << "===== 保存地图功能测试 end =====" << std::endl;
}

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

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

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

    // 3. 调用保存地图测试
    saveMapFuncTest(rpc);

    return 0;
}
