#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 CreatMapTest(agvc_interface::RpcClientPtr agv)
{
    std::cout << "\n===== 切换建图模式测试 start =====" << std::endl;

    // 1. 抢夺控制权
    agv->releasePriority();
    agv->setPriority("user", "");

    // 2. 切换建图模式
    agvc_interface::RunningMode running_mode = agvc_interface::RunningMode::MAPPING;
    agvc_interface::Header command_header{};
    command_header.id = "create_map";

    agv->changeRunningMode(running_mode, command_header);

    // 3. 判断是否成功切换建图模式
    while (true) {
        agvc_interface::AsyncInterfaceResultStatus mode_status = agv->getAsyncInterfaceResultStatus();

        if (mode_status.change_running_mode_status.status == agvc_interface::ChangeRunningModeStatus::FINISHED) {
            agv->changeRunningMode(running_mode, command_header);
            std::cout << "切换建图模式成功" << std::endl;
            break;
        }
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }
    std::cout << "===== 切换建图模式测试 end =====" << std::endl;
}

// 02_agv开环控制扫描地图
void MappingTest(agvc_interface::RpcClientPtr agv)
{
    std::cout << "\n===== AGV开环控制扫描地图 start =====" << std::endl;

    // 1. 开环控制让车前进 2s
    auto start = std::chrono::high_resolution_clock::now();
    while (true) {
        auto end = std::chrono::high_resolution_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::seconds>(end - start);
        if (duration.count() < 2) {
            agvc_interface::Speed speed;
            speed.v = 0.5;
            speed.w = 0.0;
            agv->setControlSpeed(speed);
        } else {
            start = std::chrono::high_resolution_clock::now();
            break;
        }
    }

    // 2. 开环控制让车旋转 2s
    while (true) {
        auto end = std::chrono::high_resolution_clock::now();
        auto duration = std::chrono::duration_cast<std::chrono::seconds>(end - start);
        if (duration.count() < 2) {
            agvc_interface::Speed speed;
            speed.v = 0.0;
            speed.w = 0.5;
            agv->setControlSpeed(speed);
        } else {
            agvc_interface::Speed speed_stop;
            speed_stop.v = 0;
            speed_stop.w = 0;
            agv->setControlSpeed(speed_stop);
            break;
        }
    }
    std::cout << "AGV开环控制完成" << std::endl;
    std::cout << "===== AGV开环控制扫描地图 end =====" << std::endl;
}

// 03_保存地图
void SaveMapTest(agvc_interface::RpcClientPtr agv)
{
    std::cout << "\n===== 保存地图测试 start =====" << std::endl;

    // 1. 设置保存地图信息，调用保存地图接口
    agvc_interface::Header map_header{};
    map_header.id     = "save_map_test";
    map_header.map_id = "map";
    map_header.name   = "test_map";

    agv->saveMap(map_header);

    // 2. 判断保存地图是否成功
    while (true) {
        agvc_interface::AsyncInterfaceResultStatus mode_status = agv->getAsyncInterfaceResultStatus();
        if (mode_status.save_map_status.status == agvc_interface::ChangeRunningModeStatus::FINISHED) {
            agv->saveMap(map_header);
            std::cout << "保存地图成功" << std::endl;
            break;
        }
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }
    std::cout << "===== 保存地图测试 end =====" << std::endl;
}

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

    // 1. 设置切换地图信息，调用切换地图接口
    agvc_interface::Header map_header{};
    map_header.id     = "switch_map_test";
    map_header.map_id = "map";

    agv->switchMap(map_header);

    // 2. 判断切换地图是否成功
    while (true) {
        agvc_interface::AsyncInterfaceResultStatus mode_status = agv->getAsyncInterfaceResultStatus();
        if (mode_status.switch_map_status.status == agvc_interface::ChangeRunningModeStatus::FINISHED) {
            agv->switchMap(map_header);
            std::cout << "切换地图成功" << std::endl;
            break;
        }
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }
    std::cout << "===== 切换地图测试 end =====" << std::endl;
}

int main()
{
    std::cout << "AGVC Mapping Test Start..." << 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");

    // 执行完整测试流程
    CreatMapTest(agv);    // 切换建图模式
    MappingTest(agv);     // AGV开环运动
    SaveMapTest(agv);     // 保存地图
    SwitchMapTest(agv);   // 切换地图

    return 0;
}
