AIDT 온디바이스 국방 MUM-T 해군 함정 탑재 초공동 고속 수중 기총(Underwater Gun) 탄도 안정화 및 사격통제 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.MumT.UnderwaterGun
{
    /// <summary>
    /// 함정 포탑 자이로 및 수중 표적 원격 계측 상태
    /// </summary>
    public readonly ref struct UnderwaterGunTelemetryState
    {
        public readonly double TargetDepthMeters;     // 수중 표적 심도 [m]
        public readonly double TargetSlantRangeMeters; // 표적 직선 거리 [m]
        public readonly double TargetSpeedKnots;       // 표적 이동 속력 [kt]
        public readonly double ShipRollAngleDeg;       // 선체 롤 요동각 [deg]
        public readonly double ShipPitchAngleDeg;      // 선체 피치 요동각 [deg]
        public readonly double TurretAzimuthDeg;       // 포탑 현재 방위각 [deg]
        public readonly double TurretElevationDeg;     // 포탑 현재 고각 [deg]

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public UnderwaterGunTelemetryState(
            double depth,
            double range,
            double speed,
            double roll,
            double pitch,
            double az,
            double el)
        {
            TargetDepthMeters = depth;
            TargetSlantRangeMeters = range;
            TargetSpeedKnots = speed;
            ShipRollAngleDeg = roll;
            ShipPitchAngleDeg = pitch;
            TurretAzimuthDeg = az;
            TurretElevationDeg = el;
        }
    }

    /// <summary>
    /// 포탑 구동 토크 및 사격 통제 출력
    /// </summary>
    public readonly ref struct UnderwaterGunControlOutput
    {
        public readonly double AzimuthTorqueNm;      // 방위각 구동 모터 토크 [N*m]
        public readonly double ElevationTorqueNm;    // 고각 구동 모터 토크 [N*m]
        public readonly double AimingErrorMrad;      // 조준 오차 [mrad]
        public readonly bool IsFirePermitted;        // 조준 오차 < 1.0mrad 사격 인가 플래그

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public UnderwaterGunControlOutput(double azTorque, double elTorque, double errMrad, bool fire)
        {
            AzimuthTorqueNm = azTorque;
            ElevationTorqueNm = elTorque;
            AimingErrorMrad = errMrad;
            IsFirePermitted = fire;
        }
    }

    /// <summary>
    /// 10,000Hz 함정 초공동 수중 기총 사격통제 서보 엔진
    /// </summary>
    public ref struct SupercavitatingGunServoEngine
    {
        private readonly double _kpAz;
        private readonly double _kdAz;
        private readonly double _kpEl;
        private readonly double _kdEl;
        private readonly double _maxTorqueNm;

        private double _prevErrAz;
        private double _prevErrEl;

        public SupercavitatingGunServoEngine(
            double kpAz,
            double kdAz,
            double kpEl,
            double kdEl,
            double maxTorqueNm)
        {
            _kpAz = kpAz;
            _kdAz = kdAz;
            _kpEl = kpEl;
            _kdEl = kdEl;
            _maxTorqueNm = maxTorqueNm;
            _prevErrAz = 0.0;
            _prevErrEl = 0.0;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public UnderwaterGunControlOutput ProcessCycle(
            in UnderwaterGunTelemetryState state,
            double targetBearingDeg,
            double dtSeconds)
        {
            // 1. 수중 탄도학 낙하각 및 수면 굴절 보정 (심도 10m당 약 1.2도 보정)
            double refractionCompDeg = state.TargetDepthMeters * 0.12;
            double desiredElevationDeg = -Math.Atan2(state.TargetDepthMeters, state.TargetSlantRangeMeters) * (180.0 / Math.PI) - refractionCompDeg;

            // 2. 선체 요동 보상 결합 목표각 산출
            double targetAz = targetBearingDeg - state.ShipRollAngleDeg * 0.5;
            double targetEl = desiredElevationDeg - state.ShipPitchAngleDeg;

            // 3. 서보 오차 연산 (deg)
            double errAz = targetAz - state.TurretAzimuthDeg;
            double errEl = targetEl - state.TurretElevationDeg;

            double dErrAz = (errAz - _prevErrAz) / dtSeconds;
            double dErrEl = (errEl - _prevErrEl) / dtSeconds;
            _prevErrAz = errAz;
            _prevErrEl = errEl;

            // 4. 모터 구동 토크 출력 (N*m)
            double torqueAz = Math.Clamp(_kpAz * errAz + _kdAz * dErrAz, -_maxTorqueNm, _maxTorqueNm);
            double torqueEl = Math.Clamp(_kpEl * errEl + _kdEl * dErrEl, -_maxTorqueNm, _maxTorqueNm);

            // 5. 총 조준 오차 mrad 환산
            double totalErrDeg = Math.Sqrt(errAz * errAz + errEl * errEl);
            double errMrad = totalErrDeg * (Math.PI / 180.0) * 1000.0;

            // 사격 인가 조건: 조준 오차 1.0mrad 이내
            bool firePermitted = errMrad <= 1.0;

            return new UnderwaterGunControlOutput(
                torqueAz,
                torqueEl,
                errMrad,
                firePermitted);
        }
    }
}