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

#include "humanoid_robot.h"
#include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h"
#include "motor/ti5_motor/ti5_motor.h"


using namespace std;
using namespace cmvr::device;

template<int DOF>
HumanoidRobot<DOF>::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) {
    try {
        id_ = cfg.getAttrString("id");
        dof_ = DOF;
        if (!pathExists(cfg.getAttrString("urdf"))) {
            throw runtime_error("urdf file does not exist");
        }
        auto rcfg = cmvr::dyn::LoadRobotFromURDF(
            cfg.getAttrString("urdf"), cfg.getAttrString("baseLink"));
        m_robot_ = std::make_shared<cmvr::dyn::Robot<DOF> >(rcfg);
        joint_names_ = splitString(cfg.getAttrString("jointNames"), ",");
        link_names_ = splitString(cfg.getAttrString("linkNames"), ",");
        if (joint_names_.size() != dof_) {
            throw runtime_error("joint names size mismatched with dof");
        }

        m_state_ = m_robot_->MakeState(link_names_, joint_names_);
        m_cctrl_ = make_shared<ctrl::CartesianController<DOF> >(m_robot_);
        upd_freq_ = cfg.getAttrDefault("updFreq", 500);
        CSP_buffer_ = make_shared<SPMCRingBuffer<JointPoint> >(cfg.getAttrDefault("bufferSize", 50));
        CSV_buffer_ = make_shared<SPMCRingBuffer<JointVelocityCommand> >(cfg.getAttrDefault("bufferSize", 50));
        CSC_buffer_ = make_shared<SPMCRingBuffer<JointCurrentCommand> >(cfg.getAttrDefault("bufferSize", 50));


        auto can_cfg = cfg.getChild("CanManger");

        auto l_can_cfg = can_cfg.getChild("LeftArmCan");
        l_motors_cfg_ = l_can_cfg.getChildren("Motor");
        l_can_client_ = std::make_shared<SocketCanClientRaw>(l_can_cfg);
        l_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
        l_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
        l_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();


        auto r_can_cfg = can_cfg.getChild("RightArmCan");
        r_motors_cfg_ = r_can_cfg.getChildren("Motor");
        r_can_client_ = std::make_shared<SocketCanClientRaw>(r_can_cfg);
        r_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
        r_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
        r_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();


        auto waist_can_cfg = can_cfg.getChild("WaistCan");
        waist_motors_cfg_ = waist_can_cfg.getChildren("Motor");
        waist_can_client_ = std::make_shared<SocketCanClientRaw>(waist_can_cfg);
        waist_can_sender_ = std::make_shared<CanSender<msgs::RobotDetail> >();
        waist_can_receiver_ = std::make_shared<CanReceiver<msgs::RobotDetail> >();
        waist_message_manager_ = std::make_shared<MessageManager<msgs::RobotDetail> >();

        upd_timer_ = make_shared<FDTimer>();
        upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000),
                          [this] { update_state_(); });
        rsm_.store(ROBOT_READY);
    } catch (exception &e) {
        LOG(ERROR) << "HumanoidRobot init failed, id=" << id_;
        throw runtime_error(e.what());
    }
}

