[단독] AIDT 온디바이스 MUM-T 함재기 캐터펄트 사출 직후 초기 천이구간 받음각 및 추력 능동 트림 서보 v1.0

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

namespace ZeliDesk.Defense.MUMT.Catapult
{
    /// 

    /// 캐터펄트 이탈 직후 비행체 센서 계측 데이터
    /// 
    public readonly record struct CatapultReleaseTelemetry(
        float TrueAirspeedMps,       // 대기속도 (m/s, 예: 82m/s)
        float AngleOfAttackDeg,      // 현재 받음각 alpha (도)
        float PitchRateDegPs,        // 피치 각속도 q (deg/s)
        float AltitudeRelFlightDeckM,// 비행갑판 기준 상대 고도 (m, 음수면 침하)
        float VerticalVelocityMps,   // 수직 상승/하강 속도 (m/s)
        float AircraftMassKg         // 기체 중량 (kg)
    );

    /// 

    /// 능동 수평미익 트림 명령 출력
    /// 
    public readonly record struct LaunchTrimServoCommand(
        float ElevatorTrimDeflectionDeg, // 수평미익 조타각 delta_e (도, -25도 ~ +15도)
        float NetVerticalG,              // 순 수직 가속도 (G)
        float PitchStallMarginDeg,       // 실속 한계까지의 여유각 (도)
        bool IsPositiveClimbEstablished  // 안전 양의 상승률(Positive Climb) 확보 여부
    );

    /// 

    /// 온디바이스 MUM-T 캐터펄트 사출 초기 트림 능동 서보
    /// 
    public ref struct CarrierCatapultLaunchTrimServo
    {
        private const float TargetAoaDeg = 13.5f;       // 최적 상승 받음각
        private const float StallAoaDeg = 18.0f;        // 실속 한계각
        private const float MinSafeAltitudeM = -1.2f;   // 허용 최대 침하 한계

        private float _integratedAoaError;
        private float _prevAoaDeg;

        public CarrierCatapultLaunchTrimServo(float initialAoa)
        {
            _integratedAoaError = 0.0f;
            _prevAoaDeg = initialAoa;
        }

        /// 

        /// 1kHz 고속 루프: 받음각 트림 및 침하 방지 미익 조타 연산
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public LaunchTrimServoCommand UpdateTrimServo(in CatapultReleaseTelemetry data, float dt)
        {
            // 1. 받음각 오차 산출
            float aoaError = TargetAoaDeg - data.AngleOfAttackDeg;
            _integratedAoaError += aoaError * dt;
            float dAoa = (data.AngleOfAttackDeg - _prevAoaDeg) / Math.Max(1e-5f, dt);
            _prevAoaDeg = data.AngleOfAttackDeg;

            // 2. 침하(Sink) 보정 게인: 고도가 낮거나 하강 중일 때 추가 기수 상승 바이어스
            float sinkBias = 0.0f;
            if (data.VerticalVelocityMps < 0.0f)
            {
                sinkBias = -data.VerticalVelocityMps * 1.5f;
            }

            // 3. 수평미익 명령 (Kp = 2.4, Ki = 0.4, Kd = 0.15)
            // 양의 수평미익 조타는 기수 상승 모멘트 유발
            float deltaE = (2.4f * aoaError + 0.4f * _integratedAoaError - 0.15f * data.PitchRateDegPs) + sinkBias;

            // 물리적 작동기 각도 클램핑 [-25도, +15도]
            deltaE = Math.Clamp(deltaE, -25.0f, 15.0f);

            // 4. 실속 마진 및 양의 상승 판단
            float stallMargin = StallAoaDeg - data.AngleOfAttackDeg;
            float netG = 1.0f + (data.VerticalVelocityMps * 0.1f);

            bool isSafeClimb = (data.VerticalVelocityMps > 1.5f) && 
                               (data.AltitudeRelFlightDeckM > MinSafeAltitudeM) && 
                               (stallMargin > 2.5f);

            return new LaunchTrimServoCommand(
                deltaE,
                netG,
                stallMargin,
                isSafeClimb
            );
        }

        /// 

        /// 다중 무인기 편대 사출 시뮬레이션 배열에 대한 SIMD 실속 마진 일괄 검증
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void BatchEvaluatePitchSafetyMarginSIMD(
            ReadOnlySpan aoaArray,
            Span outMargins)
        {
            int i = 0;
            int vectorSize = Vector256.Count;
            var stallVec = Vector256.Create(StallAoaDeg);

            for (; i <= aoaArray.Length - vectorSize; i += vectorSize)
            {
                var aoaVec = Vector256.LoadUnsafe(in aoaArray[i]);
                var marginVec = Vector256.Subtract(stallVec, aoaVec);
                marginVec.StoreUnsafe(ref outMargins[i]);
            }

            for (; i < aoaArray.Length; i++)
            {
                outMargins[i] = StallAoaDeg - aoaArray[i];
            }
        }
    }
}