ThreatSourceLibaray/ThreatSource/src/MIssile/TerminalSensitiveSubmunition.cs

using ThreatSource.Simulation;
using ThreatSource.Utils;
using ThreatSource.Sensor;
using ThreatSource.Equipment;
using System.Diagnostics;

namespace ThreatSource.Missile
{
    /// <summary>
    /// 目标探测结果结构
    /// </summary>
    /// <remarks>
    /// 包含目标探测的完整信息：
    /// - 目标角度
    /// - 目标对象引用
    /// </remarks>
    public struct DetectionResult
    {
        /// <summary>
        /// 目标角度（弧度）
        /// </summary>
        public double? Angle { get; set; }

        /// <summary>
        /// 目标对象
        /// </summary>
        public BaseEquipment? Target { get; set; }
    }

    /// <summary>
    /// 末敏子弹类，实现了多传感器融合的末端制导功能
    /// </summary>
    /// <remarks>
    /// 该类提供了末敏子弹的核心功能：
    /// - 多传感器融合（红外、毫米波、激光）
    /// - 螺旋扫描搜索
    /// - 自主目标识别
    /// - 末端精确制导
    /// 通过多种传感器的协同工作实现对目标的精确打击
    /// </remarks>
    public class TerminalSensitiveSubmunition : BaseMissile
    {
        /// <summary>
        /// 子弹飞行阶段枚举
        /// </summary>
        /// <remarks>
        /// 定义了子弹飞行的八个主要阶段：
        /// - Separation：分离阶段，从母弹分离
        /// - Deceleration：减速阶段，调整速度
        /// - ParachuteDeployment：降落伞打开阶段，稳定下降
        /// - StableScan：稳定扫描阶段，稳定下降
        /// - Detection：目标探测阶段，搜索确定目标
        /// - Attack：攻击阶段，末端制导
        /// - Explode：爆炸阶段，命中目标
        /// - SelfDestruct：自毁阶段，触发自毁
        /// </remarks>
        private enum SubmunitionStage
        {
            Separation,    // 分离阶段
            Deceleration,  // 减速阶段
            ParachuteDeployment, // 降落伞打开阶段
            StableScan,    // 稳定扫描阶段
            Detection,     // 目标探测阶段
            Attack         // 攻击阶段
        }

        /// <summary>
        /// 获取或设置当前飞行阶段
        /// </summary>
        /// <remarks>
        /// 用于控制子弹在不同阶段的行为
        /// 影响传感器工作模式和制导方式
        /// </remarks>
        private SubmunitionStage currentSubmunitionStage;

        /// <summary>
        /// 子弹配置参数
        /// </summary>
        private readonly TerminalSensitiveSubmunitionConfig config;

        /// <summary>
        /// 获取或设置螺旋扫描的当前角度
        /// </summary>
        /// <remarks>
        /// 用于控制螺旋扫描的方向
        /// 影响传感器的指向
        /// 初始角度为0(北)
        /// </remarks>
        private double spiralAngle = 0;

        /// <summary>
        /// 螺旋扫描旋转速度，单位：弧度/秒
        /// </summary>
        private readonly double SpiralRotationSpeedInRadians = 0;

        /// <summary>
        /// 扫描角度，单位：弧度
        /// </summary>
        private readonly double ScanAngleInRadians = 0;

        /// <summary>
        /// 扫描方向向量
        /// </summary>
        /// <remarks>
        /// 表示传感器的当前指向
        /// 用于目标探测和跟踪
        /// </remarks>
        private Vector3D scanDirection;

        /// <summary>
        /// 获取扫描方向向量
        /// </summary>
        /// <remarks>
        /// 表示传感器的当前指向
        /// 用于目标探测和跟踪
        /// </remarks>
        public Vector3D ScanDirection => scanDirection;

        /// <summary>
        /// 目标ID
        /// </summary>
        /// <remarks>
        /// 记录目标的唯一标识符
        /// 用于目标识别和跟踪
        /// </remarks>
        public string TargetId { get; }

