AIDT 온디바이스 STEM 방사광 X선 극저온 투과전자현미경(Cryo-TEM) 시료 드리프트 피에조 간섭계 서보 제어기

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Stem.CryoTemStageDrift
{
    /// <summary>
    /// Cryo-TEM 간섭계 및 온도 텔레메트리 상태
    /// </summary>
    public readonly ref struct CryoTemTelemetryState
    {
        public readonly double InterferometerXDisplacementNm; // 레이저 간섭계 X 변위 [nm]
        public readonly double InterferometerYDisplacementNm; // 레이저 간섭계 Y 변위 [nm]
        public readonly double StageTempKelvin;               // 헬륨 냉각단 온도 [K]
        public readonly double ElectronBeamCurrentNanoAmps;   // 시료 조사 전자빔 전류 [nA]

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public CryoTemTelemetryState(double dx, double dy, double temp, double iBeam)
        {
            InterferometerXDisplacementNm = dx;
            InterferometerYDisplacementNm = dy;
            StageTempKelvin = temp;
            ElectronBeamCurrentNanoAmps = iBeam;
        }
    }

    /// <summary>
    /// 2축 PZT 피에조 서보 전압 지령
    /// </summary>
    public readonly ref struct StagePiezoCommand
    {
        public readonly double VoltageXPztVolts;              // X축 피에조 전압 [V]
        public readonly double VoltageYPztVolts;              // Y축 피에조 전압 [V]
        public readonly double ResidualDriftRateAngstromSec;  // 잔여 드리프트율 [Å/s]
        public readonly bool AtomicDriftStabilized;           // 원자단위 10pm 안정화 잠금

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public StagePiezoCommand(double vx, double vy, double driftRate, bool locked)
        {
            VoltageXPztVolts = vx;
            VoltageYPztVolts = vy;
            ResidualDriftRateAngstromSec = driftRate;
            AtomicDriftStabilized = locked;
        }
    }

    /// <summary>
    /// 10,000Hz 0-Alloc Cryo-TEM 시료 스테이지 드리프트 보정 엔진
    /// </summary>
    public sealed class CryoTemStageDriftServoEngine
    {
        private const double PztSensitivityNmPerVolt = 25.0; // 25 nm / V
        private double _prevX;
        private double _prevY;
        private double _currentVx;
        private double _currentVy;

        public CryoTemStageDriftServoEngine()
        {
            _prevX = 0.0;
            _prevY = 0.0;
            _currentVx = 75.0; // 75V 바이어스 중심
            _currentVy = 75.0;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public StagePiezoCommand UpdateServoLoop(in CryoTemTelemetryState telemetry, double dtSeconds)
        {
            // 1. 실시간 드리프트 속도 추정 (\hat{v}_{drift} [nm/s])
            double vDriftX = (telemetry.InterferometerXDisplacementNm - _prevX) / dtSeconds;
            double vDriftY = (telemetry.InterferometerYDisplacementNm - _prevY) / dtSeconds;
            _prevX = telemetry.InterferometerXDisplacementNm;
            _prevY = telemetry.InterferometerYDisplacementNm;

            // 2. 피에조 역보상 전압 지령 (PD 제어 + 피에조 감도 역산)
            double dVx = -(telemetry.InterferometerXDisplacementNm / PztSensitivityNmPerVolt) - (vDriftX * 0.02);
            double dVy = -(telemetry.InterferometerYDisplacementNm / PztSensitivityNmPerVolt) - (vDriftY * 0.02);

            _currentVx = Math.Clamp(_currentVx + (dVx * dtSeconds * 50.0), 0.0, 150.0);
            _currentVy = Math.Clamp(_currentVy + (dVy * dtSeconds * 50.0), 0.0, 150.0);

            // 3. 잔여 드리프트율 계산 ([Å/s])
            double resDriftRate = Math.Sqrt(vDriftX * vDriftX + vDriftY * vDriftY) * 10.0; // 1 nm = 10 Å

            bool isLocked = resDriftRate < 0.2; // 0.02 nm/s = 0.2 Å/s 미만 안정화
            return new StagePiezoCommand(_currentVx, _currentVy, resDriftRate, isLocked);
        }
    }
}