//
// Created by lgv on 2025/8/4.
//


// #include "humanoid_robot.h"

#include "gtest/gtest.h"
#include <glog/logging.h>
#include "device_manager/device_manager.h"
#include <libgen.h>

#include "cmvr/msgs/can_card_parameter.grpc.pb.h"
#include "cmvr/msgs/geometry.pb.h"
#include "robot/humanoid_robot/humanoid_robot.h"

// 定义一个命令行参数 --config_path
DEFINE_string(config_path, "../config/cabin_robot.xml", "Path to the robot config XML file");
using namespace cmvr::device;
TEST(HumanoidRobotTest,MyRobotTest) {

    std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
    const XmlNode config(config_path);

    if (!config.hasChild("DeviceManager")){
        LOG(ERROR) << "Device Manager node not found";
    }
    auto dmgr_cfg = config.getChild("DeviceManager");
    auto &dmgr = DeviceManager::getInstance(dmgr_cfg);

    auto robot = dmgr.getDevice<AbstractRobot>("hc01");

    std::vector<JointPoint> cmd{};

    std::vector<std::vector<JointPoint>>  traj;

//    robot->calibrateZeroQ("R_WRIST_P");
    robot->calibrateZeroQ("R_WRIST_Y");
//    robot->calibrateZeroQ("R_WRIST_R");
     //
     cmd = {
         // {"L_SHOULDER_P", 0.0},
         // {"L_SHOULDER_R", 0.0},
         // {"L_SHOULDER_Y", 0.0},
         // {"L_ELBOW_R",    0.0},
         // {"L_WRIST_P",    0.0},
         // {"L_WRIST_Y",    0.0},
         // {"L_WRIST_R",    0.0},
         //
         // {"R_SHOULDER_P", 0.0},
         // {"R_SHOULDER_R", 0.0},
         // {"R_SHOULDER_Y", 0.0},
         // {"R_ELBOW_R",    0.0},
         // {"R_WRIST_P",    0.0},
         // {"R_WRIST_Y",    0.0},
         // {"R_WRIST_R",    0.0},

         {"WAIST_P" ,0.0},
         {"WAIST_Y" ,0.0},
     };
     robot->moveJ(cmd,0.8);


    // cmd = {
    //     {"L_SHOULDER_P", 0.0},
    //     {"L_SHOULDER_R", -1.31873},
    //     {"L_SHOULDER_Y", 0.0},
    //     {"L_ELBOW_R",    -0.537621},
    //     {"L_WRIST_P",    0.0},
    //     {"L_WRIST_Y",    0.000183204},
    //     {"L_WRIST_R",    0.0225797},
    //
    //     {"R_SHOULDER_P", 0.00203898},
    //     {"R_SHOULDER_R", 1.34062},
    //     {"R_SHOULDER_Y", 0.0},
    //     {"R_ELBOW_R",    0.522261},
    //     {"R_WRIST_P",    0.0},
    //     {"R_WRIST_Y",    -0.000210733},
    //     {"R_WRIST_R",    -0.0942364}
    // };

    // robot->moveJ(cmd,0.8);

    // std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    // robot->eStop();

    // robot->torqueOff();


    while (true) {

        // cmd = {{"L_SHOULDER_P",3.14},{"L_SHOULDER_R",3.14},{"L_SHOULDER_Y",3.14}};
        // robot->moveJ(cmd,1.0);
        // std::this_thread::sleep_for(std::chrono::milliseconds(8000));
        // cmd = {{"L_SHOULDER_P",-3.14},{"L_SHOULDER_R",-3.14},{"L_SHOULDER_Y",-3.14}};
        // robot->moveJ(cmd,1.0);
        // std::this_thread::sleep_for(std::chrono::milliseconds(8000));

        // cmd = {{"L_SHOULDER_R",3.14}};
        // robot->moveJ(cmd,0.7);
        // // std::this_thread::sleep_for(std::chrono::milliseconds(10000));
        // cmd = {{"L_SHOULDER_R",-3.14}};
        // robot->moveJ(cmd,0.7);
        std::this_thread::sleep_for(std::chrono::milliseconds(10000));
    }
}