        /// <summary>
        /// 红外探测器实例
        /// </summary>
        /// <remarks>
        /// 用于探测目标的红外辐射
        /// 提供目标方位信息
        /// </remarks>
        private readonly InfraredDetector infraredDetector;

        /// <summary>
        /// 毫米波辐射计实例
        /// </summary>
        /// <remarks>
        /// 用于探测目标的毫米波辐射
        /// 提供目标特征信息
        /// </remarks>
        private readonly MillimeterWaveRadiometer radiometer;

        /// <summary>
        /// 毫米波测高仪实例
        /// </summary>
        /// <remarks>
        /// 用于测量对地高度
        /// 控制飞行阶段转换
        /// </remarks>
        private readonly MillimeterWaveAltimeter altimeter;

        /// <summary>
        /// 激光测距仪实例
        /// </summary>
        /// <remarks>
        /// 用于精确测量目标距离
        /// 提供攻击阶段的制导信息
        /// </remarks>
        private readonly LaserRangefinder rangefinder;

        /// <summary>
        /// 是否已经探测到目标
        /// </summary>
        private bool isTargetDetected = false;

        /// <summary>
        /// 第一次探测到目标时记录的目标方位角
        /// </summary>
        private double? firstDetectionAngle = null;

        /// <summary>
        /// 获取或设置最后一次检测到目标的时间
        /// </summary>
        private double? lastDetectionTime = null;

        /// <summary>
        /// 初始化末敏子弹的新实例
        /// </summary>
        /// <param name="missileId">导弹ID</param>
        /// <param name="targetId">目标ID</param>
        /// <param name="properties">子弹配置参数</param>
        /// <param name="kinemateState">发射参数</param>
        /// <param name="submunitionConfig">子弹性能参数配置</param>
        /// <param name="manager">仿真管理器实例</param>
        /// <remarks>
        /// 构造过程：
        /// - 初始化基本属性
        /// - 创建传感器系统
        /// - 配置传感器参数
        /// - 设置初始状态
        /// </remarks>
        public TerminalSensitiveSubmunition(
            string missileId,
            string targetId,
            MissileProperties properties, 
            KinematicState kinemateState,
            TerminalSensitiveSubmunitionConfig submunitionConfig,
            ISimulationManager manager)
            : base(missileId, properties, kinemateState, manager)
        {
            TargetId = targetId;
            config = submunitionConfig;

            // 初始化扫描方向为正北
            scanDirection = new Vector3D(0, 0, 1).Normalize();
            
            // 初始化传感器
            infraredDetector = new InfraredDetector(Id+"_IR", this, submunitionConfig.InfraredDetectorConfig, manager);
            radiometer = new MillimeterWaveRadiometer(Id+"_MW", this, submunitionConfig.RadiometerConfig, manager); 
            altimeter = new MillimeterWaveAltimeter(Id+"_MW_ALT", this, submunitionConfig.AltimeterConfig, manager); 
            rangefinder = new LaserRangefinder(Id+"_LASER", this, submunitionConfig.RangefinderConfig, manager); 

            SpiralRotationSpeedInRadians = config.SpiralRotationSpeed * Math.PI / 180.0;
            ScanAngleInRadians = config.ScanAngle * Math.PI / 180.0;
        }

        /// <summary>
        /// 发射子弹
        /// </summary>
        /// <remarks>
        /// 发射过程：
        /// - 调用基类发射方法
        /// </remarks>
        public override void Fire()
        {
            base.Fire();
            currentSubmunitionStage = SubmunitionStage.Separation;
        }