template<int DOF>
void HumanoidRobot<DOF>::init() {
    // 1 === 初始化公共组件 ===
    l_can_client_->init();
    r_can_client_->init();
    waist_can_client_->init();
    auto ret = l_can_sender_->Init(l_can_client_.get(), false);
    if (ret != ErrorCode::OK) {
        LOG(ERROR) << "Failed to init can sender.";
    }
    ret = r_can_sender_->Init(r_can_client_.get(), false);
    if (ret != ErrorCode::OK) {
        LOG(ERROR) << "Failed to init can sender.";
    }
    ret = waist_can_sender_->Init(waist_can_client_.get(), false);
    if (ret != ErrorCode::OK) {
        LOG(ERROR) << "Failed to init can sender.";
    }


    ret = l_can_receiver_->Init(l_can_client_.get(), l_message_manager_.get(), false);
    if (ret != ErrorCode::OK) {
        LOG(ERROR) << "Failed to init can receiver.";
    }
    ret = r_can_receiver_->Init(r_can_client_.get(), r_message_manager_.get(), false);
    if (ret != ErrorCode::OK) {
        LOG(ERROR) << "Failed to init can receiver.";
    }
    ret = waist_can_receiver_->Init(waist_can_client_.get(), waist_message_manager_.get(), false);
    if (ret != ErrorCode::OK) {
        LOG(ERROR) << "Failed to init can receiver.";
    }


    // 2 ===  启动通讯 ===
    l_can_client_->start();
    ret = l_can_sender_->Start();
    if (ret != ErrorCode::OK) {
        LOG(ERROR) << "Failed to start can sender.";
    }

    r_can_client_->start();
    ret = r_can_sender_->Start();
    if (ret != ErrorCode::OK) {
        LOG(ERROR) << "Failed to start can sender.";
    }

    waist_can_client_->start();
    ret = waist_can_sender_->Start();
    if (ret != ErrorCode::OK) {
        LOG(ERROR) << "Failed to start can sender.";
    }

    ret = l_can_receiver_->Start();
    if (ret != ErrorCode::OK) {
        LOG(ERROR) << "Failed to start can receiver.";
    }

    ret = r_can_receiver_->Start();
    if (ret != ErrorCode::OK) {
        LOG(ERROR) << "Failed to start can receiver.";
    }

    ret = waist_can_receiver_->Start();
    if (ret != ErrorCode::OK) {
        LOG(ERROR) << "Failed to start can receiver.";
    }

    // 3 == 创建协议 ===
    auto l_canopen_protocol = std::make_shared<Ti5MotorCanopenProtocol>(l_can_sender_, l_message_manager_);
    auto r_canopen_protocol = std::make_shared<Ti5MotorCanopenProtocol>(r_can_sender_, r_message_manager_);
    auto waist_canopen_protocol = std::make_shared<Ti5MotorCanopenProtocol>(waist_can_sender_, waist_message_manager_);

    // 4 === 创建  MotorManager ===
    motor_manager_ = std::make_shared<MotorManager>();

    // for (const auto& cfg : r_motors_cfg_) {
    //     auto motor = std::make_shared<Ti5Motor>(cfg);
    //     motor->setProtocol(r_canopen_protocol);
    //     motor->init();  // 耗时操作
    //     motor_manager_->addMotor(motor);
    // }
    //
    // for (const auto& cfg : l_motors_cfg_) {
    //     auto motor = std::make_shared<Ti5Motor>(cfg);
    //     motor->setProtocol(l_canopen_protocol);
    //     motor->init();  // 耗时操作
    //     motor_manager_->addMotor(motor);
    // }

    // 5 === 并行创建电机 ===
    auto left_task = std::async(std::launch::async, [&] {
        LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing LEFT motors...";
        for (const auto &cfg: l_motors_cfg_) {
            auto motor = std::make_shared<Ti5Motor>(cfg);
            motor->setProtocol(l_canopen_protocol);
            motor->init();
            motor_manager_->addMotor(motor);
        }
    });

    auto right_task = std::async(std::launch::async, [&] {
        LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing RIGHT motors...";
        for (const auto &cfg: r_motors_cfg_) {
            auto motor = std::make_shared<Ti5Motor>(cfg);
            motor->setProtocol(r_canopen_protocol);
            motor->init();
            motor_manager_->addMotor(motor);
        }
    });

    auto waist_task = std::async(std::launch::async, [&] {
        LOG(INFO) << "[Thread " << std::this_thread::get_id() << "] Start initializing waist motors...";
        for (const auto &cfg: waist_motors_cfg_) {
            auto motor = std::make_shared<Ti5Motor>(cfg);
            motor->setProtocol(waist_canopen_protocol);
            motor->init();
            motor_manager_->addMotor(motor);
        }
    });

    // 等待两个线程完成
    left_task.get();
    right_task.get();
    waist_task.get();

    motor_manager_->getMotor("WAIST_Y")->setQ(0);
    motor_manager_->getMotor("WAIST_P")->setQ(0);

    rsm_.store(ROBOT_ESTOP);
    LOG(INFO) << "All motors initialized successfully.";
}

template<int DOF>
void HumanoidRobot<DOF>::torqueOff() {
    try {
        if (rsm_.load() == ROBOT_RUNNING) {
            throw runtime_error("robot is running");
        }

        if (rsm_.load() != ROBOT_TOROFF) {
            for (const auto &pair: motor_manager_->motorsMap()) {
                if (pair.second->jointName() != "WAIST_Y" && pair.second->jointName() != "WAIST_P"  )
                    pair.second->torqueOff();
            }
            rsm_.store(ROBOT_TOROFF);
        }
    } catch (std::exception &e) {
        throw runtime_error(e.what());
    }
}