TEST(HumanoidRobotTest,FollowJointTrajectoryTest) {
    std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
    const XmlNode config(config_path);

    if (!config.hasChild("DeviceManager")){
        LOG(ERROR) << "Device Manager node not found";
    }
    auto dmgr_cfg = config.getChild("DeviceManager");
    auto &dmgr = DeviceManager::getInstance(dmgr_cfg);

    auto robot = dmgr.getDevice<AbstractRobot>("hc01");


    // 读取轨迹
    std::ifstream file("/home/lgv/cmvr/cmvr-es/src/devices/robot/humanoid_robot/joint_positions_1.csv");
    if (!file.is_open()) {
        LOG(ERROR) << "无法打开文件" ;

    }

    std::string line;

    // 读取标题行，获取关节名称（除去第一列 idx）
    if (!std::getline(file, line)) {
        LOG(ERROR) << "文件为空或格式错误" ;
    }

    std::vector<std::string> joint_names;
    {
        std::stringstream ss(line);
        std::string cell;
        // 第一列是 idx，跳过
        std::getline(ss, cell, ',');
        // 读取关节名称列
        while (std::getline(ss, cell, ',')) {
            joint_names.push_back(cell);
        }
    }

    std::vector<std::vector<JointPoint>> traj;

    // 读取后续每行数据
    while (std::getline(file, line)) {
        std::stringstream ss(line);
        std::string cell;

        // 读取第一列 idx，暂时不使用
        std::getline(ss, cell, ',');

        std::vector<JointPoint> joints;

        // 读取每个关节角度
        for (size_t i = 0; i < joint_names.size(); ++i) {
            if (!std::getline(ss, cell, ',')) {
                LOG(ERROR) << "数据列不足，格式错误";
            }
            double angle = std::stod(cell);  // 字符串转 double

            JointPoint cmd;
            cmd.joint_name = joint_names[i];
            cmd.rad = angle;
            joints.push_back(cmd);
        }
        traj.push_back(joints);
    }

    file.close();


    // 计算速度
    double dt = 0.01; // 采样周期(s)
    size_t N = traj.size();

    for (size_t i = 0; i < N; ++i) {
        if (i == 0) {
            // 第一个点没有前一帧，速度设为 0
            for (auto &cmd : traj[i]) {
                cmd.vel = 0.0;
            }
        } else {
            // 后向差分
            for (size_t j = 0; j < traj[i].size(); ++j) {
                double pos_prev = traj[i - 1][j].rad;
                double pos_curr = traj[i][j].rad;
                traj[i][j].vel =  (pos_curr - pos_prev) / dt;
            }
        }
    }



    // // 测试打印读取结果
    // for (size_t i = 0; i < traj.size(); ++i) {
    //     std::cout << "Index " << i << ":" << std::endl;
    //     for (const auto& cmd : traj[i]) {
    //         std::cout << cmd.joint_name
    //             << " = " << cmd.rad << " rad, "
    //             << cmd.vel << " rad/s; ";
    //     }
    //     std::cout << std::endl;
    // }


    // 1：


    //
    // std::vector<JointPoint> cmd{};
    // cmd = {
    //     // {"L_SHOULDER_P", 0.0},
    //     // {"L_SHOULDER_R", 0.0},
    //     // {"L_SHOULDER_Y", 0.0},
    //     // {"L_ELBOW_R",    0.0},
    //     // {"L_WRIST_P",    0.0},
    //     // {"L_WRIST_Y",    0.0},
    //     // {"L_WRIST_R",    0.0},
    //
    //     {"R_SHOULDER_P", 0.0},
    //     {"R_SHOULDER_R", 0.0},
    //     {"R_SHOULDER_Y", 0.0},
    //     {"R_ELBOW_R",    0.0},
    //     {"R_WRIST_P",    0.0},
    //     {"R_WRIST_Y",    0.0},
    //     {"R_WRIST_R",    0.0}
    // };
    // robot->moveJ(cmd,0.8);

    // // // 先到达轨迹起点
    // LOG(INFO) << "Moving to trajectory start...";
    // robot->moveJ(traj[0],0.8);
    //
    //
    //
    // // 再移动
    // LOG(INFO) << "Reached trajectory start point";
    // robot->followJointTrajectory(traj,dt * 1000);
    // LOG(INFO) << "Trajectory execution completed";



    // std::vector<JointPoint> cmd1{
    //     {"L_SHOULDER_P", -0.747573},
    //     {"L_SHOULDER_R", -1.26911},
    //     {"L_SHOULDER_Y", -1.20811},
    //     {"L_ELBOW_R",   -1.51221},
    //     {"L_WRIST_P",    2.64099},
    //     {"L_WRIST_Y",    0.417608},
    //     {"L_WRIST_R",   -0.518287},
    //
    //     {"R_SHOULDER_P", -0.344938},
    //     {"R_SHOULDER_R",  0.935147},
    //     {"R_SHOULDER_Y",  2.27031},
    //     {"R_ELBOW_R",     1.68959},
    //     {"R_WRIST_P",    -2.32841},
    //     {"R_WRIST_Y",     0.460145},
    //     {"R_WRIST_R",     0.300996}
    // };
    //
    // std::vector<JointPoint> cmd2{
    //     {"L_SHOULDER_P", -0.747573},
    //     {"L_SHOULDER_R", -1.26911},
    //     {"L_SHOULDER_Y", -1.20811},
    //     {"L_ELBOW_R",   -1.51221},
    //     {"L_WRIST_P",    2.64099},
    //     {"L_WRIST_Y",    0.417608},
    //     {"L_WRIST_R",   -0.518287},
    //
    //     {"R_SHOULDER_P",  0.239368},
    //     {"R_SHOULDER_R",  0.871341},
    //     {"R_SHOULDER_Y",  1.86052},
    //     {"R_ELBOW_R",     1.28044},
    //     {"R_WRIST_P",    -2.49436},
    //     {"R_WRIST_Y",     0.404731},
    //     {"R_WRIST_R",     0.280016}
    // };


    std::vector<JointPoint> cmd1{
        {"L_SHOULDER_P", -0.747573},
        {"L_SHOULDER_R", -1.26911},
        {"L_SHOULDER_Y", -1.20811},
        {"L_ELBOW_R",   -1.51221},
        {"L_WRIST_P",    2.64099},
        {"L_WRIST_Y",    0.417608},
        {"L_WRIST_R",   -0.518287},

        {"R_SHOULDER_P", -0.956276},
        {"R_SHOULDER_R",  1.0244},
        {"R_SHOULDER_Y",  2.70621},
        {"R_ELBOW_R",     2.02276},
        {"R_WRIST_P",    -1.99653},
        {"R_WRIST_Y",     0.68523},
        {"R_WRIST_R",     0.477066}
    };

    std::vector<JointPoint> cmd2{
        {"L_SHOULDER_P", -0.747573},
        {"L_SHOULDER_R", -1.26911},
        {"L_SHOULDER_Y", -1.20811},
        {"L_ELBOW_R",   -1.51221},
        {"L_WRIST_P",    2.64099},
        {"L_WRIST_Y",    0.417608},
        {"L_WRIST_R",   -0.518287},

        {"R_SHOULDER_P",  0.226871},
        {"R_SHOULDER_R",  0.624717},
        {"R_SHOULDER_Y",  1.15086},
        {"R_ELBOW_R",     1.30365},
        {"R_WRIST_P",    -2.14683},
        {"R_WRIST_Y",     0.196003},
        {"R_WRIST_R",     0.106678}
    };



    while (true) {
        robot->moveJ(cmd1,0.8);
        robot->moveJ(cmd2,0.8);
        std::this_thread::sleep_for(std::chrono::milliseconds(500));
    }

}


