전방 도플러 라이다(Doppler LiDAR) 청천 난류(CAT) 10km 조기 탐지 & 1,000Hz 0-Alloc C# 능동 플랩 돌풍 완화(Gust Alleviation) 비행 서보 엔진

using System;
using System.Runtime.CompilerServices;
using System.Numerics;

namespace ZeliDesk.Stem.Aerodynamics
{
    /// 

    /// 1,000Hz 전방 도플러 라이다 청천 난류(CAT) 감시 및 능동 돌풍 완화(GLA) 비행 서보
    /// GC 무할당 스택 메모리 0-Alloc 구현체
    /// 
    public ref struct DopplerLidarGustAlleviationServo
    {
        // 광학 및 비행 물리 상수
        private const float LidarWavelengthMeter = 1.55e-6f;   // 1.55 um Eye-safe 파이버 레이저
        private const float SpeedOfSoundMps = 300.0f;          // 고고도 음속
        private const float AirDensityKgM3 = 0.38f;            // FL350 고고도 대기 밀도 (kg/m^3)
        private const float WingAreaM2 = 125.0f;               // 주익 면적
        private const float AircraftMassKg = 65000.0f;         // 기체 질량 (65톤)
        private const float CruiseSpeedMps = 240.0f;           // 마하 0.8 순항 속도 (m/s)

        // 서보 제어 상태
        public float TargetAileronDeflectionDeg;
        public float TargetElevatorDeflectionDeg;
        public float EstimatedVerticalGustMps;
        public float FilteredShearIntensity;
        public float PredictedGLoad;
        public float TimeToImpactSec;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Initialize()
        {
            TargetAileronDeflectionDeg = 0.0f;
            TargetElevatorDeflectionDeg = 0.0f;
            EstimatedVerticalGustMps = 0.0f;
            FilteredShearIntensity = 0.0f;
            PredictedGLoad = 1.0f; // 기본 1G 수평 비행
            TimeToImpactSec = 0.0f;
        }

        /// 

        /// 1ms 제어 주기: 라이다 도플러 주파수 스펙트럼 피크로부터 1km 전방 하강 풍속 산출 및 상쇄 플랩 타각 지령
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void StepGustControl(
            ReadOnlySpan beatFrequencyBinsKhz, 
            ReadOnlySpan spectralPowerDb, 
            float lookAheadDistanceM,
            out float aileronCmdDeg, 
            out float elevatorCmdDeg, 
            out float deltaLiftKn)
        {
            // 1. 최대 전력 도플러 피크 주파수 검출 (0-Alloc 스택 선형 탐색)
            float maxPwr = -999.0f;
            float peakFreqKhz = 0.0f;
            int count = Math.Min(beatFrequencyBinsKhz.Length, spectralPowerDb.Length);

            for (int i = 0; i < count; i++)
            {
                if (spectralPowerDb[i] > maxPwr)
                {
                    maxPwr = spectralPowerDb[i];
                    peakFreqKhz = beatFrequencyBinsKhz[i];
                }
            }

            // 2. 도플러 편이 -> 시선 속도 역산: v_los = (lambda * delta_f) / 2
            float vLos = (LidarWavelengthMeter * (peakFreqKhz * 1000.0f)) * 0.5f;

            // 기체 진행각 대비 수직 하강 성분 분해 (앙각 5도 전방 하향 스캔 가정)
            const float ScanElevationRad = 0.087266f; // ~5 deg
            float verticalGustRaw = vLos / MathF.Sin(ScanElevationRad);

            // 저역 통과 필터링 (고주파 노이즈 억제)
            EstimatedVerticalGustMps = EstimatedVerticalGustMps * 0.95f + verticalGustRaw * 0.05f;

            // 충돌 잔여 시간(Time-to-Impact)
            TimeToImpactSec = MathF.Max(0.01f, lookAheadDistanceM / CruiseSpeedMps);

            // 3. 준정상 공기역학 양력 섭동량(Delta Lift) 예측: dL = 0.5 * rho * V * S * C_L_alpha * w_gust
            const float LiftCurveSlopeClAlpha = 5.2f; // 양력 기울기 (1/rad)
            float deltaLiftN = 0.5f * AirDensityKgM3 * CruiseSpeedMps * WingAreaM2 * LiftCurveSlopeClAlpha * (EstimatedVerticalGustMps / CruiseSpeedMps);
            deltaLiftKn = deltaLiftN * 0.001f;

            // 예측 G-로드: Delta_G = Delta_L / (m * g)
            const float GConst = 9.80665f;
            float deltaG = deltaLiftN / (AircraftMassKg * GConst);
            PredictedGLoad = 1.0f + deltaG;

            // 4. 피드포워드 능동 플랩/에일러론 상쇄 제어기 (양력 역위상 덤핑)
            // dL_cmd = -deltaLiftN
            const float ControlGainAileron = -0.45f;   // 양력 상쇄 게인 (deg / (m/s))
            const float ControlGainElevator = 0.18f;   // 피치 모멘트 보상 게인 (deg / (m/s))

            TargetAileronDeflectionDeg = EstimatedVerticalGustMps * ControlGainAileron;
            TargetElevatorDeflectionDeg = EstimatedVerticalGustMps * ControlGainElevator;

            // 기계적 타각 리미터 한계 (-20도 ~ +20도)
            TargetAileronDeflectionDeg = MathF.Max(-20.0f, MathF.Min(20.0f, TargetAileronDeflectionDeg));
            TargetElevatorDeflectionDeg = MathF.Max(-15.0f, MathF.Min(15.0f, TargetElevatorDeflectionDeg));

            aileronCmdDeg = TargetAileronDeflectionDeg;
            elevatorCmdDeg = TargetElevatorDeflectionDeg;
        }
    }
}