template<int DOF>
HumanoidRobot<DOF>::~HumanoidRobot() {
    // TODO: close can interfaces
    upd_timer_->stop();

    std::vector<JointPoint> 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.0201069},
        {"R_SHOULDER_R", 1.46698},
        {"R_SHOULDER_Y", 1.45894},
        {"R_ELBOW_R",    0.159681},
        {"R_WRIST_P",    0.0808349},
        {"R_WRIST_Y",    -0.138279},
        {"R_WRIST_R",    -0.243169},

        {"WAIST_Y",    0},
        {"WAIST_P",    0}
    };

    this->moveJ(cmd,0.8);
    this->torqueOff();
}

template<int DOF>
int HumanoidRobot<DOF>::getDOF() {
    return dof_;
}

template<int DOF>
std::vector<std::string> HumanoidRobot<DOF>::getJointNames() {
    return joint_names_;
}

template<int DOF>
std::unordered_map<std::string, double> HumanoidRobot<DOF>::getJointQ() const{
    std::unordered_map<std::string, double> joint_qs;

    for (const auto &pair : motor_manager_->motorsMap()) {
        auto motor = pair.second;
        joint_qs[motor->jointName()] = motor->getQ();
    }
    return joint_qs;
}

template<int DOF>
void HumanoidRobot<DOF>::getJointQ(std::unordered_map<std::string, double> &joint_qs) const {
    for (auto &pair : joint_qs) {
        auto motor = motor_manager_->getMotor(pair.first);
        if (motor) {
            pair.second = motor->getQ();
        } else {
            pair.second = 0.0;
        }
    }
}



template<int DOF>
std::vector<std::string> HumanoidRobot<DOF>::getLinkNames() {
    return link_names_;
}

template<int DOF>
void HumanoidRobot<DOF>::getJointsState(std::vector<JointState>& states) {
    try {
        lock_guard lock(exec_mtx_);

        states.clear();
        JointState state;
        for (const auto &pair : motor_manager_->motorsMap()) {
            auto motor = pair.second;
            state.name = motor->jointName();
            state.position = motor->getQ();
            state.velocity = motor->getQd();
            states.push_back(state);
        }
    } catch (exception &e) {
        throw runtime_error(e.what());
    }
}

template<int DOF>
void HumanoidRobot<DOF>::getState(RobotState &state) {
    try {
        lock_guard lock(exec_mtx_);
        // TODO: copy m_state_ date into state

    } catch (exception &e) {
        throw runtime_error(e.what());
    }
}

template<int DOF>
void HumanoidRobot<DOF>::torqueOn() {
    eStop();
}

template<int DOF>
void HumanoidRobot<DOF>::torqueOn(const std::string &joint_name) {
    auto motor = motor_manager_->getMotor(joint_name);
    motor->brake();
}

template<int DOF>
void HumanoidRobot<DOF>::torqueOff(const std::string &joint_name) {
    auto motor = motor_manager_->getMotor(joint_name);
    motor->torqueOff();
}

template<int DOF>
void HumanoidRobot<DOF>::eStop() {
    if (rsm_.load() != ROBOT_ESTOP) {
        CSP_buffer_->clear();
        CSV_buffer_->clear();
        CSC_buffer_->clear();
        for (const auto &pair: motor_manager_->motorsMap()) {
            pair.second->brake();
        }
        rsm_.store(ROBOT_ESTOP);
    }
}

template<int DOF>
void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &cmd, double vel, double acc) {
    try {
        if (rsm_.load() == ROBOT_RUNNING) {
            flash_cmd_.store(true);
            eStop();
        }
        if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
            rsm_.store(ROBOT_RUNNING);

            for (const auto &j: cmd) {
                auto motor = motor_manager_->getMotor(j.joint_name);
                if (motor != nullptr) {
                    // PPM 模式下 这个实际速度会超30% 左右
                    motor->setQd(vel);

                    if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
                        motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
                    }
                    motor->setQ(j.rad);
                }
            }

            //3. wait for completion
            bool completion = true;
            do {
                completion = true;
                for (const auto &j: cmd) {
                    auto motor = motor_manager_->getMotor(j.joint_name);
                    if (motor != nullptr) {
                        if (!motor->reachedTargetQ()) {
                            completion = false;
                            break;
                        }
                    }
                }
                // 4. while waiting, check flash_cmd_, if it is true, set it false then exit
                if (flash_cmd_.load()) {
                    flash_cmd_.store(false);
                    return;
                }
                std::this_thread::sleep_for(std::chrono::milliseconds(2));
            } while (!completion);

            rsm_.store(ROBOT_ESTOP);
        } else {
            throw runtime_error("rsm invalid");
        }
    } catch (exception &e) {
        throw runtime_error(e.what());
    }
}

