ThreatSourceLibaray/ThreatSource/src/Guidance/LaserSemiActiveGuidanceSystem.cs

using ThreatSource.Utils;
using ThreatSource.Simulation;
using ThreatSource.Sensor;
using ThreatSource.Indicator;
using ThreatSource.Jammer;
using System.Diagnostics;
using AirTransmission;

namespace ThreatSource.Guidance
{
    /// <summary>
    /// 激光半主动制导系统类，实现了基于激光照射的目标跟踪和制导功能
    /// </summary>
    /// <remarks>
    /// 该类提供了激光半主动制导系统的核心功能：
    /// - 激光目标照射
    /// - 反射光探测
    /// - 信号处理
    /// - 比例导引控制
    /// 用于实现精确制导打击
    /// </remarks>
    public class LaserSemiActiveGuidanceSystem : BaseGuidanceSystem
    {
        private readonly LaserSemiActiveGuidanceConfig config;

        /// <summary>
        /// 获取或设置目标位置 (使用可空类型)
        /// </summary>
        /// <remarks>
        /// 记录当前跟踪目标的三维位置
        /// 用于制导计算
        /// </remarks>
        private Vector3D? TargetPosition { get; set; }

        /// <summary>
        /// 获取或设置接收到的激光功率
        /// </summary>
        /// <remarks>
        /// 记录接收到的激光功率
        /// 用于制导计算
        /// </remarks>
        private double ReceivedLaserPower { get; set; }

        /// <summary>
        /// 获取或设置激光照射状态
        /// </summary>
        /// <remarks>
        /// 指示当前是否有激光照射目标
        /// 影响系统的工作状态
        /// </remarks>
        private bool LaserIlluminationOn { get; set; }

        /// <summary>
        /// 获取或设置期望的激光编码
        /// </summary>
        /// <remarks>
        /// 导弹期望接收的编码信息
        /// 用于验证接收到的激光信号
        /// 默认编码为PPM编码，值为1234
        /// </remarks>
        private LaserCodeConfig? InternalLaserCodeConfig { get; set; }

        /// <summary>
        /// 获取或设置支持的编码类型列表
        /// </summary>
        /// <remarks>
        /// 定义导弹支持的编码类型
        /// 默认支持PRF、PPM和PWM编码
        /// </remarks>
        private readonly List<LaserCodeType> supportedCodeTypes =
        [
            LaserCodeType.PRF,
            LaserCodeType.PPM,
            LaserCodeType.PWM
        ];

        /// <summary>
        /// 四象限探测器实例
        /// </summary>
        /// <remarks>
        /// 用于精确测量光斑位置
        /// 计算目标方向误差
        /// 提供高精度制导信号
        /// </remarks>
        private readonly QuadrantDetector quadrantDetector;

        /// <summary>
        /// 获取或设置光斑偏移灵敏度
        /// </summary>
        /// <remarks>
        /// 定义了四象限探测器对光斑偏移的响应灵敏度
        /// 影响制导系统的响应速度和稳定性
        /// 典型值为0.05
        /// </remarks>
        private double SpotOffsetSensitivity { get; set; } = 0.05;

        /// <summary>
        /// 上一次的制导加速度，用于平滑处理
        /// </summary>
        /// <remarks>
        /// 用于实现加速度平滑处理
        /// 减少加速度突变，使导弹飞行更稳定
        /// </remarks>
        private Vector3D PreviousGuidanceAcceleration { get; set; } = Vector3D.Zero;

        /// <summary>
        /// 加速度平滑系数
        /// </summary>
        /// <remarks>
        /// 范围(0,1]，值越小平滑效果越强
        /// 影响加速度的平滑程度
        /// </remarks>
        private const double AccelerationSmoothingFactor = 0.5;

        /// <summary>
        /// 烟幕衰减
        /// </summary>
        private double SmokeAttenuation { get; set; } = 1.0;

        /// <summary>
        /// 激光目标列表，包括真实目标和诱偏目标
        /// </summary>
        private readonly List<(SimulationElement Target, SimulationElement Source)> laserTargets = [];

