#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 addPathTest(agvc_interface::RpcClientPtr agv)
{

    // 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;
    }
}

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

    // 添加路径
    addPathTest(agv);

    return 0;
}
