//
// Created by lgv on 2025/11/3.
//

#ifndef CMVR_ES_SRS_IK_SLOVER_H
#define CMVR_ES_SRS_IK_SLOVER_H


#include <Eigen/Dense>
#include "srs_ik/ik_limit_analyzer.h"
namespace cmvr {
namespace utils {

    class SRSIkSlover {
    public:
        enum ConfigDirection {
            OUTWARD = 1,  // 向外
            INWARD = -1     // 向内
        };

    public:
        SRSIkSlover();
        ~SRSIkSlover(){};

        std::vector<double>  inverse_kinematics(const Eigen::MatrixXd& pose, double psi);

        Eigen::Matrix4d calc_total_transform(const std::vector<double>& joint_angles);

        bool cal_coefficient_matrix(const Eigen::MatrixXd& pose,Eigen::MatrixXd& s_mat , Eigen::MatrixXd& w_mat);
        std::vector<std::pair<double, double>>  calc_arm_angle_limits(const Eigen::MatrixXd& s_mat ,const Eigen::MatrixXd& w_mat);

        void set_shoulder_config(ConfigDirection value) {
            shoulder_config_ = value;
        }

        void set_elbow_config(ConfigDirection value) {
            elbow_config_ = value;
        }

        void set_wrist_config(ConfigDirection value) {
            wrist_config_ = value;
        }



    private:


        ConfigDirection shoulder_config_{OUTWARD};
        ConfigDirection elbow_config_{OUTWARD};
        ConfigDirection wrist_config_{OUTWARD};
        // DH参数
        Eigen::VectorXd link_lengths_;
        Eigen::MatrixXd dh_params_;

        double d_bs_, d_se_, d_ew_, d_wt_;

        // 关节物理限位
        // first : min   second : max
        std::vector<std::pair<double, double>>  joints_limits_{};

        IkLimitAnalyzer  ik_limit_analyzer_;





        // 计算参考平面相对于基坐标系的旋转矩阵
        Eigen::Matrix3d reference_plane(const Eigen::Vector3d& S, const Eigen::Vector3d& W);

        // 罗德里格斯公式
        Eigen::Matrix3d calc_rotation_matrix(const Eigen::Vector3d& rotation_axis, double rotation_angle);


        // 使用 DH 参数计算变换矩阵
         Eigen::Matrix4d calc_dh(double d, double alpha, double a, double theta);

        // 将角度归一化到 [-π, π] 范围内
        double normalize_angle(const double angle);




    };
}
}


#endif //CMVR_ES_SRS_IK_SLOVER_H