// File: Measurement_Core.c
#include "App_Common.h"

void AD5941_InitAll(void) {
    AD5940_HWReset();
    AD5940_MCUResourceInit(0);
    AD5940_Initialize();
    // CRITICAL: Re-enable Platform Interrupts/GPIOs after HW Reset
    AD5940PlatformCfg();
}

void AD5940ImpedanceStructInit(void) {
    AppIMPCfg_Type *pImpedanceCfg;
    AppIMPGetCfg(&pImpedanceCfg);
    
    pImpedanceCfg->IMPInited = bFALSE;
    pImpedanceCfg->SeqStartAddr = 0;
    pImpedanceCfg->MaxSeqLen = 512; 
    pImpedanceCfg->RcalVal = 100.0; 
    pImpedanceCfg->RtiaVal = CalibratedHstiaVal; 
    pImpedanceCfg->SinFreq = 1000.0;
    pImpedanceCfg->FifoThresh = 6; 
    pImpedanceCfg->DacVoltPP = 600.0;
    pImpedanceCfg->ExcitBufGain = EXCITBUFGAIN_0P25; 
    pImpedanceCfg->HsDacGain = HSDACGAIN_0P2;       
    pImpedanceCfg->HsDacUpdateRate = 7; 
    
    // --- Switch Matrix Configuration ---
    // D-Switch: Connect DAC to CE0 (Force +)
    pImpedanceCfg->DswitchSel = SWD_CE0;
    
    // P-Switch: Connect CE0 to P-Bus (Optional, but standard)
    pImpedanceCfg->PswitchSel = SWP_CE0; 
    
    // N-Switch: Connect SE0 to N-Bus (Force - / Current Input)
    // This is the CRITICAL path for HSTIA current measurement.
    pImpedanceCfg->NswitchSel = SWN_SE0; 
    
    // T-Switch: Only connect the Feedback Resistor (TRTIA).
    // DO NOT connect SWT_SE0 (T11), as it may short SE0 to RE0/LPTIA nodes.
    pImpedanceCfg->TswitchSel = 0; // Impedance.c adds SWT_TRTIA automatically
    
    pImpedanceCfg->HstiaRtiaSel = GetHSTIARtia(ConfigHstiaVal); 
    pImpedanceCfg->BiasVolt = 0.0;
    pImpedanceCfg->SweepCfg.SweepEn = bFALSE;
    pImpedanceCfg->SweepCfg.SweepStart = 100.0f;
    pImpedanceCfg->SweepCfg.SweepStop = 100000.0f;
    pImpedanceCfg->SweepCfg.SweepPoints = 50;
    pImpedanceCfg->SweepCfg.SweepLog = bTRUE;
    pImpedanceCfg->PwrMod = AFEPWR_LP;
    pImpedanceCfg->ADCSinc3Osr = ADCSINC3OSR_4;
    pImpedanceCfg->DftNum = DFTNUM_16384;
    pImpedanceCfg->DftSrc = DFTSRC_SINC3;
    pImpedanceCfg->ShortRe0Se0 = GlobalShortRe0Se0; 
}

void AD5940AMPStructInit(void) {
    AppAMPCfg_Type *pAMPCfg;
    AppAMPGetCfg(&pAMPCfg);

    pAMPCfg->AMPInited = bFALSE;
    pAMPCfg->WuptClkFreq = LFOSCFreq;
    pAMPCfg->SeqStartAddr = 0;
    pAMPCfg->MaxSeqLen = 512;
    pAMPCfg->RcalVal = 100.0;
    pAMPCfg->NumOfData = -1;
    pAMPCfg->AmpODR = 1.0;
    pAMPCfg->FifoThresh = 4;
    pAMPCfg->SensorBias = 0;
    pAMPCfg->LptiaRtiaSel = GetLPTIARtia(ConfigLptiaVal); 
    pAMPCfg->LpTiaRl = ConfigRLoad; 
    pAMPCfg->LpTiaRf = CurrentLpTiaRf; 
    pAMPCfg->Vzero = 1100;
    pAMPCfg->ADCRefVolt = 1.82;
    pAMPCfg->RtiaCalValue.Magnitude = CalibratedLptiaVal; 
    pAMPCfg->ShortRe0Se0 = GlobalShortRe0Se0; 
}

