#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 setNavGoalTest(agvc_interface::RpcClientPtr agv)
{
    std::cout << "\n===== 导航任务测试 start =====" << std::endl;

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

    // ==================== 第一段导航任务 ====================
    // 3. 设置导航任务参数
    agvc_interface::NavGoalType nav_goal{};
    nav_goal.header.id       = "test_nav_goal_1";
    nav_goal.header.map_id   = "current_map";
    nav_goal.type            = agvc_interface::NavType::PATH_TO_STATION;
    nav_goal.forward_flag    = false;
    nav_goal.goal_end_rotate = false;
    nav_goal.max_vel.v       = 0.5;
    nav_goal.stations_id.emplace_back("add_station_2");
    nav_goal.stations_id.emplace_back("add_station_1");

    // 4. 下发导航任务
    int ret = agv->setNavGoal(nav_goal);
    if (ret == 10100000) {
        std::cout << "第一段导航任务下发成功" << std::endl;
    } else {
        std::cout << "第一段导航任务下发失败，错误码：" << ret << std::endl;
        return;
    }
    sleep(1);

    // 5. 循环判断任务状态
    while (true) {
        auto nav_info = agv->getNavInfo();
        if (nav_info.status == agvc_interface::NavStatus::FINISHED) {
            std::cout << " nav back finish!" << std::endl;
            break;
        } else if (nav_info.status == agvc_interface::NavStatus::FAILED) {
            std::cout << " nav back failed!" << std::endl;
            break;
        } else if (nav_info.status == agvc_interface::NavStatus::RUNNING) {
            std::cout << " nav back running!" << std::endl;
        }
        sleep(1);
    }

    // ==================== 第二段导航任务 ====================
    // 6. 设置第二段导航任务参数
    nav_goal.header.id       = "test_nav_goal_2";
    nav_goal.header.map_id   = "current_map";
    nav_goal.type            = agvc_interface::NavType::PATH_TO_STATION;
    nav_goal.forward_flag    = true;
    nav_goal.goal_end_rotate = false;
    nav_goal.max_vel.v       = 0.5;
    nav_goal.stations_id.clear();
    nav_goal.stations_id.emplace_back("add_station_1");
    nav_goal.stations_id.emplace_back("add_station_2");

    // 7. 下发导航任务
    ret = agv->setNavGoal(nav_goal);
    if (ret == 10100000) {
        std::cout << "第二段导航任务下发成功" << std::endl;
    } else {
        std::cout << "第二段导航任务下发失败，错误码：" << ret << std::endl;
        return;
    }
    sleep(1);

    // 8. 循环判断任务状态
    while (true) {
        auto nav_info = agv->getNavInfo();
        if (nav_info.status == agvc_interface::NavStatus::FINISHED) {
            std::cout << " nav finish!" << std::endl;
            break;
        } else if (nav_info.status == agvc_interface::NavStatus::FAILED) {
            std::cout << " nav failed!" << std::endl;
            break;
        } else if (nav_info.status == agvc_interface::NavStatus::RUNNING) {
            std::cout << " nav running!" << std::endl;
        }
        sleep(1);
    }

    std::cout << "===== 导航任务测试 end =====" << std::endl;
}

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

    // 执行导航任务测试
    setNavGoalTest(agv);

    return 0;
}