        /// <summary>
        /// 更新子弹状态
        /// </summary>
        /// <param name="deltaTime">时间步长，单位：秒</param>
        /// <remarks>
        /// 更新过程：
        /// - 更新所有传感器
        /// - 根据当前阶段更新状态
        /// - 处理阶段转换
        /// - 调用基类更新
        /// </remarks>
        public override void Update(double deltaTime)
        {
            // 更新传感器
            infraredDetector.Update(deltaTime);
            radiometer.Update(deltaTime);
            altimeter.Update(deltaTime);
            rangefinder.Update(deltaTime);

            switch (currentSubmunitionStage)
            {
                case SubmunitionStage.Separation:
                    UpdateSeparationStage(deltaTime);
                    break;
                case SubmunitionStage.Deceleration:
                    UpdateDecelerationStage(deltaTime);
                    break;
                case SubmunitionStage.ParachuteDeployment:
                    UpdateParachuteDeploymentStage(deltaTime);
                    break;
                case SubmunitionStage.StableScan:
                    UpdateStableScanStage(deltaTime);
                    break;
                case SubmunitionStage.Detection:
                    UpdateDetectionStage(deltaTime);
                    break;
                case SubmunitionStage.Attack:
                    UpdateAttackStage(deltaTime);
                    break;
            }
            base.Update(deltaTime);
        }

        /// <summary>
        /// 更新分离阶段状态
        /// </summary>
        /// <param name="deltaTime">时间步长，单位：秒</param>
        /// <remarks>
        /// 分离阶段处理：
        /// - 激活毫米波测高雷达
        /// - 检查高度计数据有效，且高度是否小于等于减速高度
        /// - 如果是，则进入减速阶段
        /// - 否则，继续惯性飞行
        /// </remarks>
        private void UpdateSeparationStage(double deltaTime)
        {
            if(!altimeter.IsActive)
            {
                // 激活毫米波测高雷达
                altimeter.Activate();
                altimeter.Update(deltaTime);
            }

            // 检查高度计数据有效，且高度是否小于等于减速高度
            AltimeterSensorData altimeterData = (AltimeterSensorData)altimeter.GetSensorData(); 
            if(altimeterData.IsValid && altimeterData.Altitude <= config.DecelerationHeight)
            {
                // 如果是，则进入减速阶段
                currentSubmunitionStage = SubmunitionStage.Deceleration;
            }
        }

        /// <summary>
        /// 更新减速阶段状态
        /// </summary>
        /// <param name="deltaTime">时间步长，单位：秒</param>
        /// <remarks>
        /// 减速阶段处理：
        /// - 激活毫米波测高雷达
        /// - 计算减速加速度
        /// - 更新速度和位置
        /// - 检查高度条件
        /// - 如果高度小于等于开伞高度，则进入降落伞打开阶段
        /// </remarks>
        private void UpdateDecelerationStage(double deltaTime)
        {
            if (KState.Speed > config.DecelerationEndSpeed)
            {
                // 减速减旋，速度减小
                Vector3D deceleration = - KState.Velocity.Normalize() * config.DecelerationAcceleration; 
                KState.Velocity += deceleration * deltaTime;
            }

            // 检查高度计数据有效，且高度是否小于等于开伞高度
            AltimeterSensorData altimeterData = (AltimeterSensorData)altimeter.GetSensorData(); 
            if(altimeterData.IsValid && altimeterData.Altitude <= config.ParachuteDeploymentHeight)
            {
                // 如果是，则进入降落伞打开阶段
                currentSubmunitionStage = SubmunitionStage.ParachuteDeployment;
            }
        }

        /// <summary>   
        /// 更新降落伞打开阶段状态
        /// </summary>
        /// <param name="deltaTime">时间步长，单位：秒</param>
        /// <remarks>
        /// 降落伞打开阶段处理：
        /// - 打开降落伞
        /// - 降落伞减速和稳定
        /// - 调整姿态准备扫描
        /// - 激活测高雷达开始工作（第二次测高）
        /// - 在200米高度进入扫描阶段
        /// - 此时子弹速度 10 米/秒
        /// </remarks>
        private void UpdateParachuteDeploymentStage(double deltaTime)
        {
            // 如果垂直速度分量大于垂直下降速度，则降落伞减速
            if (Math.Abs(KState.Velocity.Y) > config.VerticalDeclineSpeed)
            {
                Vector3D deceleration = -KState.Velocity.Normalize() * config.ParachuteDeceleration;
                KState.Velocity += deceleration * deltaTime;
            }
            else
            {
                // 如果垂直速度分量小于等于垂直下降速度，则改为匀速垂直下降
                KState.Velocity = new Vector3D(0, -config.VerticalDeclineSpeed, 0);
            }

            AltimeterSensorData altimeterData = (AltimeterSensorData)altimeter.GetSensorData(); 
            // 检查高度计数据有效，且高度是否小于等于稳定扫描高度
            if(altimeterData.IsValid && altimeterData.Altitude <= config.StableScanHeight)
            {
                // 如果是，则进入稳定扫描阶段
                currentSubmunitionStage = SubmunitionStage.StableScan;
                // 维持稳定的垂直下降速度
                KState.Velocity = new Vector3D(0, -config.VerticalDeclineSpeed, 0);
                // 制导加速度抵消重力加速度，匀速下降
                GuidanceAcceleration = new Vector3D(0, PhysicalConstants.BeijingGravity, 0);
            }  
        }