void AD5940RampStructInit(void) {
    AppRAMPCfg_Type *pRampCfg;
    AppRAMPGetCfg(&pRampCfg);

    pRampCfg->RAMPInited = bFALSE;
    pRampCfg->SeqStartAddr = 0;
    pRampCfg->MaxSeqLen = 1024;
    pRampCfg->RcalVal = 100.0;
    pRampCfg->ADCRefVolt = 1820.0f;
    pRampCfg->FifoThresh = 4;
    pRampCfg->SysClkFreq = 16000000.0f;
    pRampCfg->LFOSCClkFreq = LFOSCFreq;
    
    pRampCfg->RampStartVolt = -500.0f;
    pRampCfg->RampPeakVolt = +500.0f;
    pRampCfg->VzeroStart = 1100.0f;
    pRampCfg->VzeroPeak = 1100.0f;
    pRampCfg->StepNumber = 100;
    pRampCfg->RampDuration = 10000;
    pRampCfg->SampleDelay = 1.0f;
    
    pRampCfg->LPTIARtiaSel = GetLPTIARtia(ConfigLptiaVal); 
    pRampCfg->LPTIARloadSel = ConfigRLoad; 
    pRampCfg->LpTiaRf = CurrentLpTiaRf; 
    pRampCfg->AdcPgaGain = ADCPGA_1P5;
    pRampCfg->RtiaValue.Magnitude = CalibratedLptiaVal; 
    pRampCfg->ShortRe0Se0 = GlobalShortRe0Se0; 
}

void Config_LPLOOP(float bias_mv) {
    uint32_t vzero_code = 32; 
    float vzero_volts = vzero_code * (2200.0f / 64.0f);
    float vbias_volts = vzero_volts + bias_mv;
    uint32_t vbias_code = (uint32_t)(vbias_volts / (2200.0f / 4095.0f));
    if(vbias_code > 4095) vbias_code = 4095;
    
    LPDACCfg_Type lpdac_cfg;
    lpdac_cfg.LpdacSel = LPDAC0;
    lpdac_cfg.LpDacVbiasMux = LPDACVBIAS_12BIT;
    lpdac_cfg.LpDacVzeroMux = LPDACVZERO_6BIT;
    lpdac_cfg.DacData6Bit = vzero_code;
    lpdac_cfg.DacData12Bit = vbias_code;
    lpdac_cfg.DataRst = bFALSE;
    lpdac_cfg.LpDacSW = LPDACSW_VBIAS2LPPA | LPDACSW_VBIAS2PIN | LPDACSW_VZERO2LPTIA | LPDACSW_VZERO2PIN;
    lpdac_cfg.LpDacRef = LPDACREF_2P5;
    lpdac_cfg.LpDacSrc = LPDACSRC_MMR;
    lpdac_cfg.PowerEn = bTRUE;
    AD5940_LPDACCfgS(&lpdac_cfg);
    
    LPLoopCfg_Type lp_loop;
    lp_loop.LpAmpCfg.LpAmpSel = LPAMP0;
    lp_loop.LpAmpCfg.LpAmpPwrMod = LPAMPPWR_NORM;
    lp_loop.LpAmpCfg.LpPaPwrEn = bTRUE;
    lp_loop.LpAmpCfg.LpTiaPwrEn = bTRUE;
    lp_loop.LpAmpCfg.LpTiaRf = CurrentLpTiaRf;
    lp_loop.LpAmpCfg.LpTiaRload = ConfigRLoad; 
    lp_loop.LpAmpCfg.LpTiaRtia = GetLPTIARtia(ConfigLptiaVal);
    
    // Base Switches
    lp_loop.LpAmpCfg.LpTiaSW = LPTIASW(5)|LPTIASW(2)|LPTIASW(4)|LPTIASW(12)|LPTIASW(13); 
    
    // Apply Short Option
    if(GlobalShortRe0Se0) {
        lp_loop.LpAmpCfg.LpTiaSW |= LPTIASW(11);
    }
    
    AD5940_LPLoopCfgS(&lp_loop);
}

