AIDT 온디바이스 STEM 방사광 X선 초극저온(10mK) 헬륨3-헬륨4 희석냉동기 나노시료대 미세진동 능동 억제 서보 제어기

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Stem.Synchrotron.DilutionCryo
{
    /// <summary>
    /// 10mK 희석냉동기 간섭계 광전압 및 PTC 밸브 상태
    /// </summary>
    public readonly ref struct DilutionVibrationTelemetryState
    {
        public readonly double FabryPerotVolt;        // 패브리-페로 간섭계 전압 [V]
        public readonly double PtcValvePhaseRad;      // 펄스 튜브 1.4Hz 회전 밸브 위상 [rad]
        public readonly double MixingChamberTempMilliK;// 믹싱 챔버 온도 [mK]

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public DilutionVibrationTelemetryState(double fpVolt, double valvePhase, double tempMk)
        {
            FabryPerotVolt = fpVolt;
            PtcValvePhaseRad = valvePhase;
            MixingChamberTempMilliK = tempMk;
        }
    }

    /// <summary>
    /// 초극저온 피에조 진동 상쇄 출력
    /// </summary>
    public readonly ref struct DilutionVibrationControlOutput
    {
        public readonly double MeasuredDisplacementNm;  // 보정 전 진동 변위 [nm]
        public readonly double PiezoCommandVolt;        // 압전 액추에이터 구동 전압 [V]
        public readonly double ResidualVibrationNm;     // 잔류 진동 진폭 [nm]
        public readonly bool IsSubNanometerStable;      // 잔류 진동 < 1.0nm 달성 여부

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public DilutionVibrationControlOutput(double measNm, double cmdVolt, double resNm, bool stable)
        {
            MeasuredDisplacementNm = measNm;
            PiezoCommandVolt = cmdVolt;
            ResidualVibrationNm = resNm;
            IsSubNanometerStable = stable;
        }
    }

    /// <summary>
    /// 10,000Hz 희석냉동기 능동 진동 억제 FxLMS 서보 엔진
    /// </summary>
    public ref struct DilutionActiveVibrationServoEngine
    {
        private readonly double _fpSensitivityNmPerVolt;
        private readonly double _fpMidVolt;
        private readonly double _piezoGainVoltPerNm;
        private readonly double _maxPiezoVolt;
        private readonly double _muLms;
        private readonly double _kp;
        private readonly double _kd;

        // 적응형 필터 가중치 (1차~8차 하모닉스 코사인/사인 성분 16개)
        private readonly Span<double> _lmsWeights;
        private double _prevErrorNm;

        public DilutionActiveVibrationServoEngine(
            double fpSensitivityNmPerVolt,
            double fpMidVolt,
            double piezoGainVoltPerNm,
            double maxPiezoVolt,
            double muLms,
            double kp,
            double kd,
            Span<double> lmsWeights)
        {
            _fpSensitivityNmPerVolt = fpSensitivityNmPerVolt;
            _fpMidVolt = fpMidVolt;
            _piezoGainVoltPerNm = piezoGainVoltPerNm;
            _maxPiezoVolt = maxPiezoVolt;
            _muLms = muLms;
            _kp = kp;
            _kd = kd;
            _lmsWeights = lmsWeights;
            _prevErrorNm = 0.0;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public DilutionVibrationControlOutput ProcessCycle(
            in DilutionVibrationTelemetryState state,
            double dtSeconds)
        {
            // 1. 간섭계 전압으로부터 순간 나노 변위 연산
            double measDispNm = (state.FabryPerotVolt - _fpMidVolt) * _fpSensitivityNmPerVolt;

            // 2. PTC 1.4Hz 기본파 및 하모닉스 기준 신호 벡터 생성
            double basePhase = state.PtcValvePhaseRad;
            double lmsFeedforwardNm = 0.0;
            int numHarmonics = _lmsWeights.Length / 2;

            for (int h = 1; h <= numHarmonics; h++)
            {
                double phaseH = h * basePhase;
                double refCos = Math.Cos(phaseH);
                double refSin = Math.Sin(phaseH);

                int idxCos = (h - 1) * 2;
                int idxSin = idxCos + 1;

                lmsFeedforwardNm += _lmsWeights[idxCos] * refCos + _lmsWeights[idxSin] * refSin;

                // FxLMS 가중치 갱신 (잔류 오차 역전파)
                _lmsWeights[idxCos] -= _muLms * measDispNm * refCos;
                _lmsWeights[idxSin] -= _muLms * measDispNm * refSin;
            }

            // 3. PD 피드백 결합
            double dErr = (measDispNm - _prevErrorNm) / dtSeconds;
            _prevErrorNm = measDispNm;
            double feedbackNm = _kp * measDispNm + _kd * dErr;

            // 4. 피에조 액추에이터 역위상 보정 전압 출력
            double compTotalNm = -(lmsFeedforwardNm + feedbackNm);
            double cmdVolt = Math.Clamp(compTotalNm * _piezoGainVoltPerNm, -_maxPiezoVolt, _maxPiezoVolt);

            // 5. 보정 후 잔류 진동 추정
            double residualNm = Math.Abs(measDispNm + (cmdVolt / _piezoGainVoltPerNm));
            bool isStable = residualNm <= 1.0; // 1nm 이하 안정화 조건

            return new DilutionVibrationControlOutput(measDispNm, cmdVolt, residualNm, isStable);
        }
    }
}