#ifndef RM_ARM_SIMULATOR_HPP
#define RM_ARM_SIMULATOR_HPP

#include <unordered_map>
#include <map>
#include <urdf_model/pose.h>
#include <std_msgs/msg/string.hpp>
#include "arm_define.h"
#include "arm_driver_define.hpp"
#include "gjk_interface.hpp"

#define ARM_COLLISION_CHECK_SIMPLE_CNT 2

typedef enum {
    FRAME_CHANGE_TYPE_ARM_BASE_TO_ORI,
    FRAME_CHANGE_TYPE_ORI_TO_ARM_BASE,
    FRAME_CHANGE_TYPE_ARM_END_TO_ORI,
    FRAME_CHANGE_TYPE_ORI_TO_ARM_END,
    FRAME_CHANGE_TYPE_ERR
} FrameChangeTypeE;

typedef std::vector<Eigen::Vector3d> Polyhedron;
struct CollisionStructure {
    std::string link_name;               // 所属链接名称
    std::string geometry_type;           // 几何类型
    std::vector<double> dimensions;      // 几何尺寸/缩放因子
    std::vector<double> origin_xyz;      // 原点坐标
    std::vector<double> origin_rpy;      // 原点姿态
    std::string mesh_filename;           // mesh文件路径（如STL）
    std::vector<double> mesh_bounds;     // 网格边界盒 [min_x, max_x, min_y, max_y, min_z, max_z]
    Polyhedron origin_points;
};

struct JointInfo {
    std::string name;               // 关节名称
    int type;               // 关节类型
    int parent_link;        // 父连杆
    int child_link;         // 子连杆
    Eigen::Quaterniond rotationAll;
    Eigen::Vector3d axis;          // 关节轴
    Eigen::Vector3d offset;        // 关节偏移
    Eigen::Quaterniond rotation; // 关节旋转
    double limit_lower;
    double limit_upper;
    double limit_v;
    double limit_e;
    double angle;                   // 关节角度
};

enum {
    SAFE = 0,
    LEFT_ARM_COLLISION,
    RIGHT_ARM_COLLISION,
};

class ArmSimulator
{
public:
    ArmSimulator(std_msgs::msg::String::SharedPtr urdf_string_ptr, float *left_now_joints, float *right_now_joints,
        KinematicsManager *leftTrajectory, KinematicsManager *rightTrajectory);
    ArmSimulator(std::string urdf_string, float *left_now_joints, float *right_now_joints,
        KinematicsManager *leftTrajectory, KinematicsManager *rightTrajectory);
    ~ArmSimulator();

    int CheckCollision();
    int CheckArmJointsAngleLimit(float *joints);
    int UpdateCollisionForNewGoal(int arm);
    int AddPolyhedron(int id, const Polyhedron& poly);
    int RemovePolyhedron(int id);

    int ChangeArmBaseFrameToArmEndPose(float *pose, int arm, int64_t frameTime);
    int ChangeOriFrameToArmBasePose(float *pose, int arm);
    int ChangeArmEndPoseToArmBaseFrame(float *pose, int arm, int64_t frameTime);
    int ChangeArmBasePoseToOriFrame(float *pose, int arm);

    int GetFrameChange(float *pose, int arm, int64_t frameTime, JointInfo *jointInfo);

    KinematicsManager *leftTrajectory_;
    KinematicsManager *rightTrajectory_;

    std::vector<CollisionStructure> collision_structures_;
    std::map<int, JointInfo> jointMap_;
    std::vector<Polyhedron> armPolyhedrons_[ARM_COLLISION_CHECK_SIMPLE_CNT];
    std::vector<OBB> addedOBBs_[ARM_COLLISION_CHECK_SIMPLE_CNT];

    int simpleUpdateIndex_;
    float leftArmJoints_[ARM_COLLISION_CHECK_SIMPLE_CNT][USED_ARM_DOF];
    float rightArmJoints_[ARM_COLLISION_CHECK_SIMPLE_CNT][USED_ARM_DOF];
private:
    std::unordered_map<int, Polyhedron> addedPolyhedrons_;
};

#endif // RM_ARM_SIMULATOR_HPP