void Calibrate_HSDAC(float freq) {
    HSDACCal_Type hsdac_cal;
    ADCPGACal_Type adcpga_cal;
    CLKCfg_Type clk_cfg;

    // Configure Clock based on frequency (matches Do_WaveGen logic)
    clk_cfg.ADCClkDiv = ADCCLKDIV_1;
    clk_cfg.ADCCLkSrc = ADCCLKSRC_HFOSC;
    clk_cfg.SysClkDiv = SYSCLKDIV_1;
    clk_cfg.SysClkSrc = SYSCLKSRC_HFOSC;
    clk_cfg.HFXTALEn = bFALSE;
    clk_cfg.LFOSCEn = bTRUE;

    if(freq > 80000) {
        clk_cfg.HfOSC32MHzMode = bTRUE;
        clk_cfg.HFOSCEn = bTRUE;
        AD5940_CLKCfg(&clk_cfg);
        AD5940_HPModeEn(bTRUE);
    } else {
        clk_cfg.HfOSC32MHzMode = bFALSE;
        clk_cfg.HFOSCEn = bTRUE;
        AD5940_CLKCfg(&clk_cfg);
        AD5940_HPModeEn(bFALSE);
    }

    AD5940_AFEPwrBW(AFEPWR_LP, AFEBW_250KHZ);

    adcpga_cal.AdcClkFreq = 16000000;
    adcpga_cal.ADCPga = ADCPGA_1P5;
    adcpga_cal.ADCSinc2Osr = ADCSINC2OSR_1333;
    adcpga_cal.ADCSinc3Osr = ADCSINC3OSR_4;
    adcpga_cal.PGACalType = PGACALTYPE_OFFSET;
    adcpga_cal.TimeOut10us = 1000;
    adcpga_cal.VRef1p11 = 1.11;
    adcpga_cal.VRef1p82 = 1.82;
    AD5940_ADCPGACal(&adcpga_cal);

    hsdac_cal.ExcitBufGain = EXCITBUFGAIN_2;
    hsdac_cal.HsDacGain = HSDACGAIN_1;
    hsdac_cal.AfePwrMode = AFEPWR_LP;
    hsdac_cal.ADCSinc2Osr = ADCSINC2OSR_1333;
    hsdac_cal.ADCSinc3Osr = ADCSINC3OSR_4;
    AD5940_HSDACCal(&hsdac_cal);

    hsdac_cal.ExcitBufGain = EXCITBUFGAIN_2;
    hsdac_cal.HsDacGain = HSDACGAIN_0P2;
    AD5940_HSDACCal(&hsdac_cal);

    hsdac_cal.ExcitBufGain = EXCITBUFGAIN_0P25;
    hsdac_cal.HsDacGain = HSDACGAIN_1;
    AD5940_HSDACCal(&hsdac_cal);

    hsdac_cal.ExcitBufGain = EXCITBUFGAIN_0P25;
    hsdac_cal.HsDacGain = HSDACGAIN_0P2;
    AD5940_HSDACCal(&hsdac_cal);
}