        /// <summary>
        /// 初始化激光半主动制导系统的新实例
        /// </summary>
        /// <param name="id">制导系统ID</param>
        /// <param name="maxAcceleration">最大加速度，单位：米/平方秒</param>
        /// <param name="guidanceCoefficient">制导系数</param>
        /// <param name="laserCodeConfig">激光编码配置</param>
        /// <param name="guidanceConfig">激光半主动导引系统配置</param>
        /// <param name="simulationManager">仿真管理器实例</param>
        /// <remarks>
        /// 构造过程：
        /// - 初始化基类参数
        /// - 初始化目标信息
        /// - 初始化激光参数
        /// - 创建四象限探测器
        /// - 加载干扰阈值配置
        /// </remarks>
        public LaserSemiActiveGuidanceSystem(
            string id, 
            double maxAcceleration, 
            double guidanceCoefficient, 
            LaserCodeConfig laserCodeConfig,
            LaserSemiActiveGuidanceConfig guidanceConfig,
            ISimulationManager simulationManager) 
            : base(id, maxAcceleration, guidanceCoefficient, simulationManager)
        {
            config = guidanceConfig;
            
            LaserIlluminationOn = false;
            InternalLaserCodeConfig = laserCodeConfig;
            
            // 创建四象限探测器实例，使用配置中的参数
            quadrantDetector = new QuadrantDetector(
                config.SensorDiameter,
                config.FocusedSpotDiameter,
                config.LockThreshold);
            
            // 设置光斑偏移灵敏度
            SpotOffsetSensitivity = config.SpotOffsetSensitivity;
            
            // 初始化加速度平滑处理
            PreviousGuidanceAcceleration = Vector3D.Zero;
            
            InitializeJamming(guidanceConfig.JammingResistanceThreshold, [JammingType.Laser, JammingType.SmokeScreen]);
        }

        /// <summary>
        /// 激活制导系统
        /// </summary>
        /// <remarks>
        /// 激活过程：
        /// - 调用基类激活  
        /// - 订阅激光照射事件
        /// - 订阅激光干扰事件
        /// </remarks>
        public override void Activate()
        {
            base.Activate();
            // 订阅激光照射事件
            SimulationManager.SubscribeToEvent<LaserIlluminationUpdateEvent>(OnLaserIlluminationUpdate);
            SimulationManager.SubscribeToEvent<LaserIlluminationStopEvent>(OnLaserIlluminationStop);
            // 订阅激光干扰事件
            SimulationManager.SubscribeToEvent<LaserJammingEvent>(OnLaserJamming);
            // 订阅烟幕事件
            SimulationManager.SubscribeToEvent<SmokeScreenEvent>(OnSmokeScreen);

            // 订阅诱偏目标照射事件
            SimulationManager.SubscribeToEvent<LaserDecoyEvent>(OnLaserDecoy);
            SimulationManager.SubscribeToEvent<LaserDecoyStopEvent>(OnLaserDecoyStop);
        }

        /// <summary>
        /// 停用制导系统
        /// </summary>
        /// <remarks>
        /// 停用过程：
        /// - 调用基类停用
        /// - 取消订阅激光照射事件
        /// </remarks>
        public override void Deactivate()
        {
            base.Deactivate();
            // 取消订阅激光照射事件
            SimulationManager.UnsubscribeFromEvent<LaserIlluminationUpdateEvent>(OnLaserIlluminationUpdate);
            SimulationManager.UnsubscribeFromEvent<LaserIlluminationStopEvent>(OnLaserIlluminationStop);

            // 取消订阅激光干扰事件
            SimulationManager.UnsubscribeFromEvent<LaserJammingEvent>(OnLaserJamming);
            // 取消订阅烟幕事件
            SimulationManager.UnsubscribeFromEvent<SmokeScreenEvent>(OnSmokeScreen);

            // 取消订阅诱偏目标照射事件
            SimulationManager.UnsubscribeFromEvent<LaserDecoyEvent>(OnLaserDecoy);
            SimulationManager.UnsubscribeFromEvent<LaserDecoyStopEvent>(OnLaserDecoyStop);
            TargetPosition = null;
        }

        /// <summary>
        /// 处理激光照射更新事件
        /// </summary>
        /// <param name="evt">激光照射更新事件</param>
        private void OnLaserIlluminationUpdate(LaserIlluminationUpdateEvent evt)
        {
            if (evt?.LaserDesignatorId != null && evt?.TargetId != null)
            {
                try 
                {
                    LaserDesignator laserDesignator = SimulationManager.GetEntityById(evt.LaserDesignatorId) as LaserDesignator ?? throw new Exception("激光指示器不存在");
                    SimulationElement target = SimulationManager.GetEntityById(evt.TargetId) as SimulationElement ?? throw new Exception("目标不存在");
                                        
                    // 添加激光目标
                    if (!laserTargets.Any(t => t.Target.Id == target.Id))
                    {
                        laserTargets.Add((target, laserDesignator));
                    }

                    // 处理激光照射更新事件
                    ProcessLaserIlluminationUpdateEvent(evt);
                }
                catch (Exception ex)
                {
                    Trace.WriteLine($"处理激光照射更新事件时出错: {ex.Message}");
                }
            }
            else
            {
                Trace.WriteLine("警告：激光照射更新事件缺少必要参数");
            }
        }
        
