// File: Measurement_Routines.c
#include "App_Common.h"
#include "Impedance.h"

// Forward declaration if not in header
// int32_t ImpedanceShowResult(uint32_t *pData, uint32_t DataCount);

void Routine_CalibrateLFO(void) {
  printf(">> Calibrating LFOSC...\n");
  if (CurrentMode == MODE_IMPEDANCE)
    AppIMPCleanup();
  else if (CurrentMode == MODE_AMPEROMETRIC)
    AppAMPCtrl(AMPCTRL_SHUTDOWN, 0);
  else if (CurrentMode == MODE_RAMP)
    AppRAMPCtrl(APPCTRL_SHUTDOWN, 0);

  LFOSCMeasure_Type cal_cfg;
  cal_cfg.CalDuration = 1000.0;
  cal_cfg.CalSeqAddr = 0;
  cal_cfg.SystemClkFreq = 16000000.0;

  if (AD5940_LFOSCMeasure(&cal_cfg, &LFOSCFreq) == AD5940ERR_OK) {
    printf(">> LFOSC Calibrated: %.2f Hz\n", LFOSCFreq);
  } else {
    printf(">> LFOSC Calibration Failed.\n");
  }
}

// Helper to show results (adapted from test/AD5940Main.c)
void ImpedanceShowResult(uint32_t *pData, uint32_t DataCount) {
  float freq;

  fImpPol_Type *pImp = (fImpPol_Type *)pData;
  AppIMPCtrl(IMPCTRL_GETFREQ, &freq);

  /*Process data*/
  for (int i = 0; i < DataCount; i++) {
    printf("DATA,%.2f,0,0,%.6f,%.6f\n", freq, pImp[i].Magnitude, pImp[i].Phase);
  }
}

void Routine_Measure(float freq, float bias_mv) {
  if (CurrentMode == MODE_AMPEROMETRIC)
    AppAMPCtrl(AMPCTRL_SHUTDOWN, 0);
  else if (CurrentMode == MODE_RAMP)
    AppRAMPCtrl(APPCTRL_SHUTDOWN, 0);
  CurrentMode = MODE_IMPEDANCE;

  // Cleanup any previous run
  AppIMPCleanup();

  // Initialize Structure
  AD5940ImpedanceStructInit();

  AppIMPCfg_Type *pCfg;
  AppIMPGetCfg(&pCfg);

  pCfg->WuptClkFreq = LFOSCFreq;
  pCfg->SweepCfg.SweepEn = bFALSE;
  pCfg->SinFreq = freq;
  pCfg->NumOfData = -1; // Changes based on new logic? Test uses -1 for single
                        // point? No, test creates sequence.

  // Apply Global Settings
  pCfg->ShortRe0Se0 = GlobalShortRe0Se0;
  pCfg->HstiaRtiaSel = GetHSTIARtia(ConfigHstiaVal);
  // pCfg->RtiaVal = (float)ConfigHstiaVal; // removed in new Impedance.c? Check
  // struct.
  pCfg->BiasVolt = bias_mv;

  pCfg->bParaChanged = bTRUE;

  // Initialize App
  if (AppIMPInit(AppBuff, APPBUFF_SIZE) != AD5940ERR_OK) {
    printf("ERROR: Init Failed\n");
    return;
  }

  // Calibrate RTIA (New implementation requires this)
  AppIMPRtiaCal();

  // Start Measurement
  AppIMPCtrl(IMPCTRL_START, 0);

  // Poll for result (Blocking, similar to sweep) - or rely on Interrupt?
  // The test code uses polling for sweep. For single measure, we can use the
  // ISR method currently in main.c, BUT we need to make sure AppIMPInit didn't
  // break anything. Actually, let's keep it consistent with the Sweep logic for
  // now to ensure it works.

  while (1) {
    if (AD5940_GetMCUIntFlag()) {
      AD5940_ClrMCUIntFlag();
      uint32_t temp = APPBUFF_SIZE;
      AppIMPISR(AppBuff, &temp);
      ImpedanceShowResult(AppBuff, temp);
      break;
    }
    // Timeout check?
  }
}