TEST(HumanoidRobotTest,MoveDeltaTest) {
    std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
    const XmlNode config(config_path);

    if (!config.hasChild("DeviceManager")){
        LOG(ERROR) << "Device Manager node not found";
    }
    auto dmgr_cfg = config.getChild("DeviceManager");
    auto &dmgr = DeviceManager::getInstance(dmgr_cfg);

    auto robot = dmgr.getDevice<AbstractRobot>("hc01");

    cmvr::msgs::Pose3d pose;
    pose.mutable_position()->set_x( 0);
    pose.mutable_position()->set_y(0);
    pose.mutable_position()->set_z(0.03);

    pose.mutable_euler()->set_rx(0);
    pose.mutable_euler()->set_ry(0);
    pose.mutable_euler()->set_rz(0);

    robot->moveDeltaJ("PELVIS_S","R_WRIST_R_S",pose);



}

TEST(HumanoidRobotTest,AngleToTest) {
    double rx = M_PI / 6;  // 30°
    double ry = M_PI / 4;  // 45°
    double rz = M_PI / 3;  // 60°

    Eigen::Matrix3d R = HumanoidRobot<7>::eulerZYXToRotationMatrix(rx, ry, rz);
    Eigen::Vector3d euler = HumanoidRobot<7>::rotationMatrixToEulerZYX(R);

    std::cout << "Original Euler angles (rad):\n" << Eigen::Vector3d(rx, ry, rz).transpose() << "\n";
    std::cout << "Recovered Euler angles (rad):\n" << euler.transpose() << "\n";
}


TEST(HumanoidRobotTest,MoveIKTest) {
    std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
    const XmlNode config(config_path);

    if (!config.hasChild("DeviceManager")){
        LOG(ERROR) << "Device Manager node not found";
    }
    auto dmgr_cfg = config.getChild("DeviceManager");
    auto &dmgr = DeviceManager::getInstance(dmgr_cfg);

    auto robot = dmgr.getDevice<AbstractRobot>("hc01");


    std::vector<JointPoint> cmd{
                    {"R_SHOULDER_P",  -0.08882},
                    {"R_SHOULDER_R",  1.50297},
                    {"R_SHOULDER_Y",  1.85922},
                    {"R_ELBOW_R",     0.214816},
                    {"R_WRIST_P",    -1.69504},
                    {"R_WRIST_Y",     0.0363855},
                    {"R_WRIST_R",     0.0519807}
    };


    robot->moveJ(cmd);


    // //
    // cmvr::ctrl::PoseTarget target;
    // std::vector<cmvr::ctrl::PoseTarget> targets;
    //
    //
    // cmvr::msgs::Pose3d pose;
    //
    // pose.mutable_position()->set_x( 1.49969573e-01);
    // pose.mutable_position()->set_y(-4.00100001e-01);
    // pose.mutable_position()->set_z(-1.00102800e-01);
    //
    // pose.mutable_euler()->set_rx(0);
    // pose.mutable_euler()->set_ry(0);
    // pose.mutable_euler()->set_rz(1.57);
    //
    //
    //
    // while (true) {
    //     robot->moveJ("PELVIS_S","R_WRIST_R_S",pose);
    //
    //
    //
    //     std::this_thread::sleep_for(std::chrono::milliseconds(2000));
    //
    //
    //     robot->moveJ(cmd);
    //     std::this_thread::sleep_for(std::chrono::milliseconds(2000));
    // }

}