template<int DOF>
void HumanoidRobot<DOF>::calibrateZeroQ(const std::string &joint_name) {
    auto motor = motor_manager_->getMotor(joint_name);
    motor->calibrateZeroQ();
}

template<int DOF>
void HumanoidRobot<DOF>::moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel, double acc) {

     try {
        if (rsm_.load() == ROBOT_RUNNING) {
            flash_cmd_.store(true);
            eStop();
        }
         if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
            rsm_.store(ROBOT_RUNNING);


            // update m_state_
            Eigen::Vector<double, DOF> q_init;
            auto q_map = getJointQ();
            q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"],
                      q_map["L_WRIST_P"],    q_map["L_WRIST_Y"],    q_map["L_WRIST_R"],
                      q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"],
                      q_map["R_WRIST_P"],    q_map["R_WRIST_Y"],    q_map["R_WRIST_R"];

            // LOG(INFO) << "q_init: " << q_init;
             m_state_->SetQ(q_init);
             m_robot_->ComputeForwardKinematics(m_state_);


             Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity();
             T_target.block<3,3>(0,0) = eulerZYXToRotationMatrix(pose.euler().rx(), pose.euler().ry(), pose.euler().rz());  // 输入为弧度
             T_target(0,3) =  pose.position().x();
             T_target(1,3) =  pose.position().y();
             T_target(2,3) =  pose.position().z();

             cmvr::ctrl::PoseTarget target;
             target.T_target = T_target;
             target.w_posrot = 0.5;
             target.weight   = 1.0;
             target.link_name = ee_link;

            // slove ik
             Eigen::Vector<double, DOF> q_cmd;
            bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, ctrl::CartesianController<DOF>::Mode::Position,
                                        q_cmd, 10000, 1e-6);
            if (!ok) {
                throw runtime_error("solve IK failed");
            }
            std::vector<JointPoint> joint_points{
                    {"R_SHOULDER_P", q_cmd[7]},   {"R_SHOULDER_R", q_cmd[8]},
                    {"R_SHOULDER_Y", q_cmd[9]},   {"R_ELBOW_R",    q_cmd[10]},
                    {"R_WRIST_P",    q_cmd[11]},  {"R_WRIST_Y",    q_cmd[12]},
                    {"R_WRIST_R",    q_cmd[13]}
            };
            for (const auto &j: joint_points) {
                auto motor = motor_manager_->getMotor(j.joint_name);
                if (motor != nullptr) {
                    // PPM 模式下 这个实际速度会超30% 左右

                    if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
                        motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
                    }
                    motor->setQd(vel);
                    motor->setQ(j.rad);
                }
            }

            //3. wait for completion
            bool completion = true;
            do {
                completion = true;
                for (const auto &j: joint_points) {
                    auto motor = motor_manager_->getMotor(j.joint_name);
                    if (motor != nullptr) {
                        if (!motor->reachedTargetQ()) {
                            completion = false;
                            break;
                        }
                    }
                }
                // 4. while waiting, check flash_cmd_, if it is true, set it false then exit
                if (flash_cmd_.load()) {
                    flash_cmd_.store(false);
                    return;
                }
                std::this_thread::sleep_for(std::chrono::milliseconds(2));
            } while (!completion);
            rsm_.store(ROBOT_READY);
        } else {
            throw runtime_error("rsm invalid");
        }
    } catch (exception &e) {
        throw runtime_error(e.what());
    }
}


