
"""
调试和可视化工具模块
负责物理系统的调试可视化和性能分析
"""

from panda3d.core import Vec3, Point3, Vec4, LVector3f
from panda3d.core import NodePath, CardMaker, TextNode, LineSegs
from panda3d.core import GeomVertexFormat, GeomVertexData, GeomVertexWriter
from panda3d.core import GeomTriangles, Geom, GeomNode
from panda3d.core import ClockObject, ConfigVariableBool
import math
import time
from collections import deque


class PhysicsDebug:
    """
    物理调试类
    提供物理系统的可视化和调试功能
    """
    
    def __init__(self, physics_world, render_node):
        """
        初始化物理调试工具
        
        Args:
            physics_world (PhysicsWorld): 物理世界对象
            render_node (NodePath): 渲染节点
        """
        self.physics_world = physics_world
        self.render_node = render_node
        
        # 调试可视化设置
        self.debug_enabled = ConfigVariableBool('physics-debug-enabled', False).getValue()
        self.show_constraints = ConfigVariableBool('physics-show-constraints', True).getValue()
        self.show_aabbs = ConfigVariableBool('physics-show-aabbs', False).getValue()
        self.show_contacts = ConfigVariableBool('physics-show-contacts', False).getValue()
        self.show_normals = ConfigVariableBool('physics-show-normals', False).getValue()
        self.show_velocities = ConfigVariableBool('physics-show-velocities', False).getValue()
        self.show_forces = ConfigVariableBool('physics-show-forces', False).getValue()
        self.show_axes = ConfigVariableBool('physics-show-axes', False).getValue()
        self.show_mass_centers = ConfigVariableBool('physics-show-mass-centers', False).getValue()
        self.show_collision_shapes = ConfigVariableBool('physics-show-collision-shapes', True).getValue()
        self.show_sleeping_states = ConfigVariableBool('physics-show-sleeping-states', True).getValue()
        self.show_particle_trails = ConfigVariableBool('physics-show-particle-trails', False).getValue()
        
        # 可视化节点
        self.debug_root = self.render_node.attachNewNode("PhysicsDebug")
        self.constraint_node = self.debug_root.attachNewNode("Constraints")
        self.aabb_node = self.debug_root.attachNewNode("AABBs")
        self.contact_node = self.debug_root.attachNewNode("Contacts")
        self.normal_node = self.debug_root.attachNewNode("Normals")
        self.velocity_node = self.debug_root.attachNewNode("Velocities")
        self.force_node = self.debug_root.attachNewNode("Forces")
        self.axes_node = self.debug_root.attachNewNode("Axes")
        self.mass_center_node = self.debug_root.attachNewNode("MassCenters")
        self.collision_shape_node = self.debug_root.attachNewNode("CollisionShapes")
        self.sleeping_node = self.debug_root.attachNewNode("SleepingStates")
        self.particle_trail_node = self.debug_root.attachNewNode("ParticleTrails")
        
        # 调试颜色配置
        self.colors = {
            'static': Vec4(0.5, 0.5, 0.5, 0.7),      # 灰色 - 静态物体
            'dynamic': Vec4(0.0, 1.0, 0.0, 0.7),     # 绿色 - 动态物体
            'kinematic': Vec4(0.0, 0.0, 1.0, 0.7),   # 蓝色 - 运动物体
            'sleeping': Vec4(0.3, 0.3, 0.3, 0.5),   # 深灰色 - 休眠物体
            'constraint': Vec4(1.0, 1.0, 0.0, 0.8),  # 黄色 - 约束
            'contact': Vec4(1.0, 0.0, 0.0, 0.8),     # 红色 - 接触点
            'normal': Vec4(0.0, 1.0, 1.0, 0.8),      # 青色 - 法线
            'velocity': Vec4(1.0, 0.5, 0.0, 0.8),    # 橙色 - 速度
            'force': Vec4(1.0, 0.0, 1.0, 0.8),       # 紫色 - 力
            'positive_axis': Vec4(1.0, 0.0, 0.0, 0.9),  # 红色 - X轴
            'negative_axis': Vec4(0.0, 1.0, 0.0, 0.9),  # 绿色 - Y轴
            'neutral_axis': Vec4(0.0, 0.0, 1.0, 0.9),   # 蓝色 - Z轴
            'mass_center': Vec4(1.0, 1.0, 0.0, 0.9),    # 黄色 - 质心
            'particle_trail': Vec4(0.8, 0.8, 1.0, 0.6),  # 浅蓝色 - 粒子轨迹
        }
        
        # 可视化参数
        self.line_thickness = 2.0
        self.axis_length = 1.0
        self.velocity_scale = 0.1
        self.force_scale = 0.01
        self.contact_point_size = 0.1
        self.normal_scale = 0.5
        self.aabb_line_style = 'solid'  # solid, dashed, dotted
        self.collision_shape_style = 'wireframe'  # wireframe, solid, transparent
        
        # 性能监控
        self.debug_update_frequency = 30  # 每秒更新次数
        self.debug_update_counter = 0
        self.last_debug_update = 0.0
        
        # 调试文本显示
        self.debug_text_enabled = True
        self.debug_text_node = None
        self.debug_text = ""
        
        # 碰撞可视化历史
        self.contact_history = deque(maxlen=100)
        self.collision_history = deque(maxlen=50)
        
        # 性能统计显示
        self.stats_display_enabled = True
        self.stats_text_node = None
        
        # 初始化调试文本
        self._init_debug_text()
        
        # 根据初始状态显示或隐藏
        if self.debug_enabled:
            self.debug_root.show()
        else:
            self.debug_root.hide()
            
        print("物理调试工具初始化完成")
        
    def _init_debug_text(self):
        """
        初始化调试文本显示
        """
        # 创建调试文本节点
        self.debug_text_node = TextNode("PhysicsDebugText")
        self.debug_text_node.setText("")
        self.debug_text_node.setTextColor(1, 1, 1, 1)  # 白色
        self.debug_text_node.setAlign(TextNode.ALeft)
        self.debug_text_node.setFontSize(16)
        
        # 创建文本节点路径
        text_np = self.render_node.attachNewNode(self.debug_text_node)
        text_np.setScale(0.05)  # 调整文本大小
        text_np.setPos(-1.5, 0, 0.9)  # 屏幕左上角
        text_np.setBin("fixed", 100)  # 确保在最上层
        text_np.setDepthWrite(False)
        text_np.setDepthTest(False)
        
        # 创建性能统计文本节点
        self.stats_text_node = TextNode("PhysicsStatsText")
        self.stats_text_node.setText("")
        self.stats_text_node.setTextColor(1, 1, 1, 1)  # 白色
        self.stats_text_node.setAlign(TextNode.ARight)
        self.stats_text_node.setFontSize(14)
        
        # 创建统计文本节点路径
        stats_np = self.render_node.attachNewNode(self.stats_text_node)
        stats_np.setScale(0.04)  # 调整文本大小
        stats_np.setPos(1.5, 0, 0.9)  # 屏幕右上角
        stats_np.setBin("fixed", 100)
        stats_np.setDepthWrite(False)
        stats_np.setDepthTest(False)
        
    def enable_debug(self, enabled=True):
        """
        启用或禁用调试可视化
        
        Args:
            enabled (bool): 是否启用调试
        """
        self.debug_enabled = enabled
        if enabled:
            self.debug_root.show()
        else:
            self.debug_root.hide()
            
    def set_debug_options(self, show_constraints=None, show_aabbs=None, 
                         show_contacts=None, show_normals=None, show_velocities=None,
                         show_forces=None, show_axes=None, show_mass_centers=None,
                         show_collision_shapes=None, show_sleeping_states=None,
                         show_particle_trails=None):
        """
        设置调试选项
        
        Args:
            show_constraints (bool): 是否显示约束
            show_aabbs (bool): 是否显示包围盒
            show_contacts (bool): 是否显示接触点
            show_normals (bool): 是否显示法线
            show_velocities (bool): 是否显示速度
            show_forces (bool): 是否显示力
            show_axes (bool): 是否显示坐标轴
            show_mass_centers (bool): 是否显示质心
            show_collision_shapes (bool): 是否显示碰撞形状
            show_sleeping_states (bool): 是否显示休眠状态
            show_particle_trails (bool): 是否显示粒子轨迹
        """
        if show_constraints is not None:
            self.show_constraints = show_constraints
            if show_constraints:
                self.constraint_node.show()
            else:
                self.constraint_node.hide()
                
        if show_aabbs is not None:
            self.show_aabbs = show_aabbs
            if show_aabbs:
                self.aabb_node.show()
            else:
                self.aabb_node.hide()
                
        if show_contacts is not None:
            self.show_contacts = show_contacts
            if show_contacts:
                self.contact_node.show()
            else:
                self.contact_node.hide()
                
        if show_normals is not None:
            self.show_normals = show_normals
            if show_normals:
                self.normal_node.show()
            else:
                self.normal_node.hide()
                
        if show_velocities is not None:
            self.show_velocities = show_velocities
            if show_velocities:
                self.velocity_node.show()
            else:
                self.velocity_node.hide()
                
        if show_forces is not None:
            self.show_forces = show_forces
            if show_forces:
                self.force_node.show()
            else:
                self.force_node.hide()
                
        if show_axes is not None:
            self.show_axes = show_axes
            if show_axes:
                self.axes_node.show()
            else:
                self.axes_node.hide()
                
        if show_mass_centers is not None:
            self.show_mass_centers = show_mass_centers
            if show_mass_centers:
                self.mass_center_node.show()
            else:
                self.mass_center_node.hide()
                
        if show_collision_shapes is not None:
            self.show_collision_shapes = show_collision_shapes
            if show_collision_shapes:
                self.collision_shape_node.show()
            else:
                self.collision_shape_node.hide()
                
        if show_sleeping_states is not None:
            self.show_sleeping_states = show_sleeping_states
            if show_sleeping_states:
                self.sleeping_node.show()
            else:
                self.sleeping_node.hide()
                
        if show_particle_trails is not None:
            self.show_particle_trails = show_particle_trails
            if show_particle_trails:
                self.particle_trail_node.show()
            else:
                self.particle_trail_node.hide()
                
    def update_debug_visualization(self):
        """
        更新调试可视化
        """
        if not self.debug_enabled:
            return
            
        # 控制更新频率
        current_time = ClockObject.getGlobalClock().getFrameTime()
        if current_time - self.last_debug_update < 1.0 / self.debug_update_frequency:
            return
            
        self.last_debug_update = current_time
        self.debug_update_counter += 1
        
        # 清除之前的可视化
        self._clear_debug_visualization()
        
        # 可视化刚体
        self._visualize_rigid_bodies()
        
        # 可视化约束
        if self.show_constraints:
            self._visualize_constraints()
            
        # 可视化接触点
        if self.show_contacts:
            self._visualize_contacts()
            
        # 可视化法线
        if self.show_normals:
            self._visualize_normals()
            
        # 可视化速度
        if self.show_velocities:
            self._visualize_velocities()
            
        # 可视化力
        if self.show_forces:
            self._visualize_forces()
            
        # 可视化坐标轴
        if self.show_axes:
            self._visualize_axes()
            
        # 可视化质心
        if self.show_mass_centers:
            self._visualize_mass_centers()
            
        # 可视化碰撞形状
        if self.show_collision_shapes:
            self._visualize_collision_shapes()
            
        # 可视化休眠状态
        if self.show_sleeping_states:
            self._visualize_sleeping_states()
            
        # 可视化粒子轨迹
        if self.show_particle_trails:
            self._visualize_particle_trails()
            
        # 更新调试文本
        if self.debug_text_enabled:
            self._update_debug_text()
            
        # 更新性能统计
        if self.stats_display_enabled:
            self._update_stats_display()
            
    def _clear_debug_visualization(self):
        """
        清除调试可视化
        """
        # 清除所有子节点
        for child in self.constraint_node.getChildren():
            child.removeNode()
        for child in self.aabb_node.getChildren():
            child.removeNode()
        for child in self.contact_node.getChildren():
            child.removeNode()
        for child in self.normal_node.getChildren():
            child.removeNode()
        for child in self.velocity_node.getChildren():
            child.removeNode()
        for child in self.force_node.getChildren():
            child.removeNode()
        for child in self.axes_node.getChildren():
            child.removeNode()
        for child in self.mass_center_node.getChildren():
            child.removeNode()
        for child in self.collision_shape_node.getChildren():
            child.removeNode()
        for child in self.sleeping_node.getChildren():
            child.removeNode()
        for child in self.particle_trail_node.getChildren():
            child.removeNode()
            
    def _visualize_rigid_bodies(self):
        """
        可视化刚体
        """
        for rigid_body in self.physics_world.rigid_bodies:
            # 获取刚体的AABB
            aabb = rigid_body.bullet_body.getAabb()
            min_point = aabb.getMin()
            max_point = aabb.getMax()
            
            # 创建包围盒可视化
            if self.show_aabbs:
                self._draw_aabb(min_point, max_point, rigid_body)
                
    def _draw_aabb(self, min_point, max_point, rigid_body):
        """
        绘制AABB包围盒
        
        Args:
            min_point (Point3): 最小点
            max_point (Point3): 最大点
            rigid_body (RigidBody): 刚体对象
        """
        # 确定颜色
        if rigid_body.body_type == rigid_body.STATIC:
            color = self.colors['static']
        elif rigid_body.body_type == rigid_body.KINEMATIC:
            color = self.colors['kinematic']
        else:
            color = self.colors['dynamic']
            
        # 如果显示休眠状态且刚体休眠，使用休眠颜色
        if self.show_sleeping_states and not rigid_body.bullet_body.isActive():
            color = self.colors['sleeping']
            
        # 创建线条几何体
        vformat = GeomVertexFormat.getV3c4()
        vdata = GeomVertexData('aabb', vformat, Geom.UHStatic)
        vertex = GeomVertexWriter(vdata, 'vertex')
        color_writer = GeomVertexWriter(vdata, 'color')
        
        # 添加8个顶点
        vertices = [
            Point3(min_point.getX(), min_point.getY(), min_point.getZ()),
            Point3(max_point.getX(), min_point.getY(), min_point.getZ()),
            Point3(max_point.getX(), max_point.getY(), min_point.getZ()),
            Point3(min_point.getX(), max_point.getY(), min_point.getZ()),
            Point3(min_point.getX(), min_point.getY(), max_point.getZ()),
            Point3(max_point.getX(), min_point.getY(), max_point.getZ()),
            Point3(max_point.getX(), max_point.getY(), max_point.getZ()),
            Point3(min_point.getX(), max_point.getY(), max_point.getZ())
        ]
        
        for vert in vertices:
            vertex.addData3(vert)
            color_writer.addData4(color)
            
        # 创建线条
        geom = Geom(vdata)
        lines = GeomLines(Geom.UHStatic)
        
        # 添加12条边
        edges = [
            (0, 1), (1, 2), (2, 3), (3, 0),  # 底面
            (4, 5), (5, 6), (6, 7), (7, 4),  # 顶面
            (0, 4), (1, 5), (2, 6), (3, 7)   # 垂直边
        ]
        
        for start, end in edges:
            lines.addVertex(start)
            lines.addVertex(end)
            lines.closePrimitive()
            
        geom.addPrimitive(lines)
        
        # 创建节点
        node = GeomNode('aabb_geom')
        node.addGeom(geom)
        
        # 添加到场景
        aabb_np = self.aabb_node.attachNewNode(node)
        aabb_np.setRenderModeWireframe()
        
    def _visualize_constraints(self):
        """
        可视化约束
        """
        for constraint_info in self.physics_world.constraints:
            constraint = constraint_info['constraint']
            body_a = constraint_info['body_a']
            body_b = constraint_info['body_b']
            
            # 获取连接点
            if hasattr(constraint, 'getPivotA'):
                pivot_a = constraint.getPivotA()
                pivot_b = constraint.getPivotB()
                
                # 转换到世界坐标
                world_pivot_a = body_a.physics_node.getTransform().getPos() + pivot_a
                world_pivot_b = body_b.physics_node.getTransform().getPos() + pivot_b
                
                # 绘制连接线
                self._draw_line(world_pivot_a, world_pivot_b, self.colors['constraint'])
                
    def _visualize_contacts(self):
        """
        可视化接触点
        """
        # 注意：Bullet不直接提供接触点信息，需要通过碰撞回调获取
        # 这里使用简化的实现，从历史记录中获取
        for contact_info in list(self.contact_history):
            position = contact_info['position']
            normal = contact_info['normal']
            
            # 绘制接触点
            self._draw_point(position, self.colors['contact'], self.contact_point_size)
            
            # 绘制法线
            if self.show_normals:
                end_point = position + normal * self.normal_scale
                self._draw_line(position, end_point, self.colors['normal'])
                
    def _visualize_normals(self):
        """
        可视化法线
        """
        # 从接触历史中绘制法线
        for contact_info in list(self.contact_history):
            position = contact_info['position']
            normal = contact_info['normal']
            end_point = position + normal * self.normal_scale
            self._draw_line(position, end_point, self.colors['normal'])
            
    def _visualize_velocities(self):
        """
        可视化速度
        """
        for rigid_body in self.physics_world.rigid_bodies:
            velocity = rigid_body.bullet_body.getLinearVelocity()
            if velocity.length() > 0.1:  # 只显示大于阈值的速度
                position = rigid_body.physics_node.getPos()
                end_point = position + velocity * self.velocity_scale
                self._draw_line(position, end_point, self.colors['velocity'])
                
                # 添加箭头
                self._draw_arrow_head(end_point, velocity, self.colors['velocity'])
                
    def _visualize_forces(self):
        """
        可视化力
        """
        # 从刚体历史中获取力信息
        for rigid_body in self.physics_world.rigid_bodies:
            # 这里需要从刚体中获取力历史，简化实现
            pass
            
    def _visualize_axes(self):
        """
        可视化坐标轴
        """
        for rigid_body in self.physics_world.rigid_bodies:
            position = rigid_body.physics_node.getPos()
            rotation = rigid_body.physics_node.getHpr()
            
            # 创建坐标系变换
            from panda3d.core import TransformState
            transform = TransformState.makePosHpr(position, rotation)
            
            # X轴（红色）
            x_end = position + transform.getQuat().getRight() * self.axis_length
            self._draw_line(position, x_end, self.colors['positive_axis'])
            self._draw_arrow_head(x_end, transform.getQuat().getRight(), self.colors['positive_axis'])
            
            # Y轴（绿色）
            y_end = position + transform.getQuat().getForward() * self.axis_length
            self._draw_line(position, y_end, self.colors['negative_axis'])
            self._draw_arrow_head(y_end, transform.getQuat().getForward(), self.colors['negative_axis'])
            
            # Z轴（蓝色）
            z_end = position + transform.getQuat().getUp() * self.axis_length
            self._draw_line(position, z_end, self.colors['neutral_axis'])
            self._draw_arrow_head(z_end, transform.getQuat().getUp(), self.colors['neutral_axis'])
            
    def _visualize_mass_centers(self):
        """
        可视化质心
        """
        for rigid_body in self.physics_world.rigid_bodies:
            # 获取质心位置
            center_of_mass = rigid_body.bullet_body.getCenterOfMassTransform().getPos()
            world_position = rigid_body.physics_node.getTransform().getPos() + center_of_mass
            
            # 绘制质心点
            self._draw_point(world_position, self.colors['mass_center'], 0.05)
            
    def _visualize_collision_shapes(self):
        """
        可视化碰撞形状
        """
        for rigid_body in self.physics_world.rigid_bodies:
            # 获取碰撞形状信息并绘制
            # 这里需要更复杂的实现来准确绘制不同形状
            pass
            
    def _visualize_sleeping_states(self):
        """
        可视化休眠状态
        """
        # 休眠状态已经在其他可视化中体现，这里可以添加额外的标识
        for rigid_body in self.physics_world.rigid_bodies:
            if not rigid_body.bullet_body.isActive():
                # 在休眠物体上添加特殊标记
                position = rigid_body.physics_node.getPos()
                self._draw_point(position + Vec3(0, 0, 0.1), self.colors['sleeping'], 0.03)
                
    def _visualize_particle_trails(self):
        """
        可视化粒子轨迹
        """
        # 从粒子系统中获取轨迹信息
        for particle_system in self.physics_world.particles:
            # 绘制每个粒子的轨迹
            # 这里需要访问粒子系统的轨迹数据
            pass
            
    def _draw_line(self, start, end, color, thickness=None):
        """
        绘制线条
        
        Args:
            start (Point3): 起点
            end (Point3): 终点
            color (Vec4): 颜色
            thickness (float): 线条粗细
        """
        if thickness is None:
            thickness = self.line_thickness
            
        # 使用LineSegs绘制线条
        segs = LineSegs('debug_line')
        segs.setThickness(thickness)
        segs.setColor(color)
        segs.moveTo(start)
        segs.drawTo(end)
        
        # 创建节点
        node = segs.create()
        line_np = self.debug_root.attachNewNode(node)
        
    def _draw_point(self, position, color, size=0.1):
        """
        绘制点
        
        Args:
            position (Point3): 位置
            color (Vec4): 颜色
            size (float): 点大小
        """
        # 创建一个小球体表示点
        cm = CardMaker('debug_point')
        cm.setFrame(-size, size, -size, size)
        point_np = self.debug_root.attachNewNode(cm.generate())
        point_np.setPos(position)
        point_np.setColor(color)
        
    def _draw_arrow_head(self, tip_position, direction, color, size=0.1):
        """
        绘制箭头头部
        
        Args:
            tip_position (Point3): 箭头顶点位置
            direction (Vec3): 方向向量
            color (Vec4): 颜色
            size (float): 箭头大小
        """
        # 标准化方向向量
        direction.normalize()
        
        # 计算垂直向量
        perpendicular = direction.cross(Vec3(0, 0, 1))
        if perpendicular.length() < 0.001:
            perpendicular = direction.cross(Vec3(1, 0, 0))
        perpendicular.normalize()
        
        # 计算箭头翼点
        wing1 = tip_position - direction * size + perpendicular * size * 0.5
        wing2 = tip_position - direction * size - perpendicular * size * 0.5
        
        # 绘制箭头翼
        self._draw_line(tip_position, wing1, color)
        self._draw_line(tip_position, wing2, color)
        
    def _update_debug_text(self):
        """
        更新调试文本
        """
        if not self.debug_text_node:
            return
            
        # 构建调试文本
        debug_lines = []
        
        # 添加基本统计
        stats = self.physics_world.get_performance_stats()
        debug_lines.append(f"Physics Debug Info:")
        debug_lines.append(f"  FPS: {stats.get('current_fps', 0):.1f}")
        debug_lines.append(f"  Update Time: {stats.get('last_update_time', 0)*1000:.2f}ms")
        debug_lines.append(f"  Objects: {len(self.physics_world.rigid_bodies)} rigid bodies")
        debug_lines.append(f"  Constraints: {len(self.physics_world.constraints)} constraints")
        debug_lines.append(f"  Particles: {len(self.physics_world.particles)} systems")
        
        # 添加活动物体统计
        active_count = sum(1 for body in self.physics_world.rigid_bodies if body.bullet_body.isActive())
        sleeping_count = len(self.physics_world.rigid_bodies) - active_count
        debug_lines.append(f"  Active Bodies: {active_count}")
        debug_lines.append(f"  Sleeping Bodies: {sleeping_count}")
        
        # 更新文本节点
        self.debug_text = '\n'.join(debug_lines)
        self.debug_text_node.setText(self.debug_text)
        
    def _update_stats_display(self):
        """
        更新性能统计显示
        """
        if not self.stats_text_node:
            return
            
        # 获取性能统计
        stats = self.physics_world.get_performance_stats()
        
        # 构建统计文本
        stat_lines = []
        stat_lines.append("Performance Stats:")
        stat_lines.append(f"Avg Update: {stats.get('avg_update_time', 0)*1000:.2f}ms")
        stat_lines.append(f"Max Update: {stats.get('max_update_time', 0)*1000:.2f}ms")
        stat_lines.append(f"Min Update: {stats.get('min_update_time', 0)*1000:.2f}ms")
        stat_lines.append(f"Objects: {stats.get('objects', {}).get('rigid_bodies', 0)} bodies")
        stat_lines.append(f"Constraints: {stats.get('objects', {}).get('constraints', 0)}")
        stat_lines.append(f"Particles: {stats.get('objects', {}).get('particles', 0)}")
        
        # 更新统计文本节点
        stats_text = '\n'.join(stat_lines)
        self.stats_text_node.setText(stats_text)
        
    def add_contact_point(self, position, normal, impulse):
        """
        添加接触点信息（供外部调用）
        
        Args:
            position (Point3): 接触点位置
            normal (Vec3): 接触法线
            impulse (float): 冲量
        """
        contact_info = {
            'position': position,
            'normal': normal,
            'impulse': impulse,
            'timestamp': time.time()
        }
        self.contact_history.append(contact_info)
        
    def add_collision_event(self, body_a, body_b, contact_points):
        """
        添加碰撞事件（供外部调用）
        
        Args:
            body_a (RigidBody): 第一个刚体
            body_b (RigidBody): 第二个刚体
            contact_points (list): 接触点列表
        """
        collision_info = {
            'body_a': body_a,
            'body_b': body_b,
            'contact_points': contact_points,
            'timestamp': time.time()
        }
        self.collision_history.append(collision_info)
        
    def set_line_thickness(self, thickness):
        """
        设置线条粗细
        
        Args:
            thickness (float): 线条粗细
        """
        self.line_thickness = max(0.1, thickness)
        
    def set_axis_length(self, length):
        """
        设置坐标轴长度
        
        Args:
            length (float): 坐标轴长度
        """
        self.axis_length = max(0.1, length)
        
    def set_velocity_scale(self, scale):
        """
        设置速度可视化缩放
        
        Args:
            scale (float): 速度缩放因子
        """
        self.velocity_scale = max(0.001, scale)
        
    def set_force_scale(self, scale):
        """
        设置力可视化缩放
        
        Args:
            scale (float): 力缩放因子
        """
        self.force_scale = max(0.0001, scale)
        
    def set_contact_point_size(self, size):
        """
        设置接触点大小
        
        Args:
            size (float): 接触点大小
        """
        self.contact_point_size = max(0.01, size)
        
    def set_normal_scale(self, scale):
        """
        设置法线可视化缩放
        
        Args:
            scale (float): 法线缩放因子
        """
        self.normal_scale = max(0.01, scale)
        
    def enable_debug_text(self, enabled=True):
        """
        启用或禁用调试文本显示
        
        Args:
            enabled (bool): 是否启用调试文本
        """
        self.debug_text_enabled = enabled
        if self.debug_text_node:
            if enabled:
                self.debug_text_node.show()
            else:
                self.debug_text_node.hide()
                
    def enable_stats_display(self, enabled=True):
        """
        启用或禁用性能统计显示
        
        Args:
            enabled (bool): 是否启用性能统计显示
        """
        self.stats_display_enabled = enabled
        if self.stats_text_node:
            if enabled:
                self.stats_text_node.show()
            else:
                self.stats_text_node.hide()
                
    def set_debug_update_frequency(self, frequency):
        """
        设置调试更新频率
        
        Args:
            frequency (int): 更新频率（Hz）
        """
        self.debug_update_frequency = max(1, frequency)
        
    def hide_debug_visualization(self):
        """
        隐藏调试可视化
        """
        self.debug_root.hide()
        
    def show_debug_visualization(self):
        """
        显示调试可视化
        """
        if self.debug_enabled:
            self.debug_root.show()
            
    def destroy(self):
        """
        销毁调试工具
        """
        # 清除所有可视化
        self._clear_debug_visualization()
        
        # 移除文本节点
        if self.debug_text_node and not self.debug_text_node.isEmpty():
            self.debug_text_node.removeNode()
        if self.stats_text_node and not self.stats_text_node.isEmpty():
            self.stats_text_node.removeNode()
            
        # 移除节点
        if self.debug_root and not self.debug_root.isEmpty():
            self.debug_root.removeNode()
            
        # 清空历史记录
        self.contact_history.clear()
        self.collision_history.clear()
        
        print("物理调试工具已销毁")


