AIDT 온디바이스 국방 MUM-T 잠수함 발사 자율 음향 기만기(ADC) 능·수동 음향 표적 신호 합성 및 회피 기동 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.MumT.SubmarineDecoy
{
    /// <summary>
    /// 기만기 수신 하이드로폰 및 어뢰 음향 센서 계측 상태
    /// </summary>
    public readonly ref struct TorpedoAcousticPingState
    {
        public readonly double PingFrequencyKhz;     // 수신된 어뢰 능동 핑 주파수 [kHz]
        public readonly double PingReceivedLevelDb;  // 수신 음압 레벨 (RL) [dB]
        public readonly double TorpedoBearingRad;    // 어뢰 방위각 [rad]
        public readonly double CurrentDepthMeters;   // 기만기 현재 심도 [m]
        public readonly double CurrentHeadingRad;    // 기만기 현재 침로 [rad]
        public readonly double DecoySpeedKnots;      // 기만기 속력 [kt]

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public TorpedoAcousticPingState(double freq, double rl, double bearing, double depth, double heading, double speed)
        {
            PingFrequencyKhz = freq;
            PingReceivedLevelDb = rl;
            TorpedoBearingRad = bearing;
            CurrentDepthMeters = depth;
            CurrentHeadingRad = heading;
            DecoySpeedKnots = speed;
        }
    }

    /// <summary>
    /// 기만 음향 방출 지령 및 회피 조타 명령
    /// </summary>
    public readonly ref struct DecoyDefenseOutput
    {
        public readonly double TransmitSoundLevelDb; // 송신 반향 합성 음압 레벨 [dB]
        public readonly double DopplerShiftHz;       // 도플러 변조 주파수 편위 [Hz]
        public readonly double RudderCommandRad;     // 방향타 조타각 [rad]
        public readonly double SternPlaneCommandRad; // 잠타(승강타) 조타각 [rad]
        public readonly bool IsDecoyActive;          // 기만 음향 방출 유효 플래그

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public DecoyDefenseOutput(double sl, double doppler, double rudder, double stern, bool active)
        {
            TransmitSoundLevelDb = sl;
            DopplerShiftHz = doppler;
            RudderCommandRad = rudder;
            SternPlaneCommandRad = stern;
            IsDecoyActive = active;
        }
    }

    /// <summary>
    /// 10,000Hz 잠수함 자율 음향 기만기(ADC) 신호 합성 및 조타 서보 엔진
    /// </summary>
    public ref struct SubmarineAcousticDecoyServoEngine
    {
        private const double SoundSpeedWater = 1500.0; // 수중 음속 [m/s]
        private readonly double _virtualSubmarineTsDb; // 가상 잠수함 표적 반향 강도 (TS) [dB]
        private readonly double _kpHeading;
        private readonly double _kdHeading;
        private readonly double _kpDepth;
        private readonly double _kdDepth;

        private double _prevHeadingErr;
        private double _prevDepthErr;

        public SubmarineAcousticDecoyServoEngine(
            double virtualSubmarineTsDb,
            double kpHeading,
            double kdHeading,
            double kpDepth,
            double kdDepth)
        {
            _virtualSubmarineTsDb = virtualSubmarineTsDb;
            _kpHeading = kpHeading;
            _kdHeading = kdHeading;
            _kpDepth = kpDepth;
            _kdDepth = kdDepth;
            _prevHeadingErr = 0.0;
            _prevDepthErr = 0.0;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public DecoyDefenseOutput ProcessCycle(
            in TorpedoAcousticPingState state,
            double targetDepthMeters,
            double targetHeadingRad,
            double dtSeconds)
        {
            // 1. 적 어뢰 능동 핑 수신 판정 (RL > 70dB)
            bool isPingDetected = state.PingReceivedLevelDb >= 70.0;

            // 2. 표적 반향 신호 음압 레벨 및 가상 도플러 연산
            double slEchoDb = 0.0;
            double dopplerHz = 0.0;

            if (isPingDetected)
            {
                // SL = RL + TS
                slEchoDb = state.PingReceivedLevelDb + _virtualSubmarineTsDb;
                slEchoDb = Math.Clamp(slEchoDb, 120.0, 210.0); // 트랜스듀서 출력 한계

                // 도플러 시프트: fd = f0 * (2 * v * cos(theta)) / c
                double speedMps = state.DecoySpeedKnots * 0.514444;
                double relAngle = state.TorpedoBearingRad - state.CurrentHeadingRad;
                dopplerHz = (state.PingFrequencyKhz * 1000.0) * (2.0 * speedMps * Math.Cos(relAngle)) / SoundSpeedWater;
            }

            // 3. 침로 제어 루프 (PD)
            double headingErr = targetHeadingRad - state.CurrentHeadingRad;
            // 각도 랩핑 (-pi ~ +pi)
            while (headingErr > Math.PI) headingErr -= 2.0 * Math.PI;
            while (headingErr < -Math.PI) headingErr += 2.0 * Math.PI;

            double derHeading = (headingErr - _prevHeadingErr) / dtSeconds;
            _prevHeadingErr = headingErr;
            double rudderCmd = Math.Clamp(_kpHeading * headingErr + _kdHeading * derHeading, -0.436, 0.436); // +-25도

            // 4. 심도 제어 루프 (PD)
            double depthErr = targetDepthMeters - state.CurrentDepthMeters;
            double derDepth = (depthErr - _prevDepthErr) / dtSeconds;
            _prevDepthErr = depthErr;
            double sternCmd = Math.Clamp(_kpDepth * depthErr + _kdDepth * derDepth, -0.35, 0.35); // +-20도

            return new DecoyDefenseOutput(slEchoDb, dopplerHz, rudderCmd, sternCmd, isPingDetected);
        }
    }
}