template<int DOF>
void HumanoidRobot<DOF>::moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel,
                                  double acc)  {
    try {
        if (rsm_.load() == ROBOT_RUNNING) {
            flash_cmd_.store(true);
            eStop();
        }
         if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
            rsm_.store(ROBOT_RUNNING);


            // update m_state_
            Eigen::Vector<double, DOF> q_init;
            auto q_map = getJointQ();
            q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"],
                      q_map["L_WRIST_P"],    q_map["L_WRIST_Y"],    q_map["L_WRIST_R"],
                      q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"],
                      q_map["R_WRIST_P"],    q_map["R_WRIST_Y"],    q_map["R_WRIST_R"];

            LOG(INFO) << "q_init: " << q_init;
             m_state_->SetQ(q_init);
            m_robot_->ComputeForwardKinematics(m_state_);

            // slove ik
             Eigen::Vector<double, DOF> q_cmd;
            bool ok = m_cctrl_->compute(m_state_, base_link, targets, 0.002, ctrl::CartesianController<DOF>::Mode::Position,
                                        q_cmd, 10000, 1e-6);
            if (!ok) {
                throw runtime_error("solve IK failed");
            }
            std::vector<JointPoint> joint_points{
                    {"R_SHOULDER_P", q_cmd[7]},   {"R_SHOULDER_R", q_cmd[8]},
                    {"R_SHOULDER_Y", q_cmd[9]},   {"R_ELBOW_R",    q_cmd[10]},
                    {"R_WRIST_P",    q_cmd[11]},  {"R_WRIST_Y",    q_cmd[12]},
                    {"R_WRIST_R",    q_cmd[13]}
            };
            // for (const auto &j: joint_points) {
            //     auto motor = motor_manager_->getMotor(j.joint_name);
            //     if (motor != nullptr) {
            //         // PPM 模式下 这个实际速度会超30% 左右
            //         motor->setQd(vel);
            //         if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) {
            //             motor->setMode(msgs::RUN_MODE_PROFILE_POSITION);
            //         }
            //         motor->setQ(j.rad);
            //     }
            // }
            //
            // //3. wait for completion
            // bool completion = true;
            // do {
            //     completion = true;
            //     for (const auto &j: joint_points) {
            //         auto motor = motor_manager_->getMotor(j.joint_name);
            //         if (motor != nullptr) {
            //             if (!motor->reachedTargetQ()) {
            //                 completion = false;
            //                 break;
            //             }
            //         }
            //     }
            //     // 4. while waiting, check flash_cmd_, if it is true, set it false then exit
            //     if (flash_cmd_.load()) {
            //         flash_cmd_.store(false);
            //         return;
            //     }
            //     std::this_thread::sleep_for(std::chrono::milliseconds(2));
            // } while (!completion);
            rsm_.store(ROBOT_READY);
        } else {
            throw runtime_error("rsm invalid");
        }
    } catch (exception &e) {
        throw runtime_error(e.what());
    }
}

template<int DOF>
void HumanoidRobot<DOF>::moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel,
                               double acc) {
    try {
        if (rsm_.load() == ROBOT_RUNNING) {
            flash_cmd_.store(true);
            eStop();
        } else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
            rsm_.store(ROBOT_RUNNING);
            // TODO:
            //  1. interpolate line waypoint by vel and acc
            //  2. for each waypoint, call cartesian controller to solve joint positions
            //  3. for each waypoint, call motor Cyclic Synchronous Position (CSP) command with Timer
            //  4. in the loop, check flash_cmd_, if it is true, set it false then exit
            //            Eigen::Vector<double, DOF> q_cmd;
            //            m_state_->SetQ(state_.joint_positions);
            //            bool ok = m_cctrl_.compute(m_state_, base_link, targets, 1, ctrl::CartesianController<DOF>::Mode::Position, q_cmd, 60, 1e-4);
            //            if (!ok) {
            //                throw runtime_error("solve IK failed");
            //            }
            rsm_.store(ROBOT_READY);
        } else {
            throw runtime_error("rsm invalid");
        }
    } catch (exception &e) {
        throw runtime_error(e.what());
    }
}

template<int DOF>
void HumanoidRobot<DOF>::speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) {
    try {
        if (rsm_.load() == ROBOT_RUNNING) {
            flash_cmd_.store(true);
            eStop();
        } else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
            rsm_.store(ROBOT_RUNNING);
            // TODO:
            //  1. set joint speed and acc
            //  2. set joint speed by PROFILE VELOCITY MODE (PVM)
            //            rsm_.store(ROBOT_READY); -> should not set rsm_ to ready because motor is running
        } else {
            throw runtime_error("rsm invalid");
        }
    } catch (exception &e) {
        throw runtime_error(e.what());
    }
}

