광핀셋(Optical Tweezers) 광자 운동량 전달 & 1,000Hz 0-Alloc C# 단일 분자 피코뉴턴(pN) 포획 서보 제어기

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Stem.BioOptics
{
    /// <summary>
    /// 광핀셋 포스 클램프 텔레메트리 구조체 (16-byte 스택 전용)
    /// </summary>
    public readonly ref struct OpticalTrapState
    {
        public readonly float MeasuredForcePicoNewton; // 실측 피코뉴턴 힘 (pN)
        public readonly float DisplacementNanoMeter;   // 비드 중심 편차 Δx (nm)
        public readonly float ForceErrorPicoNewton;    // 목표 대비 힘 오차 (pN)
        public readonly float AomPwmDutyCycle;         // AOM 레이저 파워 제어 듀티 (0.0~1.0)

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public OpticalTrapState(float force, float disp, float err, float duty)
        {
            MeasuredForcePicoNewton = force;
            DisplacementNanoMeter = disp;
            ForceErrorPicoNewton = err;
            AomPwmDutyCycle = duty;
        }
    }

    /// <summary>
    /// 1,000Hz Zero-Allocation 광핀셋 포스 클램프 서보 엔진
    /// </summary>
    public ref struct OpticalTweezersServoEngine
    {
        private readonly float _trapStiffnessPnPerNm; // 트랩 강성 k_trap (e.g. 0.12 pN/nm)
        private readonly float _qpdSensitivityNmPerV; // QPD 차동 감도 (nm/V)
        private readonly float _targetForcePn;        // 목표 클램핑 인장력 (e.g. 15.0 pN)
        private float _currentAomDuty;                // 현재 AOM 구동 듀티
        private float _integralForceError;            // 적분 누적 오차

        private const float MinAomDuty = 0.05f;
        private const float MaxAomDuty = 0.95f;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public OpticalTweezersServoEngine(float kTrap, float qpdSens, float targetForce)
        {
            _trapStiffnessPnPerNm = kTrap;
            _qpdSensitivityNmPerV = qpdSens;
            _targetForcePn = targetForce;
            _currentAomDuty = 0.5f;
            _integralForceError = 0.0f;
        }

        /// <summary>
        /// 1ms 주기(1,000Hz) QPD 4분할 센서 차동 전압으로부터 힘 계산 및 AOM 피드백 제어 (0-Alloc)
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public OpticalTrapState Update1Khz(float qpdDiffVoltX, float dtSeconds)
        {
            // 1) QPD 전압으로부터 비드 중심 나노미터 변위 산출: Delta_x = V_diff * Sensitivity
            float displacementNm = qpdDiffVoltX * _qpdSensitivityNmPerV;

            // 2) 훅의 법칙에 따른 순시 트랩 작용력 F = k_trap * Delta_x 산출 (pN)
            float measuredForcePn = displacementNm * _trapStiffnessPnPerNm;

            // 3) 포스 클램프 오차 산출 (목표 힘과 실측 힘의 편차)
            float forceErrorPn = _targetForcePn - measuredForcePn;
            _integralForceError += forceErrorPn * dtSeconds;
            _integralForceError = Math.Clamp(_integralForceError, -5.0f, 5.0f);

            // 4) PI 제어를 통한 AOM 감쇄 듀티 갱신 (레이저 파워 가변으로 힘 일정 유지)
            float deltaDuty = (forceErrorPn * 0.035f) + (_integralForceError * 0.008f);
            _currentAomDuty += deltaDuty;
            _currentAomDuty = Math.Clamp(_currentAomDuty, MinAomDuty, MaxAomDuty);

            return new OpticalTrapState(measuredForcePn, displacementNm, forceErrorPn, _currentAomDuty);
        }
    }
}