        /// <summary>
        /// 更新稳定扫描阶段状态
        /// </summary>
        /// <param name="deltaTime">时间步长，单位：秒</param>
        /// <remarks>
        /// 扫描阶段处理：
        /// - 激活激光测距仪
        /// - 执行螺旋扫描
        /// - 处理目标探测
        /// - 检查目标确认条件
        /// </remarks>
        private void UpdateStableScanStage(double deltaTime)
        {
            if(!rangefinder.IsActive)
            {
                // 激活激光测距仪
                rangefinder.Activate();
                rangefinder.Update(deltaTime);
            }

            // 更新螺旋角度（顺时针旋转，标准方位角）
            spiralAngle += SpiralRotationSpeedInRadians * deltaTime;  // 顺时针角度增加
            while (spiralAngle >= 2 * Math.PI)
            {
                spiralAngle -= 2 * Math.PI;
            }

            // 更新扫描方向（标准方位角，顺时针为正）
            scanDirection = new Vector3D(
                Math.Sin(spiralAngle) * Math.Sin(ScanAngleInRadians),
                -Math.Cos(ScanAngleInRadians),
                Math.Cos(spiralAngle) * Math.Sin(ScanAngleInRadians)
            ).Normalize();

            // 更新末敏弹的朝向，指向扫描方向
            KState.Orientation = Orientation.FromVector(scanDirection);

            // 如果距离小于等于目标探测距离，则进入目标探测阶段
            RangefinderSensorData rangefinderData = (RangefinderSensorData)rangefinder.GetSensorData(); 
            if (rangefinderData.IsValid && rangefinderData.Distance <= config.TargetDetectionDistance)
            {
                currentSubmunitionStage = SubmunitionStage.Detection;
            }
        }