// Sweep Implementation matching test/AD5940Main.c
void Routine_Sweep(float start, float end, int steps, float bias_mv) {
  if (CurrentMode == MODE_AMPEROMETRIC)
    AppAMPCtrl(AMPCTRL_SHUTDOWN, 0);
  else if (CurrentMode == MODE_RAMP)
    AppRAMPCtrl(APPCTRL_SHUTDOWN, 0);
  CurrentMode = MODE_IMPEDANCE;

  AppIMPCfg_Type *pCfg;
  AppIMPGetCfg(&pCfg);

  // Initial Setup
  AD5940ImpedanceStructInit();
  pCfg->WuptClkFreq = LFOSCFreq;
  pCfg->ShortRe0Se0 = GlobalShortRe0Se0;
  pCfg->HstiaRtiaSel = GetHSTIARtia(ConfigHstiaVal);
  pCfg->BiasVolt = bias_mv;

  // Calculate Sweep Points
  float log_step = pow(end / start, 1.0 / (steps - 1));
  float curr_freq = start;

  printf("Starting Software Sweep: %.2f Hz to %.2f Hz, %d points\n", start, end,
         steps);

  for (int i = 0; i < steps; i++) {
    // Check for Abort from UART (User wants to stop)
    int c = getchar_timeout_us(0);
    if (c == 'x') {
      printf("Sweep Aborted by User.\n");
      AppIMPCleanup();
      break;
    }

    uint32_t temp;
    /* Update Frequency */
    pCfg->SinFreq = curr_freq;
    pCfg->bParaChanged = bTRUE; // Force sequence regeneration

    /* Re-Initialize App to apply new frequency (this regenerates sequences) */
    printf("DEBUG: Before Init for %.2f\n", curr_freq);
    if (AppIMPInit(AppBuff, APPBUFF_SIZE) != AD5940ERR_OK) {
      printf("ERROR: Init failed at %.2f\n", curr_freq);
      break;
    }
    printf("DEBUG: After Init\n");

    /* Calibrate RTIA at this frequency (Already done in AppIMPInit) */
    // AppIMPRtiaCal();

    /* Measure Impedance */
    printf("DEBUG: Starting Measurement\n");
    AppIMPCtrl(IMPCTRL_START, 0);

    /* Wait for result */
    int timeout = 100000; // 1s timeout approx
    while (timeout > 0) {
      if (AD5940_GetMCUIntFlag()) {
        // printf("DEBUG: Interrupt Triggered\n");
        AD5940_ClrMCUIntFlag();
        temp = APPBUFF_SIZE;
        AppIMPISR(AppBuff, &temp);

        if (temp > 0) {
          // printf("DEBUG: ISR processed %d items\n", temp);
          ImpedanceShowResult(AppBuff, temp);
          break; // Done with this point only if we got data
        } else {
          // Spurious interrupt or not enough data yet
          // printf("DEBUG: Spurious INT, 0 items. Continue waiting.\n");
        }
      }
      AD5940_Delay10us(1);
      timeout--;
    }

    if (timeout <= 0) {
      printf("timeout at freq: %.2f\n", curr_freq);
    }

    /* Next Frequency */
    curr_freq *= log_step;
  }
  AppIMPCleanup(); // Stop hardware
  printf("Sweep Completed.\n");
}

void Routine_Amperometric(float bias_mv) {
  if (CurrentMode == MODE_IMPEDANCE)
    AppIMPCleanup();
  else if (CurrentMode == MODE_RAMP)
    AppRAMPCtrl(APPCTRL_SHUTDOWN, 0);
  CurrentMode = MODE_AMPEROMETRIC;

  printf(">> Starting Amperometry (Bias: %.1f mV, LP Range: %d)...\n", bias_mv,
         ConfigLptiaVal);

  AppAMPCfg_Type *pCfg;
  AppAMPGetCfg(&pCfg);
  AD5940AMPStructInit(); // Reload config with current LP settings
  pCfg->SensorBias = bias_mv;
  pCfg->ReDoRtiaCal = bFALSE; // Use pre-calibrated value

  if (AppAMPInit(AppBuff, APPBUFF_SIZE) == AD5940ERR_OK) {
    AppAMPCtrl(AMPCTRL_START, 0);
  } else {
    printf("ERROR: AMP Init Failed\n");
  }
}

