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

#ifndef CMVR_ES_HUMANOID_ROBOT_H
#define CMVR_ES_HUMANOID_ROBOT_H

#include "devices/abstract_robot.h"
#include "utils/controller/cartesian_controller.h"
#include "utils/base/os.h"
#include "utils/base/timer.h"
#include "utils/base/ring_buffer.h"

#include "devices/abstract_canbus.h"
#include "canbus/can_comm/can_sender.h"
#include "canbus/can_comm/can_receiver.h"
#include "canbus/can_comm/message_manager.h"
#include "cmvr/msgs/robot_detail.pb.h"
#include "motor/motor_manager.h"
#include <thread>
#include <queue>
#include <mutex>
#include <condition_variable>
#include <atomic>
#include <vector>
#include <memory>
#include <chrono>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace cmvr::device{

    template<int DOF>
    class HumanoidRobot: public AbstractRobot {
        typedef enum {
            ROBOT_DISABLED,
            ROBOT_READY,
            ROBOT_RUNNING,
            ROBOT_ESTOP,
            ROBOT_TOROFF,
            ROBOT_ERROR
        } RobotStateMachine;

//        typedef enum {
//            PPM_MODE, PVM_MODE, PTM_MODE, CSP_MODE, CSV_MODE, CSC_MODE
//        } RobotRunningMode;

    public:
        explicit HumanoidRobot(const XmlNode& cfg);
        ~HumanoidRobot();

        void init() override;

        /* get robot state and info*/
        int getDOF() override;
        std::vector<std::string> getJointNames() override;
        std::vector<std::string> getLinkNames() override;

        std::unordered_map<std::string,double> getJointQ() const override;
        void getJointQ(std::unordered_map<std::string,double> &joint_qs) const override;

        void getState(RobotState &state) override;
        math::Pose3d getTransform(std::string &bask_link, std::string &target_link) override;

        /* torque on and off */
        void torqueOn() override;
        void torqueOff() override;
        void calibrateZeroQ(const std::string &joint_name) override;

        /* robot basic command*/
        void eStop() override;
        void moveJ(std::vector<JointPoint> &cmd, double vel = 0.5, double acc = 0.1) override;
        void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel, double acc) override;
        void moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc)  override ;
        void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) override;

        void moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) override;
        void moveL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d target_pose, double vel, double acc) override;
        void moveDeltaL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d delta_pose, double vel, double acc) override;


        void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) override;
        void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) override;

        void followJointTrajectory(std::vector<std::vector<JointPoint>> &traj, double dt) override;
        void followPoseTrajectory(std::string &base_link, std::vector<std::vector<cmvr::ctrl::PoseTarget>> &targets, double dt) override;

        void servoJ(std::vector<JointPoint> &joints, double dt) override;
        void servoJ(std::vector<JointPoint> &joints, double vel, double dt) override;
        void servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel, double acc) override;
        void servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) override;
        void servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) override;

    protected:
        bool check_joint_traj_(std::vector<std::vector<JointPoint>> &traj, double dt);
        void update_state_();
        int exec_PPM_cmd_(std::vector<int> &ids, std::vector<double> &pos, std::vector<double> &vel, std::vector<double> &acc);
        int exec_PVM_cmd_(std::vector<int> &ids, std::vector<double> &vel, std::vector<double> &acc);
        int exec_PTM_cmd_(std::vector<int> &ids, std::vector<double> &acc);
        int exec_CSP_cmd_(std::vector<int> &ids, std::vector<double> &pos, int dt);
        int exec_CSV_cmd_(std::vector<int> &ids, std::vector<double> &vel, int dt);
        int exec_CSC_cmd_(std::vector<int> &ids, std::vector<double> &cur, int dt);





        // 添加这三个函数的声明
        bool computeJacobian(const std::shared_ptr<cmvr::dyn::State<DOF>>& state,
                            const std::string& base_link,
                            const std::string& ee_link,
                            Eigen::Matrix<double, 6, DOF>& J);

        Eigen::Vector3d getEndEffectorPosition(const std::shared_ptr<cmvr::dyn::State<DOF>>& state,
                                              const std::string& ee_link);

        Eigen::Matrix3d getEndEffectorOrientation(const std::shared_ptr<cmvr::dyn::State<DOF>>& state,
                                                 const std::string& ee_link);



    public:
        // 将欧拉角(rx, ry, rz)转为旋转矩阵，旋转顺序 Y→Y→Z
         static Eigen::Matrix3d eulerZYXToRotationMatrix(double rx, double ry, double rz);
         static Eigen::Vector3d rotationMatrixToEulerZYX(const Eigen::Matrix3d &R);


        double calculateMoveTime(double distance, double vel, double acc);
        void generateSTrapezoidalProfile(double total_distance, double max_vel, double max_acc,
                                                   double total_time, size_t num_points,
                                                   std::vector<double>& time_points,
                                                   std::vector<double>& distance_ratios);
        msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link);

    private:
        std::string id_;
        int upd_freq_;
        std::shared_ptr<FDTimer> upd_timer_;
        std::atomic<RobotStateMachine> rsm_{ROBOT_DISABLED};
