// 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 = 4;
  pImpedanceCfg->DacVoltPP = 800.0;
  pImpedanceCfg->ExcitBufGain = EXCITBUFGAIN_2;
  pImpedanceCfg->HsDacGain = HSDACGAIN_1;
  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)
  pImpedanceCfg->NswitchSel = SWN_SE0;

  // T-Switch: Connect SE0LOAD to T-Node (Effectively SE0 with RLOAD=0)
  pImpedanceCfg->TswitchSel = SWT_SE0LOAD;

  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->LptiaRtiaSel = GetLPTIARtia(ConfigLptiaVal);
  pAMPCfg->LpTiaRl = LPTIARLOAD_SHORT;
  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->LPTIARtiaSel = GetLPTIARtia(ConfigLptiaVal);
  pRampCfg->LPTIARloadSel = LPTIARLOAD_SHORT;
  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.LpTiaRf = CurrentLpTiaRf;
  lp_loop.LpAmpCfg.LpTiaRload = LPTIARLOAD_SHORT;
  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_0R;
  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;
  // T-Switch: Connect SE0LOAD to T-Node
  HpLoopCfg.SWMatCfg.Tswitch = SWT_SE0LOAD;

  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);
}