[단독] AIDT 온디바이스 MUM-T 함재기 광학 착함 유도시스템(FLOLS/IFLOLS) 가상 미트볼(Meatball) 활공각 시각 정렬 서보 v1.0

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

namespace ZeliDesk.Defense.MUMT.Landing
{
    /// 

    /// IFLOLS 시각 미트볼 센서 및 함정 모션 텔레메트리
    /// 
    public readonly record struct IflolsOpticalTelemetry(
        float RangeToTouchdownM,    // 접촉점까지의 잔여 거리 (m, 100m ~ 1800m)
        float ApproachAirspeedMps,  // 착함 접근 속도 (m/s, 예: 72m/s)
        float MeatballAngularErrorDeg,// 미트볼-데이텀 간 각오차 (도, 양수: High, 음수: Low)
        float DeckHeaveVelocityMps, // 착함 갑판 상하 수직 요동 속도 (m/s)
        float DeckPitchDeg          // 함정 피칭 각도 (도)
    );

    /// 

    /// 활공각 추종 비행제어 서보 출력
    /// 
    public readonly record struct GlideSlopeServoOutput(
        float DesiredSinkRateMps,   // 요구 강하율 (m/s)
        float ThrottleTrimDelta,    // 오토스로틀 보정량 [-1.0 ~ +1.0]
        float ElevatorPitchDeltaDeg,// 승강타 피치 보정각 (도)
        bool IsOnGlideSlopeCenter   // 최적 활공 경로 락온(미트볼 센터) 여부
    );

    /// 

    /// 온디바이스 MUM-T IFLOLS 가상 미트볼 활공각 서보
    /// 
    public ref struct IflolsMeatballOpticalServo
    {
        private const float NominalGlideSlopeDeg = 3.5f;
        private const float DegToRad = MathF.PI / 180.0f;
        private float _integratedMeatballError;
        private float _prevMeatballError;

        public IflolsMeatballOpticalServo(float initialError)
        {
            _integratedMeatballError = 0.0f;
            _prevMeatballError = initialError;
        }

        /// 

        /// 100Hz 고속 루프: 미트볼 광학 각도 오차 기반 요구 강하율 및 스로틀 제어
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public GlideSlopeServoOutput UpdateServo(in IflolsOpticalTelemetry data, float dt)
        {
            // 1. 공칭 강하율 산출: V_app * sin(3.5도)
            float nominalSinkRate = data.ApproachAirspeedMps * MathF.Sin(NominalGlideSlopeDeg * DegToRad);

            // 2. 미트볼 각도 오차 미적분
            _integratedMeatballError += data.MeatballAngularErrorDeg * dt;
            float dError = (data.MeatballAngularErrorDeg - _prevMeatballError) / Math.Max(1e-5f, dt);
            _prevMeatballError = data.MeatballAngularErrorDeg;

            // 3. 요구 수직 강하율 제어 루프 (Kp = 1.8, Ki = 0.25, Kd = 0.4)
            // 미트볼이 높으면(High, 양수) 강하율 증가, 낮으면(Low, 음수) 강하율 감소
            float correctionSinkRate = (1.8f * data.MeatballAngularErrorDeg + 
                                        0.25f * _integratedMeatballError + 
                                        0.4f * dError);

            // 함정 갑판 히브 속도 피드포워드 차감
            float targetSinkRate = nominalSinkRate + correctionSinkRate - (data.DeckHeaveVelocityMps * 0.7f);

            // 4. 오토스로틀 및 승강타 보정량 연산
            float throttleTrim = -0.2f * (targetSinkRate - nominalSinkRate);
            throttleTrim = Math.Clamp(throttleTrim, -0.5f, 0.5f);

            float elevatorTrim = -1.2f * data.MeatballAngularErrorDeg;
            elevatorTrim = Math.Clamp(elevatorTrim, -5.0f, 5.0f);

            bool isCentered = MathF.Abs(data.MeatballAngularErrorDeg) < 0.08f;

            return new GlideSlopeServoOutput(
                targetSinkRate,
                throttleTrim,
                elevatorTrim,
                isCentered
            );
        }

        /// 

        /// 다채널 무인 편대 접근 배열에 대한 SIMD 미트볼 각거리 일괄 벡터 산출
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void BatchComputeMeatballDisplacementSIMD(
            ReadOnlySpan ballPixelsY,
            ReadOnlySpan datumPixelsY,
            float arcsecPerPixel,
            Span outErrorDeg)
        {
            int i = 0;
            int vectorSize = Vector256.Count;
            float degPerPixel = arcsecPerPixel / 3600.0f;
            var scaleVec = Vector256.Create(degPerPixel);

            for (; i <= ballPixelsY.Length - vectorSize; i += vectorSize)
            {
                var ballVec = Vector256.LoadUnsafe(in ballPixelsY[i]);
                var datumVec = Vector256.LoadUnsafe(in datumPixelsY[i]);

                var diffVec = Vector256.Subtract(datumVec, ballVec); // 픽셀 Y축 반전 고려
                var degVec = Vector256.Multiply(diffVec, scaleVec);

                degVec.StoreUnsafe(ref outErrorDeg[i]);
            }

            for (; i < ballPixelsY.Length; i++)
            {
                outErrorDeg[i] = (datumPixelsY[i] - ballPixelsY[i]) * degPerPixel;
            }
        }
    }
}