//        std::atomic<RobotRunningMode> rmode_{PPM_MODE};

        std::shared_ptr<cmvr::dyn::State<DOF>> m_state_;
        std::shared_ptr<cmvr::dyn::Robot<DOF>> m_robot_;
        std::shared_ptr<cmvr::ctrl::CartesianController<DOF>> m_cctrl_;

        std::vector<std::string> joint_names_;
        std::vector<std::string> link_names_;

        std::atomic<bool> flash_cmd_{false};
        std::mutex state_mtx_{};
        std::mutex exec_mtx_{};

        std::shared_ptr<SPMCRingBuffer<JointPoint>> CSP_buffer_;
        std::shared_ptr<SPMCRingBuffer<JointVelocityCommand>> CSV_buffer_;
        std::shared_ptr<SPMCRingBuffer<JointCurrentCommand>> CSC_buffer_;


        struct ControlPoint {
            Eigen::Vector<double, DOF> joint_positions;
            Eigen::Vector<double, DOF> joint_velocities;
            double timestamp;
            ControlPoint() : timestamp(0.0) {}
            ControlPoint(const Eigen::Vector<double, DOF>& pos, const Eigen::Vector<double, DOF>& vel, double time): joint_positions(pos), joint_velocities(vel), timestamp(time) {}
        };
            // 线程间通信的队列和同步变量
        std::queue<ControlPoint> control_queue_;
        std::mutex queue_mutex_;
        std::condition_variable queue_cv_;
        std::atomic<bool> trajectory_completed_{false};
        std::atomic<bool> control_thread_ready_{false};
        std::atomic<bool> stop_requested_{false};

        // 线程对象
        std::unique_ptr<std::thread> trajectory_thread_;
        std::unique_ptr<std::thread> control_thread_;




    private:
        std::vector<XmlNode>  l_motors_cfg_;
        std::vector<XmlNode>  r_motors_cfg_;
        std::vector<XmlNode>  waist_motors_cfg_;


        std::shared_ptr<AbstractCanbus> l_can_client_{nullptr};
        std::shared_ptr<AbstractCanbus> r_can_client_{nullptr};
        std::shared_ptr<AbstractCanbus> waist_can_client_{nullptr};

        std::shared_ptr<CanReceiver<msgs::RobotDetail>>  l_can_receiver_{nullptr};
        std::shared_ptr<CanReceiver<msgs::RobotDetail>>  r_can_receiver_{nullptr};
        std::shared_ptr<CanReceiver<msgs::RobotDetail>>  waist_can_receiver_{nullptr};

        std::shared_ptr<CanSender<msgs::RobotDetail>>  l_can_sender_{nullptr};
        std::shared_ptr<CanSender<msgs::RobotDetail>>  r_can_sender_{nullptr};
        std::shared_ptr<CanSender<msgs::RobotDetail>>  waist_can_sender_{nullptr};

        std::shared_ptr<MessageManager<msgs::RobotDetail>>  l_message_manager_{nullptr};
        std::shared_ptr<MessageManager<msgs::RobotDetail>>  r_message_manager_{nullptr};
        std::shared_ptr<MessageManager<msgs::RobotDetail>>  waist_message_manager_{nullptr};

        std::shared_ptr<MotorManager>  motor_manager_{nullptr};




    };


}


#endif //CMVR_ES_HUMANOID_ROBOT_H