        /// <summary>
        /// 处理激光照射停止事件
        /// </summary>
        /// <param name="evt">激光照射停止事件</param>
        private void OnLaserIlluminationStop(LaserIlluminationStopEvent evt)
        {           
            LaserIlluminationOn = false;
            HasGuidance = false; // 禁用制导
            PreviousGuidanceAcceleration = Vector3D.Zero; // 重置历史加速度
            TargetPosition = null;
        }

        /// <summary>
        /// 处理诱偏目标照射事件
        /// </summary>
        /// <param name="evt">诱偏目标照射事件</param>
        private void OnLaserDecoy(LaserDecoyEvent evt)
        {
            if (evt.LaserDecoyId != null && evt.SourceId != null)
            {
                LaserDecoy decoyTarget = SimulationManager.GetEntityById(evt.LaserDecoyId) as LaserDecoy ?? throw new Exception("诱偏目标不存在");
                SimulationElement decoySource = SimulationManager.GetEntityById(evt.SourceId) as SimulationElement ?? throw new Exception("诱偏源不存在");
                // 添加激光目标
                if (!laserTargets.Any(t => t.Target.Id == decoyTarget.Id))
                {
                    laserTargets.Add((decoyTarget, decoySource));
                }
            }
        }

        /// <summary>
        /// 处理诱偏目标照射停止事件
        /// </summary>
        /// <param name="evt">诱偏目标照射停止事件</param>
        private void OnLaserDecoyStop(LaserDecoyStopEvent evt)
        {
            if (evt?.LaserDecoyId != null)
            {
                laserTargets.RemoveAll(t => t.Target.Id == evt.LaserDecoyId);
            }
        }

        /// <summary>
        /// 处理激光干扰事件
        /// </summary>
        /// <param name="evt">激光干扰事件</param>
        private void OnLaserJamming(LaserJammingEvent evt)
        {
            if (evt == null) return;

            // 创建干扰参数
            var parameters = new JammingParameters
            {
                Type = JammingType.Laser,
                Power = evt.JammingPower,
                Direction = evt.JammingDirection,
                SourcePosition = evt.JammingSourcePosition,
                AngleRange = evt.JammingAngleRange,
                Mode = evt.JammingMode,
                Duration = evt.Duration
            };

            // 使用JammableComponent进行干扰判断
            ApplyJamming(parameters);
            
            // 如果被硬干扰，切换到搜索模式
            if (IsHardJammed)
            {
                // 清除目标信息
                TargetPosition = null;
                LaserIlluminationOn = false;
                // 记录干扰信息
                Trace.WriteLine($"激光半主动制导系统被硬干扰 - 功率: {evt.JammingPower}W, 位置: {evt.JammingSourcePosition}, 方向: {evt.JammingDirection}");
            }
        }

        /// <summary>
        /// 处理烟幕事件
        /// </summary>
        /// <param name="evt">烟幕事件</param>
        private void OnSmokeScreen(SmokeScreenEvent evt)
        {
            if (evt != null && evt.SmokeGrenadeId != null)
            {
                // 获取烟幕弹的配置
                if (SimulationManager.GetEntityById(evt.SmokeGrenadeId) is SmokeGrenade smokeGrenade)
                {
                    var config = smokeGrenade.config;
                    // 创建干扰参数
                    var parameters = new JammingParameters
                    {
                        Type = JammingType.SmokeScreen,
                        JammerId = smokeGrenade.Id,
                        SourcePosition = smokeGrenade.Position,
                        Direction = smokeGrenade.Orientation.ToVector(),
                        AngleRange = config.SmokeType == SmokeScreenType.Wall ? Math.PI : Math.PI * 2, // 墙状烟幕为半球形覆盖，云状为全方位
                        SmokeConcentration = config.Concentration,
                        SmokeType = config.SmokeType,
                        SmokeThickness = config.Thickness,
                        Duration = config.Duration,
                        Mode = JammingMode.Obscuration
                    };

                    // 使用JammableComponent进行干扰判断
                    ApplyJamming(parameters);
                }
            }
        }

