AIDT 온디바이스 국방 MUM-T 대함 미사일 밀리미터파/IIR 복합 시커 디코이 분리 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.AntiShipDualSeeker
{
    /// <summary>
    /// MMW/IIR 복합 시커 텔레메트리 상태
    /// </summary>
    public readonly ref struct DualSeekerSensorTelemetryState
    {
        public readonly double MmwAzimuthErrorDeg;       // MMW 레이더 방위각 오차 [deg]
        public readonly double MmwElevationErrorDeg;     // MMW 레이더 고저각 오차 [deg]
        public readonly double IirAzimuthErrorDeg;       // IIR 영상 방위각 오차 [deg]
        public readonly double IirElevationErrorDeg;     // IIR 영상 고저각 오차 [deg]
        public readonly double TargetDopplerVelocityMps; // 표적 도플러 속도 [m/s]
        public readonly double ClosingVelocityMps;       // 미사일 접근 속도 [m/s]
        public readonly double DecoyConfusionIndex;      // 디코이 교란 지수 (0~1)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public DualSeekerSensorTelemetryState(
            double mAz, double mEl, double iAz, double iEl, double vDop, double vClose, double decoy)
        {
            MmwAzimuthErrorDeg = mAz;
            MmwElevationErrorDeg = mEl;
            IirAzimuthErrorDeg = iAz;
            IirElevationErrorDeg = iEl;
            TargetDopplerVelocityMps = vDop;
            ClosingVelocityMps = vClose;
            DecoyConfusionIndex = decoy;
        }
    }

    /// <summary>
    /// 복합 시커 짐벌 및 비행 유도 명령
    /// </summary>
    public readonly ref struct DualSeekerGuidanceCommand
    {
        public readonly double FusedAzimuthCommandDeg;   // 융합 방위각 유도 지령 [deg]
        public readonly double FusedElevationCommandDeg; // 융합 고저각 유도 지령 [deg]
        public readonly double CommandedGuidanceG;       // 비례항법 유도 가속도 [G]
        public readonly bool DecoyRejected;              // 기만체 분리 배제 성공 여부

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public DualSeekerGuidanceCommand(double az, double el, double g, bool rejected)
        {
            FusedAzimuthCommandDeg = az;
            FusedElevationCommandDeg = el;
            CommandedGuidanceG = g;
            DecoyRejected = rejected;
        }
    }

    /// <summary>
    /// 10,000Hz 주기 대함 미사일 복합 시커 디코이 분리 유도 서보 엔진
    /// </summary>
    public ref struct AntiShipDualSeekerGuidanceServoEngine
    {
        private const double NavGain = 3.9;
        private double _prevAzimuth;
        private double _prevElevation;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public DualSeekerGuidanceCommand CalculateGuidance(
            in DualSeekerSensorTelemetryState state, double dtSeconds)
        {
            // 1. 도플러 기반 채프 배제 & 센서 신뢰도 가중치 동적 산출
            bool isChaffPresent = state.DecoyConfusionIndex > 0.5;
            double wMmw = isChaffPresent ? 0.25 : 0.60;
            double wIir = 1.0 - wMmw;

            // 2. 센서 융합 방위각 및 고저각
            double fusedAz = wMmw * state.MmwAzimuthErrorDeg + wIir * state.IirAzimuthErrorDeg;
            double fusedEl = wMmw * state.MmwElevationErrorDeg + wIir * state.IirElevationErrorDeg;

            // 시선 각속도 계산
            double azRate = (dtSeconds > 1e-9) ? (fusedAz - _prevAzimuth) / dtSeconds : 0.0;
            double elRate = (dtSeconds > 1e-9) ? (fusedEl - _prevElevation) / dtSeconds : 0.0;
            _prevAzimuth = fusedAz;
            _prevElevation = fusedEl;

            // 3. 비례항법 유도 가속도 지령
            double totalLosRateRad = Math.Sqrt(azRate * azRate + elRate * elRate) * (Math.PI / 180.0);
            double cmdAccel = NavGain * state.ClosingVelocityMps * totalLosRateRad;
            double cmdG = cmdAccel / 9.80665;
            cmdG = Math.Clamp(cmdG, -25.0, 25.0);

            // 기만체 분리 판정: 도플러 속도 차이 확보 및 융합 오차 정합
            bool decoyRejected = Math.Abs(state.TargetDopplerVelocityMps) > 5.0; // 5m/s 이상 함정 기동

            return new DualSeekerGuidanceCommand(
                fusedAz,
                fusedEl,
                cmdG,
                decoyRejected
            );
        }
    }
}