AIDT 온디바이스 STEM 테라헤르츠 시간영역 분광(THz-TDS) 비파괴 복합소재 검사 및 공진 모드 벡터 서보 엔진

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

namespace ZeliDesk.Stem.TerahertzTds
{
    public readonly ref struct ThzSpectralParameters
    {
        public readonly float FrequencyTHz;
        public readonly float RefractiveIndex;
        public readonly float ExtinctionCoeff;
        public readonly float AbsorptionCoeffCm1;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ThzSpectralParameters(float freqTHz, float n, float kappa, float alphaCm1)
        {
            FrequencyTHz = freqTHz;
            RefractiveIndex = n;
            ExtinctionCoeff = kappa;
            AbsorptionCoeffCm1 = alphaCm1;
        }
    }

    public ref struct TerahertzTdsWaveformVectorServo
    {
        private const float LightSpeedVacuumCmS = 2.99792458e10f; // cm/s
        private const float TwoPi = 6.28318530718f;

        private readonly float _sampleThicknessCm;
        private float _delayStagePositionUm;
        private float _lockInPhaseRad;
        private float _pidIntegral;
        private float _prevError;

        public TerahertzTdsWaveformVectorServo(float sampleThicknessCm)
        {
            _sampleThicknessCm = MathF.Max(1e-4f, sampleThicknessCm);
            _delayStagePositionUm = 0.0f;
            _lockInPhaseRad = 0.0f;
            _pidIntegral = 0.0f;
            _prevError = 0.0f;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public ThzSpectralParameters InvertComplexIndex(
            float freqHz, 
            float sampleAmp, 
            float refAmp, 
            float phaseDifferenceRad)
        {
            float omega = TwoPi * freqHz;
            float freqTHz = freqHz * 1e-12f;

            // 1. Refractive Index n(omega) = 1 + (c / (omega * d)) * phi
            float n = 1.0f + (LightSpeedVacuumCmS / (omega * _sampleThicknessCm)) * phaseDifferenceRad;
            if (n < 1.0f) n = 1.0f; // 물리적 최소 한계 하한선

            // 2. Amplitude Ratio rho = |E_sample| / |E_ref|
            float rho = MathF.Max(1e-9f, sampleAmp / MathF.Max(1e-9f, refAmp));

            // 3. Fresnel Transmission loss factor T12 * T21 = 4n / (n + 1)^2
            float fresnelTrans = (4.0f * n) / MathF.Pow(n + 1.0f, 2.0f);
            fresnelTrans = MathF.Max(1e-6f, fresnelTrans);

            // 4. Extinction Coefficient kappa(omega)
            float lnTerm = MathF.Log(MathF.Max(1e-9f, fresnelTrans / rho));
            float kappa = (LightSpeedVacuumCmS / (omega * _sampleThicknessCm)) * lnTerm;
            if (kappa < 0.0f) kappa = 0.0f;

            // 5. Absorption Coefficient alpha(omega) = 2 * omega * kappa / c [cm^-1]
            float alphaCm1 = (2.0f * omega * kappa) / LightSpeedVacuumCmS;

            return new ThzSpectralParameters(freqTHz, n, kappa, alphaCm1);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public float UpdateDelayStageServo(float targetPositionUm, float currentPositionUm, float dt, float kp, float ki, float kd)
        {
            float error = targetPositionUm - currentPositionUm;
            _pidIntegral += error * dt;
            float derivative = (error - _prevError) / MathF.Max(1e-6f, dt);
            _prevError = error;

            float driveVoltage = (kp * error) + (ki * _pidIntegral) + (kd * derivative);
            _delayStagePositionUm = currentPositionUm + (driveVoltage * dt * 100.0f);
            return driveVoltage;
        }
    }
}