심해 6,000m AUV 열수 분출공 플룸 난류 역학 & 1,000Hz 0-Alloc C# 초음파 도플러(ADCP) 부력 평형 서보 엔진

using System;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;

namespace ZeliDesk.Stem.AuvDeepSeaControl
{
    /// <summary>
    /// 심해 환경 및 계측 데이터 (온도, 염분, 압력, 4빔 도플러 편이)
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Pack = 8)]
    public readonly struct AuvOceanSensorData
    {
        public readonly double TemperatureC;
        public readonly double SalinityPsu;
        public readonly double DepthMeters;
        public readonly double VerticalUpdraftSpeedM_S; // ADCP 수직 상승류 유속

        public AuvOceanSensorData(double temp, double sal, double depth, double wSpeed)
        {
            TemperatureC = temp;
            SalinityPsu = sal;
            DepthMeters = depth;
            VerticalUpdraftSpeedM_S = wSpeed;
        }
    }

    /// <summary>
    /// 가변 부력 엔진(VBE) 오일 피스톤 및 수직 추진기 제어 명령
    /// </summary>
    public ref struct AuvBuoyancyCommand
    {
        public double VariableBuoyancyVolumeDeltaCc; // cm^3 피스톤 변위
        public double VerticalThrusterForceN;       // 수직 추진기 추력 (N)
        public bool UpdraftWarning;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public AuvBuoyancyCommand(double vbeCc, double thrustN, bool alert)
        {
            VariableBuoyancyVolumeDeltaCc = vbeCc;
            VerticalThrusterForceN = thrustN;
            UpdraftWarning = alert;
        }
    }

    /// <summary>
    /// 1,000Hz 무할당 심해 AUV 부력 평형 서보 엔진
    /// </summary>
    public ref struct AuvPlumeBuoyancyEngine
    {
        private const double VehicleDisplacementM3 = 0.85; // AUV 기준 배수 체적 (m^3)
        private const double VehicleDryMassKg = 870.0;     // AUV 질량 (kg)
        private const double AlphaThermal = 2.1e-4;        // 열팽창 계수 (1/K)
        private const double BetaSaline = 7.4e-4;          // 염분 수축 계수 (1/PSU)
        private const double NominalDensityKgM3 = 1028.5;  // 6,000m 기준 해수 밀도

        private double _targetDepthMeters;
        private double _depthErrorIntegral;
        private double _filteredDensity;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void SetTargetDepth(double depthM)
        {
            _targetDepthMeters = depthM;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ExecuteControlCycle(
            in AuvOceanSensorData sensor,
            double verticalVelocityM_S,
            out AuvBuoyancyCommand command,
            out double netBuoyancyForceN)
        {
            // 1. 국소 해수 밀도 계산 (열수 플룸 진입에 따른 고온 저밀도 효과)
            double deltaT = sensor.TemperatureC - 2.0; // 심해 배경 수온 2도 기준
            double deltaS = sensor.SalinityPsu - 34.8;
            double currentDensity = NominalDensityKgM3 * (1.0 - AlphaThermal * deltaT + BetaSaline * deltaS);
            _filteredDensity = 0.9 * _filteredDensity + 0.1 * currentDensity;

            // 2. 순 부력 계산 (아르키메데스 원리)
            double buoyancyForce = _filteredDensity * VehicleDisplacementM3 * 9.80665;
            double gravityForce = VehicleDryMassKg * 9.80665;
            netBuoyancyForceN = buoyancyForce - gravityForce; // 양수: 상승 부력, 음수: 침강력

            // 3. 심도 제어 루프 (PI 제어)
            double depthError = _targetDepthMeters - sensor.DepthMeters;
            _depthErrorIntegral += depthError * 0.001;

            double kp = 150.0;
            double kd = 350.0;
            double ki = 25.0;

            // 목표 수직 제어력 (플룸 상승 기류 및 순 부력 보상)
            double plumeHydrodynamicLift = 0.5 * _filteredDensity * 0.45 * 0.6 * Math.Pow(sensor.VerticalUpdraftSpeedM_S, 2.0);
            double requiredControlForceN = (kp * depthError - kd * verticalVelocityM_S + ki * _depthErrorIntegral) 
                                           - netBuoyancyForceN - plumeHydrodynamicLift;

            // 4. 가변 부력 엔진(저속 고효율) 및 수직 추진기(고속 동적 응답) 출력 분배
            double vbeDeltaCc = Math.Clamp(-netBuoyancyForceN / (NominalDensityKgM3 * 9.80665) * 1.0e6, -500.0, 500.0);
            double thrusterN = Math.Clamp(requiredControlForceN, -800.0, 800.0);

            bool alert = sensor.VerticalUpdraftSpeedM_S > 1.2 || Math.Abs(deltaT) > 15.0;

            command = new AuvBuoyancyCommand(vbeDeltaCc, thrusterN, alert);
        }
    }
}