"""
车辆系统模块
负责车辆物理的创建和管理
"""

from panda3d.core import Vec3, Point3, LVector3f
from panda3d.bullet import BulletVehicle, BulletWheel, BulletRigidBodyNode
import math
import time
from collections import deque


class VehicleSystem:
    """
    车辆系统类
    管理车辆的物理特性和控制
    """
    
    def __init__(self, physics_world, chassis_body, vehicle_config=None):
        """
        初始化车辆系统
        
        Args:
            physics_world (PhysicsWorld): 物理世界对象
            chassis_body (RigidBody): 底盘刚体
            vehicle_config (dict): 车辆配置参数
        """
        self.physics_world = physics_world
        self.chassis_body = chassis_body
        
        # 车辆配置
        self.config = {
            'max_engine_force': 3000.0,
            'max_brake_force': 100.0,
            'max_steering': math.pi / 4,  # 45度
            'steering_return_speed': 2.0,  # 转向回正速度
            'steering_sensitivity': 1.0,   # 转向灵敏度
            'center_of_mass_offset': Vec3(0, 0, -0.5),  # 质心偏移
            'suspension_stiffness': 50.0,
            'suspension_damping': 2.3,
            'suspension_compression': 4.4,
            'suspension_rest_length': 0.5,
            'roll_influence': 0.1,
            'friction_slip': 100.0,
            'wheel_radius': 0.3,
            'wheel_width': 0.2,
            'wheel_mass': 1.0,
            'anti_roll_bar_force': 100.0,  # 防倾杆力
            'aerodynamic_downforce': 0.0,  # 空气动力学下压力
            'aerodynamic_drag': 0.0,       # 空气动力学阻力
            'differential_type': 'open',   # 差速器类型: open, locked, limited_slip
            'gear_ratios': [2.66, 1.78, 1.30, 1.00, 0.74, 0.50],  # 齿轮比
            'final_drive_ratio': 3.42,     # 最终传动比
            'max_rpm': 7000,               # 最大转速
            'idle_rpm': 800,               # 怠速转速
            'engine_torque_curve': [(0, 0), (1000, 200), (2000, 300), (3000, 350), 
                                  (4000, 380), (5000, 350), (6000, 300), (7000, 200)],  # 扭矩曲线
        }
        
        # 应用自定义配置
        if vehicle_config:
            self.config.update(vehicle_config)
            
        # 创建Bullet车辆
        self.bullet_vehicle = BulletVehicle(
            physics_world.bullet_world.getBroadphase(), 
            chassis_body.bullet_body
        )
        self.bullet_vehicle.setCoordinateSystem(0, 1, 2)  # Right, Up, Forward
        
        # 车轮列表
        self.wheels = []
        self.wheel_info = []  # 详细的车轮信息
        
        # 车辆参数
        self.max_engine_force = self.config['max_engine_force']
        self.max_brake_force = self.config['max_brake_force']
        self.max_steering = self.config['max_steering']
        self.steering_return_speed = self.config['steering_return_speed']
        self.steering_sensitivity = self.config['steering_sensitivity']
        
        # 控制状态
        self.steering = 0.0
        self.target_steering = 0.0
        self.engine_force = 0.0
        self.brake_force = 0.0
        self.handbrake_force = 0.0
        self.gear = 1  # 当前档位
        self.rpm = self.config['idle_rpm']  # 发动机转速
        self.clutch_engaged = True  # 离合器状态
        
        # 车辆状态
        self.speed = 0.0
        self.speed_kmh = 0.0
        self.acceleration = 0.0
        self.lateral_acceleration = 0.0
        self.yaw_rate = 0.0
        self.pitch = 0.0
        self.roll = 0.0
        
        # 历史数据
        self.speed_history = deque(maxlen=100)
        self.acceleration_history = deque(maxlen=100)
        self.steering_history = deque(maxlen=100)
        
        # 高级特性
        self.anti_lock_brake_enabled = True
        self.traction_control_enabled = True
        self.stability_control_enabled = True
        self.abs_threshold = 0.8  # ABS触发阈值
        self.tc_slip_threshold = 0.2  # 牵引力控制滑移阈值
        
        # 车辆损伤系统
        self.damage = {
            'engine': 0.0,      # 发动机损伤
            'transmission': 0.0, # 传动系统损伤
            'suspension': 0.0,   # 悬挂损伤
            'wheels': [0.0] * 4, # 车轮损伤
            'body': 0.0,        # 车身损伤
        }
        
        # 燃料系统
        self.fuel_capacity = 60.0  # 升
        self.fuel_level = 60.0     # 升
        self.fuel_consumption_rate = 0.01  # 升/秒/单位力
        
        # 时间戳
        self.creation_time = time.time()
        self.last_update_time = self.creation_time
        
        # 添加到物理世界
        physics_world.add_vehicle(self)
        
        # 设置底盘质心偏移
        self._set_center_of_mass()
        
        print("车辆系统初始化完成")
        
    def _set_center_of_mass(self):
        """
        设置底盘质心偏移
        """
        com_offset = self.config['center_of_mass_offset']
        if com_offset.length() > 0:
            self.chassis_body.bullet_body.setCenterOfMassTransform(
                TransformState.makePos(com_offset)
            )
            
    def add_wheel(self, connection_point, wheel_direction, wheel_axle, wheel_radius=None,
                  suspension_rest_length=None, suspension_stiffness=None,
                  damping_relaxation=None, damping_compression=None,
                  friction_slip=None, roll_influence=None, wheel_width=None,
                  wheel_mass=None, is_front_wheel=True, is_steered_wheel=True,
                  is_driven_wheel=True, is_braked_wheel=True):
        """
        添加车轮
        
        Args:
            connection_point (Point3): 底盘连接点
            wheel_direction (Vec3): 车轮方向（通常向下）
            wheel_axle (Vec3): 车轮轴向（通常向右）
            wheel_radius (float): 车轮半径
            suspension_rest_length (float): 悬挂静止长度
            suspension_stiffness (float): 悬挂刚度
            damping_relaxation (float): 放松阻尼
            damping_compression (float): 压缩阻尼
            friction_slip (float): 滑动摩擦
            roll_influence (float): 翻滚影响
            wheel_width (float): 车轮宽度
            wheel_mass (float): 车轮质量
            is_front_wheel (bool): 是否为前轮
            is_steered_wheel (bool): 是否为转向轮
            is_driven_wheel (bool): 是否为驱动轮
            is_braked_wheel (bool): 是否为刹车轮
            
        Returns:
            BulletWheel: 车轮对象
        """
        # 使用配置参数或默认值
        wheel_radius = wheel_radius or self.config['wheel_radius']
        suspension_rest_length = suspension_rest_length or self.config['suspension_rest_length']
        suspension_stiffness = suspension_stiffness or self.config['suspension_stiffness']
        damping_relaxation = damping_relaxation or self.config['suspension_damping']
        damping_compression = damping_compression or self.config['suspension_compression']
        friction_slip = friction_slip or self.config['friction_slip']
        roll_influence = roll_influence or self.config['roll_influence']
        wheel_width = wheel_width or self.config['wheel_width']
        wheel_mass = wheel_mass or self.config['wheel_mass']
        
        wheel = self.bullet_vehicle.createWheel()
        
        # 设置车轮参数
        wheel.setChassisConnectionPointCs(connection_point)
        wheel.setWheelDirectionCs(wheel_direction)
        wheel.setWheelAxleCs(wheel_axle)
        wheel.setWheelRadius(wheel_radius)
        wheel.setWheelWidth(wheel_width)
        wheel.setMaxSuspensionTravelCm(500.0)
        wheel.setSuspensionStiffness(suspension_stiffness)
        wheel.setWheelsDampingRelaxation(damping_relaxation)
        wheel.setWheelsDampingCompression(damping_compression)
        wheel.setFrictionSlip(friction_slip)
        wheel.setRollInfluence(roll_influence)
        wheel.setSteering(is_steered_wheel)
        wheel.setDriving(is_driven_wheel)
        wheel.setBraking(is_braked_wheel)
        
        # 创建车轮刚体
        wheel_shape = BulletCylinderShape(wheel_radius, wheel_width, 1)  # Y轴
        wheel_body = BulletRigidBodyNode(f'Wheel_{len(self.wheels)}')
        wheel_body.addShape(wheel_shape)
        wheel_body.setMass(wheel_mass)
        
        # 保存车轮信息
        wheel_info = {
            'wheel': wheel,
            'body': wheel_body,
            'connection_point': connection_point,
            'direction': wheel_direction,
            'axle': wheel_axle,
            'radius': wheel_radius,
            'width': wheel_width,
            'mass': wheel_mass,
            'properties': {
                'suspension_rest_length': suspension_rest_length,
                'suspension_stiffness': suspension_stiffness,
                'damping_relaxation': damping_relaxation,
                'damping_compression': damping_compression,
                'friction_slip': friction_slip,
                'roll_influence': roll_influence
            },
            'is_front_wheel': is_front_wheel,
            'is_steered_wheel': is_steered_wheel,
            'is_driven_wheel': is_driven_wheel,
            'is_braked_wheel': is_braked_wheel,
            'rotation': 0.0,
            'rpm': 0.0,
            'skid_info': 0.0,
            'suspension_relative_velocity': 0.0,
            'clipped_inv_contact_dot_suspension': 0.0,
            'suspension_force': 0.0,
            'side_impulse': 0.0,
            'forward_impulse': 0.0,
            'side_friction_stiffness': 1.0,
            'forward_friction_stiffness': 1.0,
            'skid_energy': 0.0,
            'slip_energy': 0.0,
            'temperature': 20.0,  # 车轮温度
            'wear': 0.0,          # 车轮磨损
            'flat_spot': False,   # 是否有平点
        }
        
        self.wheels.append(wheel_info)
        self.wheel_info.append(wheel_info)
        
        # 更新车轮损伤数组大小
        while len(self.damage['wheels']) < len(self.wheels):
            self.damage['wheels'].append(0.0)
            
        return wheel
        
    def set_steering(self, steering):
        """
        设置转向
        
        Args:
            steering (float): 转向值 (-1.0 到 1.0)
        """
        self.target_steering = max(-1.0, min(1.0, steering))
        
    def set_engine_force(self, force):
        """
        设置引擎力
        
        Args:
            force (float): 引擎力 (-1.0 到 1.0)
        """
        self.engine_force = max(-1.0, min(1.0, force))
        
    def set_brake_force(self, force):
        """
        设置刹车力
        
        Args:
            force (float): 刹车力 (0.0 到 1.0)
        """
        self.brake_force = max(0.0, min(1.0, force))
        
    def set_handbrake_force(self, force):
        """
        设置手刹力
        
        Args:
            force (float): 手刹力 (0.0 到 1.0)
        """
        self.handbrake_force = max(0.0, min(1.0, force))
        
    def shift_up(self):
        """
        升档
        """
        if self.gear < len(self.config['gear_ratios']):
            self.gear += 1
            
    def shift_down(self):
        """
        降档
        """
        if self.gear > 1:
            self.gear -= 1
            
    def set_gear(self, gear):
        """
        设置档位
        
        Args:
            gear (int): 档位
        """
        self.gear = max(1, min(len(self.config['gear_ratios']), gear))
        
    def update(self, dt):
        """
        更新车辆状态
        
        Args:
            dt (float): 时间增量
        """
        current_time = time.time()
        self.last_update_time = current_time
        
        # 平滑转向控制
        steering_diff = self.target_steering - self.steering
        if abs(steering_diff) > 0.01:
            steering_change = steering_diff * self.steering_sensitivity * dt * 10
            self.steering += steering_change
        else:
            # 转向回正
            if abs(self.steering) > 0.01:
                self.steering -= self.steering * self.steering_return_speed * dt
            else:
                self.steering = 0.0
                
        # 应用转向到前轮
        self._apply_steering()
        
        # 计算发动机转速
        self._calculate_engine_rpm()
        
        # 应用引擎力和刹车力
        self._apply_forces_and_brakes(dt)
        
        # 应用手刹
        self._apply_handbrake()
        
        # 更新车辆状态
        self._update_vehicle_state(dt)
        
        # 更新车轮状态
        self._update_wheel_states()
        
        # 更新燃料消耗
        self._update_fuel_consumption(dt)
        
        # 记录历史数据
        self._record_history()
        
    def _apply_steering(self):
        """
        应用转向
        """
        steering_angle = self.steering * self.max_steering
        
        # 应用转向到转向轮（假设前两个轮子是前轮）
        for i, wheel_info in enumerate(self.wheels):
            if wheel_info['is_steered_wheel']:
                wheel_info['wheel'].setSteering(steering_angle)
                
    def _calculate_engine_rpm(self):
        """
        计算发动机转速
        """
        # 简化的转速计算
        wheel_rpm = 0.0
        driven_wheels = 0
        
        for wheel_info in self.wheels:
            if wheel_info['is_driven_wheel']:
                wheel_rpm += abs(wheel_info['rpm'])
                driven_wheels += 1
                
        if driven_wheels > 0:
            avg_wheel_rpm = wheel_rpm / driven_wheels
            # 考虑当前档位和最终传动比
            gear_ratio = self.config['gear_ratios'][self.gear - 1]
            final_ratio = self.config['final_drive_ratio']
            self.rpm = avg_wheel_rpm * gear_ratio * final_ratio
            
        # 限制转速范围
        self.rpm = max(self.config['idle_rpm'], min(self.config['max_rpm'], self.rpm))
        
    def _apply_forces_and_brakes(self, dt):
        """
        应用引擎力和刹车力
        
        Args:
            dt (float): 时间增量
        """
        # 计算实际引擎力（考虑损伤和燃料）
        actual_engine_force = self.engine_force * self.max_engine_force
        if self.damage['engine'] > 0.5:
            actual_engine_force *= (1.0 - self.damage['engine'] * 0.8)
        if self.fuel_level <= 0:
            actual_engine_force = 0.0
            
        # 计算实际刹车力
        actual_brake_force = self.brake_force * self.max_brake_force
        
        # 应用牵引力控制
        if self.traction_control_enabled:
            actual_engine_force = self._apply_traction_control(actual_engine_force, dt)
            
        # 应用到驱动轮
        for i, wheel_info in enumerate(self.wheels):
            if wheel_info['is_driven_wheel']:
                wheel_info['wheel'].setEngineForce(actual_engine_force)
            if wheel_info['is_braked_wheel']:
                wheel_info['wheel'].setBrake(actual_brake_force)
                
    def _apply_traction_control(self, engine_force, dt):
        """
        应用牵引力控制
        
        Args:
            engine_force (float): 引擎力
            dt (float): 时间增量
            
        Returns:
            float: 调整后的引擎力
        """
        # 检查是否有车轮打滑
        slip_detected = False
        for wheel_info in self.wheels:
            if wheel_info['is_driven_wheel']:
                slip_ratio = wheel_info['skid_info']
                if slip_ratio > self.tc_slip_threshold:
                    slip_detected = True
                    break
                    
        # 如果检测到打滑，减少引擎力
        if slip_detected:
            return engine_force * 0.5
            
        return engine_force
        
    def _apply_handbrake(self):
        """
        应用手刹
        """
        handbrake_force = self.handbrake_force * self.max_brake_force * 2  # 手刹力度更大
        
        # 应用到后轮（假设后两个轮子是后轮）
        for i, wheel_info in enumerate(self.wheels):
            if not wheel_info['is_front_wheel']:  # 后轮
                wheel_info['wheel'].setBrake(
                    wheel_info['wheel'].getBrake() + handbrake_force
                )
                
    def _update_vehicle_state(self, dt):
        """
        更新车辆状态
        
        Args:
            dt (float): 时间增量
        """
        # 获取当前速度
        linear_velocity = self.chassis_body.get_linear_velocity()
        self.speed = linear_velocity.length()
        self.speed_kmh = self.speed * 3.6  # 转换为km/h
        
        # 计算加速度
        if len(self.speed_history) > 0:
            prev_speed = self.speed_history[-1][0]
            time_diff = dt
            if time_diff > 0:
                self.acceleration = (self.speed - prev_speed) / time_diff
        else:
            self.acceleration = 0.0
            
        # 计算横向加速度
        # 这里需要更复杂的计算，简化处理
        self.lateral_acceleration = 0.0
        
        # 计算横摆角速度
        angular_velocity = self.chassis_body.get_angular_velocity()
        self.yaw_rate = angular_velocity.getZ()
        
        # 计算俯仰和翻滚
        rotation = self.chassis_body.get_rotation()
        self.pitch = rotation.getY()
        self.roll = rotation.getX()
        
        # 应用空气动力学效应
        self._apply_aerodynamics(linear_velocity, dt)
        
        # 应用防倾杆力
        self._apply_anti_roll_bars()
        
    def _apply_aerodynamics(self, velocity, dt):
        """
        应用空气动力学效应
        
        Args:
            velocity (Vec3): 速度向量
            dt (float): 时间增量
        """
        speed = velocity.length()
        if speed > 0:
            # 下压力（随速度平方增加）
            downforce = self.config['aerodynamic_downforce'] * speed * speed
            if downforce > 0:
                downforce_vector = Vec3(0, 0, -downforce)
                self.chassis_body.apply_force(downforce_vector)
                
            # 空气阻力
            drag_coefficient = self.config['aerodynamic_drag']
            drag_force = drag_coefficient * speed * speed
            if drag_force > 0:
                drag_direction = -velocity.normalized()
                drag_vector = drag_direction * drag_force
                self.chassis_body.apply_force(drag_vector)
                
    def _apply_anti_roll_bars(self):
        """
        应用防倾杆力
        """
        if len(self.wheels) >= 4:
            # 简化的防倾杆实现
            front_left = self.wheels[0]['wheel'].getSuspensionRelativeVelocity()
            front_right = self.wheels[1]['wheel'].getSuspensionRelativeVelocity()
            rear_left = self.wheels[2]['wheel'].getSuspensionRelativeVelocity()
            rear_right = self.wheels[3]['wheel'].getSuspensionRelativeVelocity()
            
            # 前轴防倾
            front_roll = (front_left - front_right) * self.config['anti_roll_bar_force']
            # 后轴防倾
            rear_roll = (rear_left - rear_right) * self.config['anti_roll_bar_force']
            
            # 这里应该应用力到车身上，简化处理
            
    def _update_wheel_states(self):
        """
        更新车轮状态
        """
        for i, wheel_info in enumerate(self.wheels):
            wheel = wheel_info['wheel']
            
            # 更新车轮旋转
            wheel_info['rotation'] = wheel.getWheelRotation()
            
            # 更新车轮转速
            wheel_info['rpm'] = wheel.getWheelRpm()
            
            # 更新滑移信息
            wheel_info['skid_info'] = wheel.getSkidInfo()
            
            # 更新悬挂信息
            wheel_info['suspension_relative_velocity'] = wheel.getSuspensionRelativeVelocity()
            wheel_info['clipped_inv_contact_dot_suspension'] = wheel.getClippedInvContactDotSuspension()
            wheel_info['suspension_force'] = wheel.getWheelsSuspensionForce()
            
            # 更新冲量信息
            wheel_info['side_impulse'] = wheel.getSideImpulse()
            wheel_info['forward_impulse'] = wheel.getForwardImpulse()
            
            # 计算能量损耗（用于磨损计算）
            skid_energy = abs(wheel_info['skid_info']) * abs(wheel_info['side_impulse'])
            slip_energy = abs(wheel.getWheelRotation()) * abs(wheel_info['forward_impulse'])
            wheel_info['skid_energy'] = skid_energy
            wheel_info['slip_energy'] = slip_energy
            
            # 更新车轮温度（简化）
            energy_input = skid_energy + slip_energy
            wheel_info['temperature'] += energy_input * 0.001 * 0.1  # 简化的热模型
            wheel_info['temperature'] = max(20.0, wheel_info['temperature'] * 0.99)  # 冷却
            
            # 更新车轮磨损
            wear_rate = (skid_energy + slip_energy) * 0.0001
            wheel_info['wear'] = min(1.0, wheel_info['wear'] + wear_rate)
            
            # 检查是否产生平点
            if wheel_info['temperature'] > 100.0 and wheel_info['wear'] > 0.5:
                wheel_info['flat_spot'] = True
                
    def _update_fuel_consumption(self, dt):
        """
        更新燃料消耗
        
        Args:
            dt (float): 时间增量
        """
        # 基础燃料消耗（怠速）
        base_consumption = 0.001  # 升/秒
        
        # 负荷燃料消耗
        load_consumption = abs(self.engine_force) * self.fuel_consumption_rate
        
        # 转速燃料消耗
        rpm_factor = self.rpm / self.config['max_rpm']
        rpm_consumption = rpm_factor * 0.002
        
        total_consumption = (base_consumption + load_consumption + rpm_consumption) * dt
        self.fuel_level = max(0.0, self.fuel_level - total_consumption)
        
    def _record_history(self):
        """
        记录历史数据
        """
        current_time = time.time()
        self.speed_history.append((self.speed, current_time))
        self.acceleration_history.append((self.acceleration, current_time))
        self.steering_history.append((self.steering, current_time))
        
    def get_current_speed_km_hour(self):
        """
        获取当前速度（km/h）
        
        Returns:
            float: 当前速度
        """
        return self.bullet_vehicle.getCurrentSpeedKmHour()
        
    def get_wheel_info(self, wheel_index):
        """
        获取车轮信息
        
        Args:
            wheel_index (int): 车轮索引
            
        Returns:
            dict: 车轮信息
        """
        if 0 <= wheel_index < len(self.wheels):
            return self.wheels[wheel_index]
        return None
        
    def get_wheel_rotation(self, wheel_index):
        """
        获取车轮旋转角度
        
        Args:
            wheel_index (int): 车轮索引
            
        Returns:
            float: 旋转角度
        """
        if 0 <= wheel_index < len(self.wheels):
            return self.wheels[wheel_index]['wheel'].getWheelRotation()
        return 0.0
        
    def get_wheel_rpm(self, wheel_index):
        """
        获取车轮转速（RPM）
        
        Args:
            wheel_index (int): 车轮索引
            
        Returns:
            float: 转速（RPM）
        """
        if 0 <= wheel_index < len(self.wheels):
            return self.wheels[wheel_index]['wheel'].getWheelRpm()
        return 0.0
        
    def get_wheel_skid_info(self, wheel_index):
        """
        获取车轮滑移信息
        
        Args:
            wheel_index (int): 车轮索引
            
        Returns:
            float: 滑移信息
        """
        if 0 <= wheel_index < len(self.wheels):
            return self.wheels[wheel_index]['wheel'].getSkidInfo()
        return 0.0
        
    def set_vehicle_parameters(self, max_engine_force=None, max_brake_force=None, 
                             max_steering=None, steering_sensitivity=None):
        """
        设置车辆参数
        
        Args:
            max_engine_force (float): 最大引擎力
            max_brake_force (float): 最大刹车力
            max_steering (float): 最大转向角度
            steering_sensitivity (float): 转向灵敏度
        """
        if max_engine_force is not None:
            self.max_engine_force = max_engine_force
        if max_brake_force is not None:
            self.max_brake_force = max_brake_force
        if max_steering is not None:
            self.max_steering = max_steering
        if steering_sensitivity is not None:
            self.steering_sensitivity = steering_sensitivity
            
    def apply_handbrake(self, force=1.0):
        """
        应用手刹
        
        Args:
            force (float): 手刹力度 (0.0 到 1.0)
        """
        self.set_handbrake_force(force)
        
    def reset_controls(self):
        """
        重置控制状态
        """
        self.steering = 0.0
        self.target_steering = 0.0
        self.engine_force = 0.0
        self.brake_force = 0.0
        self.handbrake_force = 0.0
        
        # 重置所有车轮控制
        for wheel_info in self.wheels:
            wheel_info['wheel'].setSteering(0.0)
            wheel_info['wheel'].setEngineForce(0.0)
            wheel_info['wheel'].setBrake(0.0)
            
    def get_vehicle_state(self):
        """
        获取车辆状态
        
        Returns:
            dict: 车辆状态信息
        """
        return {
            'speed': self.speed,
            'speed_kmh': self.speed_kmh,
            'acceleration': self.acceleration,
            'lateral_acceleration': self.lateral_acceleration,
            'yaw_rate': self.yaw_rate,
            'pitch': self.pitch,
            'roll': self.roll,
            'steering': self.steering,
            'engine_force': self.engine_force,
            'brake_force': self.brake_force,
            'handbrake_force': self.handbrake_force,
            'gear': self.gear,
            'rpm': self.rpm,
            'fuel_level': self.fuel_level,
            'fuel_percentage': self.fuel_level / self.fuel_capacity * 100,
            'damage': self.damage.copy(),
        }
        
    def get_engine_torque(self):
        """
        获取发动机扭矩
        
        Returns:
            float: 发动机扭矩
        """
        torque_curve = self.config['engine_torque_curve']
        
        # 在扭矩曲线上插值
        for i in range(len(torque_curve) - 1):
            rpm1, torque1 = torque_curve[i]
            rpm2, torque2 = torque_curve[i + 1]
            
            if rpm1 <= self.rpm <= rpm2:
                t = (self.rpm - rpm1) / (rpm2 - rpm1) if rpm2 != rpm1 else 0
                return torque1 + (torque2 - torque1) * t
                
        # 如果超出范围，返回最近点的扭矩
        if self.rpm < torque_curve[0][0]:
            return torque_curve[0][1]
        else:
            return torque_curve[-1][1]
            
    def get_engine_power(self):
        """
        获取发动机功率（马力）
        
        Returns:
            float: 发动机功率（马力）
        """
        torque = self.get_engine_torque()
        # 功率 = 扭矩 * 转速 / 常数
        power_kw = torque * self.rpm / 9549.3  # 转换为千瓦
        power_hp = power_kw * 1.34102  # 转换为马力
        return power_hp
        
    def apply_damage(self, component, amount):
        """
        应用车辆损伤
        
        Args:
            component (str): 组件名称
            amount (float): 损伤量 (0.0 到 1.0)
        """
        if component in self.damage:
            if isinstance(self.damage[component], list):
                # 车轮损伤
                for i in range(len(self.damage[component])):
                    self.damage[component][i] = min(1.0, self.damage[component][i] + amount)
            else:
                # 其他组件损伤
                self.damage[component] = min(1.0, self.damage[component] + amount)
                
    def repair_component(self, component, amount):
        """
        修复车辆组件
        
        Args:
            component (str): 组件名称
            amount (float): 修复量 (0.0 到 1.0)
        """
        if component in self.damage:
            if isinstance(self.damage[component], list):
                # 车轮修复
                for i in range(len(self.damage[component])):
                    self.damage[component][i] = max(0.0, self.damage[component][i] - amount)
            else:
                # 其他组件修复
                self.damage[component] = max(0.0, self.damage[component] - amount)
                
    def refuel(self, amount):
        """
        加油
        
        Args:
            amount (float): 燃料量（升）
        """
        self.fuel_level = min(self.fuel_capacity, self.fuel_level + amount)
        
    def get_performance_stats(self):
        """
        获取性能统计
        
        Returns:
            dict: 性能统计信息
        """
        # 计算平均速度
        if len(self.speed_history) > 0:
            avg_speed = sum([speed for speed, _ in self.speed_history]) / len(self.speed_history)
        else:
            avg_speed = 0.0
            
        # 计算最大速度
        max_speed = max([speed for speed, _ in self.speed_history]) if len(self.speed_history) > 0 else 0.0
        
        # 计算平均加速度
        if len(self.acceleration_history) > 0:
            avg_acceleration = sum([acc for acc, _ in self.acceleration_history]) / len(self.acceleration_history)
        else:
            avg_acceleration = 0.0
            
        return {
            'current_speed_kmh': self.speed_kmh,
            'average_speed_kmh': avg_speed * 3.6,
            'max_speed_kmh': max_speed * 3.6,
            'average_acceleration': avg_acceleration,
            'engine_rpm': self.rpm,
            'engine_power_hp': self.get_engine_power(),
            'engine_torque_nm': self.get_engine_torque(),
            'fuel_level_l': self.fuel_level,
            'fuel_percentage': self.fuel_level / self.fuel_capacity * 100,
            'gear': self.gear,
            'steering_input': self.steering,
            'vehicle_age': time.time() - self.creation_time,
            'wheel_count': len(self.wheels),
            'damage_stats': self.damage.copy(),
        }
        
    def enable_anti_lock_brake(self, enabled=True):
        """
        启用或禁用防抱死刹车
        
        Args:
            enabled (bool): 是否启用
        """
        self.anti_lock_brake_enabled = enabled
        
    def enable_traction_control(self, enabled=True):
        """
        启用或禁用牵引力控制
        
        Args:
            enabled (bool): 是否启用
        """
        self.traction_control_enabled = enabled
        
    def enable_stability_control(self, enabled=True):
        """
        启用或禁用车身稳定控制
        
        Args:
            enabled (bool): 是否启用
        """
        self.stability_control_enabled = enabled
        
    def destroy(self):
        """
        销毁车辆
        """
        # 重置控制
        self.reset_controls()
        
        # 从物理世界移除
        if self.physics_world:
            self.physics_world.remove_vehicle(self)
            
        # 清空车轮列表
        self.wheels.clear()
        self.wheel_info.clear()
        
        # 清空历史数据
        self.speed_history.clear()
        self.acceleration_history.clear()
        self.steering_history.clear()
        
        print("车辆系统已销毁")