void Routine_LSV(float start_mv, float end_mv, int steps, int duration_ms) {
  if (CurrentMode == MODE_IMPEDANCE)
    AppIMPCleanup();
  else if (CurrentMode == MODE_AMPEROMETRIC)
    AppAMPCtrl(AMPCTRL_SHUTDOWN, 0);
  CurrentMode = MODE_RAMP;

  printf(
      ">> Starting LSV (%.1f to %.1f mV, %d steps, %d ms, LP Range: %d)...\n",
      start_mv, end_mv, steps, duration_ms, ConfigLptiaVal);

  AppRAMPCfg_Type *pCfg;
  AppRAMPGetCfg(&pCfg);
  AD5940RampStructInit(); // Reload config with current LP settings

  pCfg->RampStartVolt = start_mv;
  pCfg->RampPeakVolt = end_mv;
  pCfg->StepNumber = steps;
  pCfg->RampDuration = duration_ms;
  pCfg->bRampOneDir = bTRUE;
  pCfg->bParaChanged = bTRUE;

  if (AppRAMPInit(AppBuff, APPBUFF_SIZE) == AD5940ERR_OK) {
    AppRAMPCtrl(APPCTRL_START, 0);
  } else {
    printf("ERROR: RAMP Init Failed\n");
  }
}

extern void __AD5940_ReferenceON(void);