class PhysicsProfiler:
    """
    物理性能分析器
    监控和分析物理系统的性能
    """
    
    def __init__(self, physics_world):
        """
        初始化性能分析器
        
        Args:
            physics_world (PhysicsWorld): 物理世界对象
        """
        self.physics_world = physics_world
        
        # 性能数据
        self.update_times = deque(maxlen=1000)
        self.rigid_body_counts = deque(maxlen=1000)
        self.constraint_counts = deque(maxlen=1000)
        self.particle_counts = deque(maxlen=1000)
        self.collision_counts = deque(maxlen=1000)
        self.active_body_counts = deque(maxlen=1000)
        self.sleeping_body_counts = deque(maxlen=1000)
        
        # 分析参数
        self.max_history = 1000  # 最大历史记录数
        self.sampling_interval = 1.0  # 采样间隔（秒）
        self.last_sample_time = 0.0
        
        # 性能阈值
        self.warning_threshold = 0.016  # 警告阈值（秒）
        self.error_threshold = 0.033   # 错误阈值（秒）
        
        # 统计计数器
        self.total_updates = 0
        self.total_collisions = 0
        self.total_constraints_solved = 0
        self.peak_performance = {
            'max_update_time': 0.0,
            'max_rigid_bodies': 0,
            'max_constraints': 0,
            'max_particles': 0,
            'max_collisions': 0
        }
        
        print("物理性能分析器初始化完成")
        
    def record_frame(self):
        """
        记录帧数据
        """
        current_time = time.time()
        
        # 检查采样间隔
        if current_time - self.last_sample_time < self.sampling_interval:
            return
            
        self.last_sample_time = current_time
        
        # 获取性能统计
        stats = self.physics_world.get_performance_stats()
        
        # 记录更新时间
        update_time = stats.get('last_update_time', 0.0)
        self.update_times.append(update_time)
        
        # 更新峰值性能记录
        if update_time > self.peak_performance['max_update_time']:
            self.peak_performance['max_update_time'] = update_time
            
        # 记录对象数量
        objects = stats.get('objects', {})
        rigid_bodies = objects.get('rigid_bodies', 0)
        constraints = objects.get('constraints', 0)
        particles = objects.get('particles', 0)
        
        self.rigid_body_counts.append(rigid_bodies)
        self.constraint_counts.append(constraints)
        self.particle_counts.append(particles)
        
        # 更新峰值记录
        if rigid_bodies > self.peak_performance['max_rigid_bodies']:
            self.peak_performance['max_rigid_bodies'] = rigid_bodies
        if constraints > self.peak_performance['max_constraints']:
            self.peak_performance['max_constraints'] = constraints
        if particles > self.peak_performance['max_particles']:
            self.peak_performance['max_particles'] = particles
            
        # 记录活动和休眠物体数量
        active_count = 0
        sleeping_count = 0
        for body in self.physics_world.rigid_bodies:
            if body.bullet_body.isActive():
                active_count += 1
            else:
                sleeping_count += 1
                
        self.active_body_counts.append(active_count)
        self.sleeping_body_counts.append(sleeping_count)
        
        # 更新总计数器
        self.total_updates += 1
        
        # 检查性能警告
        self._check_performance_warnings(update_time)
        
    def _check_performance_warnings(self, update_time):
        """
        检查性能警告
        
        Args:
            update_time (float): 更新时间
        """
        if update_time > self.error_threshold:
            print(f"[ERROR] Physics update time exceeded error threshold: {update_time*1000:.2f}ms")
        elif update_time > self.warning_threshold:
            print(f"[WARNING] Physics update time exceeded warning threshold: {update_time*1000:.2f}ms")
            
    def get_average_update_time(self):
        """
        获取平均更新时间
        
        Returns:
            float: 平均更新时间（秒）
        """
        if not self.update_times:
            return 0.0
        return sum(self.update_times) / len(self.update_times)
        
    def get_max_update_time(self):
        """
        获取最大更新时间
        
        Returns:
            float: 最大更新时间（秒）
        """
        if not self.update_times:
            return 0.0
        return max(self.update_times)
        
    def get_min_update_time(self):
        """
        获取最小更新时间
        
        Returns:
            float: 最小更新时间（秒）
        """
        if not self.update_times:
            return 0.0
        return min(self.update_times)
        
    def get_average_fps(self):
        """
        获取平均帧率
        
        Returns:
            float: 平均帧率
        """
        avg_update_time = self.get_average_update_time()
        if avg_update_time > 0:
            return 1.0 / avg_update_time
        return 0.0
        
    def get_object_counts(self):
        """
        获取对象数量统计
        
        Returns:
            dict: 对象数量统计
        """
        return {
            'rigid_bodies': len(self.physics_world.rigid_bodies),
            'constraints': len(self.physics_world.constraints),
            'vehicles': len(self.physics_world.vehicles),
            'particles': sum([ps.get_particle_count() 
                            for ps in self.physics_world.particles]),
            'soft_bodies': len(self.physics_world.soft_bodies),
            'ghost_objects': len(self.physics_world.ghost_objects)
        }
        
    def get_average_object_counts(self):
        """
        获取平均对象数量
        
        Returns:
            dict: 平均对象数量统计
        """
        if not self.rigid_body_counts:
            return self.get_object_counts()
            
        return {
            'rigid_bodies': sum(self.rigid_body_counts) / len(self.rigid_body_counts),
            'constraints': sum(self.constraint_counts) / len(self.constraint_counts),
            'particles': sum(self.particle_counts) / len(self.particle_counts),
            'active_bodies': sum(self.active_body_counts) / len(self.active_body_counts) if self.active_body_counts else 0,
            'sleeping_bodies': sum(self.sleeping_body_counts) / len(self.sleeping_body_counts) if self.sleeping_body_counts else 0
        }
        
    def get_performance_report(self):
        """
        获取性能报告
        
        Returns:
            dict: 性能报告
        """
        return {
            'average_update_time': self.get_average_update_time(),
            'max_update_time': self.get_max_update_time(),
            'min_update_time': self.get_min_update_time(),
            'average_fps': self.get_average_fps(),
            'current_fps': 1.0 / self.get_average_update_time() if self.get_average_update_time() > 0 else 0,
            'object_counts': self.get_object_counts(),
            'average_object_counts': self.get_average_object_counts(),
            'peak_performance': self.peak_performance.copy(),
            'total_updates': self.total_updates,
            'total_collisions': self.total_collisions,
            'history_length': len(self.update_times),
            'sampling_interval': self.sampling_interval
        }
        
    def get_performance_analysis(self):
        """
        获取性能分析
        
        Returns:
            dict: 性能分析结果
        """
        report = self.get_performance_report()
        
        # 计算性能等级
        avg_update_time = report['average_update_time']
        if avg_update_time <= 0.008:  # 8ms
            performance_grade = "Excellent"
        elif avg_update_time <= 0.016:  # 16ms
            performance_grade = "Good"
        elif avg_update_time <= 0.024:  # 24ms
            performance_grade = "Acceptable"
        elif avg_update_time <= 0.033:  # 33ms
            performance_grade = "Poor"
        else:
            performance_grade = "Critical"
            
        # 分析瓶颈
        bottlenecks = []
        if report['object_counts']['rigid_bodies'] > 500:
            bottlenecks.append("High rigid body count")
        if report['object_counts']['constraints'] > 100:
            bottlenecks.append("High constraint count")
        if report['object_counts']['particles'] > 1000:
            bottlenecks.append("High particle count")
        if report['average_object_counts']['active_bodies'] > 200:
            bottlenecks.append("Too many active bodies")
            
        return {
            'performance_grade': performance_grade,
            'bottlenecks': bottlenecks,
            'recommendations': self._generate_recommendations(report),
            'report': report
        }
        
    def _generate_recommendations(self, report):
        """
        生成性能优化建议
        
        Args:
            report (dict): 性能报告
            
        Returns:
            list: 优化建议列表
        """
        recommendations = []
        
        # 基于性能等级的建议
        if report['performance_grade'] == "Poor":
            recommendations.append("Consider reducing physics complexity")
        elif report['performance_grade'] == "Critical":
            recommendations.append("URGENT: Physics performance critical - immediate optimization needed")
            
        # 基于对象数量的建议
        if report['object_counts']['rigid_bodies'] > 500:
            recommendations.append("Reduce rigid body count through batching or simplification")
        if report['object_counts']['constraints'] > 100:
            recommendations.append("Optimize constraint usage - consider removing unnecessary constraints")
        if report['object_counts']['particles'] > 1000:
            recommendations.append("Reduce particle system complexity or limit particle count")
            
        # 基于活动物体的建议
        avg_active = report['average_object_counts']['active_bodies']
        if avg_active > 200:
            recommendations.append("Improve sleeping thresholds to reduce active bodies")
            
        # 基于更新时间的建议
        if report['average_update_time'] > 0.016:
            recommendations.append("Profile physics update to identify slow operations")
            recommendations.append("Consider multithreading if available")
            
        return recommendations
        
    def reset_statistics(self):
        """
        重置统计信息
        """
        self.update_times.clear()
        self.rigid_body_counts.clear()
        self.constraint_counts.clear()
        self.particle_counts.clear()
        self.collision_counts.clear()
        self.active_body_counts.clear()
        self.sleeping_body_counts.clear()
        
        self.total_updates = 0
        self.total_collisions = 0
        self.total_constraints_solved = 0
        
        # 重置峰值记录（保留一些）
        self.peak_performance['max_update_time'] = 0.0
        self.peak_performance['max_collisions'] = 0
        
        print("性能统计已重置")
        
    def export_statistics(self, filepath):
        """
        导出统计信息到文件
        
        Args:
            filepath (str): 导出文件路径
        """
        import json
        
        # 构建统计数据
        stats_data = {
            'performance_report': self.get_performance_report(),
            'performance_analysis': self.get_performance_analysis(),
            'raw_data': {
                'update_times': list(self.update_times),
                'rigid_body_counts': list(self.rigid_body_counts),
                'constraint_counts': list(self.constraint_counts),
                'particle_counts': list(self.particle_counts),
                'collision_counts': list(self.collision_counts),
                'active_body_counts': list(self.active_body_counts),
                'sleeping_body_counts': list(self.sleeping_body_counts)
            },
            'export_timestamp': time.time(),
            'export_date': time.strftime('%Y-%m-%d %H:%M:%S')
        }
        
        # 导出到JSON文件
        try:
            with open(filepath, 'w', encoding='utf-8') as f:
                json.dump(stats_data, f, indent=2, ensure_ascii=False)
            print(f"性能统计已导出到: {filepath}")
        except Exception as e:
            print(f"导出性能统计失败: {e}")
            
    def destroy(self):
        """
        销毁性能分析器
        """
        self.reset_statistics()
        print("物理性能分析器已销毁")
