AIDT 온디바이스 STEM 양자시뮬레이션 광격자 양자가스 현미경(Quantum Gas Microscope) 단일 원자 형광 분해능 위치 서보 제어기

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Stem.QuantumGasMicroscopy
{
    /// <summary>
    /// 비점수차 4분할 포토다이오드(QPD) 광전압 입력 상태
    /// </summary>
    public readonly ref struct AstigmaticQpdVoltages
    {
        public readonly double VoltA;
        public readonly double VoltB;
        public readonly double VoltC;
        public readonly double VoltD;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public AstigmaticQpdVoltages(double a, double b, double c, double d)
        {
            VoltA = a;
            VoltB = b;
            VoltC = c;
            VoltD = d;
        }
    }

    /// <summary>
    /// 대물렌즈 피에조 Z-초점 제어 출력
    /// </summary>
    public readonly ref struct ObjectiveFocusCorrectionOutput
    {
        public readonly double FocusErrorNm;       // 초점 이탈 변위 [nm]
        public readonly double PiezoCommandVolt;    // 피에조 Z-구동 전압 [V]
        public readonly bool IsInSingleAtomFocus;   // 단일 원자 결상 허용(<15nm) 플래그

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ObjectiveFocusCorrectionOutput(double errNm, double volt, bool focused)
        {
            FocusErrorNm = errNm;
            PiezoCommandVolt = volt;
            IsInSingleAtomFocus = focused;
        }
    }

    /// <summary>
    /// 10,000Hz 양자가스 현미경 대물렌즈 서브-나노 초점 서보 엔진
    /// </summary>
    public ref struct QuantumGasObjectiveServoEngine
    {
        private readonly double _kp;
        private readonly double _ki;
        private readonly double _kd;
        private readonly double _fesSensitivityNmPerNorm; // 비점수차 정규화 신호당 nm 감도
        private readonly double _piezoGainVoltPerNm;      // 피에조 전압 변환율 [V/nm]
        private readonly double _maxPiezoVolt;

        private double _integral;
        private double _prevErrorNm;

        public QuantumGasObjectiveServoEngine(
            double kp,
            double ki,
            double kd,
            double fesSensitivityNmPerNorm,
            double piezoGainVoltPerNm,
            double maxPiezoVolt)
        {
            _kp = kp;
            _ki = ki;
            _kd = kd;
            _fesSensitivityNmPerNorm = fesSensitivityNmPerNorm;
            _piezoGainVoltPerNm = piezoGainVoltPerNm;
            _maxPiezoVolt = maxPiezoVolt;
            _integral = 0.0;
            _prevErrorNm = 0.0;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ObjectiveFocusCorrectionOutput Update(
            in AstigmaticQpdVoltages qpd,
            double dtSeconds)
        {
            double sum = qpd.VoltA + qpd.VoltB + qpd.VoltC + qpd.VoltD;
            if (sum < 1e-6)
            {
                // 광신호 부족 시 이전 상태 유지
                return new ObjectiveFocusCorrectionOutput(0.0, 0.0, false);
            }

            // 비점수차 포커스 에러 정규화 계산
            double diff = (qpd.VoltA + qpd.VoltD) - (qpd.VoltB + qpd.VoltC);
            double normFes = diff / sum;

            // 물리적 초점 이탈 변위 추정 [nm]
            double currentErrorNm = normFes * _fesSensitivityNmPerNorm;

            // PID 연산
            _integral += currentErrorNm * dtSeconds;
            double derivative = (currentErrorNm - _prevErrorNm) / dtSeconds;
            _prevErrorNm = currentErrorNm;

            double controlNm = _kp * currentErrorNm + _ki * _integral + _kd * derivative;

            // 피에조 구동 전압 변환 및 리미터
            double cmdVolt = Math.Clamp(controlNm * _piezoGainVoltPerNm, -_maxPiezoVolt, _maxPiezoVolt);

            // 단일 원자 해상도(초점 이탈 15nm 미만) 판정
            bool isFocused = Math.Abs(currentErrorNm) <= 15.0;

            return new ObjectiveFocusCorrectionOutput(currentErrorNm, cmdVolt, isFocused);
        }
    }
}