void Configure_Filters(float freq) {
    ADCFilterCfg_Type adc_filter;
    DFTCfg_Type dft_cfg;
    FIFOCfg_Type fifo_cfg;

    fifo_cfg.FIFOEn = bFALSE;
    fifo_cfg.FIFOMode = FIFOMODE_FIFO;
    fifo_cfg.FIFOSize = FIFOSIZE_4KB;
    fifo_cfg.FIFOSrc = FIFOSRC_DFT;
    fifo_cfg.FIFOThresh = 2;
    AD5940_FIFOCfg(&fifo_cfg);
    fifo_cfg.FIFOEn = bTRUE;
    AD5940_FIFOCfg(&fifo_cfg);

    AD5940_ADCMuxCfgS(ADCMUXP_HSTIA_P, ADCMUXN_HSTIA_N);
    AD5940_StructInit(&adc_filter, sizeof(adc_filter));
    AD5940_StructInit(&dft_cfg, sizeof(dft_cfg));

    if (freq < 0.51f) {
        adc_filter.ADCAvgNum = ADCAVGNUM_16;
        adc_filter.ADCSinc2Osr = ADCSINC2OSR_267;
        adc_filter.ADCSinc3Osr = ADCSINC3OSR_5;
        adc_filter.BpNotch = bTRUE;
        adc_filter.BpSinc3 = bFALSE;
        adc_filter.Sinc2NotchEnable = bTRUE;
        adc_filter.ADCRate = ADCRATE_800KHZ;
        dft_cfg.DftNum = DFTNUM_8192;
        dft_cfg.DftSrc = DFTSRC_SINC2NOTCH;
    } else if(freq < 5.0f) {
        adc_filter.ADCAvgNum = ADCAVGNUM_16;
        adc_filter.ADCSinc2Osr = ADCSINC2OSR_178;
        adc_filter.ADCSinc3Osr = ADCSINC3OSR_4;
        adc_filter.BpNotch = bTRUE;
        adc_filter.BpSinc3 = bFALSE;
        adc_filter.Sinc2NotchEnable = bTRUE;
        adc_filter.ADCRate = ADCRATE_800KHZ;
        dft_cfg.DftNum = DFTNUM_8192;
        dft_cfg.DftSrc = DFTSRC_SINC2NOTCH;
    } else if(freq < 450.0f) {
        adc_filter.ADCAvgNum = ADCAVGNUM_16;
        adc_filter.ADCSinc2Osr = ADCSINC2OSR_44;
        adc_filter.ADCSinc3Osr = ADCSINC3OSR_4;
        adc_filter.BpNotch = bTRUE;
        adc_filter.BpSinc3 = bFALSE;
        adc_filter.Sinc2NotchEnable = bTRUE;
        adc_filter.ADCRate = ADCRATE_800KHZ;
        dft_cfg.DftNum = DFTNUM_4096;
        dft_cfg.DftSrc = DFTSRC_SINC2NOTCH;
    } else if(freq < 80000.0f) {
        adc_filter.ADCAvgNum = ADCAVGNUM_16;
        adc_filter.ADCSinc2Osr = ADCSINC2OSR_178;
        adc_filter.ADCSinc3Osr = ADCSINC3OSR_4;
        adc_filter.BpNotch = bTRUE;
        adc_filter.BpSinc3 = bFALSE;
        adc_filter.Sinc2NotchEnable = bFALSE;
        adc_filter.ADCRate = ADCRATE_800KHZ;
        dft_cfg.DftNum = DFTNUM_16384;
        dft_cfg.DftSrc = DFTSRC_SINC3;
    } else {
        adc_filter.ADCAvgNum = ADCAVGNUM_16;
        adc_filter.ADCSinc2Osr = ADCSINC2OSR_178;
        adc_filter.ADCSinc3Osr = ADCSINC3OSR_2;
        adc_filter.BpNotch = bTRUE;
        adc_filter.BpSinc3 = bFALSE;
        adc_filter.Sinc2NotchEnable = bFALSE;
        adc_filter.ADCRate = ADCRATE_1P6MHZ;
        dft_cfg.DftNum = DFTNUM_16384;
        dft_cfg.DftSrc = DFTSRC_SINC3;
    }
    dft_cfg.HanWinEn = bTRUE;

    AD5940_ADCFilterCfgS(&adc_filter);
    AD5940_DFTCfgS(&dft_cfg);
}