template<int DOF>
void HumanoidRobot<DOF>::speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) {
    try {
        if (rsm_.load() == ROBOT_RUNNING) {
            flash_cmd_.store(true);
            eStop();
        } else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
            rsm_.store(ROBOT_RUNNING);
            // TODO: ???
            //            rsm_.store(ROBOT_READY); -> should not set rsm_ to ready because motor is running
        } else {
            throw runtime_error("rsm invalid");
        }
    } catch (exception &e) {
        throw runtime_error(e.what());
    }
}

template<int DOF>
void HumanoidRobot<DOF>::followJointTrajectory(std::vector<std::vector<JointPoint> > &traj, double dt) {
    try {
        if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY || rsm_.load() == ROBOT_TOROFF) {
            auto ok = check_joint_traj_(traj, dt);
            if (!ok) { throw runtime_error("joint traj invalid"); }

            rsm_.store(ROBOT_RUNNING);
            // TODO: need to optimize callback loop
            for (auto i = 0; i < traj.size(); i++) {
                if (flash_cmd_.load()) {
                    flash_cmd_.store(false);
                    LOG(INFO) << "followJointTrajectory is canceled";
                    return;
                }
                servoJ(traj[i], dt);
                this_thread::sleep_for(chrono::milliseconds((int) dt));
            }
            rsm_.store(ROBOT_ESTOP);
        } else {
            throw runtime_error("rsm invalid");
        }
    } catch (exception &e) {
        throw runtime_error(e.what());
    }
}

template<int DOF>
void HumanoidRobot<DOF>::followPoseTrajectory(std::string &base_link,
                                              std::vector<std::vector<cmvr::ctrl::PoseTarget> > &targets, double dt) {
    try {
        if (rsm_.load() == ROBOT_RUNNING) {
            flash_cmd_.store(true);
            eStop();
        } else if (rsm_.load() == ROBOT_ESTOP || rsm_.load() == ROBOT_READY) {
            rsm_.store(ROBOT_RUNNING);
            // TODO:
            //  1. set Timer(dt)
            //  2. for each timestamp, use Cyclic Synchronous Position (CSP) Mode to set joint position
            //  3. if flash_cmd_ is set, set it to false and exit
            //  3. join timer
            rsm_.store(ROBOT_READY);
        } else {
            throw runtime_error("rsm invalid");
        }
    } catch (exception &e) {
        throw runtime_error(e.what());
    }
}

template<int DOF>
void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double dt) {
    for (const auto &j: joints) {
        auto motor = motor_manager_->getMotor(j.joint_name);
        if (motor != nullptr) {
            if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
                motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
            }
            motor->setQd(j.vel);
            motor->setQ(j.rad);
        }
    }
    rsm_.store(ROBOT_READY);
}

template<int DOF>
void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double vel, double dt) {
    for (const auto &j: joints) {
        auto motor = motor_manager_->getMotor(j.joint_name);
        if (motor != nullptr) {

            if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
                motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
            }
            motor->setQd(vel);
            motor->setQ(j.rad);
        }
    }
}