// Custom LPTIA Calibration using Internal HSTIA as Reference
// Routing: Excitation -> RE0/CE0, Short SE0
// This allows using HSTIA internal RTIA (e.g. 1k, 5k, etc) as the reference
// Rcal instead of the fixed external 100 Ohm Rcal which causes saturation.
AD5940Err AD5940_LPRtiaCal_UserCustom(LPRTIACal_Type *pCalCfg, void *pResult) {
  HSLoopCfg_Type hs_loop;
  LPLoopCfg_Type lp_loop;
  DSPCfg_Type dsp_cfg;
  ADCBaseCfg_Type *pADCBaseCfg;
  SWMatrixCfg_Type *pSWCfg;
  uint32_t INTCCfg;
  BoolFlag bADCClk32MHzMode = bFALSE;

  float ExcitVolt;
  uint32_t RtiaVal;
  uint32_t const LpRtiaTable[] = {
      0,     110,   1000,   2000,   3000,   4000,   6000,   8000,   10000,
      12000, 16000, 20000,  24000,  30000,  32000,  40000,  48000,  64000,
      85000, 96000, 100000, 120000, 128000, 160000, 196000, 256000, 512000};
  float const ADCPGAGainTable[] = {1, 1.5, 2, 4, 9};
  uint32_t WgAmpWord;
  uint32_t ADCPgaGainRtia, ADCPgaGainRcal;
  float GainRatio;
  iImpCar_Type DftRcal, DftRtia;

  if (pCalCfg == NULL)
    return AD5940ERR_NULLP;
  if (pResult == NULL)
    return AD5940ERR_NULLP;

  if (pCalCfg->AdcClkFreq > (32000000 * 0.8))
    bADCClk32MHzMode = bTRUE;

  // Initialize Pointers
  pSWCfg = &hs_loop.SWMatCfg;
  pADCBaseCfg = &dsp_cfg.ADCBaseCfg;

  RtiaVal = LpRtiaTable[pCalCfg->LpTiaRtia];

  // Calculate Excitation Voltage
  // Note: We are using HSTIA as Reference (Rcal).
  // Let's assume we use 1k Internal HSTIA RTIA as Rcal for this calculation to
  // aim for 800mVpp
  float RcalEffective = 1000.0f; // Approximate
  ExcitVolt = 2000 * 0.8 * RcalEffective / RtiaVal;
  WgAmpWord = ((uint32_t)(ExcitVolt / 2200 * 2047 * 2) + 1) >> 1;
  if (WgAmpWord > 0x7FF)
    WgAmpWord = 0x7FF;

  // Store original AFECON
  // uint32_t reg_afecon = AD5940_ReadReg(REG_AFE_AFECON); // Unused in this
  // snippet but good practice to restore if needed

  // INTC Config
  INTCCfg = AD5940_INTCGetCfg(AFEINTC_1);
  AD5940_INTCCfg(AFEINTC_1, AFEINTSRC_DFTRDY | AFEINTSRC_SINC2RDY, bTRUE);
  AD5940_INTCClrFlag(AFEINTSRC_ALLINT);

  AD5940_AFECtrlS(AFECTRL_ALL, bFALSE);
  /* Helper to turn on reference - adapted from AD5940_ReferenceON */
  AFERefCfg_Type aferef_cfg;
  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);

  // __AD5940_ReferenceON();

  // DSP Config
  AD5940_StructInit(&dsp_cfg, sizeof(dsp_cfg));
  dsp_cfg.ADCFilterCfg.ADCAvgNum = ADCAVGNUM_16;
  dsp_cfg.ADCFilterCfg.ADCRate =
      bADCClk32MHzMode ? ADCRATE_1P6MHZ : ADCRATE_800KHZ;
  dsp_cfg.ADCFilterCfg.ADCSinc2Osr = pCalCfg->ADCSinc2Osr;
  dsp_cfg.ADCFilterCfg.ADCSinc3Osr = pCalCfg->ADCSinc3Osr;
  dsp_cfg.ADCFilterCfg.BpNotch = bTRUE;
  dsp_cfg.ADCFilterCfg.BpSinc3 = bFALSE;
  dsp_cfg.ADCFilterCfg.Sinc2NotchEnable = bTRUE;
  memcpy(&dsp_cfg.DftCfg, &pCalCfg->DftCfg, sizeof(pCalCfg->DftCfg));
  AD5940_DSPCfgS(&dsp_cfg);

  // LP Loop Config
  AD5940_StructInit(&lp_loop, sizeof(lp_loop));
  lp_loop.LpDacCfg.LpdacSel = LPDAC0;
  lp_loop.LpDacCfg.DacData12Bit = 0x800;
  lp_loop.LpDacCfg.DacData6Bit = 32;
  lp_loop.LpDacCfg.DataRst = bFALSE;
  lp_loop.LpDacCfg.LpDacSW =
      LPDACSW_VBIAS2LPPA | LPDACSW_VZERO2HSTIA; // Vzero to HSTIA
  lp_loop.LpDacCfg.LpDacRef = LPDACREF_2P5;
  lp_loop.LpDacCfg.LpDacSrc = LPDACSRC_WG;
  lp_loop.LpDacCfg.LpDacVbiasMux = LPDACVBIAS_6BIT;
  lp_loop.LpDacCfg.LpDacVzeroMux = LPDACVZERO_12BIT;
  lp_loop.LpDacCfg.PowerEn = bTRUE;

  lp_loop.LpAmpCfg.LpAmpSel = pCalCfg->LpAmpSel;
  lp_loop.LpAmpCfg.LpAmpPwrMod = pCalCfg->LpAmpPwrMod;
  lp_loop.LpAmpCfg.LpPaPwrEn = bTRUE;
  lp_loop.LpAmpCfg.LpTiaPwrEn = bTRUE;
  lp_loop.LpAmpCfg.LpTiaRload = LPTIARLOAD_100R;
  lp_loop.LpAmpCfg.LpTiaRtia = pCalCfg->LpTiaRtia;
  lp_loop.LpAmpCfg.LpTiaRf = LPTIARF_OPEN;
  // Use User Routing for LPTIA Switches if needed, but standard should work for
  // TIA part
  lp_loop.LpAmpCfg.LpTiaSW =
      LPTIASW(6) | LPTIASW(8) | (pCalCfg->bWithCtia == bTRUE ? LPTIASW(5) : 0);
  AD5940_LPLoopCfgS(&lp_loop);

  // HS Loop Config
  AD5940_StructInit(&hs_loop, sizeof(hs_loop));
  hs_loop.HsTiaCfg.DiodeClose = bFALSE;
  hs_loop.HsTiaCfg.HstiaBias = HSTIABIAS_VZERO0; // Bias from LPDAC Vzero
  hs_loop.HsTiaCfg.HstiaCtia = 31;
  hs_loop.HsTiaCfg.HstiaDeRload = HSTIADERLOAD_OPEN;
  hs_loop.HsTiaCfg.HstiaDeRtia = HSTIADERTIA_OPEN;
  hs_loop.HsTiaCfg.HstiaDe1Rload = HSTIADERLOAD_OPEN;
  hs_loop.HsTiaCfg.HstiaDe1Rtia = HSTIADERTIA_OPEN;
  // Use 1k Internal RTIA as Reference (or match approximate LPTIA range if
  // possible, but 1k is a safe start)
  hs_loop.HsTiaCfg.HstiaRtiaSel = HSTIARTIA_1K;

  hs_loop.HsDacCfg.ExcitBufGain = 0;
  hs_loop.HsDacCfg.HsDacGain = 0;
  hs_loop.HsDacCfg.HsDacUpdateRate = 255;

  // USER ROUTING: RE0, CE0, SE0(Short)
  // Dswitch: Connect SE0
  // Pswitch: Connect RE0
  // Nswitch: Connect SE0LOAD? Or use SE0 as N?
  // User said "utilizing reo and setting it to re0 and ce0 then shorting se0"
  // -> SE0 is shorted to RE0? Let's try standard Ladder Routing logic:
  // Excitation via CE0/RE0 (P/N switches?) or High Power loop?
  // Actually, for LPTIA Cal, we use LPDAC/WG.
  // We need to route the current through LPTIA and the Reference.
  // Standard Cal uses RCAL0/RCAL1.
  // We want to use HSTIA.
  // Connect HSTIA to SE0?

  // Implementation based on interpretation:
  // SWP = RE0 (Excitation P side)
  // SWN = SE0 (Excitation N side?) No, SE0 is usually sense.
  // SWT = TRTIA (Connects HSTIA to T-Matrix)

  hs_loop.SWMatCfg.Dswitch = SWD_SE0;
  hs_loop.SWMatCfg.Pswitch = SWP_RE0 | SWP_CE0; // "setting it to re0 and ce0"
  hs_loop.SWMatCfg.Nswitch =
      SWN_SE0; // "shorting se0" - maybe use SE0 as return?
  hs_loop.SWMatCfg.Tswitch =
      SWT_TRTIA | SWT_SE0LOAD; // Connect HSTIA and SE0LOAD?

  AD5940_HSLoopCfgS(&hs_loop);

  // Waveform Generator
  hs_loop.WgCfg.WgType = WGTYPE_SIN;
  hs_loop.WgCfg.GainCalEn = bTRUE;
  hs_loop.WgCfg.OffsetCalEn = bTRUE;
  hs_loop.WgCfg.SinCfg.SinFreqWord =
      AD5940_WGFreqWordCal(pCalCfg->fFreq, pCalCfg->SysClkFreq);
  hs_loop.WgCfg.SinCfg.SinAmplitudeWord = WgAmpWord;
  hs_loop.WgCfg.SinCfg.SinOffsetWord = 0;
  hs_loop.WgCfg.SinCfg.SinPhaseWord = 0;
  AD5940_HSLoopCfgS(&hs_loop);

  AD5940_AFECtrlS(AFECTRL_HSTIAPWR | AFECTRL_INAMPPWR | AFECTRL_EXTBUFPWR |
                      AFECTRL_SINC2NOTCH,
                  bTRUE);

  // 1. Measure Reference (HSTIA Internal RTIA)
  // We need to measure voltage across HSTIA.
  // Mux P = HSTIA_P, Mux N = HSTIA_N
  AD5940_ADCMuxCfgS(ADCMUXP_HSTIA_P, ADCMUXN_HSTIA_N);
  AD5940_AFECtrlS(AFECTRL_WG | AFECTRL_ADCPWR, bTRUE);
  AD5940_Delay10us(25);
  AD5940_AFECtrlS(AFECTRL_ADCCNV | AFECTRL_DFT, bTRUE);
  while (AD5940_INTCTestFlag(AFEINTC_1, AFEINTSRC_DFTRDY) == bFALSE)
    ;
  AD5940_AFECtrlS(AFECTRL_ADCCNV | AFECTRL_DFT | AFECTRL_WG | AFECTRL_ADCPWR,
                  bFALSE);
  AD5940_INTCClrFlag(AFEINTSRC_DFTRDY);

  DftRcal.Real = AD5940_ReadAfeResult(AFERESULT_DFTREAL);
  DftRcal.Image = AD5940_ReadAfeResult(AFERESULT_DFTIMAGE);

  // 2. Measure LPTIA
  // Mux P = LPTIA_P, Mux N = LPTIA_N
  AD5940_ADCMuxCfgS(ADCMUXP_LPTIA0_P, ADCMUXN_LPTIA0_N);
  AD5940_AFECtrlS(AFECTRL_WG | AFECTRL_ADCPWR, bTRUE);
  AD5940_Delay10us(25);
  AD5940_AFECtrlS(AFECTRL_ADCCNV | AFECTRL_DFT, bTRUE);
  while (AD5940_INTCTestFlag(AFEINTC_1, AFEINTSRC_DFTRDY) == bFALSE)
    ;
  AD5940_AFECtrlS(AFECTRL_ADCCNV | AFECTRL_DFT | AFECTRL_WG | AFECTRL_ADCPWR,
                  bFALSE);
  AD5940_INTCClrFlag(AFEINTSRC_DFTRDY);

  DftRtia.Real = AD5940_ReadAfeResult(AFERESULT_DFTREAL);
  DftRtia.Image = AD5940_ReadAfeResult(AFERESULT_DFTIMAGE);

  // Calculation: Rtia = (V_Rtia / V_Rcal) * Rcal_Value
  // Here Rcal_Value is 1000 Ohm (HSTIA 1k)
  fImpCar_Type temp;
  temp = AD5940_ComplexDivInt(&DftRtia, &DftRcal);
  temp.Real *= 1000.0f;
  temp.Image *= 1000.0f;

  if (pCalCfg->bPolarResult == bFALSE) {
    *(fImpCar_Type *)pResult = temp;
  } else {
    ((fImpPol_Type *)pResult)->Magnitude = AD5940_ComplexMag(&temp);
    ((fImpPol_Type *)pResult)->Phase = AD5940_ComplexPhase(&temp);
  }

  // Restore INTC
  if (INTCCfg & AFEINTSRC_DFTRDY)
    ;
  else
    AD5940_INTCCfg(AFEINTC_1, AFEINTSRC_DFTRDY, bFALSE);

  return AD5940ERR_OK;
}

