AIDT 온디바이스 국방 MUM-T 무인수상정(USV) 초공동(Supercavitating) 수중 로켓 어뢰 사출 캐비테이터 자세안정화 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.MumT.SupercavitatingTorpedo
{
    /// <summary>
    /// 초공동 어뢰 주행 동역학 및 기포 내부 상태
    /// </summary>
    public readonly ref struct SupercavitationKinematicState
    {
        public readonly double SpeedMetersPerSec;    // 수중 속도 [m/s]
        public readonly double DepthMeters;          // 잠항 심도 [m]
        public readonly double PitchAngleRad;        // 피치각 [rad]
        public readonly double PitchRateRadPerSec;   // 피치 각속도 [rad/s]
        public readonly double TailVerticalOffsetM;  // 미부 중심 상하 편위 [m]
        public readonly double CavityInternalPressPa;// 기포 내부 가스압 [Pa]

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public SupercavitationKinematicState(
            double speed,
            double depth,
            double pitch,
            double q,
            double tailOffset,
            double pCavity)
        {
            SpeedMetersPerSec = speed;
            DepthMeters = depth;
            PitchAngleRad = pitch;
            PitchRateRadPerSec = q;
            TailVerticalOffsetM = tailOffset;
            CavityInternalPressPa = pCavity;
        }
    }

    /// <summary>
    /// 노즈 캐비테이터 각도 및 가스 분사 서보 지령
    /// </summary>
    public readonly ref struct CavitatorActuatorCommand
    {
        public readonly double CavitatorPitchDeflectionRad; // 캐비테이터 피치 타각 [rad]
        public readonly double VentilatedGasMassFlowKgPerS; // 환형 가스 분사 유량 [kg/s]
        public readonly double EstimatedClearanceMm;        // 미부-기포벽 간극 [mm]
        public readonly bool IsTailSlapWarning;             // 테일 슬랩 충돌 임계 경보

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public CavitatorActuatorCommand(double deltaRad, double gasFlow, double clearanceMm, bool warning)
        {
            CavitatorPitchDeflectionRad = deltaRad;
            VentilatedGasMassFlowKgPerS = gasFlow;
            EstimatedClearanceMm = clearanceMm;
            IsTailSlapWarning = warning;
        }
    }

    /// <summary>
    /// 10,000Hz 초공동 수중 로켓 어뢰 캐비테이터 자세안정화 서보 엔진
    /// </summary>
    public ref struct SupercavitatingTorpedoServoEngine
    {
        private const double WaterDensity = 1025.0; // 해수 밀도 [kg/m^3]
        private const double Gravity = 9.80665;
        private const double AtmosphericPressPa = 101325.0;

        private readonly double _cavitatorDiameterM;
        private readonly double _bodyRadiusM;
        private readonly double _tailDistanceM;
        private readonly double _kpAngle;
        private readonly double _kdRate;
        private readonly double _kgGas;

        private double _prevClearanceM;

        public SupercavitatingTorpedoServoEngine(
            double cavitatorDiameterM,
            double bodyRadiusM,
            double tailDistanceM,
            double kpAngle,
            double kdRate,
            double kgGas)
        {
            _cavitatorDiameterM = cavitatorDiameterM;
            _bodyRadiusM = bodyRadiusM;
            _tailDistanceM = tailDistanceM;
            _kpAngle = kpAngle;
            _kdRate = kdRate;
            _kgGas = kgGas;
            _prevClearanceM = 0.05; // 초기 50mm 간극
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public CavitatorActuatorCommand ProcessControlCycle(
            in SupercavitationKinematicState state,
            double dtSeconds)
        {
            // 1. 심도에 따른 정수압 계산
            double ambientPressPa = AtmosphericPressPa + WaterDensity * Gravity * state.DepthMeters;
            double dynPress = 0.5 * WaterDensity * state.SpeedMetersPerSec * state.SpeedMetersPerSec;

            // 2. 캐비테이션 수 sigma 계산
            double sigma = Math.Max(0.01, (ambientPressPa - state.CavityInternalPressPa) / dynPress);

            // 3. 미부 위치에서의 캐비티 반경 Rc 계산 (로그 근사 모델)
            double cd0 = 0.82;
            double maxCavityRadius = (_cavitatorDiameterM / 2.0) * Math.Sqrt((cd0 * (1.0 + sigma)) / sigma);
            double cavityLength = _cavitatorDiameterM * (Math.Sqrt(cd0 * (1.0 + sigma)) / sigma) * Math.Log(1.0 / sigma);

            double xRatio = Math.Clamp(_tailDistanceM / cavityLength, 0.0, 1.0);
            double tailCavityRadius = maxCavityRadius * Math.Sqrt(Math.Max(0.0, 1.0 - Math.Pow(1.0 - 2.0 * xRatio, 2)));

            // 4. 미부-기포벽 여유 간극 계산
            double currentClearanceM = tailCavityRadius - (_bodyRadiusM + Math.Abs(state.TailVerticalOffsetM));
            double clearanceMm = currentClearanceM * 1000.0;

            // 5. 캐비테이터 피치 타각 계산 (각속도 댐핑 + 오프셋 보정)
            double targetPitchRad = -(_kpAngle * state.TailVerticalOffsetM + _kdRate * state.PitchRateRadPerSec);
            double clampedPitchCmdRad = Math.Clamp(targetPitchRad, -0.26, 0.26); // +-15도 한계

            // 6. 환형 가스 인젝션 유량 계산 (간극 20mm 미만 시 가스 분사 급증)
            const double safeClearanceM = 0.020; // 20mm
            double gasFlow = 0.2; // 기본 유지 유량 [kg/s]
            if (currentClearanceM < safeClearanceM)
            {
                double dClearance = (currentClearanceM - _prevClearanceM) / dtSeconds;
                double deficit = safeClearanceM - currentClearanceM;
                gasFlow += _kgGas * deficit - 0.5 * dClearance;
            }
            gasFlow = Math.Clamp(gasFlow, 0.0, 5.0); // 최대 5kg/s 분사
            _prevClearanceM = currentClearanceM;

            bool isWarning = clearanceMm < 10.0; // 10mm 미만 충돌 임계

            return new CavitatorActuatorCommand(
                clampedPitchCmdRad,
                gasFlow,
                clearanceMm,
                isWarning);
        }
    }
}