#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 addStationsTest(agvc_interface::RpcClientPtr agv)
{
    std::cout << "\n===== 添加站点测试 start =====" << std::endl;

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

    // 3. 设置站点属性
    std::vector<agvc_interface::StationMark> stations;
    agvc_interface::StationMark station1, station2;

    station1.header.id     = "add_station_1"; // 站点的id
    station1.header.map_id = "current_map";   // 当前地图的id
    station1.header.name   = "ST1";           // 站点的名称
    station1.pose.x        = 0.0;
    station1.pose.y        = 0.0;
    station1.pose.yaw      = 0.0;
    station1.type          = "ST";
    stations.emplace_back(station1);

    station2.header.id     = "add_station_2";
    station2.header.map_id = "current_map";
    station2.header.name   = "ST2";
    station2.pose.x        = 1.0;
    station2.pose.y        = 0.0;
    station2.pose.yaw      = 0.0;
    station2.type          = "ST";
    stations.emplace_back(station2);

    // 4. 调用接口并判断结果
    int ret = agv->addStations(stations);
    if (ret == 10100000) {
        std::cout << "添加站点成功！" << std::endl;
    } else {
        std::cout << "添加站点失败，错误码：" << ret << std::endl;
    }

    std::cout << "===== 添加站点测试 end =====" << std::endl;
}

// 添加路径（正确流程）
void addPathTest(agvc_interface::RpcClientPtr agv)
{
    std::cout << "\n===== 添加路径测试 start =====" << std::endl;

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

    // 3. 设置路径属性
    agvc_interface::PathStation path{};
    path.header.id        = "new_path_1-2";
    path.header.map_id    = "current_map";
    path.header.name      = "path1-2";
    path.start_station_id = "add_station_1";
    path.end_station_id   = "add_station_2";
    path.shape            = agvc_interface::PathShape::LINE;
    path.use_direction    = false;
    path.max_speed        = 1.0;

    // 4. 调用接口并判断结果
    int ret = agv->generatePath(path);
    if (ret == 10100000) {
        std::cout << "添加路径成功！" << std::endl;
    } else {
        std::cout << "添加路径失败，错误码：" << ret << std::endl;
    }

    std::cout << "===== 添加路径测试 end =====" << std::endl;
}

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

    // 执行测试
    addStationsTest(agv);  // 添加站点
    addPathTest(agv);     // 添加路径

    return 0;
}