        /// <summary>
        /// 更新目标探测阶段状态
        /// </summary>
        /// <param name="deltaTime">时间步长，单位：秒</param>
        /// <remarks>
        /// 目标探测阶段处理：
        /// - 多传感器协同工作
        /// - 进行目标特征识别
        /// - 执行二次扫描确认
        /// - 为攻击阶段做准备
        /// </remarks>
        private void UpdateDetectionStage(double deltaTime)
        {
            // 激活传感器
            if(!radiometer.IsActive)
            {
                radiometer.Activate();
                radiometer.Update(deltaTime);
            }
            if(!infraredDetector.IsActive)
            {
                infraredDetector.Activate();
                infraredDetector.Update(deltaTime);
            }

            // 更新扫描角度（顺时针旋转）
            spiralAngle += SpiralRotationSpeedInRadians * deltaTime;  // 顺时针角度增加
            while (spiralAngle >= 2 * Math.PI)
            {
                spiralAngle -= 2 * Math.PI;
            }

            // 更新扫描方向
            scanDirection = new Vector3D(
                Math.Sin(spiralAngle) * Math.Sin(ScanAngleInRadians),
                -Math.Cos(ScanAngleInRadians),
                Math.Cos(spiralAngle) * Math.Sin(ScanAngleInRadians)
            ).Normalize();

            // 更新末敏弹的朝向，指向扫描方向
            KState.Orientation = Orientation.FromVector(scanDirection);

            // 获取传感器数据
            RadiometerSensorData radiometerData = (RadiometerSensorData)radiometer.GetSensorData();
            InfraredSensorData infraredData = (InfraredSensorData)infraredDetector.GetSensorData();

            bool isRadiometerDetected = radiometerData.IsValid && radiometerData.IsTargetDetected;
            bool isInfraredDetected = infraredData.IsValid && infraredData.IsTargetDetected;

            // 目标探测逻辑
            if (!isTargetDetected)
            {
                double? targetAngle = null;

                if (isRadiometerDetected && radiometerData.TargetAngle.HasValue)
                {
                    targetAngle = radiometerData.TargetAngle.Value;
                }
                else if (isInfraredDetected && infraredData.TargetAngle.HasValue)
                {
                    targetAngle = infraredData.TargetAngle.Value;
                }

                // 记录首次探测信息
                if (targetAngle.HasValue)
                {
                    isTargetDetected = true;
                    lastDetectionTime = FlightTime;
                    firstDetectionAngle = targetAngle.Value;
                    Debug.WriteLine($"首次探测到目标，方位角: {firstDetectionAngle.Value * 180 / Math.PI:F2}°");
                }
            }
            else  // 已经探测到目标，等待转过一圈进行二次确认
            {
                // 确保lastDetectionTime不为null
                if (!lastDetectionTime.HasValue)
                {
                    lastDetectionTime = FlightTime;
                }
                
                double timeSinceLastDetection = FlightTime - lastDetectionTime.Value;
                if (timeSinceLastDetection >= 0.75 * 2 * Math.PI / SpiralRotationSpeedInRadians)  // 转过3/4圈以后
                {
                    // 确保firstDetectionAngle不为null
                    if (!firstDetectionAngle.HasValue)
                    {
                        firstDetectionAngle = spiralAngle;
                    }
                    
                    // 检查当前角度是否接近记录的目标方位角
                    double angleDiff = Math.Abs(spiralAngle - firstDetectionAngle.Value);
                    // 根据仿真步长和转速计算允许的角度误差
                    double allowedError = deltaTime * SpiralRotationSpeedInRadians / 2;
                    if (angleDiff <= allowedError)
                    {
                        if (isRadiometerDetected || isInfraredDetected)
                        {
                            // 二次确认成功，进入攻击阶段
                            Debug.WriteLine($"二次确认成功，进入攻击阶段，当前角度: {spiralAngle * 180 / Math.PI:F2}°");
                            currentSubmunitionStage = SubmunitionStage.Attack;
                            return;
                        }
                        else
                        {
                            // 二次确认失败，重新开始扫描
                            Debug.WriteLine($"二次确认失败，重新开始扫描");
                            isTargetDetected = false;
                            lastDetectionTime = null;
                            firstDetectionAngle = null;
                        }
                    }
                }
            }
        }

        /// <summary>
        /// 更新攻击阶段状态
        /// </summary>
        /// <param name="deltaTime">时间步长，单位：秒</param>
        /// <remarks>
        /// 攻击阶段处理：
        /// - 使用记录的攻击角度计算攻击方向
        /// - 使用扫描方向作为攻击方向，弹丸没有传感器，只进行弹道运动
        /// - 更新速度和位置
        /// - 检查命中条件：使用纯几何计算
        /// </remarks>
        private void UpdateAttackStage(double deltaTime)
        {
            // 关闭传感器
            if (radiometer.IsActive)
            {
                radiometer.Deactivate();
            }
            if (infraredDetector.IsActive)
            {
                infraredDetector.Deactivate();
            }

            KState.Velocity = scanDirection * config.AttackSpeed;
        }

