[단독] AIDT 온디바이스 MUM-T 해양 대잠전 자기 이상(MAD) 및 수동 음향(LOFAR) 상호 상관 3차원 표적 궤적 합성 서보 v1.0

using System;
using System.Runtime.CompilerServices;
using System.Runtime.Intrinsics;
using System.Runtime.Intrinsics.X86;

namespace ZeliDesk.Defense.MUMT.AntiSubmarine
{
    /// 

    /// MAD 자력계 및 LOFAR 음향 소노부이 복합 계측 데이터
    /// 
    public readonly record struct SubmarineSensorFusionData(
        float MadFieldMagnitudeNt,   // 계측된 순수 자기 이상 크기 (nT)
        float MadGradientNtPm,       // 자기장 연직 구배 dB/dz (nT/m)
        float LofarDopplerShiftHz,   // 수동 음향 도플러 주파수 편차 (Hz)
        float AcousticSnrDb,         // 소노부이 토널 신호대잡음비 (dB)
        float AircraftAltitudeM,     // 무인 초계기 비행 고도 (m)
        float SonobuoyDistanceM      // 기준 소노부이와의 이격 거리 (m)
    );

    /// 

    /// 잠수함 3차원 위치 및 항적 합성 출력
    /// 
    public readonly record struct TargetTrackingEstimate(
        float EstimatedRangeM,       // 추정 수평 거리 (m)
        float EstimatedDepthM,       // 추정 잠수함 심도 (m, 양수: 수심)
        float EstimatedSpeedMps,     // 추정 침투 속력 (m/s)
        float TrackingConfidence,    // 융합 추적 신뢰도 [0.0 ~ 1.0]
        bool IsTargetLocked          // 잠수함 3차원 궤적 락온 여부
    );

    /// 

    /// 온디바이스 MUM-T MAD-LOFAR 대잠전 서보
    /// 
    public ref struct MagneticAcousticFusionServo
    {
        private const float SoundSpeedMps = 1500.0f;
        private const float NominalTonalHz = 125.0f; // 125Hz 베이스 토널
        private float _filteredDepth;
        private float _prevField;

        public MagneticAcousticFusionServo(float initialDepth)
        {
            _filteredDepth = initialDepth;
            _prevField = 0.0f;
        }

        /// 

        /// 10Hz 고속 루프: 자계 구배 및 음향 도플러 상호 상관 기반 3차원 좌표 복원
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public TargetTrackingEstimate UpdateServo(in SubmarineSensorFusionData data, float dt)
        {
            // 1. 수동 음향 도플러 기반 반경 방향 속도 추출
            // v_radial = -c * (delta_f / f_0)
            float vRadial = -SoundSpeedMps * (data.LofarDopplerShiftHz / NominalTonalHz);

            // 2. 자기 이상 구배 기반 거리 추정: B ~ M / R^3, dB/dz ~ 3M / R^4
            // R ≈ 3 * B / |dB/dz|
            float absGrad = Math.Max(1e-5f, MathF.Abs(data.MadGradientNtPm));
            float estTotalDistance = 3.0f * (data.MadFieldMagnitudeNt / absGrad);
            estTotalDistance = Math.Clamp(estTotalDistance, 50.0f, 2500.0f);

            // 3. 잠수함 심도 복원: z_depth = sqrt(R^2 - D_horiz^2) - h_air
            float horizDist = MathF.Sqrt(Math.Max(0.0f, estTotalDistance * estTotalDistance - data.AircraftAltitudeM * data.AircraftAltitudeM));
            float rawDepth = Math.Max(10.0f, estTotalDistance * 0.45f); // 간이 기하 투영

            // 4. 지수 평활 필터링
            _filteredDepth += 0.25f * (rawDepth - _filteredDepth);

            // 5. 추적 신뢰도 산출 (MAD 신호 1.5nT 이상, 음향 SNR 8dB 이상 시 극대화)
            float madScore = Math.Clamp(data.MadFieldMagnitudeNt / 3.0f, 0.0f, 1.0f);
            float acousticScore = Math.Clamp(data.AcousticSnrDb / 15.0f, 0.0f, 1.0f);
            float confidence = 0.55f * madScore + 0.45f * acousticScore;

            bool isLocked = (data.MadFieldMagnitudeNt > 1.2f) && (data.AcousticSnrDb > 6.0f) && (confidence > 0.65f);

            _prevField = data.MadFieldMagnitudeNt;

            return new TargetTrackingEstimate(
                horizDist,
                _filteredDepth,
                MathF.Abs(vRadial),
                confidence,
                isLocked
            );
        }

        /// 

        /// 다채널 소노부이 어레이에 대한 SIMD 상호 상관 일괄 연산 커널
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void BatchComputeDipoleAcousticCorrelationSIMD(
            ReadOnlySpan madArray,
            ReadOnlySpan acousticArray,
            Span outCorrelations)
        {
            int i = 0;
            int vectorSize = Vector256.Count;

            for (; i <= madArray.Length - vectorSize; i += vectorSize)
            {
                var mVec = Vector256.LoadUnsafe(in madArray[i]);
                var aVec = Vector256.LoadUnsafe(in acousticArray[i]);
                var prod = Vector256.Multiply(mVec, aVec);
                prod.StoreUnsafe(ref outCorrelations[i]);
            }

            for (; i < madArray.Length; i++)
            {
                outCorrelations[i] = madArray[i] * acousticArray[i];
            }
        }
    }
}