        /// <summary>
        /// 处理系统被干扰的事件
        /// </summary>
        /// <param name="parameters">干扰参数</param>
        protected override void HandleJammingApplied(JammingParameters parameters)
        {
            base.HandleJammingApplied(parameters);

            if (parameters.Type == JammingType.Laser)
            {               
                // 在硬干扰下切换到搜索模式
                if (LaserIlluminationOn)
                {
                    LaserIlluminationOn = false;
                    PreviousGuidanceAcceleration = Vector3D.Zero; // 重置历史加速度
                }
            }
            else if (parameters.Type == JammingType.SmokeScreen)
            {
                if (SimulationManager.GetEntityById(parameters.JammerId) is SmokeGrenade smokeGrenade)
                {
                    // 检查目标位置是否有效，并且激光照射是否开启 (烟幕只影响接收到的信号)
                    if (TargetPosition != null && LaserIlluminationOn)
                    {
                        // 计算烟幕衰减 (1.0 表示无衰减)
                        SmokeAttenuation = smokeGrenade.GetSmokeTransmittanceOnLine(Position, TargetPosition, config.LaserWavelength);
                        Console.WriteLine($"[烟幕干扰应用 Laser] 视线透过率/衰减系数: {SmokeAttenuation:F3}");
                    }
                    else
                    {   
                        // 目标无效或激光未照射，暂不计算特定视线衰减
                        SmokeAttenuation = 1.0;
                        Console.WriteLine("[烟幕干扰应用 Laser] 目标位置无效或激光未照射，暂不计算烟幕衰减。");
                    }
                }
                else
                {
                     // 未找到烟幕弹，假定无衰减
                    SmokeAttenuation = 1.0;
                }
            }
        }

        /// <summary>
        /// 处理系统干扰被清除的事件
        /// </summary>  
        /// <param name="type">被清除的干扰类型</param> 
        protected override void HandleJammingCleared(JammingType type)
        {
            base.HandleJammingCleared(type);
        }

        /// <summary>
        /// 更新制导系统的状态和计算结果
        /// </summary>
        /// <param name="deltaTime">时间步长，单位：秒</param>
        /// <param name="missilePosition">导弹位置，单位：米</param>
        /// <param name="missileVelocity">导弹速度，单位：米/秒</param>
        public override void Update(double deltaTime, Vector3D missilePosition, Vector3D missileVelocity)
        {
            base.Update(deltaTime, missilePosition, missileVelocity);
                        
            // 处理接收到的所有激光信号
            ProcessLaserTargets();
            
            if (LaserIlluminationOn && !IsHardJammed)
            {
                // 更新制导状态
                HasGuidance = quadrantDetector.IsTargetLocked;
                
                if (HasGuidance)
                {
                    CalculateGuidanceAcceleration(deltaTime);
                }
                else
                {
                    GuidanceAcceleration = Vector3D.Zero;
                    PreviousGuidanceAcceleration = Vector3D.Zero; // 重置历史加速度
                }
            }
            else
            {
                HasGuidance = false;
                GuidanceAcceleration = Vector3D.Zero;
                PreviousGuidanceAcceleration = Vector3D.Zero; // 重置历史加速度
            }
        }
        
        /// <summary>
        /// 处理接收到的所有激光目标
        /// </summary>
        /// <remarks>
        /// 基于接收到的激光目标计算合成光斑位置
        /// </remarks>
        private void ProcessLaserTargets()
        {
            try
            {
                Console.WriteLine($"处理激光信号: 激光目标数量={laserTargets.Count}");

                // 如果没有激光源，返回
                if (laserTargets.Count == 0)
                {
                    LaserIlluminationOn = false;
                    return;
                }
                
                // 计算所有激光源的总接收功率和加权位置
                ReceivedLaserPower = 0.0;
                Vector3D weightedPosition = Vector3D.Zero;
                
                foreach (var target in laserTargets)
                {
                    // 计算角度偏差，判断是否在视野范围内
                    double angleDeviation = CalculateAngleDeviation(target.Target.Position);
                    if (angleDeviation > config.FieldOfViewAngleInRadians / 2)
                    {
                        Console.WriteLine($"处理激光信号: 目标超出视野范围，目标ID: {target.Target.Id}, 角度偏差: {angleDeviation:F2}弧度, 视野范围: {config.FieldOfViewAngleInRadians:F2}弧度");
                        continue; // 目标超出视野范围
                    }

                    double receivedPower = 0;
                    if (target.Target is LaserDecoy decoy)
                    {
                        // 计算接收功率
                        receivedPower = CalculateReceivedPower(target.Source.Position, target.Target.Position, decoy.DecoyPower, decoy.DecoyLaserDivergenceAngle);
                        Console.WriteLine($"处理激光信号: 诱偏目标接收功率={receivedPower:E}W, 诱偏目标ID: {target.Target.Id}");
                    }
                    else if (target.Source is LaserDesignator laserDesignator)
                    {
                        // 计算接收功率
                        receivedPower = CalculateReceivedPower(target.Source.Position, target.Target.Position, laserDesignator.LaserPower, laserDesignator.LaserDivergenceAngle);
                        Console.WriteLine($"处理激光信号: 真实目标接收功率={receivedPower:E}W, 真实目标ID: {target.Target.Id}");
                    }
                    
                    // 累加功率
                    ReceivedLaserPower += receivedPower;

                    // 加权位置
                    weightedPosition += target.Target.Position * receivedPower;
                    Console.WriteLine($"处理激光信号: 累加功率={ReceivedLaserPower:E}W, 加权位置={weightedPosition}");
                }
                
                // 如果总功率为0，表示没有在视野范围内的激光源
                if (ReceivedLaserPower <= 0)
                {
                    LaserIlluminationOn = false;
                    return;
                }
                
                // 计算加权平均位置
                TargetPosition = weightedPosition / ReceivedLaserPower;

                Console.WriteLine($"处理激光信号: 总功率={ReceivedLaserPower:E}W, 加权平均目标位置={TargetPosition}");
                
                // 更新激光照射参数
                LaserIlluminationOn = true;                
                // 计算光斑偏移
                Vector2D spotOffset = CalculateSpotOffset();
                
                // 将合成激光信号传递给四象限探测器
                quadrantDetector.ProcessLaserSignal(ReceivedLaserPower, spotOffset);
                
                Debug.WriteLine($"处理激光信号: 总功率={ReceivedLaserPower:E}W, 目标位置={TargetPosition}");
            }
            catch (Exception ex)
            {
                Trace.WriteLine($"处理激光信号时出错: {ex.Message}");
            }
        }

