"""
约束和关节系统模块
负责各种物理约束和关节的创建和管理
"""

from panda3d.core import Point3, Vec3, TransformState, LVector3f
from panda3d.bullet import BulletConstraint
from panda3d.bullet import BulletPoint2PointConstraint, BulletHingeConstraint
from panda3d.bullet import BulletSliderConstraint, BulletConeTwistConstraint
from panda3d.bullet import BulletGeneric6DofConstraint, BulletGeneric6DofSpringConstraint
from panda3d.bullet import BulletGearConstraint, BulletFixedConstraint
import math
import time
from collections import deque


class ConstraintManager:
    """
    约束管理器类
    管理各种类型的物理约束和关节
    """
    
    CONSTRAINT_POINT2POINT = 'point2point'
    CONSTRAINT_HINGE = 'hinge'
    CONSTRAINT_SLIDER = 'slider'
    CONSTRAINT_CONETWIST = 'conetwist'
    CONSTRAINT_6DOF = '6dof'
    CONSTRAINT_6DOF_SPRING = '6dof_spring'
    CONSTRAINT_GEAR = 'gear'
    CONSTRAINT_FIXED = 'fixed'
    
    def __init__(self):
        """
        初始化约束管理器
        """
        self.constraints = []
        self.constraint_groups = {}  # 约束组
        self.constraint_limits = {}  # 约束限制
        self.constraint_motors = {}  # 约束马达
        self.constraint_springs = {}  # 约束弹簧
        
        print("约束管理器初始化完成")
        
    def create_point2point_constraint(self, body_a, body_b, pivot_a, pivot_b, 
                                    collision=True, breaking_threshold=0.0,
                                    disable_collision_between_linked_bodies=True):
        """
        创建点对点约束
        
        Args:
            body_a (RigidBody): 第一个刚体
            body_b (RigidBody): 第二个刚体
            pivot_a (Point3): 第一个刚体上的连接点
            pivot_b (Point3): 第二个刚体上的连接点
            collision (bool): 是否允许两个刚体碰撞
            breaking_threshold (float): 破裂阈值
            disable_collision_between_linked_bodies (bool): 是否禁用连接刚体间的碰撞
            
        Returns:
            BulletPoint2PointConstraint: 点对点约束对象
        """
        constraint = BulletPoint2PointConstraint(
            body_a.bullet_body, body_b.bullet_body, pivot_a, pivot_b)
            
        # 设置约束属性
        constraint.setOverrideEnabled(not collision)
        if breaking_threshold > 0:
            constraint.setBreakingImpulseThreshold(breaking_threshold)
            
        # 禁用连接刚体间的碰撞
        if disable_collision_between_linked_bodies:
            body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, True)
            
        # 保存约束信息
        constraint_info = {
            'type': self.CONSTRAINT_POINT2POINT,
            'constraint': constraint,
            'body_a': body_a,
            'body_b': body_b,
            'properties': {
                'pivot_a': pivot_a,
                'pivot_b': pivot_b,
                'collision': collision,
                'breaking_threshold': breaking_threshold,
                'disable_collision': disable_collision_between_linked_bodies
            },
            'creation_time': time.time(),
            'is_enabled': True,
            'group': None,
            'tags': set(),
            'feedback': None
        }
        
        self.constraints.append(constraint_info)
        return constraint
        
    def create_hinge_constraint(self, body_a, body_b, pivot_a, pivot_b, axis_a, axis_b,
                              collision=True, breaking_threshold=0.0, limits=None,
                              motor=None, disable_collision_between_linked_bodies=True):
        """
        创建铰链约束
        
        Args:
            body_a (RigidBody): 第一个刚体
            body_b (RigidBody): 第二个刚体
            pivot_a (Point3): 第一个刚体上的连接点
            pivot_b (Point3): 第二个刚体上的连接点
            axis_a (Vec3): 第一个刚体上的轴向
            axis_b (Vec3): 第二个刚体上的轴向
            collision (bool): 是否允许两个刚体碰撞
            breaking_threshold (float): 破裂阈值
            limits (tuple): 旋转限制 (min_angle, max_angle) 或 None
            motor (dict): 马达参数 {'target_velocity': float, 'max_impulse': float}
            disable_collision_between_linked_bodies (bool): 是否禁用连接刚体间的碰撞
            
        Returns:
            BulletHingeConstraint: 铰链约束对象
        """
        constraint = BulletHingeConstraint(
            body_a.bullet_body, body_b.bullet_body, pivot_a, pivot_b, axis_a, axis_b)
            
        # 设置约束属性
        constraint.setOverrideEnabled(not collision)
        if breaking_threshold > 0:
            constraint.setBreakingImpulseThreshold(breaking_threshold)
            
        # 设置旋转限制
        if limits:
            min_angle, max_angle = limits
            constraint.setLimit(min_angle, max_angle)
            
        # 设置马达
        if motor:
            target_velocity = motor.get('target_velocity', 0.0)
            max_impulse = motor.get('max_impulse', 1.0)
            constraint.enableAngularMotor(True, target_velocity, max_impulse)
            
        # 禁用连接刚体间的碰撞
        if disable_collision_between_linked_bodies:
            body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, True)
            
        # 保存约束信息
        constraint_info = {
            'type': self.CONSTRAINT_HINGE,
            'constraint': constraint,
            'body_a': body_a,
            'body_b': body_b,
            'properties': {
                'pivot_a': pivot_a,
                'pivot_b': pivot_b,
                'axis_a': axis_a,
                'axis_b': axis_b,
                'collision': collision,
                'breaking_threshold': breaking_threshold,
                'limits': limits,
                'motor': motor,
                'disable_collision': disable_collision_between_linked_bodies
            },
            'creation_time': time.time(),
            'is_enabled': True,
            'group': None,
            'tags': set(),
            'feedback': None,
            'motor_enabled': motor is not None,
            'current_angle': 0.0,
            'angular_velocity': 0.0
        }
        
        self.constraints.append(constraint_info)
        return constraint
        
    def create_slider_constraint(self, body_a, body_b, frame_a=None, frame_b=None,
                               collision=True, breaking_threshold=0.0, limits=None,
                               motor=None, disable_collision_between_linked_bodies=True):
        """
        创建滑动约束
        
        Args:
            body_a (RigidBody): 第一个刚体
            body_b (RigidBody): 第二个刚体
            frame_a (TransformState): 第一个刚体上的参考帧
            frame_b (TransformState): 第二个刚体上的参考帧
            collision (bool): 是否允许两个刚体碰撞
            breaking_threshold (float): 破裂阈值
            limits (tuple): 滑动限制 (linear_min, linear_max, angular_min, angular_max) 或 None
            motor (dict): 马达参数 {'target_linear_velocity': float, 'max_linear_force': float,
                                  'target_angular_velocity': float, 'max_angular_force': float}
            disable_collision_between_linked_bodies (bool): 是否禁用连接刚体间的碰撞
            
        Returns:
            BulletSliderConstraint: 滑动约束对象
        """
        # 如果没有提供参考帧，创建默认的
        if frame_a is None:
            frame_a = TransformState.makePos(Point3(0, 0, 0))
        if frame_b is None:
            frame_b = TransformState.makePos(Point3(0, 0, 0))
            
        constraint = BulletSliderConstraint(
            body_a.bullet_body, body_b.bullet_body, frame_a, frame_b, useLinearReferenceFrameA=True)
            
        # 设置约束属性
        constraint.setOverrideEnabled(not collision)
        if breaking_threshold > 0:
            constraint.setBreakingImpulseThreshold(breaking_threshold)
            
        # 设置滑动限制
        if limits:
            linear_min, linear_max, angular_min, angular_max = limits
            constraint.setLowerLinLimit(linear_min)
            constraint.setUpperLinLimit(linear_max)
            constraint.setLowerAngLimit(angular_min)
            constraint.setUpperAngLimit(angular_max)
            
        # 设置马达
        if motor:
            # 滑动约束的马达设置比较复杂，需要分别设置线性和角马达
            pass
            
        # 禁用连接刚体间的碰撞
        if disable_collision_between_linked_bodies:
            body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, True)
            
        # 保存约束信息
        constraint_info = {
            'type': self.CONSTRAINT_SLIDER,
            'constraint': constraint,
            'body_a': body_a,
            'body_b': body_b,
            'properties': {
                'frame_a': frame_a,
                'frame_b': frame_b,
                'collision': collision,
                'breaking_threshold': breaking_threshold,
                'limits': limits,
                'motor': motor,
                'disable_collision': disable_collision_between_linked_bodies
            },
            'creation_time': time.time(),
            'is_enabled': True,
            'group': None,
            'tags': set(),
            'feedback': None,
            'motor_enabled': motor is not None,
            'current_linear_position': 0.0,
            'current_angular_position': 0.0
        }
        
        self.constraints.append(constraint_info)
        return constraint
        
    def create_cone_twist_constraint(self, body_a, body_b, frame_a=None, frame_b=None,
                                   collision=True, breaking_threshold=0.0, limits=None,
                                   motor=None, disable_collision_between_linked_bodies=True):
        """
        创建锥形扭转约束
        
        Args:
            body_a (RigidBody): 第一个刚体
            body_b (RigidBody): 第二个刚体
            frame_a (TransformState): 第一个刚体上的参考帧
            frame_b (TransformState): 第二个刚体上的参考帧
            collision (bool): 是否允许两个刚体碰撞
            breaking_threshold (float): 破裂阈值
            limits (tuple): 限制 (swing_span1, swing_span2, twist_span) 或 None
            motor (dict): 马达参数
            disable_collision_between_linked_bodies (bool): 是否禁用连接刚体间的碰撞
            
        Returns:
            BulletConeTwistConstraint: 锥形扭转约束对象
        """
        # 如果没有提供参考帧，创建默认的
        if frame_a is None:
            frame_a = TransformState.makePos(Point3(0, 0, 0))
        if frame_b is None:
            frame_b = TransformState.makePos(Point3(0, 0, 0))
            
        constraint = BulletConeTwistConstraint(
            body_a.bullet_body, body_b.bullet_body, frame_a, frame_b)
            
        # 设置约束属性
        constraint.setOverrideEnabled(not collision)
        if breaking_threshold > 0:
            constraint.setBreakingImpulseThreshold(breaking_threshold)
            
        # 设置限制
        if limits:
            swing_span1, swing_span2, twist_span = limits
            constraint.setLimit(swing_span1, swing_span2, twist_span)
            
        # 禁用连接刚体间的碰撞
        if disable_collision_between_linked_bodies:
            body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, True)
            
        # 保存约束信息
        constraint_info = {
            'type': self.CONSTRAINT_CONETWIST,
            'constraint': constraint,
            'body_a': body_a,
            'body_b': body_b,
            'properties': {
                'frame_a': frame_a,
                'frame_b': frame_b,
                'collision': collision,
                'breaking_threshold': breaking_threshold,
                'limits': limits,
                'motor': motor,
                'disable_collision': disable_collision_between_linked_bodies
            },
            'creation_time': time.time(),
            'is_enabled': True,
            'group': None,
            'tags': set(),
            'feedback': None
        }
        
        self.constraints.append(constraint_info)
        return constraint
        
    def create_6dof_constraint(self, body_a, body_b, frame_a=None, frame_b=None,
                             collision=True, breaking_threshold=0.0, linear_limits=None,
                             angular_limits=None, motors=None, springs=None,
                             disable_collision_between_linked_bodies=True):
        """
        创建6自由度约束
        
        Args:
            body_a (RigidBody): 第一个刚体
            body_b (RigidBody): 第二个刚体
            frame_a (TransformState): 第一个刚体上的参考帧
            frame_b (TransformState): 第二个刚体上的参考帧
            collision (bool): 是否允许两个刚体碰撞
            breaking_threshold (float): 破裂阈值
            linear_limits (tuple): 线性限制 (x_min, x_max, y_min, y_max, z_min, z_max) 或 None
            angular_limits (tuple): 角度限制 (x_min, x_max, y_min, y_max, z_min, z_max) 或 None
            motors (dict): 马达参数 {'linear': {axis: {'target_velocity', 'max_force'}},
                                   'angular': {axis: {'target_velocity', 'max_force'}}}
            springs (dict): 弹簧参数 {'linear': {axis: {'stiffness', 'damping', 'equilibrium'}},
                                    'angular': {axis: {'stiffness', 'damping', 'equilibrium'}}}
            disable_collision_between_linked_bodies (bool): 是否禁用连接刚体间的碰撞
            
        Returns:
            BulletGeneric6DofConstraint: 6自由度约束对象
        """
        # 如果没有提供参考帧，创建默认的
        if frame_a is None:
            frame_a = TransformState.makePos(Point3(0, 0, 0))
        if frame_b is None:
            frame_b = TransformState.makePos(Point3(0, 0, 0))
            
        constraint = BulletGeneric6DofConstraint(
            body_a.bullet_body, body_b.bullet_body, frame_a, frame_b, useLinearReferenceFrameA=True)
            
        # 设置约束属性
        constraint.setOverrideEnabled(not collision)
        if breaking_threshold > 0:
            constraint.setBreakingImpulseThreshold(breaking_threshold)
            
        # 设置线性限制
        if linear_limits:
            x_min, x_max, y_min, y_max, z_min, z_max = linear_limits
            constraint.setLinearLowerLimit(Vec3(x_min, y_min, z_min))
            constraint.setLinearUpperLimit(Vec3(x_max, y_max, z_max))
            
        # 设置角度限制
        if angular_limits:
            x_min, x_max, y_min, y_max, z_min, z_max = angular_limits
            constraint.setAngularLowerLimit(Vec3(x_min, y_min, z_min))
            constraint.setAngularUpperLimit(Vec3(x_max, y_max, z_max))
            
        # 设置马达
        if motors:
            linear_motors = motors.get('linear', {})
            angular_motors = motors.get('angular', {})
            
            for axis in range(3):
                # 线性马达
                if axis in linear_motors:
                    motor_info = linear_motors[axis]
                    constraint.setTargetVelocity(axis, motor_info.get('target_velocity', 0.0))
                    constraint.setMaxMotorForce(axis, motor_info.get('max_force', 1.0))
                    constraint.setParam(axis, 1)  # 启用马达
                    
                # 角马达
                if axis in angular_motors:
                    motor_info = angular_motors[axis]
                    constraint.setTargetVelocity(axis + 3, motor_info.get('target_velocity', 0.0))
                    constraint.setMaxMotorForce(axis + 3, motor_info.get('max_force', 1.0))
                    constraint.setParam(axis + 3, 1)  # 启用马达
                    
        # 禁用连接刚体间的碰撞
        if disable_collision_between_linked_bodies:
            body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, True)
            
        # 保存约束信息
        constraint_info = {
            'type': self.CONSTRAINT_6DOF,
            'constraint': constraint,
            'body_a': body_a,
            'body_b': body_b,
            'properties': {
                'frame_a': frame_a,
                'frame_b': frame_b,
                'collision': collision,
                'breaking_threshold': breaking_threshold,
                'linear_limits': linear_limits,
                'angular_limits': angular_limits,
                'motors': motors,
                'springs': springs,
                'disable_collision': disable_collision_between_linked_bodies
            },
            'creation_time': time.time(),
            'is_enabled': True,
            'group': None,
            'tags': set(),
            'feedback': None,
            'motor_enabled': motors is not None,
            'spring_enabled': springs is not None
        }
        
        self.constraints.append(constraint_info)
        return constraint
        
    def create_6dof_spring_constraint(self, body_a, body_b, frame_a=None, frame_b=None,
                                    collision=True, breaking_threshold=0.0, linear_limits=None,
                                    angular_limits=None, springs=None, motors=None,
                                    disable_collision_between_linked_bodies=True):
        """
        创建6自由度弹簧约束
        
        Args:
            body_a (RigidBody): 第一个刚体
            body_b (RigidBody): 第二个刚体
            frame_a (TransformState): 第一个刚体上的参考帧
            frame_b (TransformState): 第二个刚体上的参考帧
            collision (bool): 是否允许两个刚体碰撞
            breaking_threshold (float): 破裂阈值
            linear_limits (tuple): 线性限制 (x_min, x_max, y_min, y_max, z_min, z_max) 或 None
            angular_limits (tuple): 角度限制 (x_min, x_max, y_min, y_max, z_min, z_max) 或 None
            springs (dict): 弹簧参数 {'linear': {axis: {'stiffness', 'damping', 'equilibrium'}},
                                    'angular': {axis: {'stiffness', 'damping', 'equilibrium'}}}
            motors (dict): 马达参数
            disable_collision_between_linked_bodies (bool): 是否禁用连接刚体间的碰撞
            
        Returns:
            BulletGeneric6DofSpringConstraint: 6自由度弹簧约束对象
        """
        # 如果没有提供参考帧，创建默认的
        if frame_a is None:
            frame_a = TransformState.makePos(Point3(0, 0, 0))
        if frame_b is None:
            frame_b = TransformState.makePos(Point3(0, 0, 0))
            
        constraint = BulletGeneric6DofSpringConstraint(
            body_a.bullet_body, body_b.bullet_body, frame_a, frame_b, useLinearReferenceFrameA=True)
            
        # 设置约束属性
        constraint.setOverrideEnabled(not collision)
        if breaking_threshold > 0:
            constraint.setBreakingImpulseThreshold(breaking_threshold)
            
        # 设置线性限制
        if linear_limits:
            x_min, x_max, y_min, y_max, z_min, z_max = linear_limits
            constraint.setLinearLowerLimit(Vec3(x_min, y_min, z_min))
            constraint.setLinearUpperLimit(Vec3(x_max, y_max, z_max))
            
        # 设置角度限制
        if angular_limits:
            x_min, x_max, y_min, y_max, z_min, z_max = angular_limits
            constraint.setAngularLowerLimit(Vec3(x_min, y_min, z_min))
            constraint.setAngularUpperLimit(Vec3(x_max, y_max, z_max))
            
        # 设置弹簧
        if springs:
            linear_springs = springs.get('linear', {})
            angular_springs = springs.get('angular', {})
            
            for axis in range(3):
                # 线性弹簧
                if axis in linear_springs:
                    spring_info = linear_springs[axis]
                    constraint.setStiffness(axis, spring_info.get('stiffness', 0.0))
                    constraint.setDamping(axis, spring_info.get('damping', 0.0))
                    constraint.setEquilibriumPoint(axis, spring_info.get('equilibrium', 0.0))
                    constraint.enableSpring(axis, True)
                    
                # 角弹簧
                if axis in angular_springs:
                    spring_info = angular_springs[axis]
                    constraint.setStiffness(axis + 3, spring_info.get('stiffness', 0.0))
                    constraint.setDamping(axis + 3, spring_info.get('damping', 0.0))
                    constraint.setEquilibriumPoint(axis + 3, spring_info.get('equilibrium', 0.0))
                    constraint.enableSpring(axis + 3, True)
                    
        # 禁用连接刚体间的碰撞
        if disable_collision_between_linked_bodies:
            body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, True)
            
        # 保存约束信息
        constraint_info = {
            'type': self.CONSTRAINT_6DOF_SPRING,
            'constraint': constraint,
            'body_a': body_a,
            'body_b': body_b,
            'properties': {
                'frame_a': frame_a,
                'frame_b': frame_b,
                'collision': collision,
                'breaking_threshold': breaking_threshold,
                'linear_limits': linear_limits,
                'angular_limits': angular_limits,
                'springs': springs,
                'motors': motors,
                'disable_collision': disable_collision_between_linked_bodies
            },
            'creation_time': time.time(),
            'is_enabled': True,
            'group': None,
            'tags': set(),
            'feedback': None,
            'spring_enabled': springs is not None,
            'motor_enabled': motors is not None
        }
        
        self.constraints.append(constraint_info)
        return constraint
        
    def create_gear_constraint(self, body_a, body_b, axis_a, axis_b, ratio=1.0,
                             collision=True, breaking_threshold=0.0):
        """
        创建齿轮约束
        
        Args:
            body_a (RigidBody): 第一个刚体
            body_b (RigidBody): 第二个刚体
            axis_a (Vec3): 第一个刚体上的轴向
            axis_b (Vec3): 第二个刚体上的轴向
            ratio (float): 齿轮比
            collision (bool): 是否允许两个刚体碰撞
            breaking_threshold (float): 破裂阈值
            
        Returns:
            BulletGearConstraint: 齿轮约束对象
        """
        constraint = BulletGearConstraint(
            body_a.bullet_body, body_b.bullet_body, axis_a, axis_b, ratio)
            
        # 设置约束属性
        constraint.setOverrideEnabled(not collision)
        if breaking_threshold > 0:
            constraint.setBreakingImpulseThreshold(breaking_threshold)
            
        # 保存约束信息
        constraint_info = {
            'type': self.CONSTRAINT_GEAR,
            'constraint': constraint,
            'body_a': body_a,
            'body_b': body_b,
            'properties': {
                'axis_a': axis_a,
                'axis_b': axis_b,
                'ratio': ratio,
                'collision': collision,
                'breaking_threshold': breaking_threshold
            },
            'creation_time': time.time(),
            'is_enabled': True,
            'group': None,
            'tags': set(),
            'feedback': None
        }
        
        self.constraints.append(constraint_info)
        return constraint
        
    def create_fixed_constraint(self, body_a, body_b, frame_a=None, frame_b=None,
                              collision=True, breaking_threshold=0.0,
                              disable_collision_between_linked_bodies=True):
        """
        创建固定约束
        
        Args:
            body_a (RigidBody): 第一个刚体
            body_b (RigidBody): 第二个刚体
            frame_a (TransformState): 第一个刚体上的参考帧
            frame_b (TransformState): 第二个刚体上的参考帧
            collision (bool): 是否允许两个刚体碰撞
            breaking_threshold (float): 破裂阈值
            disable_collision_between_linked_bodies (bool): 是否禁用连接刚体间的碰撞
            
        Returns:
            BulletFixedConstraint: 固定约束对象
        """
        # 如果没有提供参考帧，创建默认的
        if frame_a is None:
            frame_a = TransformState.makePos(Point3(0, 0, 0))
        if frame_b is None:
            frame_b = TransformState.makePos(Point3(0, 0, 0))
            
        constraint = BulletFixedConstraint(
            body_a.bullet_body, body_b.bullet_body, frame_a, frame_b)
            
        # 设置约束属性
        constraint.setOverrideEnabled(not collision)
        if breaking_threshold > 0:
            constraint.setBreakingImpulseThreshold(breaking_threshold)
            
        # 禁用连接刚体间的碰撞
        if disable_collision_between_linked_bodies:
            body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, True)
            
        # 保存约束信息
        constraint_info = {
            'type': self.CONSTRAINT_FIXED,
            'constraint': constraint,
            'body_a': body_a,
            'body_b': body_b,
            'properties': {
                'frame_a': frame_a,
                'frame_b': frame_b,
                'collision': collision,
                'breaking_threshold': breaking_threshold,
                'disable_collision': disable_collision_between_linked_bodies
            },
            'creation_time': time.time(),
            'is_enabled': True,
            'group': None,
            'tags': set(),
            'feedback': None
        }
        
        self.constraints.append(constraint_info)
        return constraint
        
    def remove_constraint(self, constraint):
        """
        移除约束
        
        Args:
            constraint: 约束对象
        """
        for constraint_info in self.constraints:
            if constraint_info['constraint'] == constraint:
                # 重新启用刚体间的碰撞
                if constraint_info['properties'].get('disable_collision', False):
                    body_a = constraint_info['body_a']
                    body_b = constraint_info['body_b']
                    body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, False)
                    
                self.constraints.remove(constraint_info)
                return True
        return False
        
    def get_constraint_info(self, constraint):
        """
        获取约束信息
        
        Args:
            constraint: 约束对象
            
        Returns:
            dict: 约束信息字典
        """
        for constraint_info in self.constraints:
            if constraint_info['constraint'] == constraint:
                return constraint_info
        return None
        
    def set_motor(self, constraint, axis, target_velocity, max_impulse):
        """
        设置约束马达
        
        Args:
            constraint: 约束对象
            axis (int): 轴向 (0=X, 1=Y, 2=Z for linear, 3=X, 4=Y, 5=Z for angular)
            target_velocity (float): 目标速度
            max_impulse (float): 最大冲量
        """
        constraint_info = self.get_constraint_info(constraint)
        if not constraint_info:
            return
            
        constraint_type = constraint_info['type']
        if constraint_type == self.CONSTRAINT_HINGE:
            constraint.setMotorTargetVelocity(target_velocity)
            constraint.setMaxMotorImpulse(max_impulse)
            constraint.enableAngularMotor(True)
            constraint_info['motor_enabled'] = True
        elif constraint_type == self.CONSTRAINT_6DOF:
            constraint.setTargetVelocity(axis, target_velocity)
            constraint.setMaxMotorForce(axis, max_impulse)
            constraint.setParam(axis, 1)  # 启用马达
            constraint_info['motor_enabled'] = True
        elif constraint_type == self.CONSTRAINT_6DOF_SPRING:
            constraint.setTargetVelocity(axis, target_velocity)
            constraint.setMaxMotorForce(axis, max_impulse)
            constraint.setParam(axis, 1)  # 启用马达
            constraint_info['motor_enabled'] = True
            
    def disable_motor(self, constraint):
        """
        禁用约束马达
        
        Args:
            constraint: 约束对象
        """
        constraint_info = self.get_constraint_info(constraint)
        if not constraint_info:
            return
            
        constraint_type = constraint_info['type']
        if constraint_type == self.CONSTRAINT_HINGE:
            constraint.enableAngularMotor(False)
            constraint_info['motor_enabled'] = False
        elif constraint_type == self.CONSTRAINT_6DOF:
            # 禁用所有轴的马达
            for axis in range(6):
                constraint.setParam(axis, 0)
            constraint_info['motor_enabled'] = False
        elif constraint_type == self.CONSTRAINT_6DOF_SPRING:
            # 禁用所有轴的马达
            for axis in range(6):
                constraint.setParam(axis, 0)
            constraint_info['motor_enabled'] = False
            
    def set_spring(self, constraint, axis, stiffness, damping, equilibrium=0.0):
        """
        设置约束弹簧
        
        Args:
            constraint: 约束对象
            axis (int): 轴向 (0=X, 1=Y, 2=Z for linear, 3=X, 4=Y, 5=Z for angular)
            stiffness (float): 刚度
            damping (float): 阻尼
            equilibrium (float): 平衡点
        """
        constraint_info = self.get_constraint_info(constraint)
        if not constraint_info:
            return
            
        constraint_type = constraint_info['type']
        if constraint_type == self.CONSTRAINT_6DOF_SPRING:
            constraint.setStiffness(axis, stiffness)
            constraint.setDamping(axis, damping)
            constraint.setEquilibriumPoint(axis, equilibrium)
            constraint.enableSpring(axis, True)
            constraint_info['spring_enabled'] = True
            
    def disable_spring(self, constraint, axis):
        """
        禁用约束弹簧
        
        Args:
            constraint: 约束对象
            axis (int): 轴向
        """
        constraint_info = self.get_constraint_info(constraint)
        if not constraint_info:
            return
            
        constraint_type = constraint_info['type']
        if constraint_type == self.CONSTRAINT_6DOF_SPRING:
            constraint.enableSpring(axis, False)
            constraint_info['spring_enabled'] = False
            
    def set_limit(self, constraint, axis, lower_limit, upper_limit):
        """
        设置约束限制
        
        Args:
            constraint: 约束对象
            axis (int): 轴向
            lower_limit (float): 下限
            upper_limit (float): 上限
        """
        constraint_info = self.get_constraint_info(constraint)
        if not constraint_info:
            return
            
        constraint_type = constraint_info['type']
        if constraint_type == self.CONSTRAINT_6DOF:
            if axis < 3:  # 线性轴
                current_lower = constraint.getLinearLowerLimit()
                current_upper = constraint.getLinearUpperLimit()
                if axis == 0:
                    current_lower.setX(lower_limit)
                    current_upper.setX(upper_limit)
                elif axis == 1:
                    current_lower.setY(lower_limit)
                    current_upper.setY(upper_limit)
                elif axis == 2:
                    current_lower.setZ(lower_limit)
                    current_upper.setZ(upper_limit)
                constraint.setLinearLowerLimit(current_lower)
                constraint.setLinearUpperLimit(current_upper)
            else:  # 角轴
                current_lower = constraint.getAngularLowerLimit()
                current_upper = constraint.getAngularUpperLimit()
                axis_index = axis - 3
                if axis_index == 0:
                    current_lower.setX(lower_limit)
                    current_upper.setX(upper_limit)
                elif axis_index == 1:
                    current_lower.setY(lower_limit)
                    current_upper.setY(upper_limit)
                elif axis_index == 2:
                    current_lower.setZ(lower_limit)
                    current_upper.setZ(upper_limit)
                constraint.setAngularLowerLimit(current_lower)
                constraint.setAngularUpperLimit(current_upper)
                
    def enable_constraint(self, constraint, enabled=True):
        """
        启用或禁用约束
        
        Args:
            constraint: 约束对象
            enabled (bool): 是否启用
        """
        constraint_info = self.get_constraint_info(constraint)
        if not constraint_info:
            return
            
        constraint_info['is_enabled'] = enabled
        
    def add_constraint_to_group(self, constraint, group_name):
        """
        将约束添加到组
        
        Args:
            constraint: 约束对象
            group_name (str): 组名
        """
        constraint_info = self.get_constraint_info(constraint)
        if not constraint_info:
            return
            
        constraint_info['group'] = group_name
        if group_name not in self.constraint_groups:
            self.constraint_groups[group_name] = []
        if constraint not in self.constraint_groups[group_name]:
            self.constraint_groups[group_name].append(constraint)
            
    def remove_constraint_from_group(self, constraint, group_name):
        """
        将约束从组中移除
        
        Args:
            constraint: 约束对象
            group_name (str): 组名
        """
        constraint_info = self.get_constraint_info(constraint)
        if not constraint_info:
            return
            
        constraint_info['group'] = None
        if group_name in self.constraint_groups and constraint in self.constraint_groups[group_name]:
            self.constraint_groups[group_name].remove(constraint)
            
    def add_constraint_tag(self, constraint, tag):
        """
        为约束添加标签
        
        Args:
            constraint: 约束对象
            tag (str): 标签
        """
        constraint_info = self.get_constraint_info(constraint)
        if not constraint_info:
            return
            
        constraint_info['tags'].add(tag)
        
    def remove_constraint_tag(self, constraint, tag):
        """
        为约束移除标签
        
        Args:
            constraint: 约束对象
            tag (str): 标签
        """
        constraint_info = self.get_constraint_info(constraint)
        if not constraint_info:
            return
            
        constraint_info['tags'].discard(tag)
        
    def get_constraints_by_tag(self, tag):
        """
        根据标签获取约束
        
        Args:
            tag (str): 标签
            
        Returns:
            list: 约束列表
        """
        result = []
        for constraint_info in self.constraints:
            if tag in constraint_info['tags']:
                result.append(constraint_info['constraint'])
        return result
        
    def get_constraints_by_group(self, group_name):
        """
        根据组名获取约束
        
        Args:
            group_name (str): 组名
            
        Returns:
            list: 约束列表
        """
        return self.constraint_groups.get(group_name, [])
        
    def get_constraint_state(self, constraint):
        """
        获取约束状态
        
        Args:
            constraint: 约束对象
            
        Returns:
            dict: 约束状态
        """
        constraint_info = self.get_constraint_info(constraint)
        if not constraint_info:
            return None
            
        state = {
            'type': constraint_info['type'],
            'is_enabled': constraint_info['is_enabled'],
            'group': constraint_info['group'],
            'tags': list(constraint_info['tags']),
            'creation_time': constraint_info['creation_time'],
            'age': time.time() - constraint_info['creation_time']
        }
        
        # 添加特定于约束类型的状态信息
        if constraint_info['type'] == self.CONSTRAINT_HINGE:
            state['current_angle'] = constraint.getHingeAngle()
            state['motor_enabled'] = constraint_info.get('motor_enabled', False)
        elif constraint_info['type'] == self.CONSTRAINT_6DOF:
            state['motor_enabled'] = constraint_info.get('motor_enabled', False)
            state['spring_enabled'] = constraint_info.get('spring_enabled', False)
        elif constraint_info['type'] == self.CONSTRAINT_6DOF_SPRING:
            state['motor_enabled'] = constraint_info.get('motor_enabled', False)
            state['spring_enabled'] = constraint_info.get('spring_enabled', False)
            
        return state
        
    def clear_all_constraints(self):
        """
        清除所有约束
        """
        # 重新启用所有刚体间的碰撞
        for constraint_info in self.constraints:
            if constraint_info['properties'].get('disable_collision', False):
                body_a = constraint_info['body_a']
                body_b = constraint_info['body_b']
                body_a.bullet_body.setIgnoreCollisionCheck(body_b.bullet_body, False)
                
        self.constraints.clear()
        self.constraint_groups.clear()
        self.constraint_limits.clear()
        self.constraint_motors.clear()
        self.constraint_springs.clear()
        
        print("所有约束已清除")
        
    def update_constraint_feedback(self, time_delta):
        """
        更新约束反馈信息
        
        Args:
            time_delta (float): 时间增量
        """
        for constraint_info in self.constraints:
            constraint = constraint_info['constraint']
            constraint_type = constraint_info['type']
            
            # 更新约束反馈信息
            if constraint_type == self.CONSTRAINT_HINGE:
                constraint_info['current_angle'] = constraint.getHingeAngle()
                # 可以添加更多反馈信息
            elif constraint_type == self.CONSTRAINT_SLIDER:
                constraint_info['current_linear_position'] = constraint.getLinearPos()
                constraint_info['current_angular_position'] = constraint.getAngularPos()
                
    def get_constraint_statistics(self):
        """
        获取约束统计信息
        
        Returns:
            dict: 统计信息
        """
        type_counts = {}
        group_counts = {}
        tag_counts = {}
        
        for constraint_info in self.constraints:
            # 统计类型
            constraint_type = constraint_info['type']
            type_counts[constraint_type] = type_counts.get(constraint_type, 0) + 1
            
            # 统计组
            group = constraint_info['group']
            if group:
                group_counts[group] = group_counts.get(group, 0) + 1
                
            # 统计标签
            for tag in constraint_info['tags']:
                tag_counts[tag] = tag_counts.get(tag, 0) + 1
                
        return {
            'total_constraints': len(self.constraints),
            'type_distribution': type_counts,
            'group_distribution': group_counts,
            'tag_distribution': tag_counts,
            'groups': list(self.constraint_groups.keys())
        }