AIDT 온디바이스 국방 MUM-T 기지방호 근접방공(C-RAM) 분당 4,500발 초고발사율 개틀링 건 탄도 예측 사격통제 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.MumT.CramGatling
{
    /// <summary>
    /// C-RAM 추적 레이더 수신 표적 상태
    /// </summary>
    public readonly ref struct RamTargetRadarState
    {
        public readonly double TargetPosXM;           // 표적 X 좌표 [m]
        public readonly double TargetPosYM;           // 표적 Y 좌표 [m]
        public readonly double TargetPosZM;           // 표적 Z 고도 [m]
        public readonly double TargetVelXMps;         // 표적 X 속도 [m/s]
        public readonly double TargetVelYMps;         // 표적 Y 속도 [m/s]
        public readonly double TargetVelZMps;         // 표적 Z 속도 [m/s]
        public readonly double CurrentTurretAzDeg;    // 현재 포탑 방위각 [deg]
        public readonly double CurrentTurretElDeg;    // 현재 포탑 고각 [deg]

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public RamTargetRadarState(
            double x, double y, double z,
            double vx, double vy, double vz,
            double az, double el)
        {
            TargetPosXM = x;
            TargetPosYM = y;
            TargetPosZM = z;
            TargetVelXMps = vx;
            TargetVelYMps = vy;
            TargetVelZMps = vz;
            CurrentTurretAzDeg = az;
            CurrentTurretElDeg = el;
        }
    }

    /// <summary>
    /// C-RAM 포탑 모터 토크 및 격발 제어 출력
    /// </summary>
    public readonly ref struct CramFireControlOutput
    {
        public readonly double AzimuthMotorTorqueNm;   // 방위각 구동 토크 [N*m]
        public readonly double ElevationMotorTorqueNm; // 고각 구동 토크 [N*m]
        public readonly double TimeToInterceptSec;     // 탄착 충돌 예상 시간 [s]
        public readonly double AimingErrorMrad;        // 조준 오차 [mrad]
        public readonly bool FireTriggerPermitted;     // 4,500rpm 격발 허용 플래그

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public CramFireControlOutput(double azTorque, double elTorque, double ttc, double errMrad, bool fire)
        {
            AzimuthMotorTorqueNm = azTorque;
            ElevationMotorTorqueNm = elTorque;
            TimeToInterceptSec = ttc;
            AimingErrorMrad = errMrad;
            FireTriggerPermitted = fire;
        }
    }

    /// <summary>
    /// 10,000Hz C-RAM 개틀링 건 탄도 예측 사격통제 서보 엔진
    /// </summary>
    public ref struct CramGatlingFireControlServoEngine
    {
        private const double MuzzleVelocityMps = 1050.0; // 포구속도 [m/s]
        private const double GravityMps2 = 9.80665;
        private readonly double _kp;
        private readonly double _kd;
        private readonly double _maxTorqueNm;

        private double _prevErrAz;
        private double _prevErrEl;

        public CramGatlingFireControlServoEngine(
            double kp,
            double kd,
            double maxTorqueNm)
        {
            _kp = kp;
            _kd = kd;
            _maxTorqueNm = maxTorqueNm;
            _prevErrAz = 0.0;
            _prevErrEl = 0.0;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public CramFireControlOutput ProcessCycle(
            in RamTargetRadarState state,
            double dtSeconds)
        {
            // 1. 초기 표적 거리 및 1차 비행 시간(Time of Flight) 추정
            double currentRange = Math.Sqrt(state.TargetPosXM * state.TargetPosXM +
                                            state.TargetPosYM * state.TargetPosYM +
                                            state.TargetPosZM * state.TargetPosZM);
            double tFlight = currentRange / MuzzleVelocityMps;

            // 2. 뉴턴-랩슨 2차 반복 미래 탄착점(PIP) 계산
            double pipX = state.TargetPosXM + state.TargetVelXMps * tFlight;
            double pipY = state.TargetPosYM + state.TargetVelYMps * tFlight;
            double pipZ = state.TargetPosZM + state.TargetVelZMps * tFlight - 0.5 * GravityMps2 * tFlight * tFlight;

            // 포환 낙하 보정 결합 고각 계산
            double groundPipDist = Math.Sqrt(pipX * pipX + pipY * pipY);
            double gravityDropM = 0.5 * GravityMps2 * tFlight * tFlight;
            double targetAzDeg = Math.Atan2(pipY, pipX) * (180.0 / Math.PI);
            double targetElDeg = Math.Atan2(pipZ + gravityDropM, groundPipDist) * (180.0 / Math.PI);

            // 3. 서보 조준 오차 연산 (deg)
            double errAz = targetAzDeg - state.CurrentTurretAzDeg;
            double errEl = targetElDeg - state.CurrentTurretElDeg;

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

            // 4. 고토크 모터 지령 (N*m)
            double torqueAz = Math.Clamp(_kp * errAz + _kd * dErrAz, -_maxTorqueNm, _maxTorqueNm);
            double torqueEl = Math.Clamp(_kp * errEl + _kd * dErrEl, -_maxTorqueNm, _maxTorqueNm);

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

            // 조준 오차 0.8mrad 이내 시 4,500rpm 격발 인가
            bool firePermit = errMrad <= 0.8 && currentRange <= 2500.0;

            return new CramFireControlOutput(
                torqueAz,
                torqueEl,
                tFlight,
                errMrad,
                firePermit);
        }
    }
}