template<int DOF>
void HumanoidRobot<DOF>::servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel,
                                double acc) {
    try {
        // update m_state_
        Eigen::Vector<double, DOF> q_init;
        auto q_map = getJointQ();
        q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"],
                q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"],
                q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"],
                q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"];

        LOG(INFO) << "q_init: " << q_init;
        m_state_->SetQ(q_init);
        m_robot_->ComputeForwardKinematics(m_state_);


        Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity();
        T_target.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(pose.euler().rx(), pose.euler().ry(), pose.euler().rz());
        // 输入为弧度
        T_target(0, 3) = pose.position().x();
        T_target(1, 3) = pose.position().y();
        T_target(2, 3) = pose.position().z();

        cmvr::ctrl::PoseTarget target;
        target.T_target = T_target;
        target.w_posrot = 0.5;
        target.weight = 1.0;
        target.link_name = ee_link;

        // slove ik
        Eigen::Vector<double, DOF> q_cmd;
        bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002,
                                    ctrl::CartesianController<DOF>::Mode::Position,
                                    q_cmd, 10000, 1e-6);
        if (!ok) {
            throw runtime_error("solve IK failed");
        }
        std::vector<JointPoint> joint_points{
            {"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]},
            {"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]},
            {"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]},
            {"R_WRIST_R", q_cmd[13]}
        };
        servoJ(joint_points, vel, 0.1);
    } catch (exception &e) {
        throw runtime_error(e.what());
    }
}



template<int DOF>
void HumanoidRobot<DOF>::servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) {
    try {
        // 1 : 计算当前位姿
        auto cur_pose = fk(base_link, ee_link);

        // 2 : 计算目标角度   target pos = cur_pose + delta_pose
        cmvr::msgs::Pose3d target_pose;
        target_pose.mutable_position()->set_x(cur_pose.position().x() + delta_pose.position().x());
        target_pose.mutable_position()->set_y(cur_pose.position().y() + delta_pose.position().y());
        target_pose.mutable_position()->set_z(cur_pose.position().z() + delta_pose.position().z());

        target_pose.mutable_euler()->set_rx(cur_pose.euler().rx() + delta_pose.euler().rx());
        target_pose.mutable_euler()->set_ry(cur_pose.euler().ry() + delta_pose.euler().ry());
        target_pose.mutable_euler()->set_rz(cur_pose.euler().rz() + delta_pose.euler().rz());

        //3 :
        servoJ(base_link, ee_link, target_pose, vel, acc);
    } catch (exception &e) {
        throw runtime_error(e.what());
    }
}



template<int DOF>
void HumanoidRobot<DOF>::servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) {
    try {
        Eigen::Vector<double, DOF> q_cmd;
        bool ok = m_cctrl_->compute(m_state_, base_link, targets, 1, ctrl::CartesianController<DOF>::Mode::Position,
                                    q_cmd, 60, 1e-4);
        if (!ok) {
            LOG(WARNING) << "[HumanoidRobot] (servoL): solve IK failed, id=" << id_;
            throw runtime_error("IK failed");
        }
        std::vector<JointPoint> joints(dof_);
        for (size_t i = 0; i < dof_; i++) {
            joints[i].joint_name = joint_names_[i];
            joints[i].rad = q_cmd[i];
        }
        servoJ(joints, dt);
    } catch (exception &e) {
        throw runtime_error(e.what());
    }
}

template<int DOF>
bool HumanoidRobot<DOF>::check_joint_traj_(std::vector<std::vector<JointPoint> > &traj, double dt) {
    // TODO: to be implemented
    return true;
}

template<int DOF>
void HumanoidRobot<DOF>::moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,
                                    double vel, double acc) {
    try {
        // 1 : 计算当前位姿
        auto cur_pose = fk(base_link, ee_link);

        // 2 : 计算目标角度   target pos = cur_pose + delta_pose
        cmvr::msgs::Pose3d target_pose;
        target_pose.mutable_position()->set_x(cur_pose.position().x() + delta_pose.position().x());
        target_pose.mutable_position()->set_y(cur_pose.position().y() + delta_pose.position().y());
        target_pose.mutable_position()->set_z(cur_pose.position().z() + delta_pose.position().z());

        target_pose.mutable_euler()->set_rx(cur_pose.euler().rx() + delta_pose.euler().rx());
        target_pose.mutable_euler()->set_ry(cur_pose.euler().ry() + delta_pose.euler().ry());
        target_pose.mutable_euler()->set_rz(cur_pose.euler().rz() + delta_pose.euler().rz());

        //3 :
        moveJ(base_link, ee_link, target_pose, vel, acc);
    } catch (exception &e) {
        throw runtime_error(e.what());
    }
}


template<int DOF>
void HumanoidRobot<DOF>::update_state_() {
    std::lock_guard lock(state_mtx_);
    // TODO: set m_state_
    //    m_state_->SetQ();
    //    m_state_->SetQdot();
    //    m_state_->SetQddot();
}

template<int DOF>
Eigen::Matrix3d HumanoidRobot<DOF>::eulerZYXToRotationMatrix(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;

    return R_x * R_y * R_z;
}

