ThreatSourceLibaray/ThreatSource/src/Sensor/LaserRangefinder.cs

using ThreatSource.Missile;
using ThreatSource.Jammer;
using System.Diagnostics;
using ThreatSource.Simulation;
using ThreatSource.Utils;

namespace ThreatSource.Sensor
{
    /// <summary>
    /// 激光测距仪类，实现了目标距离测量和数据采集功能
    /// </summary>
    /// <remarks>
    /// 该类提供了激光测距仪的核心功能：
    /// - 实时距离测量
    /// - 脉冲频率控制
    /// - 量程范围控制
    /// - 数据采集
    /// 用于目标距离的精确测量和跟踪
    /// </remarks>
    public class LaserRangefinder : BaseSensor
    {
        /// <summary>
        /// 激光测距仪配置
        /// </summary>
        public readonly LaserRangefinderConfig config;

        /// <summary>
        /// 定义设备支持的干扰类型
        /// </summary>
        /// <remarks>
        /// 激光测距仪支持的干扰类型：
        /// - 激光干扰
        /// - 烟雾干扰
        /// </remarks>
        public override IEnumerable<JammingType> SupportedJammingTypes => [JammingType.Laser, JammingType.SmokeGrenade];

        /// <summary>
        /// 获取设备支持的阻塞干扰类型
        /// </summary>
        public override IEnumerable<JammingType> SupportedBlockingJammingTypes => [JammingType.Laser];

        /// <summary>
        /// 激光测距仪数据
        /// </summary>
        private RangefinderSensorData sensorData;

        /// <summary>
        /// 当前测量距离，单位：米
        /// </summary>
        /// <remarks>
        /// 包含测量精度引入的随机误差
        /// </remarks>
        private double currentDistance;

        /// <summary>
        /// 末敏子弹实例
        /// </summary>
        /// <remarks>
        /// 关联的末敏子弹对象
        /// 用于获取位置和姿态信息
        /// </remarks>
        private readonly TerminalSensitiveSubmunition submunition;

        /// <summary>
        /// 初始化激光测距仪的新实例
        /// </summary>
        /// <param name="id">激光测距仪的ID</param>
        /// <param name="submunition">末敏子弹实例</param>
        /// <param name="config">激光测距仪配置</param>
        /// <param name="simulationManager">仿真管理器实例</param>
        /// <remarks>
        /// 构造过程：
        /// - 设置位置和姿态
        /// - 配置测量参数
        /// - 初始化工作状态
        /// </remarks>
        public LaserRangefinder(string id, TerminalSensitiveSubmunition submunition, LaserRangefinderConfig config, ISimulationManager simulationManager)
            : base(id, new KinematicState(submunition.KState.Position, submunition.KState.Orientation, submunition.KState.Speed), simulationManager)
        {
            this.config = config;
            this.submunition = submunition;
            currentDistance = 0;
            sensorData = new RangefinderSensorData();

            // 初始化干扰处理，设置干扰抗性阈值和支持的干扰类型
            InitializeJamming(config.JammingResistanceThreshold, SupportedJammingTypes, SupportedBlockingJammingTypes);
        }

        /// <summary>
        /// 更新激光测距仪状态
        /// </summary>
        /// <param name="deltaTime">时间步长，单位：秒</param>
        public override void Update(double deltaTime)
        {
            // 先调用基类的Update方法，处理干扰状态等通用逻辑
            base.Update(deltaTime);

            // 基类的Update方法会在合适的时候调用UpdateSensor
        }

        /// <summary>
        /// 更新激光测距仪特定功能
        /// </summary>
        /// <param name="deltaTime">时间步长，单位：秒</param>
        protected override void UpdateSensor(double deltaTime)
        {
            // 从submunition更新位置和朝向
            KState = submunition.KState;

            // 实时计算烟幕衰减
            double smokeAttenuation = 1.0; // 默认无衰减
            var activeSmokeJammerIds = _jammingComponent.GetActiveJammerIdsOfType(JammingType.SmokeGrenade);

            foreach (var jammerId in activeSmokeJammerIds)
            {
                if (SimulationManager.GetEntityById(jammerId) is SmokeGrenade smokeGrenade)
                {
                    // 计算视线目标点 (地面交点)
                    var targetPointForLos = MotionAlgorithm.CalculateIntersectionWithGround(KState.Position, KState.Orientation.ToVector(), 0);
                    if (targetPointForLos != null)
                    {
                        var wavelength = config.Wavelength;

                        // 计算此烟幕的透射率
                        double singleSmokeTransmittance = smokeGrenade.GetSmokeTransmittanceOnLine(KState.Position, targetPointForLos.Value, wavelength);
                        smokeAttenuation *= singleSmokeTransmittance; // 累乘透射率
                    }
                }
            }

            if ((!IsJammed || IsJammed && !IsBlockingJammed) && smokeAttenuation > 0.5)
            {
                // 执行距离测量
                currentDistance = GetTargetDistance();
                if(currentDistance > 0)
                {
                    sensorData.Distance = currentDistance;
                    sensorData.IsValid = true; // 成功测量，数据有效
                }
                else
                {
                    sensorData.IsValid = false;
                    sensorData.Distance = 0;
                }
            }
            else
            {
                sensorData.IsValid = false;
                sensorData.Distance = 0;
            }
        }

