//
// Created by xtkuang on 2025/7/31.
//

#include "utils/dynamics/robot.h"
#include "utils/controller/cartesian_controller.h"
#include <Eigen/Dense>
#include <iostream>
#include <memory>
#include <string>
#include <vector>
#include <cmath>
#include <glog/logging.h>

constexpr int DOF = 14;
using RobotT = cmvr::dyn::Robot<DOF>;
using ControllerT = cmvr::ctrl::CartesianController<DOF>;

// 将欧拉角(rx, ry, rz)转为旋转矩阵，旋转顺序 Z→Y→X
Eigen::Matrix3d eulerToRotationMatrix(double rx, double ry, double rz)
{
    Eigen::Matrix3d R_x;
    R_x << 1, 0, 0,
           0, cos(rx), -sin(rx),
           0, sin(rx), cos(rx);

    Eigen::Matrix3d R_y;
    R_y << cos(ry), 0, sin(ry),
           0, 1, 0,
          -sin(ry), 0, cos(ry);

    Eigen::Matrix3d R_z;
    R_z << cos(rz), -sin(rz), 0,
           sin(rz), cos(rz), 0,
           0, 0, 1;

    // ✅ 按照输入顺序 X → Y → Z 旋转
    return R_x * R_y * R_z;
}

int main(int argc, char **argv)
{
    if (argc != 9)
    {
        std::cerr << "用法: " << argv[0] << " <base link> <target link> <x> <y> <z> <rx> <ry> <rz>\n";
        return -1;
    }

    std::vector<std::string> link_names = {
        "PELVIS_S",
        "L_SHOULDER_P_S", "L_SHOULDER_R_S", "L_SHOULDER_Y_S", "L_ELBOW_R_S", "L_WRIST_P_S", "L_WRIST_Y_S", "L_WRIST_R_S",
        "R_SHOULDER_P_S", "R_SHOULDER_R_S", "R_SHOULDER_Y_S", "R_ELBOW_R_S", "R_WRIST_P_S", "R_WRIST_Y_S", "R_WRIST_R_S", "R_CAM", "R_FINGER_TIP"
    };

    std::vector<std::string> joint_names = {
        "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",
        "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R",
    };

    std::string BASE_LINK = argv[1];
    std::string target_link = argv[2];
    bool find_base_link = false;
    bool find_target_link = false;
    for (auto link_name : link_names) {
        if (BASE_LINK == link_name) {
            find_base_link = true;
        }
        if (target_link == link_name) {
            find_target_link = true;
        }
    }
    if (!find_base_link) {
        std::cerr << "base link 无效" << std::endl;
        return -1;
    }
    if (!find_target_link) {
        std::cerr << "target link 无效" << std::endl;
        return -1;
    }

    double x = std::stod(argv[3]);
    double y = std::stod(argv[4]);
    double z = std::stod(argv[5]);
    double rx = std::stod(argv[6]) * M_PI / 180.0;
    double ry = std::stod(argv[7]) * M_PI / 180.0;
    double rz = std::stod(argv[8]) * M_PI / 180.0;

    // 1. 加载机器人模型
    auto rcfg = LoadRobotFromURDF(
        "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
        /*root*/ "PELVIS_S");
    auto robot = std::make_shared<RobotT>(rcfg);

    auto state = robot->MakeState(link_names, joint_names);

    // 初始关节角度
    Eigen::Vector<double, DOF> q_init;
    q_init << -0.3647738137, -1.4556045962, 1.6057029118, -1.8483036779, 2.9024825461, 0.1850049007, -0.5602506899,
             -0.23905230057945082, 1.2838583858530206, 1.8486727698400174, 1.3036511045462158, -2.824727848601091, -0.02012495463116552, -0.202540520242054;
    state->SetQ(q_init);
    robot->ComputeForwardKinematics(state);

    // 2. 构造目标位姿矩阵
    Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity();
    T_target.block<3,3>(0,0) = eulerToRotationMatrix(rx, ry, rz);  // 输入为弧度
    T_target(0,3) = x;
    T_target(1,3) = y;
    T_target(2,3) = z;

    // 3. 创建控制器
    ControllerT ctrl(robot);
    constexpr double DT = 0.002; // 2ms

    std::vector<cmvr::ctrl::PoseTarget> targets = {
        {target_link, T_target, 0.5, 1.0}
    };

    Eigen::Vector<double, DOF> q_cmd;
    bool ok = ctrl.compute(state, BASE_LINK, targets, DT, ControllerT::Mode::Position, q_cmd, 10000, 1e-6);

    // ✅ 格式化输出
    std::cout << "[IK Solve] success=" << std::boolalpha << ok << "\n";
    std::cout << "left:  ";
    for(int i=0; i<7; ++i) std::cout << q_cmd[i] << (i<6?" ":"");
    std::cout << "\nright: ";
    for(int i=7; i<14; ++i) std::cout << q_cmd[i] << (i<13?" ":"");
    std::cout << std::endl;

    return 0;
}