단일 광자 애벌랜치 광다이오드(SPAD) 어레이 양자 라이다(ToF) & 1,000Hz 0-Alloc C# TCSPC 포아송 통계 복원 및 피에조 MEMS 스캔 서보 엔진

using System;
using System.Runtime.CompilerServices;
using System.Numerics;

namespace ZeliDesk.Stem.QuantumOptics
{
    /// 

    /// 1,000Hz SPAD 어레이 양자 라이다 ToF 히스토그램 피크 검출 및 피에조 MEMS 스캔 서보
    /// GC 무할당 스택 메모리 0-Alloc 구현체
    /// 
    public ref struct SpadQuantumLidarServo
    {
        // 양자 광학 물리 상수
        private const float SpeedOfLightMps = 299792458.0f;
        private const float TdcBinWidthPs = 64.0f;           // TDC 빈 너비 64 ps
        private const float LaserRepetitionRateKhz = 250.0f;  // 레이저 반복률 250 kHz
        private const float MaxScanAngleDeg = 15.0f;          // MEMS 미러 최대 주사각

        // 서보 제어 상태
        public float TargetDistanceM;
        public float ConfidenceLevelPercent;
        public float CurrentMemsAngleXDeg;
        public float CurrentMemsAngleYDeg;
        public float PiezoDacVoltageVx;
        public float PiezoDacVoltageVy;
        public int TotalIntegratedPhotons;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Initialize()
        {
            TargetDistanceM = 0.0f;
            ConfidenceLevelPercent = 0.0f;
            CurrentMemsAngleXDeg = 0.0f;
            CurrentMemsAngleYDeg = 0.0f;
            PiezoDacVoltageVx = 0.0f;
            PiezoDacVoltageVy = 0.0f;
            TotalIntegratedPhotons = 0;
        }

        /// 

        /// 1ms 제어 주기: 256-Bin TCSPC 광자 계수 히스토그램으로부터 서브 빈 피크 역산 및 MEMS 조향 전압 출력
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void StepLidarScanning(
            ReadOnlySpan tcspcHistogramBins, 
            float targetAzimuthDeg, 
            float targetElevationDeg, 
            out float estimatedRangeM, 
            out float snrDb, 
            out float memsDriveVx, 
            out float memsDriveVy)
        {
            // 1. 히스토그램 최대 광자 피크 빈 및 배경 노이즈 추정 (Zero-Alloc 스택 탐색)
            int peakBin = 0;
            ushort maxCount = 0;
            int sumCounts = 0;
            int binCount = tcspcHistogramBins.Length;

            for (int i = 0; i < binCount; i++)
            {
                ushort val = tcspcHistogramBins[i];
                sumCounts += val;
                if (val > maxCount)
                {
                    maxCount = val;
                    peakBin = i;
                }
            }
            TotalIntegratedPhotons = sumCounts;

            // 평균 배경 잡음 레벨
            float avgNoise = (sumCounts - maxCount) / (float)Math.Max(1, binCount - 1);

            // 2. 가우시안 3점 보간 서브 빈 정밀 ToF 역산
            float subBinOffset = 0.0f;
            if (peakBin > 0 && peakBin < binCount - 1)
            {
                float y1 = tcspcHistogramBins[peakBin - 1];
                float y2 = tcspcHistogramBins[peakBin];
                float y3 = tcspcHistogramBins[peakBin + 1];

                float denom = 2.0f * (2.0f * y2 - y1 - y3) + 1e-5f;
                subBinOffset = (y1 - y3) / denom;
                subBinOffset = MathF.Max(-0.5f, MathF.Min(0.5f, subBinOffset));
            }

            float finePeakBin = peakBin + subBinOffset;
            float flightTimeSec = (finePeakBin * TdcBinWidthPs * 1e-12f);
            TargetDistanceM = (SpeedOfLightMps * flightTimeSec) * 0.5f;
            estimatedRangeM = TargetDistanceM;

            // 3. 신호 대 잡음비(SNR) 산출
            float signalNet = MathF.Max(1.0f, maxCount - avgNoise);
            float noiseStd = MathF.Sqrt(avgNoise + 1.0f);
            float linearSnr = signalNet / noiseStd;
            snrDb = 20.0f * MathF.Log10(MathF.Max(1.0f, linearSnr));
            ConfidenceLevelPercent = MathF.Min(100.0f, linearSnr * 10.0f);

            // 4. 1,000Hz 2축 피에조 MEMS 스캐너 서보 구동 (게인: 0.5 V/deg)
            const float MemsGainVPerDeg = 0.50f;
            CurrentMemsAngleXDeg = MathF.Max(-MaxScanAngleDeg, MathF.Min(MaxScanAngleDeg, targetAzimuthDeg));
            CurrentMemsAngleYDeg = MathF.Max(-MaxScanAngleDeg, MathF.Min(MaxScanAngleDeg, targetElevationDeg));

            PiezoDacVoltageVx = CurrentMemsAngleXDeg * MemsGainVPerDeg;
            PiezoDacVoltageVy = CurrentMemsAngleYDeg * MemsGainVPerDeg;

            memsDriveVx = PiezoDacVoltageVx;
            memsDriveVy = PiezoDacVoltageVy;
        }
    }
}