#include "aubo_sdk/rpc.h"
#include "aubo_sdk/rtde.h"

#include <robot_math/robot_math.h>

#include <array>
#include <atomic>
#include <chrono>
#include <cmath>
#include <iostream>
#include <mutex>
#include <stdexcept>
#include <string>
#include <thread>
#include <vector>
#include "skill_interface/tcp_auto_calib.h"

#ifdef WIN32
#include <Windows.h>
#endif

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

using namespace arcs::common_interface;
using namespace arcs::aubo_sdk;

constexpr const char *kRobotIp = "192.168.6.11";
constexpr int kRobotRpcPort = 30004;
constexpr int kRobotRtdePort = 30010;
constexpr const char *kUser = "aubo";
constexpr const char *kPassword = "123456";

void printPose(const std::vector<double> &v)
{
    std::cout << "[ ";
    for (int i = 0; i < v.size(); i++) {
        std::cout << v[i] << ", ";
    }
    std::cout << " ]" << std::endl;
}

int main(int argc, char **argv)
{
    (void)argc;
    (void)argv;
#ifdef WIN32
    SetConsoleOutputCP(CP_UTF8);
#endif
    auto rpc = std::make_shared<RpcClient>();
    auto rtde_cli = std::make_shared<RtdeClient>();
    int task_id = -1;

    try {
        rpc->setRequestTimeout(5000);
        rpc->connect(kRobotIp, kRobotRpcPort);
        if (!rpc->hasConnected()) {
            std::cerr << "RPC connect failed" << std::endl;
            return 1;
        }
        rpc->login(kUser, kPassword);
        if (!rpc->hasLogined()) {
            std::cerr << "RPC login failed" << std::endl;
            return 1;
        }

        // 接口调用: 连接到 RTDE 服务
        rtde_cli->connect(kRobotIp, kRobotRtdePort);
        // 接口调用: 登录
        rtde_cli->login(kUser, kPassword);

        TcpAutoCalib tcp_auto_calib(rpc, rtde_cli);

        auto result = tcp_auto_calib.calibrateSensorPoseWithTCP(1, 3);
        if (std::get<1>(result) != 0) {
            std::cout << "通过TCP标定传感器位置失败." << std::endl;
        } else {
            std::cout << "sensor_pose:";
            printPose(std::get<0>(result));
        }

        //        std::vector sensor_pose = { 0.460921, 0.340546,   -0.0225517,
        //                                    3.08853,  -0.0197323, 2.40585 };
        //        auto result = tcp_auto_calib.tcpAutoCorrect(sensor_pose, 1,
        //        3); if (std::get<1>(result) != 0) {
        //            std::cout << "TCP纠偏失败." << std::endl;
        //        } else {
        //            std::cout << "新的TCP:";
        //            printPose(std::get<0>(result));
        //        }

        //        std::vector<std::vector<double>> poses;
        //        poses.push_back({ 0.4235989107595406, 0.3452219112901429,
        //                          0.13967565904508186, 3.0539148448644116,
        //                          0.2100639154923824, 2.4347151266751053 });
        //        poses.push_back({ 0.4765796729520416, 0.301614894612744,
        //                          0.1385872008626541, 3.1146824345782496,
        //                          -0.25948717801437626, 2.2340917493450623 });
        //        poses.push_back({ 0.5070082903984907, 0.33130968420657214,
        //                          0.1375007157543185, -2.864135841287073,
        //                          -0.23139670383797054, 2.164867989348182 });
        //        poses.push_back({ 0.4466712480162999, 0.39461406695518225,
        //                          0.1305209962255966, -2.8395321705222543,
        //                          0.2691619191333885, 2.561952466505771 });

        //        auto result = tcp_auto_calib.tcpAutoCalibrate(poses, 1, 3);
        //        if (std::get<1>(result) != 0) {
        //            std::cout << "TCP自动标定失败." << std::endl;
        //        } else {
        //            std::cout << "TCP标定结果:";
        //            printPose(std::get<0>(result));
        //        }

        rtde_cli->logout();
        rtde_cli->disconnect();

        rpc->logout();
        rpc->disconnect();
    } catch (const std::exception &e) {
        std::cerr << "Exception: " << e.what() << std::endl;
        return 1;
    }
    return 0;
}
