AIDT 온디바이스 국방 MUM-T 잠수함 수중 사출형 무인기(UAV) 캐니스터 이탈 충격 완충 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.SubmarineLaunchedUav
{
    /// <summary>
    /// 수중 사출 UAV 가속도 및 캐니스터 분리 텔레메트리
    /// </summary>
    public readonly ref struct SubUavLaunchTelemetryState
    {
        public readonly double BaseVerticalAccelG;        // 기저 수직 가속도 [G]
        public readonly double RelativeDisplacementMm;    // 충격 격리단 상대 변위 [mm]
        public readonly double WaterExitDetectorBar;      // 외부 정압 센서 [bar]
        public readonly double WingDeploymentAngleDeg;    // 주익 전개 각도 [deg]

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SubUavLaunchTelemetryState(double aG, double xMm, double pWater, double wingDeg)
        {
            BaseVerticalAccelG = aG;
            RelativeDisplacementMm = xMm;
            WaterExitDetectorBar = pWater;
            WingDeploymentAngleDeg = wingDeg;
        }
    }

    /// <summary>
    /// MR 댐퍼 및 날개 전개 서보 지령
    /// </summary>
    public readonly ref struct UavSeparationCommand
    {
        public readonly double MrDamperCurrentAmps;       // MR 유체 댐퍼 제어 전류 [A]
        public readonly double WingServoTorqueNm;         // 날개 전개 서보 토크 [Nm]
        public readonly bool FlightModeReady;             // 공중 활공 모드 전환 준비 완료

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public UavSeparationCommand(double iDamper, double torqueWing, bool ready)
        {
            MrDamperCurrentAmps = iDamper;
            WingServoTorqueNm = torqueWing;
            FlightModeReady = ready;
        }
    }

    /// <summary>
    /// 10,000Hz 0-Alloc 잠수함 사출 UAV 충격 완충 서보 엔진
    /// </summary>
    public sealed class SubmarineUavLaunchServoEngine
    {
        private const double MaxDamperAmps = 2.5;         // MR 댐퍼 최대 전류 2.5A
        private double _prevDisplacement;
        private bool _canisterJettisoned;

        public SubmarineUavLaunchServoEngine()
        {
            _prevDisplacement = 0.0;
            _canisterJettisoned = false;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public UavSeparationCommand UpdateServoLoop(in SubUavLaunchTelemetryState telemetry, double dtSeconds)
        {
            // 1. 상대 변위 속도 계산
            double vRel = (telemetry.RelativeDisplacementMm - _prevDisplacement) / dtSeconds;
            _prevDisplacement = telemetry.RelativeDisplacementMm;

            // 2. 수면 돌파 감지 (대기압 1.0 bar 진입 및 급격한 가속도 피크)
            if (telemetry.WaterExitDetectorBar < 1.1 && telemetry.BaseVerticalAccelG > 8.0)
            {
                _canisterJettisoned = true;
            }

            // 3. 스카이훅(Skyhook) 지반 격리 MR 댐퍼 능동 가변 전류 지령
            double iDamper = 0.0;
            if (telemetry.BaseVerticalAccelG > 2.0)
            {
                iDamper = Math.Clamp(Math.Abs(telemetry.BaseVerticalAccelG) * 0.18, 0.2, MaxDamperAmps);
            }

            // 4. 날개 전개 시점 동조 서보 (충격 가속도가 3G 이하로 진정된 후 90도 전개)
            double wingTorque = 0.0;
            if (_canisterJettisoned && telemetry.BaseVerticalAccelG < 3.0)
            {
                double wingErr = 90.0 - telemetry.WingDeploymentAngleDeg;
                wingTorque = Math.Clamp(wingErr * 0.45, 0.0, 15.0); // 최대 15 Nm 전개 토크
            }

            bool isReady = _canisterJettisoned && (telemetry.WingDeploymentAngleDeg > 88.0);
            return new UavSeparationCommand(iDamper, wingTorque, isReady);
        }
    }
}