void Routine_CalibrateSystem(void) {
  if (CurrentMode == MODE_AMPEROMETRIC)
    AppAMPCtrl(AMPCTRL_SHUTDOWN, 0);
  else if (CurrentMode == MODE_RAMP)
    AppRAMPCtrl(APPCTRL_SHUTDOWN, 0);
  CurrentMode = MODE_IMPEDANCE;

  AppIMPCfg_Type *pCfg;
  AppIMPGetCfg(&pCfg);
  AppIMPCleanup();

  ADCPGACal_Type adcpga_cal;
  adcpga_cal.AdcClkFreq = 16000000.0;
  adcpga_cal.SysClkFreq = 16000000.0;
  adcpga_cal.ADCSinc3Osr = ADCSINC3OSR_4;
  adcpga_cal.ADCSinc2Osr = ADCSINC2OSR_22;
  adcpga_cal.ADCPga = ADCPGA_1P5;
  adcpga_cal.PGACalType = PGACALTYPE_OFFSET;
  adcpga_cal.TimeOut10us = 1000;
  adcpga_cal.VRef1p11 = 1.11;
  adcpga_cal.VRef1p82 = 1.82;
  printf(">> Calibrating ADC Offset...\n");
  AD5940_ADCPGACal(&adcpga_cal);

  // --- 1. Calibrate LPTIA (Low Power Loop) ---
  LPRTIACal_Type lprtia_cal;
  fImpPol_Type LpRes;
  memset(&lprtia_cal, 0, sizeof(lprtia_cal));
  lprtia_cal.AdcClkFreq = 16000000.0;
  lprtia_cal.SysClkFreq = 16000000.0;
  lprtia_cal.ADCSinc3Osr = ADCSINC3OSR_4;
  lprtia_cal.ADCSinc2Osr = ADCSINC2OSR_22;
  lprtia_cal.bPolarResult = bTRUE;
  lprtia_cal.fRcal = pCfg->RcalVal;
  lprtia_cal.LpTiaRtia = GetLPTIARtia(ConfigLptiaVal);
  lprtia_cal.LpAmpPwrMod = LPAMPPWR_NORM;
  lprtia_cal.bWithCtia = bFALSE;
  lprtia_cal.fFreq = 100.0f;
  lprtia_cal.DftCfg.DftNum = DFTNUM_2048;
  lprtia_cal.DftCfg.DftSrc = DFTSRC_SINC3;
  lprtia_cal.DftCfg.HanWinEn = bTRUE;

  printf(">> Calibrating LPTIA %d Ohm using Custom Routing (HSTIA Ref)...\n",
         ConfigLptiaVal);

  // CALL CUSTOM CALIBRATION FUNCTION
  if (AD5940_LPRtiaCal_UserCustom(&lprtia_cal, &LpRes) == AD5940ERR_OK) {
    printf("RCAL,LPTIA,%f,%f\n", LpRes.Magnitude, LpRes.Phase);
    CalibratedLptiaVal = LpRes.Magnitude;
  } else {
    printf("RCAL,LPTIA,FAIL\n");
  }

  // --- 2. Calibrate HSTIA (High Speed Loop) ---
  HSDACCfg_Type hsdac_cfg;
  hsdac_cfg.ExcitBufGain = EXCITBUFGAIN_0P25;
  hsdac_cfg.HsDacGain = HSDACGAIN_0P2;
  hsdac_cfg.HsDacUpdateRate = 7;
  AD5940_HSDacCfgS(&hsdac_cfg);

  HSRTIACal_Type hsrtia_cal;
  fImpPol_Type HsRes;
  memset(&hsrtia_cal, 0, sizeof(hsrtia_cal));
  hsrtia_cal.fFreq = 1000.0f;
  hsrtia_cal.AdcClkFreq = 16000000.0;
  hsrtia_cal.SysClkFreq = 16000000.0; // Orig was 16M
  hsrtia_cal.ADCSinc3Osr = ADCSINC3OSR_4;
  hsrtia_cal.ADCSinc2Osr = ADCSINC2OSR_22;
  hsrtia_cal.bPolarResult = bTRUE;
  hsrtia_cal.fRcal = 100.0;
  hsrtia_cal.HsTiaCfg.DiodeClose = bFALSE;
  hsrtia_cal.HsTiaCfg.HstiaBias = HSTIABIAS_1P1;
  hsrtia_cal.HsTiaCfg.HstiaCtia = 31;
  hsrtia_cal.HsTiaCfg.HstiaRtiaSel = GetHSTIARtia(ConfigHstiaVal);
  hsrtia_cal.HsTiaCfg.HstiaDeRtia = HSTIADERTIA_OPEN;
  hsrtia_cal.HsTiaCfg.HstiaDeRload = HSTIADERLOAD_OPEN;
  hsrtia_cal.DftCfg.DftNum = DFTNUM_16384;
  hsrtia_cal.DftCfg.DftSrc = DFTSRC_SINC3;

  printf(">> Calibrating HSTIA %d Ohm...\n", ConfigHstiaVal);
  if (AD5940_HSRtiaCal(&hsrtia_cal, &HsRes) == AD5940ERR_OK) {
    printf("RCAL,HSTIA,%f,%f\n", HsRes.Magnitude, HsRes.Phase);
    CalibratedHstiaVal = HsRes.Magnitude;
  } else {
    printf("RCAL,HSTIA,FAIL\n");
  }

  // Cleanup to prevent spurious interrupts in main loop
  AppIMPCleanup();
  CurrentMode = MODE_IDLE;
}