        /// <summary>
        /// 获取激光测距仪的最新数据
        /// </summary>
        /// <returns>包含距离测量结果的数据对象</returns>
        /// <remarks>
        /// 返回数据：
        /// - 目标距离值
        /// </remarks>
        public override SensorData GetSensorData()
        {
            return sensorData;
        }

        /// <summary>
        /// 判断是否应该处理传入的干扰参数
        /// </summary>
        /// <param name="parameters">干扰参数</param>
        /// <returns>如果应该处理则返回 true，否则返回 false</returns>
        protected override bool ShouldHandleJamming(JammingParameters parameters)
        {
            // 首先调用基类检查是否支持该干扰类型
            if (!base.ShouldHandleJamming(parameters))
            {
                return false;
            }

            // 检查干扰类型是否为激光
            if (parameters.Type == JammingType.Laser)
            {
                // 检查波长是否与工作波长匹配 (单位: 微米)
                if (!IsWavelengthCompatible(parameters.Wavelength ?? 0))
                {
                    Debug.WriteLine($"[LaserRangefinder] 忽略干扰：波长 {parameters.Wavelength}um 与工作波长 {config.Wavelength}um 不匹配。", "Jamming");
                    return false;
                }
            }

            // 如果所有检查通过，则处理此干扰
            return true;
        }

        /// <summary>
        /// 检查干扰波长是否与激光测距仪波段匹配
        /// </summary>
        /// <param name="wavelength">干扰波长（微米）</param>
        /// <returns>如果波长匹配返回true，否则返回false</returns>
        private bool IsWavelengthCompatible(double wavelength)
        {
            // 激光测距仪使用特定的激光波长，需要精确匹配
            // 允许小范围的偏差(±0.01μm)以适应现实中的波长漂移
            return Math.Abs(wavelength - config.Wavelength) <= 0.01;
        }

        /// <summary>
        /// 处理激光干扰应用
        /// </summary>
        protected override void HandleJammingApplied(JammingParameters parameters)
        {
            base.HandleJammingApplied(parameters);
            if (parameters.Type == JammingType.Laser)
            {         
                Debug.WriteLine($"[LaserRangefinder] 激光测距仪受到有效干扰，功率：{parameters.Power}瓦特", "Jamming");
                sensorData.IsValid = false;
                sensorData.Distance = 0;  // 在干扰状态下将距离设置为0
            }
        }

        /// <summary>
        /// 处理激光干扰清除
        /// </summary>
        protected override void HandleJammingCleared(JammingParameters parameters)
        {
            base.HandleJammingCleared(parameters);
            if (parameters.Type == JammingType.Laser)
            {
                Debug.WriteLine("[LaserRangefinder] 激光测距仪干扰已清除", "Jamming");
                // 只有在整体未被干扰时才恢复有效状态
                if (!IsJammed)
                {
                    sensorData.IsValid = true;
                    sensorData.Distance = currentDistance; // 恢复上次测量的距离
                }
                 else
                {
                    sensorData.IsValid = false; // 如果仍被干扰，保持无效
                    sensorData.Distance = 0;
                }
            }
        }

        /// <summary>
        /// 计算目标距离
        /// </summary>
        /// <returns>测量的目标距离，单位：米</returns>
        /// <remarks>
        /// 计算逻辑：
        /// - 计算视线与地面的交点，或最大探测距离处的点。
        /// - 计算当前位置到该目标点的距离。
        /// - 限制在最大探测距离内。
        /// - 添加测量误差。
        /// </remarks>
        private double GetTargetDistance()
        {
            // 获取视线目标点
            var targetPoint = MotionAlgorithm.CalculateIntersectionWithGround(KState.Position, KState.Orientation.ToVector(), 0);

            if (targetPoint != null)
            {
                // 计算到目标点的斜距
                double dx = targetPoint.Value.X - KState.Position.X;
                double dy = targetPoint.Value.Y - KState.Position.Y;
                double dz = targetPoint.Value.Z - KState.Position.Z;
                double calculatedSlantRange = Math.Sqrt(dx * dx + dy * dy + dz * dz);

                // 确保距离不超过最大探测范围
                double baseSlantRange = Math.Min(calculatedSlantRange, config.MaxDetectionRange);
                
                // 添加测量精度引入的随机误差
                return baseSlantRange + (2 * new Random().NextDouble() - 1) * config.Accuracy;
            }
            else
            {
                return 0;
            }
        }
    }
}