        /// <summary>
        /// 计算从特定位置接收到的激光功率
        /// </summary>
        /// <param name="sourcePos">激光源位置</param>
        /// <param name="targetPos">目标位置</param>
        /// <param name="laserPower">激光功率</param>
        /// <param name="laserDivergenceAngle">激光发散角</param>
        /// <returns>接收到的激光功率，单位：瓦特</returns>
        private double CalculateReceivedPower(Vector3D sourcePos, Vector3D targetPos, double laserPower, double laserDivergenceAngle)
        {
            double distanceDesignatorToTarget = (sourcePos - targetPos).Magnitude();
            double distanceMissileToTarget = (Position - targetPos).Magnitude();

            // 计算大气透过率 (1.使用从激光源到目标的单程透过率，2.使用从目标到导弹的单程透过率)
            // 如果当前天气为null，则认为大气透过率为1.0
            double atmosphericTransmittanceToTarget = 1.0;
            double atmosphericTransmittanceToMissile = 1.0;

            // 考虑烟幕衰减，计算大气透过率
            if(SimulationManager.CurrentWeather != null)
            {
                atmosphericTransmittanceToTarget = SmokeAttenuation * AtmosphereDllWrapper.CalculateTransmittance(
                    distanceDesignatorToTarget,
                    RadiationType.Laser,
                    config.LaserWavelength, 
                    SimulationManager.CurrentWeather);
                
                atmosphericTransmittanceToMissile = SmokeAttenuation * AtmosphereDllWrapper.CalculateTransmittance(
                    distanceMissileToTarget, 
                    RadiationType.Laser,
                    config.LaserWavelength, 
                    SimulationManager.CurrentWeather);
            }

            // 计算目标处的光斑面积
            double spotAreaAtTarget = Math.PI * Math.Pow(distanceDesignatorToTarget * Math.Tan(laserDivergenceAngle), 2);
    
            // 计算目标处的激光功率密度，考虑大气衰减和发射系统透过率
            double powerDensityAtTarget = laserPower * atmosphericTransmittanceToTarget * config.TransmitterEfficiency / spotAreaAtTarget;
    
            // 计算从目标反射的总功率
            double reflectedPower = powerDensityAtTarget * config.TargetReflectiveArea * config.ReflectionCoefficient;
    
            // 计算反射光在导弹处的扩散面积（假设漫反射）
            double reflectedSpotArea = 2 * Math.PI * Math.Pow(distanceMissileToTarget, 2);
    
            // 计算导弹接收到的功率，考虑大气衰减和接收系统透过率
            double receivedPower = reflectedPower * atmosphericTransmittanceToMissile * config.ReceiverEfficiency / reflectedSpotArea;

            // 计算镜头接收到的功率比例
            double lensArea = Math.PI * Math.Pow(config.LensDiameter / 2, 2);
            double illuminatedArea = Math.PI * Math.Pow(distanceMissileToTarget * Math.Tan(config.FieldOfViewAngleInRadians / 2), 2);
            double powerRatio = Math.Min(1, lensArea / illuminatedArea);

            // 计算聚焦后的功率密度增加
            double sensorArea = Math.PI * Math.Pow(config.SensorDiameter / 2, 2);
            double focusedArea = Math.PI * Math.Pow(config.FocusedSpotDiameter / 2, 2);
            double focusingFactor = sensorArea / focusedArea;

            // 计算最终接收到的功率
            double finalReceivedPower = receivedPower * powerRatio * focusingFactor;
            
            Debug.WriteLine($"激光功率计算: 源->目标距离={distanceDesignatorToTarget:F1}m (透过率={atmosphericTransmittanceToTarget:F3}), " +
                           $"目标->导弹距离={distanceMissileToTarget:F1}m (透过率={atmosphericTransmittanceToMissile:F3}), " +
                           $"最终功率={finalReceivedPower:E}W");

            return finalReceivedPower;
        }

