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

// 重定位测试（异步接口）
void locateTest(agvc_interface::RpcClientPtr agv)
{
    std::cout << "\n===== 重定位测试 start =====" << std::endl;

    // 1. 释放控制权
    agv->releasePriority();
    // 2. 获取控制权
    agv->setPriority("user", "");

    // 3. 获取运行信息
    auto running_info = agv->getRunningInfo();

    // 4. 执行重定位
    if (running_info.localization_loss)
    {
        // 设置重定位参数
        agvc_interface::Pose2d init_pose{};
        agvc_interface::Header command_header{};
        init_pose.x       = 0.0;
        init_pose.y       = 0.0;
        init_pose.yaw     = 0.0;
        command_header.id = "test_relocation";

        // 调用重定位接口
        int ret = agv->relocation(init_pose, command_header);

        // 等待异步执行完成
        while (true)
        {
            auto mode_status = agv->getAsyncInterfaceResultStatus();
            if (mode_status.relocation_status.status == agvc_interface::ChangeRunningModeStatus::FINISHED)
            {
                // 重新调用接口查询最终结果
                ret = agv->relocation(init_pose, command_header);

                // 判断是否成功
                if (ret == 10100000){
                    std::cout << "重定位成功！" << std::endl;
                }
                else{
                    std::cout << "重定位失败，错误码：" << ret << std::endl;
                }
                break;
            }
            std::this_thread::sleep_for(std::chrono::seconds(1));
        }
    }

    std::cout << "===== 重定位测试 end =====" << std::endl;
}

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

    // 初始化 AGV 客户端
    auto agv = std::make_shared<agvc_interface::RpcClient>();
    agv->setRequestTimeout(100);
    agv->connect("192.168.192.100", 30104);
    agv->login("aa", "bb");

    // 执行重定位测试
    locateTest(agv);

    return 0;
}
