#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"

// 获取AGV信息打印
void getAgvInfoTest(agvc_interface::RpcClientPtr agv)
{
    std::cout << "\n===== 获取AGV信息测试 start =====" << std::endl;

    // 1. 获取运行信息
    agvc_interface::RunningInfo run_info = agv->getRunningInfo();

    std::cout << "------------------ running info ------------------" << std::endl;
    std::cout << "sn                 : " << run_info.header.id << std::endl;
    std::cout << "name               : " << run_info.header.name << std::endl;
    std::cout << "batter_current     : " << run_info.battery_current << std::endl;
    std::cout << "battery_voltage    : " << run_info.battery_voltage << std::endl;
    std::cout << "current_dist       : " << run_info.current_dist << std::endl;
    std::cout << "current_speed v    : " << run_info.current_speed.v << std::endl;
    std::cout << "current_speed w    : " << run_info.current_speed.w << std::endl;
    std::cout << "map_id             : " << run_info.header.map_id << std::endl;
    std::cout << "location_score     : " << run_info.location_score << std::endl;
    std::cout << "total_dist         : " << run_info.total_dist << std::endl;
    std::cout << "total_running_time : " << run_info.total_running_time << std::endl;
    std::cout << "low_battery        : " << run_info.low_battery << std::endl;
    std::cout << "pause              : " << run_info.pause << std::endl;
    std::cout << "emergency_stop     : " << run_info.emergency_stop << std::endl;
    std::cout << "blocked            : " << run_info.blocked << std::endl;
    std::cout << "localization_loss  : " << run_info.localization_loss << std::endl;
    std::cout << "fault_alarm        : " << run_info.fault_alarm << std::endl;
    std::cout << "---------------------------------------------------" << std::endl;

    // 2. 获取AGV详细信息
    auto agv_details = agv->getAgvDetails();
    std::cout << "---------------- firmware version -----------------" << std::endl;
    std::cout << "hardware_abstraction_version  : " << agv_details.hardware_abstraction_version << std::endl;
    std::cout << "left_motor_firmware_version   : " << agv_details.left_motor_firmware_version << std::endl;
    std::cout << "right_motor_firmware_version  : " << agv_details.right_motor_firmware_version << std::endl;
    std::cout << "master_board_firmware_version : " << agv_details.master_board_firmware_version << std::endl;
    std::cout << "battery_version               : " << agv_details.battery_version << std::endl;
    std::cout << "communition version           : " << agv_details.communication_version << std::endl;
    std::cout << "loop_times                    : " << agv_details.loop_times << std::endl;
    std::cout << "surplus_capacity              : " << agv_details.surplus_capacity << std::endl;
    std::cout << "max_speed                     : " << agv_details.max_speed << std::endl;
    std::cout << "MFD                           : " << agv_details.MFD << std::endl;
    std::cout << "name                          : " << agv_details.name << std::endl;
    std::cout << "SN                            : " << agv_details.SN << std::endl;
    std::cout << "version                       : " << agv_details.version << std::endl;
    std::cout << "---------------------------------------------------" << std::endl;

    std::cout << "===== 获取AGV信息测试 end =====" << std::endl;
}

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

    // 获取AGV信息
    getAgvInfoTest(agv);

    return 0;
}