        /// <summary>
        /// 检测目标是否在视场内
        /// </summary>
        /// <param name="fieldOfView">视场角，单位：度</param>
        /// <returns>返回探测结果。如果目标在视场内返回角度和目标引用，否则返回null</returns>
        /// <remarks>
        /// 检测过程：
        /// - 获取目标位置
        /// - 计算目标方向
        /// - 判断是否在视场内
        /// </remarks>
        public DetectionResult DetectTarget(double fieldOfView)
        {
            // 获取目标对象
            if (SimulationManager.GetEntityById(TargetId) is not SimulationElement target) return new DetectionResult();

            // 获取目标的尺寸属性
            if (target is not BaseEquipment equipment) return new DetectionResult();

            double targetLength = equipment.Properties.Length;  // 目标长度
            double targetWidth = equipment.Properties.Width;    // 目标宽度
            
            // 获取目标的朝向向量
            Vector3D targetForward = target.KState.Orientation.ToVector();
            Vector3D targetRight = Vector3D.CrossProduct(Vector3D.UnitY, targetForward).Normalize();
            
            // 计算矩形四个顶点的位置
            Vector3D targetPosition = target.KState.Position;
            Vector3D[] vertices =
            [
                // 前后左右四个顶点
                targetPosition + (targetForward * (targetLength/2)) + (targetRight * (targetWidth/2)),  // 右前
                targetPosition + (targetForward * (targetLength/2)) - (targetRight * (targetWidth/2)),  // 左前
                targetPosition - (targetForward * (targetLength/2)) + (targetRight * (targetWidth/2)),  // 右后
                targetPosition - (targetForward * (targetLength/2)) - (targetRight * (targetWidth/2)),  // 左后
            ];

            // 计算目标方位角（弧度值）
            Vector3D toTarget = (targetPosition - KState.Position).Normalize();
            double targetAzimuth = Math.Atan2(toTarget.X, toTarget.Z);  // 修改为 atan2(X, Z) 使正北为0度
            if (targetAzimuth < 0)
            {
                targetAzimuth += 2 * Math.PI;  // 转换到 [0, 2π]
            }
            Debug.WriteLine($"目标方位角: {targetAzimuth * 180 / Math.PI:F2}°");
            Debug.WriteLine($"螺旋角度: {spiralAngle * 180 / Math.PI:F2}°");
            Debug.WriteLine($"扫描方向: {scanDirection}");
            Debug.WriteLine($"视场角: {fieldOfView}°");
            double targetDotProduct = Vector3D.DotProduct(scanDirection, toTarget);
            double angleInRadians = Math.Acos(Math.Clamp(targetDotProduct, -1.0, 1.0));
            Debug.WriteLine($"扫描方向与目标方向的夹角: {angleInRadians * 180 / Math.PI:F2}°");

            // 视场检测
            double cosFieldOfView = Math.Cos(fieldOfView * Math.PI / 180);
            for(int i = 0; i < vertices.Length; i++)
            {
                Vector3D toVertex = (vertices[i] - KState.Position).Normalize();
                double dotProduct = Vector3D.DotProduct(scanDirection, toVertex);
                double vertexAngle = Math.Acos(Math.Clamp(dotProduct, -1.0, 1.0)) * 180 / Math.PI;
                Debug.WriteLine($"顶点{i+1}夹角: {vertexAngle:F2}°");
                
                if(dotProduct > cosFieldOfView)
                {
                    Debug.WriteLine($"顶点{i+1}在视场角内，检测成功！");
                    // 如果目标的任何一个顶点在视场角内，返回目标方位角和目标引用
                    return new DetectionResult { 
                        Angle = targetAzimuth,
                        Target = equipment
                    };
                }
            }
            
            // 如果目标不在视场角内，返回空结果
            return new DetectionResult();
        }

        /// <summary>
        /// 激活子弹
        /// </summary>
        /// <remarks>
        /// 激活过程：
        /// - 调用基类激活方法
        /// </remarks>
        public override void Activate()
        {
            base.Activate();
        }

        /// <summary>
        /// 停用子弹
        /// </summary>
        /// <remarks>
        /// 停用过程：
        /// - 调用基类停用方法
        /// - 停用所有传感器
        /// </remarks>
        public override void Deactivate()
        {
            base.Deactivate();
            infraredDetector.Deactivate();
            radiometer.Deactivate();
            altimeter.Deactivate();
            rangefinder.Deactivate();
        }

        /// <summary>
        /// 获取末敏弹子弹是否处于制导状态
        /// </summary>
        /// <remarks>
        /// 对于末敏弹子弹，制导状态基于攻击阶段而非传统制导系统
        /// 攻击阶段表示子弹已锁定目标并进行精确攻击
        /// 与传统导弹不同，末敏弹子弹使用传感器制导而非制导系统
        /// </remarks>
        public override bool IsGuidance 
        { 
            get => currentSubmunitionStage == SubmunitionStage.Attack;
        }

