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

#include "../src/utils/dynamics/include/robot.h"
#include "../src/utils/controller/include/cartesian_controller.h"

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

int main(int argc, char **argv) {
    auto rcfg  = LoadRobotFromURDF(
            "/home/xtkuang/projects/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
            /*root*/ "PELVIS_S");

    auto robot = std::make_shared<RobotT>(rcfg);

    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"
    };

    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"
    };

    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.02679378, 0.9625459, 2.0013871, 1.4209224, -2.552366, 0.34969908, 0.10842471;
    state->SetQ(q_init);
    robot->ComputeForwardKinematics(state);
    auto base_idx = robot->GetLinkIdx("PELVIS_S");
    auto target_idx_left = robot->GetLinkIdx("L_WRIST_R_S");
    auto target_idx_right = robot->GetLinkIdx("R_WRIST_R_S");
    auto T_l = robot->GetTransformation(state, base_idx, target_idx_left);
    auto T_r = robot->GetTransformation(state, base_idx, target_idx_right);
    std::cout << "q_init: " << std::endl;
    std::cout << "PELVIS_S -> L_WRIST_R_S:" << std::endl;
    std::cout << T_l << std::endl << std::endl;
    std::cout << "PELVIS_S -> R_WRIST_R_S:" << std::endl;
    std::cout << T_r << std::endl << std::endl;

    Eigen::Matrix4d T_target_left, T_target_right;
    Eigen::Vector<double, DOF> q_result;

    T_target_left <<
    0.      , 1.        , 0.        , 0.2,
    -1.     , 0.        , 0.0       , 0.3,
    0.      , 0.        , 1.        , -0.15,
    0,          0,          0,          1;

    T_target_right <<
    0.       , -1       , 0.       , 0.2      ,
    1.       , 0.       , 0.       , -0.3     ,
    0.       , 0.       , 1.       , -0.15    ,
    0.       , 0.       , 0.       , 1        ;

    ControllerT ctrl(robot);          // d_safe / lambda 用默认即可
    constexpr char BASE_LINK[] = "PELVIS_S";
    constexpr double DT = 0.002;      // 控制周期 2 ms
    std::vector<cmvr::ctrl::PoseTarget> targets = {
            // {"L_WRIST_R_S", T_target_left, 0.5, 1.0},
            {"R_WRIST_R_S", T_target_right, 0.5, 1.0}
    };

    /* --- 3.1 位置控制（输出关节位置） --- */
    {
        state->SetQ(q_init);
        robot->ComputeForwardKinematics(state);
        Eigen::Vector<double, DOF> q_cmd;
        bool ok = ctrl.compute(
                state, BASE_LINK, targets, DT,
                ControllerT::Mode::Position, q_cmd,  80, 1e-6);

        std::cout << "\n[Position] success=" << std::boolalpha << ok
                  << "\nq_cmd = " << q_cmd.transpose() << "\n";
        state->SetQ(q_cmd);
        robot->ComputeForwardKinematics(state);
        T_l = robot->GetTransformation(state, base_idx, target_idx_left);
        T_r = robot->GetTransformation(state, base_idx, target_idx_right);
        std::cout << "PELVIS_S -> L_WRIST_R_S:" << std::endl;
        std::cout << T_l << std::endl << std::endl;
        std::cout << "PELVIS_S -> R_WRIST_R_S" << ":" << std::endl;
        std::cout << T_r << std::endl << std::endl;
    }

    return 0;
}