template<int DOF>
Eigen::Vector3d HumanoidRobot<DOF>::rotationMatrixToEulerZYX(const Eigen::Matrix3d &R) {
    double rx, ry, rz;

    // 根据 R = R_x * R_y * R_z
    // R = | cy*cz           -cy*sz           sy       |
    //     | sx*sy*cz + cx*sz  -sx*sy*sz + cx*cz  -sx*cy |
    //     | -cx*sy*cz + sx*sz  cx*sy*sz + sx*cz   cx*cy |

    // 提取 ry（绕 Y 的角度）
    ry = std::asin(R(0,2));  // R(0,2) = sin(ry)

    double cy = std::cos(ry);

    if (std::abs(cy) > 1e-6) {
        // 正常情况
        rx = std::atan2(-R(1,2), R(2,2));
        rz = std::atan2(-R(0,1), R(0,0));
    } else {
        // 万向节锁：cy ≈ 0
        rx = 0; // 任意选择
        if (ry > 0) {
            rz = std::atan2(R(1,0), R(1,1));
        } else {
            rz = std::atan2(-R(1,0), R(1,1));
        }
    }

    return Eigen::Vector3d(rx, ry, rz);
}
template<int DOF>
std::vector<double> HumanoidRobot<DOF>::ik(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose) {
    // update m_state_

    try {
        Eigen::Vector<double, DOF> q_init;
        auto q_map = getJointQ();
        q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"],
                  q_map["L_WRIST_P"],    q_map["L_WRIST_Y"],    q_map["L_WRIST_R"],
                  q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"],
                  q_map["R_WRIST_P"],    q_map["R_WRIST_Y"],    q_map["R_WRIST_R"];

        LOG(INFO) << "q_init: " << q_init;
        m_state_->SetQ(q_init);
        m_robot_->ComputeForwardKinematics(m_state_);


        Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity();
        T_target.block<3,3>(0,0) = eulerZYXToRotationMatrix(pose.euler().rx(), pose.euler().ry(), pose.euler().rz());  // 输入为弧度
        T_target(0,3) =  pose.position().x();
        T_target(1,3) =  pose.position().y();
        T_target(2,3) =  pose.position().z();

        cmvr::ctrl::PoseTarget target;
        target.T_target = T_target;
        target.w_posrot = 0.5;
        target.weight   = 1.0;
        target.link_name = ee_link;

        // slove ik
        Eigen::Vector<double, DOF> q_cmd{};
        bool ok = m_cctrl_->compute(m_state_, base_link, {target}, 0.002, ctrl::CartesianController<DOF>::Mode::Position,
                                    q_cmd, 10000, 1e-6);
        if (!ok) {
            throw std::runtime_error("IK solve failed");
        } else {
            return std::vector<double>(q_cmd.data(), q_cmd.data() + q_cmd.size());
        }
    }catch (std::exception &e) {
        throw runtime_error(e.what());
    }


}


template<int DOF>
cmvr::msgs::Pose3d HumanoidRobot<DOF>::fk(const std::string &base_link, const std::string &ee_link) {
    cmvr::msgs::Pose3d pose;

    try {
        // 获取当前关节角度
        Eigen::Vector<double, DOF> q;
        auto q_map = getJointQ(); // 类似 moveJ 中获取关节角度
        q << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"],
             q_map["L_WRIST_P"],    q_map["L_WRIST_Y"],    q_map["L_WRIST_R"],
             q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"],
             q_map["R_WRIST_P"],    q_map["R_WRIST_Y"],    q_map["R_WRIST_R"];

        // 更新状态并计算前向运动学
        m_state_->SetQ(q);
        m_robot_->ComputeForwardKinematics(m_state_);

        // 获取基座和末端索引
        auto base_idx = m_robot_->GetLinkIdx(base_link);
        auto ee_idx   = m_robot_->GetLinkIdx(ee_link);

        // 获取变换矩阵
        Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx);

        // 填充 Pose3d
        pose.mutable_position()->set_x(T(0,3));
        pose.mutable_position()->set_y(T(1,3));
        pose.mutable_position()->set_z(T(2,3));

        // 将旋转矩阵转换为欧拉角
        Eigen::Matrix3d R = T.block<3,3>(0,0);
        Eigen::Vector3d euler = rotationMatrixToEulerZYX(R); // 你需要实现或已有此工具函数
        pose.mutable_euler()->set_rx(euler(0));
        pose.mutable_euler()->set_ry(euler(1));
        pose.mutable_euler()->set_rz(euler(2));

    } catch (const std::exception &e) {
        throw std::runtime_error(std::string("FK计算失败: ") + e.what());
    }

    return pose;
}

template class cmvr::device::HumanoidRobot<7>;
template class cmvr::device::HumanoidRobot<14>;
template class cmvr::device::HumanoidRobot<20>;