[단독] AIDT 온디바이스 MUM-T 자율 기뢰 탐색(MCM) 합성개구소나(SAS) 도플러 궤적 보상 및 마이크로나비게이션 위상 서보 v1.0

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

namespace ZeliDesk.Defense.MUMT.Sonar
{
    /// 

    /// SAS 핑 수신 신호 및 관성/DVL 복합 측정 파라미터
    /// 
    public readonly record struct SasHydrophonePingData(
        float AuvVelocityMps,       // AUV 전진 속도 (m/s)
        float SoundSpeedMps,        // 수중 음속 (m/s, 통상 1500m/s)
        float CenterFreqHz,         // 중심 주파수 (Hz, 예: 150kHz)
        float PingIntervalSec,      // 핑 간격 (초, 예: 0.05s)
        float SwayDisplacementMm,   // DVL 추정 횡방향 요동 (mm)
        float HeaveDisplacementMm   // DVL 추정 수직 요동 (mm)
    );

    /// 

    /// 마이크로나비게이션 위상 보상 출력 명령
    /// 
    public readonly record struct SasPhaseCorrectionResult(
        float EstimatedRangeErrorMm,// 추정 궤적 오차 거리 (mm)
        float PhaseCorrectionRad,   // 위상 보정각 (rad)
        float ImageCoherenceMetric, // 영상 위상 일관성 지표 [0.0 ~ 1.0]
        bool IsPhaseLocked          // 위상 락온(허용 오차 이내) 여부
    );

    /// 

    /// 온디바이스 MUM-T 합성개구소나 마이크로나비게이션 위상 서보
    /// 
    public ref struct SyntheticApertureSonarServo
    {
        private const float TwoPi = 6.28318530718f;
        private float _integratedPhaseDrift;
        private float _prevRangeErrorMm;

        public SyntheticApertureSonarServo(float initialDrift)
        {
            _integratedPhaseDrift = initialDrift;
            _prevRangeErrorMm = 0.0f;
        }

        /// 

        /// 핑 단위(20~50Hz) 실시간 궤적 변위 위상 보정각 산출
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SasPhaseCorrectionResult UpdateMicronavigation(in SasHydrophonePingData ping)
        {
            // 1. 파장 계산: lambda = c / f
            float lambdaM = ping.SoundSpeedMps / ping.CenterFreqHz;
            float lambdaMm = lambdaM * 1000.0f;

            // 2. 합성 요동 오차 거리: delta_r = sqrt(sway^2 + heave^2) * 부호 보정
            float netDisplacementMm = MathF.Sqrt(ping.SwayDisplacementMm * ping.SwayDisplacementMm + 
                                                 ping.HeaveDisplacementMm * ping.HeaveDisplacementMm);

            if (ping.SwayDisplacementMm < 0.0f)
            {
                netDisplacementMm = -netDisplacementMm;
            }

            // 3. 왕복 전파 위상 편차: delta_phi = (4 * pi / lambda) * delta_r
            float phaseDeltaRad = (2.0f * TwoPi / lambdaMm) * netDisplacementMm;

            // 라디안 [-pi, +pi] 정규화
            while (phaseDeltaRad > MathF.PI) phaseDeltaRad -= TwoPi;
            while (phaseDeltaRad < -MathF.PI) phaseDeltaRad += TwoPi;

            // 4. 위상 일관성 평가 (허용 오차: lambda / 8)
            float errorRatio = MathF.Abs(netDisplacementMm) / (lambdaMm / 8.0f);
            float coherence = Math.Max(0.0f, 1.0f - Math.Min(1.0f, errorRatio));

            bool isLocked = MathF.Abs(netDisplacementMm) <= (lambdaMm / 8.0f);

            _integratedPhaseDrift += phaseDeltaRad;
            _prevRangeErrorMm = netDisplacementMm;

            return new SasPhaseCorrectionResult(
                netDisplacementMm,
                phaseDeltaRad,
                coherence,
                isLocked
            );
        }

        /// 

        /// 다채널 하이드로폰 복소 신호 배열에 대한 SIMD 고속 위상 역회전 회생 연산
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void BatchRotateHydrophonePhaseSIMD(
            ReadOnlySpan inReal,
            ReadOnlySpan inImag,
            float phaseCorrectionRad,
            Span outReal,
            Span outImag)
        {
            float cosPhi = MathF.Cos(-phaseCorrectionRad);
            float sinPhi = MathF.Sin(-phaseCorrectionRad);

            int i = 0;
            int vectorSize = Vector256.Count;

            var cosVec = Vector256.Create(cosPhi);
            var sinVec = Vector256.Create(sinPhi);

            // 복소수 곱셈: (R + jI) * (cos + j sin) = (R*cos - I*sin) + j(R*sin + I*cos)
            for (; i <= inReal.Length - vectorSize; i += vectorSize)
            {
                var rVec = Vector256.LoadUnsafe(in inReal[i]);
                var iVec = Vector256.LoadUnsafe(in inImag[i]);

                var rCos = Vector256.Multiply(rVec, cosVec);
                var iSin = Vector256.Multiply(iVec, sinVec);
                var newR = Vector256.Subtract(rCos, iSin);

                var rSin = Vector256.Multiply(rVec, sinVec);
                var iCos = Vector256.Multiply(iVec, cosVec);
                var newI = Vector256.Add(rSin, iCos);

                newR.StoreUnsafe(ref outReal[i]);
                newI.StoreUnsafe(ref outImag[i]);
            }

            // 잔여 요소 처리
            for (; i < inReal.Length; i++)
            {
                float r = inReal[i];
                float im = inImag[i];
                outReal[i] = r * cosPhi - im * sinPhi;
                outImag[i] = r * sinPhi + im * cosPhi;
            }
        }
    }
}