        /// <summary>
        /// 计算目标的角度偏差
        /// </summary>
        /// <param name="targetPos">目标位置</param>
        /// <returns>角度偏差，单位：弧度</returns>
        /// <remarks>
        /// 计算目标方向与导弹当前朝向的夹角
        /// </remarks>
        private double CalculateAngleDeviation(Vector3D targetPos)
        {
            // 计算目标方向
            Vector3D targetDirection = (targetPos - Position).Normalize();
            
            // 计算当前导弹朝向
            Vector3D missileDirection = Velocity.Normalize();
            
            // 计算夹角
            double dotProduct = Vector3D.DotProduct(targetDirection, missileDirection);
            dotProduct = Math.Max(-1.0, Math.Min(1.0, dotProduct)); // 确保在[-1,1]范围内
            
            return Math.Acos(dotProduct);
        }

        /// <summary>
        /// 计算光斑偏移量
        /// </summary>
        /// <returns>光斑中心相对于探测器中心的偏移量，单位：米</returns>
        /// <remarks>
        /// 计算过程：
        /// - 计算理想指向方向
        /// - 计算当前指向方向
        /// - 计算角度偏差
        /// - 转换为光斑偏移量
        /// </remarks>
        private Vector2D CalculateSpotOffset()
        {
            // Check TargetPosition != null before using it
            if (TargetPosition == null)
            {
                return Vector2D.Zero;
            }

            // No cast needed, just use TargetPosition directly
            Vector3D idealDirection = (TargetPosition - Position).Normalize();
            
            // 计算当前导弹前向方向
            Vector3D currentDirection = Velocity.Normalize();
            
            // 计算右向量和上向量
            Vector3D right = Vector3D.CrossProduct(Vector3D.UnitY, currentDirection).Normalize();
            Vector3D up = Vector3D.CrossProduct(currentDirection, right).Normalize();
            
            // 计算理想方向在当前坐标系中的投影
            double forwardComponent = Vector3D.DotProduct(idealDirection, currentDirection);
            double rightComponent = Vector3D.DotProduct(idealDirection, right);
            double upComponent = Vector3D.DotProduct(idealDirection, up);
            
            // 确保前向分量为正（目标在前方）
            if (forwardComponent <= 0)
            {
                // 目标在后方，无法探测
                return new Vector2D(0, 0);
            }
            
            // 计算角度偏差
            double horizontalAngle = Math.Atan2(rightComponent, forwardComponent);
            double verticalAngle = Math.Atan2(upComponent, forwardComponent);
            
            // 转换为光斑偏移量（假设小角度近似）
            double focalLength = config.SensorDiameter / (2 * Math.Tan(config.FieldOfViewAngleInRadians / 2));
            double horizontalOffset = focalLength * Math.Tan(horizontalAngle);
            double verticalOffset = focalLength * Math.Tan(verticalAngle);
            
            return new Vector2D(horizontalOffset, verticalOffset);
        }

        /// <summary>
        /// 计算制导加速度
        /// </summary>
        /// <param name="deltaTime">时间步长，单位：秒</param>
        /// <remarks>
        /// 计算过程：
        /// - 使用四象限探测器获取目标方向
        /// - 计算比例导引加速度
        /// - 限制最大加速度
        /// - 应用加速度平滑处理
        /// </remarks>
        protected void CalculateGuidanceAcceleration(double deltaTime)
        {
            // 获取当前导弹指向方向
            Vector3D currentDirection = Velocity.Normalize();

            // 使用四象限探测器获取修正后的目标方向
            Vector3D targetDirection = quadrantDetector.GetTargetDirection(currentDirection, SpotOffsetSensitivity);
            
            // 计算方向差异向量
            Vector3D directionDifference = targetDirection - currentDirection;
            
            // 确保差异向量与当前速度垂直（只保留横向分量）
            Vector3D guidanceDirection = directionDifference - currentDirection * Vector3D.DotProduct(directionDifference, currentDirection);
            
            // 如果差异太小，可以适当放大
            if (guidanceDirection.Magnitude() < 0.01)
            {
                guidanceDirection = guidanceDirection.Normalize() * 0.01;
            }
            
            // 计算新的制导加速度，与速度垂直
            Vector3D newGuidanceAcceleration = guidanceDirection * ProportionalNavigationCoefficient * Velocity.Magnitude();
            
            // 限制最大加速度
            double maxAcceleration = MaxAcceleration;
            if (newGuidanceAcceleration.Magnitude() > maxAcceleration)
            {
                newGuidanceAcceleration = newGuidanceAcceleration.Normalize() * maxAcceleration;
            }
            
            // 应用加速度平滑处理
            GuidanceAcceleration = PreviousGuidanceAcceleration * (1 - AccelerationSmoothingFactor) + 
                                 newGuidanceAcceleration * AccelerationSmoothingFactor;
            
            // 保存当前加速度用于下次平滑计算
            PreviousGuidanceAcceleration = GuidanceAcceleration;
        }