void Do_WaveGen(float freq) {
    CLKCfg_Type clk_cfg;
    AFERefCfg_Type aferef_cfg;
    HSLoopCfg_Type HpLoopCfg;
    ADCBaseCfg_Type adc_base;

    clk_cfg.ADCClkDiv = ADCCLKDIV_1;
    clk_cfg.ADCCLkSrc = ADCCLKSRC_HFOSC;
    clk_cfg.SysClkDiv = SYSCLKDIV_1;
    clk_cfg.SysClkSrc = SYSCLKSRC_HFOSC;
    clk_cfg.HFXTALEn = bFALSE;
    clk_cfg.LFOSCEn = bTRUE;

    if(freq > 80000) {
        clk_cfg.HfOSC32MHzMode = bTRUE;
        clk_cfg.HFOSCEn = bTRUE;
        AD5940_CLKCfg(&clk_cfg);
        AD5940_HPModeEn(bTRUE);
    } else {
        clk_cfg.HfOSC32MHzMode = bFALSE;
        clk_cfg.HFOSCEn = bTRUE;
        AD5940_CLKCfg(&clk_cfg);
        AD5940_HPModeEn(bFALSE);
    }

    aferef_cfg.HpBandgapEn = bTRUE;
    aferef_cfg.Hp1V1BuffEn = bTRUE;
    aferef_cfg.Hp1V8BuffEn = bTRUE;
    aferef_cfg.Disc1V1Cap = bFALSE;
    aferef_cfg.Disc1V8Cap = bFALSE;
    aferef_cfg.Hp1V8ThemBuff = bFALSE;
    aferef_cfg.Hp1V8Ilimit = bFALSE;
    aferef_cfg.Lp1V1BuffEn = bFALSE;
    aferef_cfg.Lp1V8BuffEn = bFALSE;
    aferef_cfg.LpBandgapEn = bTRUE;
    aferef_cfg.LpRefBufEn = bTRUE;
    aferef_cfg.LpRefBoostEn = bFALSE;
    AD5940_REFCfgS(&aferef_cfg);

    HpLoopCfg.HsDacCfg.ExcitBufGain = EXCITBUFGAIN_2;
    HpLoopCfg.HsDacCfg.HsDacGain = HSDACGAIN_1;
    HpLoopCfg.HsDacCfg.HsDacUpdateRate = (freq > 80000) ? 0x07 : 0x1B;

    HpLoopCfg.HsTiaCfg.DiodeClose = bFALSE;
    HpLoopCfg.HsTiaCfg.HstiaBias = HSTIABIAS_1P1;
    HpLoopCfg.HsTiaCfg.HstiaCtia = 16;
    HpLoopCfg.HsTiaCfg.HstiaDeRload = HSTIADERLOAD_OPEN;
    HpLoopCfg.HsTiaCfg.HstiaDeRtia = HSTIADERTIA_TODE;
    HpLoopCfg.HsTiaCfg.HstiaRtiaSel = GetHSTIARtia(ConfigHstiaVal);

    adc_base.ADCPga = ADCPGA_1P5;
    AD5940_ADCBaseCfgS(&adc_base);

    // --- Switch Matrix Configuration (Manual Sweep) ---
    HpLoopCfg.SWMatCfg.Dswitch = SWD_CE0;
    HpLoopCfg.SWMatCfg.Pswitch = SWP_CE0; 
    HpLoopCfg.SWMatCfg.Nswitch = SWN_SE0; 
    // FIX: Only use TRTIA (Feedback). Do NOT use SWT_SE0.
    HpLoopCfg.SWMatCfg.Tswitch = SWT_TRTIA; 

    AD5940_AFECtrlS(AFECTRL_WG, bFALSE);

    HpLoopCfg.WgCfg.WgType = WGTYPE_SIN;
    HpLoopCfg.WgCfg.GainCalEn = bFALSE;
    HpLoopCfg.WgCfg.OffsetCalEn = bFALSE;
    HpLoopCfg.WgCfg.SinCfg.SinFreqWord = AD5940_WGFreqWordCal(freq, (freq > 80000) ? 32000000.0 : 16000000.0);
    
    // Reduced Amplitude: 50mV peak (100mV pp) to prevent saturation
    // Range is +/- 607mV with Gain 2/1.
    // 50 / 607 * 2047 = ~168
    HpLoopCfg.WgCfg.SinCfg.SinAmplitudeWord = (uint32_t)(50.0f/607.0f*2047 + 0.5f); 
    HpLoopCfg.WgCfg.SinCfg.SinOffsetWord = 0;
    HpLoopCfg.WgCfg.SinCfg.SinPhaseWord = 0;
    AD5940_HSLoopCfgS(&HpLoopCfg);

    AD5940_AFECtrlS(AFECTRL_DACREFPWR, bTRUE);
    AD5940_AFECtrlS(AFECTRL_EXTBUFPWR | AFECTRL_INAMPPWR | AFECTRL_HSTIAPWR | AFECTRL_HSDACPWR, bTRUE);
    AD5940_AFECtrlS(AFECTRL_WG, bTRUE);
    AD5940_AFECtrlS(AFECTRL_DCBUFPWR, bTRUE);
    AD5940_AFEPwrBW(AFEPWR_LP, AFEBW_250KHZ);
}