        /// <summary>
        /// 检查传感器是否受到干扰
        /// </summary>
        /// <returns>如果当前阶段的关键传感器都被干扰返回true，否则返回false</returns>
        /// <remarks>
        /// 不同阶段的检查逻辑：
        /// - Deceleration阶段：测高仪被干扰才返回true
        /// - ParachuteDeployment阶段：测高仪被干扰才返回true
        /// - StableScan阶段：激光测距仪被干扰才返回true
        /// - Detection阶段：红外探测器和毫米波辐射计都被干扰才返回true
        /// - Attack阶段：红外探测器和毫米波辐射计都被干扰才返回true
        /// </remarks>
        private bool IsSensorsJammed()
        {
            // 根据当前阶段检查相应传感器的干扰状态
            switch (currentSubmunitionStage)
            {
                case SubmunitionStage.Deceleration:
                    // 减速阶段需要测高仪
                    return altimeter.IsJammed;
                case SubmunitionStage.ParachuteDeployment:
                    // 开伞阶段需要测高仪
                    return altimeter.IsJammed;
                case SubmunitionStage.StableScan:
                    // 稳定扫描阶段需要激光测距仪
                    return rangefinder.IsJammed;
                case SubmunitionStage.Detection:
                    // 目标探测阶段需要红外探测器和毫米波辐射计
                    return infraredDetector.IsJammed && radiometer.IsJammed;
                case SubmunitionStage.Attack:
                    // 攻击阶段需要红外探测器和毫米波辐射计
                    return infraredDetector.IsJammed && radiometer.IsJammed;
                default:
                    return false;
            }
        }

        /// <summary>
        /// 获取子弹状态信息
        /// </summary>
        /// <returns>包含子弹状态的详细描述</returns>
        /// <remarks>
        /// 返回信息包括：
        /// - 基本状态信息
        /// - 当前飞行阶段
        /// - 扫描和检测状态
        /// - 传感器工作状态
        /// </remarks>
        
        public override ElementStatusInfo GetStatusInfo()
        {
            var statusInfo = base.GetStatusInfo();
            // 获取目标对象并计算距离
            SimulationElement target = SimulationManager.GetEntityById(TargetId) as SimulationElement ?? throw new Exception("目标不存在");
            double distanceToTarget = (target.KState.Position - KState.Position).Magnitude();

            statusInfo.ExtendedProperties["CurrentStage"] = currentSubmunitionStage.ToString();
            statusInfo.ExtendedProperties["SpiralAngle"] = spiralAngle * 180 / Math.PI;
            statusInfo.ExtendedProperties["LastDetectionTime"] = lastDetectionTime ?? 0;
            statusInfo.ExtendedProperties["FirstDetectionAngle"] = firstDetectionAngle ?? 0;
            statusInfo.ExtendedProperties["IsTargetDetected"] = isTargetDetected;
            statusInfo.ExtendedProperties["TargetPosition"] = target.KState.Position;
            statusInfo.ExtendedProperties["DistanceToTarget"] = distanceToTarget;
            statusInfo.ExtendedProperties["IsSensorsJammed"] = IsSensorsJammed();

            return statusInfo;
        }

        /// <summary>
        /// 获取子弹状态
        /// </summary>
        /// <returns>子弹状态</returns>
        /// <remarks>
        /// 返回信息包括：
        /// - 基本状态信息
        /// - 激光测距仪状态
        /// - 红外探测器状态
        /// - 毫米波辐射计状态
        /// - 测高仪状态
        /// </remarks>
        public override string GetStatus()
        {
            return base.GetStatus() 
                + $"\n激光测距仪: {rangefinder.GetStatus()}"
                + $"\n红外探测器: {infraredDetector.GetStatus()}"
                + $"\n毫米波辐射计: {radiometer.GetStatus()}"
                + $"\n测高仪: {altimeter.GetStatus()}";
        }
    }
}