        /// <summary>
        /// 获取制导系统的详细状态信息
        /// </summary>
        /// <returns>包含完整状态参数的字符串</returns>
        /// <remarks>
        /// 返回信息：
        /// - 基本状态信息
        /// - 接收功率数据
        /// - 锁定阈值
        /// - 四象限探测器状态
        /// 用于系统监控和调试
        /// </remarks>
        public override string GetStatus()
        {
            return base.GetStatus() + 
                   $" 接收到的激光功率: {ReceivedLaserPower:E} W," + 
                   $" 锁定阈值: {config.LockThreshold:E} W," +
                   $" 四象限探测器: {quadrantDetector.GetStatus()}";
        }

        /// <summary>
        /// 获取四象限探测器的水平误差
        /// </summary>
        /// <returns>水平误差值，范围[-1, 1]</returns>
        /// <remarks>
        /// 正值表示右侧偏移，负值表示左侧偏移
        /// </remarks>
        public double GetHorizontalError()
        {
            return quadrantDetector.HorizontalError;
        }
        
        /// <summary>
        /// 获取四象限探测器的垂直误差
        /// </summary>
        /// <returns>垂直误差值，范围[-1, 1]</returns>
        /// <remarks>
        /// 正值表示上方偏移，负值表示下方偏移
        /// </remarks>
        public double GetVerticalError()
        {
            return quadrantDetector.VerticalError;
        }

        /// <summary>
        /// 设置光斑偏移灵敏度
        /// </summary>
        /// <param name="sensitivity">灵敏度值</param>
        /// <remarks>
        /// 灵敏度值越高，制导系统对光斑偏移的响应越强烈
        /// 典型值范围：0.1-1.0
        /// </remarks>
        public void SetSpotOffsetSensitivity(double sensitivity)
        {
            SpotOffsetSensitivity = sensitivity;
        }
        
        /// <summary>
        /// 获取四象限探测器的锁定状态
        /// </summary>
        /// <returns>如果四象限探测器锁定目标则返回true，否则返回false</returns>
        /// <remarks>
        /// 用于外部系统监控四象限探测器的工作状态
        /// </remarks>
        public bool IsQuadrantDetectorLocked()
        {
            return quadrantDetector.IsTargetLocked;
        }

        /// <summary>
        /// 设置期望的激光编码
        /// </summary>
        /// <param name="codeType">编码类型</param>
        /// <param name="codeValue">编码值</param>
        /// <remarks>
        /// 设置过程：
        /// - 检查编码类型是否支持
        /// - 创建新的编码对象
        /// - 设置编码类型和值
        /// </remarks>
        public void SetExpectedLaserCode(LaserCodeType codeType, int codeValue)
        {
            if (supportedCodeTypes.Contains(codeType))
            {
                InternalLaserCodeConfig = new LaserCodeConfig
                {
                    Code = new LaserCode { CodeType = codeType, CodeValue = codeValue }
                };
                Debug.WriteLine($"激光半主动制导系统设置期望编码：类型={codeType}，值={codeValue}");
            }
            else
            {
                Debug.WriteLine($"激光半主动制导系统不支持编码类型：{codeType}");
            }
        }

        /// <summary>
        /// 添加期望编码参数
        /// </summary>
        /// <param name="key">参数名称</param>
        /// <param name="value">参数值</param>
        /// <remarks>
        /// 添加特定编码类型的额外参数
        /// 如PPM编码的脉冲位置模式、PWM编码的脉冲宽度等
        /// </remarks>
        public void AddExpectedCodeParameter(string key, object value)
        {
            if (InternalLaserCodeConfig != null)
            {
                InternalLaserCodeConfig.Code.Parameters[key] = value;
                Debug.WriteLine($"激光半主动制导系统添加期望编码参数：{key}={value}");
            }
        }

