AIDT 온디바이스 STEM 방사광 X선 극저온 나노단층촬영(Cryo-Nano-CT) 시료 회전축 나노 흔들림(Runout) 보정 서보 제어기

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Stem.Synchrotron.CryoTomography
{
    /// <summary>
    /// 100K 극저온 시료 회전축 나노 런아웃 계측 및 피에조 보정 상태
    /// </summary>
    public readonly ref struct CryoRunoutSampleState
    {
        public readonly double RotationAngleRad;   // 시료 회전 각도 theta [rad]
        public readonly double XInterferometerNm;   // X축 레이저 간섭계 계측값 [nm]
        public readonly double YInterferometerNm;   // Y축 레이저 간섭계 계측값 [nm]
        public readonly double CryoTemperatureK;    // 시료 챔버 온도 [K]

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public CryoRunoutSampleState(double angle, double xNm, double yNm, double tempK)
        {
            RotationAngleRad = angle;
            XInterferometerNm = xNm;
            YInterferometerNm = yNm;
            CryoTemperatureK = tempK;
        }
    }

    /// <summary>
    /// 피에조 3축 서보 제어 출력
    /// </summary>
    public readonly ref struct CryoPiezoCorrectionOutput
    {
        public readonly double DeltaXPztVolt;      // X축 피에조 구동 전압 [V]
        public readonly double DeltaYPztVolt;      // Y축 피에조 구동 전압 [V]
        public readonly double ResidualErrorNm;    // 보정 후 잔류 축 편심 오차 [nm]
        public readonly bool IsStableForProjection; // 촬영 허용 잔류 오차(<5nm) 여부

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public CryoPiezoCorrectionOutput(double vx, double vy, double errNm, bool stable)
        {
            DeltaXPztVolt = vx;
            DeltaYPztVolt = vy;
            ResidualErrorNm = errNm;
            IsStableForProjection = stable;
        }
    }

    /// <summary>
    /// 10,000Hz 극저온 시료축 나노 런아웃 능동 보정 서보 엔진
    /// </summary>
    public ref struct SynchrotronCryoRunoutServoEngine
    {
        private readonly double _kp;
        private readonly double _ki;
        private readonly double _kd;
        private readonly double _piezoGainVoltPerNm;
        private readonly double _maxVoltLimit;

        // PID 적분 및 이전 오차 버퍼
        private double _integralX;
        private double _integralY;
        private double _prevErrX;
        private double _prevErrY;

        // 1차~4차 고조파 편심 피드포워드 룩업 계수
        private readonly Span<double> _harmonicsCosX;
        private readonly Span<double> _harmonicsSinX;
        private readonly Span<double> _harmonicsCosY;
        private readonly Span<double> _harmonicsSinY;

        public SynchrotronCryoRunoutServoEngine(
            double kp,
            double ki,
            double kd,
            double piezoGainVoltPerNm,
            double maxVoltLimit,
            Span<double> cosX,
            Span<double> sinX,
            Span<double> cosY,
            Span<double> sinY)
        {
            _kp = kp;
            _ki = ki;
            _kd = kd;
            _piezoGainVoltPerNm = piezoGainVoltPerNm;
            _maxVoltLimit = maxVoltLimit;
            _integralX = 0.0;
            _integralY = 0.0;
            _prevErrX = 0.0;
            _prevErrY = 0.0;
            _harmonicsCosX = cosX;
            _harmonicsSinX = sinX;
            _harmonicsCosY = cosY;
            _harmonicsSinY = sinY;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public CryoPiezoCorrectionOutput Update(in CryoRunoutSampleState state, double dtSeconds)
        {
            double theta = state.RotationAngleRad;

            // 1. 고조파 편심 피드포워드 기준 모델 계산
            double ffX = 0.0;
            double ffY = 0.0;
            int harmonicsCount = _harmonicsCosX.Length;
            for (int k = 1; k <= harmonicsCount; k++)
            {
                double angleK = k * theta;
                double c = Math.Cos(angleK);
                double s = Math.Sin(angleK);
                ffX += _harmonicsCosX[k - 1] * c + _harmonicsSinX[k - 1] * s;
                ffY += _harmonicsCosY[k - 1] * c + _harmonicsSinY[k - 1] * s;
            }

            // 2. 오차 연산 (이상적 축 중심 0.0nm 기준)
            double errX = -(state.XInterferometerNm);
            double errY = -(state.YInterferometerNm);

            // 3. 적분 및 미분 연산
            _integralX += errX * dtSeconds;
            _integralY += errY * dtSeconds;
            double derivX = (errX - _prevErrX) / dtSeconds;
            double derivY = (errY - _prevErrY) / dtSeconds;
            _prevErrX = errX;
            _prevErrY = errY;

            // 4. 피드백 + 피드포워드 보정 변위 [nm]
            double compNmX = ffX + (_kp * errX + _ki * _integralX + _kd * derivX);
            double compNmY = ffY + (_kp * errY + _ki * _integralY + _kd * derivY);

            // 5. 전압 변환 및 포화 한계
            double voltX = Math.Clamp(compNmX * _piezoGainVoltPerNm, -_maxVoltLimit, _maxVoltLimit);
            double voltY = Math.Clamp(compNmY * _piezoGainVoltPerNm, -_maxVoltLimit, _maxVoltLimit);

            // 6. 보정 후 잔류 반경 오차 추정
            double residualX = state.XInterferometerNm + (voltX / _piezoGainVoltPerNm);
            double residualY = state.YInterferometerNm + (voltY / _piezoGainVoltPerNm);
            double residualDistNm = Math.Sqrt(residualX * residualX + residualY * residualY);

            bool isStable = residualDistNm <= 5.0; // 5nm 이내 단층촬영 투영 허용 조건

            return new CryoPiezoCorrectionOutput(voltX, voltY, residualDistNm, isStable);
        }
    }
}