ThreatSourceLibaray/ThreatSource/src/Sensor/LaserRangefinder.cs

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

namespace ThreatSource.Sensor
{
    /// <summary>
    /// 激光测距仪类，实现了目标距离测量和数据采集功能
    /// </summary>
    /// <remarks>
    /// 该类提供了激光测距仪的核心功能：
    /// - 实时距离测量
    /// - 脉冲频率控制
    /// - 量程范围控制
    /// - 数据采集
    /// 用于目标距离的精确测量和跟踪
    /// </remarks>
    public class LaserRangefinder : Sensor
    {
        /// <summary>
        /// 定义设备支持的干扰类型
        /// </summary>
        /// <remarks>
        /// 激光测距仪支持的干扰类型：
        /// - 激光干扰
        /// </remarks>
        public override IEnumerable<JammingType> SupportedJammingTypes => [JammingType.Laser];
        
        /// <summary>
        /// 激光测距仪数据
        /// </summary>
        private RangefinderSensorData sensorData;

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

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

        /// <summary>
        /// 获取或设置最大测量距离，单位：米
        /// </summary>
        /// <remarks>
        /// 定义了测距仪的量程上限
        /// 超出此距离的测量可能不准确
        /// 受大气条件和目标反射率影响
        /// </remarks>
        public double MaxDetectionRange { get; set; }

        /// <summary>
        /// 获取或设置工作波长，单位：微米
        /// </summary>
        /// <remarks>
        /// 定义了激光测距仪的工作波段
        /// 影响测量的准确性和可靠性
        /// </remarks>
        public double Wavelength { get; set; }

        /// <summary>
        /// 获取或设置脉冲频率，单位：赫兹
        /// </summary>
        /// <remarks>
        /// 定义了激光发射的重复频率
        /// 影响测量的更新速率和精度
        /// 需要根据目标运动特性选择
        /// </remarks>
        public double PulseRate { get; set; }

        /// <summary>
        /// 获取或设置测量精度，单位：米
        /// </summary>
        /// <remarks>
        /// 定义了测距仪的测量误差范围
        /// 影响测量的可靠性和稳定性
        /// 需要根据应用场景选择合适的精度
        /// </remarks>
        public double Accuracy { get; set; }

        /// <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 MotionParameters(submunition.Position, submunition.Orientation, submunition.Speed), simulationManager)
        {
            this.submunition = submunition;
            MaxDetectionRange = config.MaxDetectionRange;
            Wavelength = config.Wavelength;
            PulseRate = config.PulseRate;
            Accuracy = config.Accuracy;
            currentDistance = 0;
            sensorData = new RangefinderSensorData();
            
            // 初始化干扰处理，设置干扰抗性阈值和支持的干扰类型
            InitializeJamming(config.JammingResistanceThreshold, [JammingType.Laser]);
        }

        /// <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更新位置和朝向
            Position = submunition.Position;
            Orientation = submunition.Orientation;

            // 执行距离测量
            currentDistance = GetTargetDistance();
            sensorData.Distance = currentDistance;
        }

        /// <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 与工作波长 {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 - Wavelength) <= 0.01;
        }

        /// <summary>
        /// 处理激光干扰应用
        /// </summary>
        protected override void HandleJammingApplied(JammingParameters parameters)
        {
            base.HandleJammingApplied(parameters);
            // 基类 JammableComponent 和 Sensor 已经处理了 IsJammed 状态
            // 这里只处理测距仪特定的响应
            if (parameters.Type == JammingType.Laser)
            {         
                // 不需要再次检查波长，因为 ShouldHandleJamming 已经做过
                Debug.WriteLine($"[LaserRangefinder] 激光测距仪受到有效干扰，功率：{parameters.Power}瓦特", "Jamming");
                sensorData.IsValid = false;
                sensorData.Distance = 0;  // 在干扰状态下将距离设置为0
            }
        }

        /// <summary>
        /// 处理激光干扰清除
        /// </summary>
        protected override void HandleJammingCleared(JammingType type)
        {
            base.HandleJammingCleared(type);
            // 基类 JammableComponent 和 Sensor 已经处理了 IsJammed 状态
            // 这里只处理测距仪特定的响应
            if (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()
        {
            // 计算到地面的垂直距离和水平距离
            double verticalDistance = submunition.Position.Y;
            double horizontalDistance = Math.Sqrt(
                submunition.Position.X * submunition.Position.X + 
                submunition.Position.Z * submunition.Position.Z);
                
            // 计算实际的斜距
            double slantRange = Math.Sqrt(verticalDistance * verticalDistance + horizontalDistance * horizontalDistance);

            // 添加测量精度引入的随机误差
            return slantRange + (2 * new Random().NextDouble() - 1) * Accuracy;
        }
    }
}