        /// <summary>
        /// 处理激光照射更新事件
        /// </summary>
        /// <param name="illuminationEvent">激光照射更新事件</param>
        /// <remarks>
        /// 处理过程：
        /// - 检查编码是否匹配
        /// - 如果要求匹配且不匹配，则忽略信号
        /// - 如果匹配或不要求匹配，则处理信号
        /// - 更新激光照射状态
        /// </remarks>
        public void ProcessLaserIlluminationUpdateEvent(LaserIlluminationUpdateEvent illuminationEvent)
        {
            if (illuminationEvent.LaserCodeConfig != null)
            {
                // 只有在编码启用的情况下才进行编码匹配检查
                if (illuminationEvent.LaserCodeConfig.IsCodeEnabled)
                {
                    bool codeMatched = InternalLaserCodeConfig?.CheckCodeMatch(illuminationEvent.LaserCodeConfig) ?? false;
                    
                    if (!codeMatched)
                    {
                        // 发布编码不匹配事件
                        PublishCodeMismatchEvent(illuminationEvent.LaserDesignatorId, illuminationEvent.LaserCodeConfig);
                        Trace.WriteLine("激光半主动制导系统接收到不匹配的激光编码，忽略信号");
                        HasGuidance = false; // 禁用制导
                        LaserIlluminationOn = false; // 禁用激光照射状态，确保四象限探测器不处理信号
                        // 重置四象限探测器状态
                        quadrantDetector.ProcessLaserSignal(0, new Vector2D(0, 0));
                        PreviousGuidanceAcceleration = Vector3D.Zero; // 重置历史加速度
                        return;
                    }
                }
                
                // 更新激光照射状态
                LaserIlluminationOn = true;
            }
        }

        /// <summary>
        /// 处理激光照射停止事件
        /// </summary>
        /// <param name="illuminationEvent">激光照射停止事件</param>
        /// <remarks>
        /// 处理过程：
        /// - 停止激光照射状态
        /// - 清理相关参数
        /// </remarks>
        public void ProcessLaserIlluminationStopEvent(LaserIlluminationStopEvent illuminationEvent)
        {
            LaserIlluminationOn = false;
            HasGuidance = false; // 禁用制导
        }

        /// <summary>
        /// 发布编码不匹配事件
        /// </summary>
        /// <param name="designatorId">激光定位器ID</param>
        /// <param name="receivedCodeConfig">接收到的编码配置</param>
        /// <remarks>
        /// 发布过程：
        /// - 创建事件对象
        /// - 设置事件属性
        /// - 发布到事件系统
        /// </remarks>
        private void PublishCodeMismatchEvent(string? designatorId, LaserCodeConfig? receivedCodeConfig)
        {
            var mismatchEvent = new LaserCodeMismatchEvent
            {
                MissileId = ParentId,
                DesignatorId = designatorId,
                ExpectedCodeConfig = InternalLaserCodeConfig,
                ReceivedCodeConfig = receivedCodeConfig
            };
            
            PublishEvent(mismatchEvent);
        }

        /// <summary>
        /// 计算烟幕对激光的衰减因子
        /// </summary>
        /// <param name="parameters">烟幕干扰参数</param>
        /// <returns>衰减因子，范围：0-1，0表示完全衰减，1表示无衰减</returns>
        private double CalculateSmokeAttenuation(JammingParameters parameters)
        {
            if (!parameters.SmokeConcentration.HasValue)
                return 1.0; // 无衰减
                
            // 获取烟幕浓度
            double concentration = parameters.SmokeConcentration.Value;
            
            // 计算烟幕厚度（设备到烟幕边缘的距离）
            double effectiveThickness = 0;
            if (parameters.SmokeType == SmokeScreenType.Cloud)
            {
                // 对于云状烟幕，使用设备到烟幕中心的距离
                double distanceToCenter = (Position - parameters.SourcePosition).Magnitude();
                double radius = parameters.SmokeThickness.HasValue ? parameters.SmokeThickness.Value / 2 : 10.0;
                effectiveThickness = Math.Max(0, radius - distanceToCenter);
                if (distanceToCenter < radius) // 如果在烟幕内部
                    effectiveThickness = 2 * (radius - distanceToCenter); // 双倍路径
            }
            else // SmokeScreenType.Wall
            {
                // 对于墙状烟幕，使用烟幕厚度
                effectiveThickness = parameters.SmokeThickness.HasValue ? parameters.SmokeThickness.Value : 5.0;
                // 可以进一步计算有效厚度，但这里简化处理
            }
            
            // 使用 AtmosphereDllWrapper 计算烟幕透过率
            double transmittance = AtmosphereDllWrapper.CalculateSmokeScreenTransmittance(
                config.LaserWavelength,  // 激光波长（微米）
                concentration,           // 烟幕浓度（g/m³）
                effectiveThickness       // 烟幕厚度（米）
            );
            
            Debug.WriteLine($"烟幕衰减计算 - 波长: {config.LaserWavelength:F2}um, 浓度: {concentration}g/m³, 厚度: {effectiveThickness}m, 透过率: {transmittance:P2}");
            
            return transmittance;
        }
    }
}