// Impedance.c
#include "ad5940.h"
#include <stdio.h>
#include "string.h"
#include "math.h"
#include "Impedance.h"

#define AD5940ERR_STOP 10

/* Default LPDAC resolution (2.5V internal reference) */
#define DAC12BITVOLT_1LSB   (2200.0f/4095)  // mV
#define DAC6BITVOLT_1LSB    (DAC12BITVOLT_1LSB*64)  // mV

/* Forward declaration */
AD5940Err AppIMPCheckFreq(float freq);

AppIMPCfg_Type AppIMPCfg = 
{
  .bParaChanged = bFALSE,
  .SeqStartAddr = 0,
  .MaxSeqLen = 0,
  
  .SeqStartAddrCal = 0,
  .MaxSeqLenCal = 0,

  .ImpODR = 20.0,             /* Output Data Rate: 20.0 Hz */
  .NumOfData = 101,           /* Default to 101 points (matches default sweep) */
  .RealDataCount = -1,        
  .SysClkFreq = 16000000.0,
  .WuptClkFreq = 32000.0,     /* Low Frequency Oscillator (LFO) typically 32kHz */
  .AdcClkFreq = 16000000.0,
  .RcalVal = 100.0,         /* Calibration Resistor Value (Ohms) */
  .RtiaVal = 200.0,           /* TIA Gain Resistor Value (Ohms) */

  .DswitchSel = SWD_CE0,
  .PswitchSel = SWP_CE0,
  .NswitchSel = SWN_SE0,
  .TswitchSel = SWT_TRTIA,

  .PwrMod = AFEPWR_HP,

  .HstiaRtiaSel = HSTIARTIA_200,
  .ExtRtia = 0, 
  .ExcitBufGain = EXCITBUFGAIN_0P25,
  .HsDacGain = HSDACGAIN_0P2,
  .HsDacUpdateRate = 7,
  .DacVoltPP = 600.0,         /* Excitation Amplitude (mV peak-to-peak) */
  .BiasVolt = -0.0f,          /* DC Bias Voltage */

  .SinFreq = 1000.0,          /* Fixed Frequency (Hz) */

  .DftNum = DFTNUM_16384,     /* DFT Point Count */
  .DftSrc = DFTSRC_SINC3,
  .HanWinEn = bTRUE,          /* Hanning Window for spectral leakage reduction */

  .AdcPgaGain = ADCPGA_1P5,
  .ADCSinc3Osr = ADCSINC3OSR_2,
  .ADCSinc2Osr = ADCSINC2OSR_22,

  .ADCAvgNum = ADCAVGNUM_16,

  /* Default Sweep: 1kHz to 100kHz, 50 Points Per Decade (Log) */
  /* Decades = log10(100k) - log10(1k) = 2. Points = 2 * 50 + 1 = 101 */
  .SweepCfg.SweepEn = bTRUE,
  .SweepCfg.SweepStart = 1000.0,
  .SweepCfg.SweepStop = 100000.0,
  .SweepCfg.SweepPoints = 101,
  .SweepCfg.SweepLog = bTRUE,
  .SweepCfg.SweepIndex = 0,

  .FifoThresh = 6,            /* Threshold: 3 measurements * 2 (Real/Imag) = 6 words */
  .IMPInited = bFALSE,
  .StopRequired = bFALSE,
};

/* ----------------------------------------------------------------------- */
/* Helper Functions                                                        */
/* ----------------------------------------------------------------------- */

/**
* @brief Configures the Impedance Sweep parameters.
* @param start Start Frequency in Hz
* @param stop Stop Frequency in Hz
* @param ppd Points Per Decade (Resolution)
 */
void AppIMPConfigureSweep(float start, float stop, float ppd) {
    if (start <= 0 || stop <= 0 || ppd <= 0) return;

    AppIMPCfg.SweepCfg.SweepEn = bTRUE;
    AppIMPCfg.SweepCfg.SweepStart = start;
    AppIMPCfg.SweepCfg.SweepStop = stop;
    AppIMPCfg.SweepCfg.SweepLog = bTRUE;
    
    // Calculate total points based on decades and PPD
    // Formula: Points = (Decades * PPD) + 1
    float decades = log10f(stop) - log10f(start);
    if (decades < 0) decades = -decades; // Handle sweep down if needed
    
    uint32_t points = (uint32_t)(decades * ppd) + 1;
    AppIMPCfg.SweepCfg.SweepPoints = points;
    
    // Set NumOfData to stop the sequencer automatically after the sweep
    AppIMPCfg.NumOfData = points;
    
    // Reset Sweep State
    AppIMPCfg.FifoDataCount = 0;
    AppIMPCfg.SweepCfg.SweepIndex = 0;
    AppIMPCfg.SweepCurrFreq = start;
    AppIMPCfg.SweepNextFreq = start;
}

void AppIMPCleanup(void) {
    // Ensure chip is awake before sending commands
    if(AD5940_WakeUp(10) > 10) return;

    // Stop Sequencer and Wakeup Timer
    AD5940_WUPTCtrl(bFALSE);
    AD5940_SEQCtrlS(bFALSE);
    
    // Stop active conversions and Waveform Generator, keep Reference/LDOs on
    AD5940_AFECtrlS(AFECTRL_ADCCNV|AFECTRL_DFT|AFECTRL_WG, bFALSE);

    // Reset FIFO configuration
    FIFOCfg_Type fifo_cfg;
    fifo_cfg.FIFOEn = bFALSE;           
    AD5940_FIFOCfg(&fifo_cfg);          
    
    fifo_cfg.FIFOEn = bTRUE;            
    fifo_cfg.FIFOMode = FIFOMODE_FIFO;
    fifo_cfg.FIFOSize = FIFOSIZE_4KB;   
    fifo_cfg.FIFOSrc = FIFOSRC_DFT;     
    fifo_cfg.FIFOThresh = 6;            
    AD5940_FIFOCfg(&fifo_cfg);
    
    // Clear all interrupt flags
    AD5940_INTCClrFlag(AFEINTSRC_ALLINT);
}

void AppIMPCalibrateLFO(void) {
    LFOSCMeasure_Type cal_cfg;
    cal_cfg.CalDuration = 1000.0; // 1000ms for high accuracy
    cal_cfg.CalSeqAddr = 0;       // Use start of SRAM for calibration sequence
    cal_cfg.SystemClkFreq = 16000000.0;
    
    float freq;
    // Measure LFOSC frequency to calibrate Wakeup Timer
    if(AD5940_LFOSCMeasure(&cal_cfg, &freq) == AD5940ERR_OK) {
        AppIMPCfg.WuptClkFreq = freq;
    }
}

/* ----------------------------------------------------------------------- */

int32_t AppIMPGetCfg(void *pCfg)
{
  if(pCfg)
  {
    *(AppIMPCfg_Type**)pCfg = &AppIMPCfg;
    return AD5940ERR_OK;
  }
  return AD5940ERR_PARA;
}

int32_t AppIMPCtrl(uint32_t Command, void *pPara)
{
  switch (Command)
  {
    case IMPCTRL_START:
    {
      // Configure interrupts and trigger the first sequence manually
      AD5940_WUPTCtrl(bFALSE); 
      
      AD5940_INTCCfg(AFEINTC_1, AFEINTSRC_ENDSEQ, bTRUE);
      AD5940_INTCCfg(AFEINTC_1, AFEINTSRC_DATAFIFOTHRESH, bFALSE); 
      AD5940_INTCClrFlag(AFEINTSRC_ALLINT);
      
      AppIMPCfg.FifoDataCount = 0;
      AppIMPCfg.StopRequired = bFALSE;
      
      // Reset Sweep State for subsequent sweeps
      if(AppIMPCfg.SweepCfg.SweepEn)
      {
        AppIMPCfg.SweepCfg.SweepIndex = 0;
        AppIMPCfg.SweepCurrFreq = AppIMPCfg.SweepCfg.SweepStart;
        AppIMPCfg.FreqofData = AppIMPCfg.SweepCfg.SweepStart;
        
        // Calculate next frequency immediately so the ISR has the correct 'next' value
        AD5940_SweepNext(&AppIMPCfg.SweepCfg, &AppIMPCfg.SweepNextFreq);
        
        // Reset Hardware WG Frequency to Start Frequency (it was left at Stop Freq)
        AD5940_WGFreqCtrlS(AppIMPCfg.SweepCurrFreq, AppIMPCfg.SysClkFreq);
        
        // Reset SRAM Wait Times for the Start Frequency
        AppIMPCheckFreq(AppIMPCfg.SweepCurrFreq);
      }

      AD5940_SEQMmrTrig(SEQID_0);
      break;
    }
    case IMPCTRL_STOPNOW:
    {
      if(AD5940_WakeUp(10) > 10) return AD5940ERR_WAKEUP;
      AD5940_WUPTCtrl(bFALSE);
      AD5940_SEQCtrlS(bFALSE); 
      break;
    }
    case IMPCTRL_STOPSYNC:
    {
      AppIMPCfg.StopRequired = bTRUE;
      break;
    }
    case IMPCTRL_GETFREQ:
    {
      if(pPara == 0) return AD5940ERR_PARA;
      if(AppIMPCfg.SweepCfg.SweepEn == bTRUE)
        *(float*)pPara = AppIMPCfg.FreqofData;
      else
        *(float*)pPara = AppIMPCfg.SinFreq;
      break;
    }
    case IMPCTRL_SHUTDOWN:
    {
      AppIMPCtrl(IMPCTRL_STOPNOW, 0);
      AFERefCfg_Type aferef_cfg;
      LPLoopCfg_Type lp_loop;
      memset(&aferef_cfg, 0, sizeof(aferef_cfg));
      AD5940_REFCfgS(&aferef_cfg);
      memset(&lp_loop, 0, sizeof(lp_loop));
      AD5940_LPLoopCfgS(&lp_loop);
      AD5940_EnterSleepS();
      break;
    }
    default:
      break;
  }
  return AD5940ERR_OK;
}

float AppIMPGetCurrFreq(void)
{
  if(AppIMPCfg.SweepCfg.SweepEn == bTRUE)
    return AppIMPCfg.FreqofData;
  else
    return AppIMPCfg.SinFreq;
}

/* Generate Initialization Sequence */
static AD5940Err AppIMPSeqCfgGen(void)
{
  AD5940Err error = AD5940ERR_OK;
  const uint32_t *pSeqCmd;
  uint32_t SeqLen;
  AFERefCfg_Type aferef_cfg;
  HSLoopCfg_Type HsLoopCfg;
  DSPCfg_Type dsp_cfg;
  float sin_freq;

  AD5940_SEQGenCtrl(bTRUE);
  AD5940_AFECtrlS(AFECTRL_ALL, bFALSE);

  // Configure Reference System (High Power and Low Power Buffers)
  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);	
  
  // Configure High Speed Loop (DAC and TIA)
  HsLoopCfg.HsDacCfg.ExcitBufGain = AppIMPCfg.ExcitBufGain;
  HsLoopCfg.HsDacCfg.HsDacGain = AppIMPCfg.HsDacGain;
  HsLoopCfg.HsDacCfg.HsDacUpdateRate = AppIMPCfg.HsDacUpdateRate;

  HsLoopCfg.HsTiaCfg.DiodeClose = bFALSE;
  HsLoopCfg.HsTiaCfg.HstiaBias = HSTIABIAS_1P1;
  HsLoopCfg.HsTiaCfg.HstiaCtia = 31;
  HsLoopCfg.HsTiaCfg.HstiaDeRload = HSTIADERLOAD_OPEN;
  HsLoopCfg.HsTiaCfg.HstiaDeRtia = HSTIADERTIA_OPEN;
  HsLoopCfg.HsTiaCfg.HstiaRtiaSel = AppIMPCfg.HstiaRtiaSel;
  HsLoopCfg.HsTiaCfg.ExtRtia = AppIMPCfg.ExtRtia;

  // Configure Switch Matrix
  HsLoopCfg.SWMatCfg.Dswitch = AppIMPCfg.DswitchSel;
  HsLoopCfg.SWMatCfg.Pswitch = AppIMPCfg.PswitchSel;
  HsLoopCfg.SWMatCfg.Nswitch = AppIMPCfg.NswitchSel;
  HsLoopCfg.SWMatCfg.Tswitch = SWT_TRTIA|AppIMPCfg.TswitchSel;

  // Configure Waveform Generator (Sine Wave)
  HsLoopCfg.WgCfg.WgType = WGTYPE_SIN;
  HsLoopCfg.WgCfg.GainCalEn = bTRUE;
  HsLoopCfg.WgCfg.OffsetCalEn = bTRUE;
  
  if(AppIMPCfg.SweepCfg.SweepEn == bTRUE)
  {
    AppIMPCfg.FreqofData = AppIMPCfg.SweepCfg.SweepStart;
    AppIMPCfg.SweepCurrFreq = AppIMPCfg.SweepCfg.SweepStart;
    AD5940_SweepNext(&AppIMPCfg.SweepCfg, &AppIMPCfg.SweepNextFreq);
    sin_freq = AppIMPCfg.SweepCurrFreq;
  }
  else
  {
    sin_freq = AppIMPCfg.SinFreq;
    AppIMPCfg.FreqofData = sin_freq;
  }
  
  HsLoopCfg.WgCfg.SinCfg.SinFreqWord = AD5940_WGFreqWordCal(sin_freq, AppIMPCfg.SysClkFreq);
  HsLoopCfg.WgCfg.SinCfg.SinAmplitudeWord = (uint32_t)(AppIMPCfg.DacVoltPP/800.0f*2047 + 0.5f);
  HsLoopCfg.WgCfg.SinCfg.SinOffsetWord = 0;
  HsLoopCfg.WgCfg.SinCfg.SinPhaseWord = 0;
  AD5940_HSLoopCfgS(&HsLoopCfg);

  // Configure DSP and ADC
  dsp_cfg.ADCBaseCfg.ADCMuxN = ADCMUXN_HSTIA_N;
  dsp_cfg.ADCBaseCfg.ADCMuxP = ADCMUXP_HSTIA_P;
  dsp_cfg.ADCBaseCfg.ADCPga = AppIMPCfg.AdcPgaGain;
  
  memset(&dsp_cfg.ADCDigCompCfg, 0, sizeof(dsp_cfg.ADCDigCompCfg));
  
  dsp_cfg.ADCFilterCfg.ADCAvgNum = AppIMPCfg.ADCAvgNum;
  dsp_cfg.ADCFilterCfg.ADCRate = ADCRATE_800KHZ;
  dsp_cfg.ADCFilterCfg.ADCSinc2Osr = AppIMPCfg.ADCSinc2Osr;
  dsp_cfg.ADCFilterCfg.ADCSinc3Osr = AppIMPCfg.ADCSinc3Osr;
  dsp_cfg.ADCFilterCfg.BpNotch = bTRUE;
  dsp_cfg.ADCFilterCfg.BpSinc3 = bFALSE;
  dsp_cfg.ADCFilterCfg.Sinc2NotchEnable = bTRUE;
  dsp_cfg.DftCfg.DftNum = AppIMPCfg.DftNum;
  dsp_cfg.DftCfg.DftSrc = AppIMPCfg.DftSrc;
  dsp_cfg.DftCfg.HanWinEn = AppIMPCfg.HanWinEn;
  
  memset(&dsp_cfg.StatCfg, 0, sizeof(dsp_cfg.StatCfg));
  AD5940_DSPCfgS(&dsp_cfg);
    
  // Enable Power for AFE blocks
  AD5940_AFECtrlS(AFECTRL_HSTIAPWR|AFECTRL_INAMPPWR|AFECTRL_EXTBUFPWR|\
                AFECTRL_WG|AFECTRL_DACREFPWR|AFECTRL_HSDACPWR|\
                AFECTRL_SINC2NOTCH, bTRUE);

  AD5940_SEQGenInsert(SEQ_STOP()); 

  error = AD5940_SEQGenFetchSeq(&pSeqCmd, &SeqLen);
  AD5940_SEQGenCtrl(bFALSE); 
  if(error == AD5940ERR_OK)
  {
    AppIMPCfg.InitSeqInfo.SeqId = SEQID_1;
    AppIMPCfg.InitSeqInfo.SeqRamAddr = AppIMPCfg.SeqStartAddr;
    AppIMPCfg.InitSeqInfo.pSeqCmd = pSeqCmd;
    AppIMPCfg.InitSeqInfo.SeqLen = SeqLen;
    AD5940_SEQCmdWrite(AppIMPCfg.InitSeqInfo.SeqRamAddr, pSeqCmd, SeqLen);
  }
  else
    return error; 
  return AD5940ERR_OK;
}

/* Generate Measurement Sequence */
static AD5940Err AppIMPSeqMeasureGen(void)
{
  AD5940Err error = AD5940ERR_OK;
  const uint32_t *pSeqCmd;
  uint32_t SeqLen;
  uint32_t WaitClks;
  SWMatrixCfg_Type sw_cfg;
  ClksCalInfo_Type clks_cal;

  // Calculate settling time (WaitClks) based on DFT/Filter settings
  clks_cal.DataType = DATATYPE_DFT;
  clks_cal.DftSrc = AppIMPCfg.DftSrc;
  clks_cal.DataCount = 1L<<(AppIMPCfg.DftNum+2); 
  clks_cal.ADCSinc2Osr = AppIMPCfg.ADCSinc2Osr;
  clks_cal.ADCSinc3Osr = AppIMPCfg.ADCSinc3Osr;
  clks_cal.ADCAvgNum = AppIMPCfg.ADCAvgNum;
  clks_cal.RatioSys2AdcClk = AppIMPCfg.SysClkFreq/AppIMPCfg.AdcClkFreq;
  AD5940_ClksCalculate(&clks_cal, &WaitClks);

  AD5940_SEQGenCtrl(bTRUE);
  AD5940_SEQGpioCtrlS(AGPIO_Pin2); 
  AD5940_SEQGenInsert(SEQ_WAIT(16*250)); 

  /* ----------------------------------------------------------------------- */
  /* Step 1: Measure Current across RCAL (Calibration)                       */
  /* ----------------------------------------------------------------------- */
  sw_cfg.Dswitch = SWD_RCAL0;
  sw_cfg.Pswitch = SWP_RCAL0;
  sw_cfg.Nswitch = SWN_RCAL1;
  sw_cfg.Tswitch = SWT_RCAL1|SWT_TRTIA;
  AD5940_SWMatrixCfgS(&sw_cfg);
  
  // ADC Mux for Current (HSTIA)
  AD5940_ADCMuxCfgS(ADCMUXP_HSTIA_P, ADCMUXN_HSTIA_N);

  // Enable AFE Power and Waveform Generator
  AD5940_AFECtrlS(AFECTRL_HSTIAPWR|AFECTRL_INAMPPWR|AFECTRL_EXTBUFPWR|\
                AFECTRL_WG|AFECTRL_DACREFPWR|AFECTRL_HSDACPWR|\
                AFECTRL_SINC2NOTCH, bTRUE);
  AD5940_AFECtrlS(AFECTRL_WG|AFECTRL_ADCPWR, bTRUE); 
  AD5940_SEQGenInsert(SEQ_WAIT(16*2000)); 
  AD5940_AFECtrlS(AFECTRL_ADCCNV|AFECTRL_DFT, bTRUE); 
  
  // Store address to update wait times later during sweep
  AD5940_SEQGenFetchSeq(NULL, &AppIMPCfg.SeqWaitAddr[0]); 
  AD5940_SEQGenInsert(SEQ_WAIT(WaitClks/2));
  AD5940_SEQGenInsert(SEQ_WAIT(WaitClks/2));
  
  // Stop ADC/DFT first, wait 10us, then stop WG
  AD5940_AFECtrlS(AFECTRL_ADCCNV|AFECTRL_DFT, bFALSE); 
  AD5940_SEQGenInsert(SEQ_WAIT(16*10)); 
  AD5940_AFECtrlS(AFECTRL_WG, bFALSE);

  /* ----------------------------------------------------------------------- */
  /* Step 2: Measure Sensor Current (I)                                      */
  /* ----------------------------------------------------------------------- */
  sw_cfg.Dswitch = AppIMPCfg.DswitchSel;
  sw_cfg.Pswitch = AppIMPCfg.PswitchSel;
  sw_cfg.Nswitch = AppIMPCfg.NswitchSel;
  sw_cfg.Tswitch = SWT_TRTIA|AppIMPCfg.TswitchSel;
  AD5940_SWMatrixCfgS(&sw_cfg);
  
  // ADC Mux for Current (HSTIA)
  AD5940_ADCMuxCfgS(ADCMUXP_HSTIA_P, ADCMUXN_HSTIA_N);

  AD5940_AFECtrlS(AFECTRL_ADCPWR|AFECTRL_WG, bTRUE); 
  AD5940_SEQGenInsert(SEQ_WAIT(16*2000)); 
  AD5940_AFECtrlS(AFECTRL_ADCCNV|AFECTRL_DFT, bTRUE); 
  
  AD5940_SEQGenFetchSeq(NULL, &AppIMPCfg.SeqWaitAddr[1]); 
  AD5940_SEQGenInsert(SEQ_WAIT(WaitClks/2));
  AD5940_SEQGenInsert(SEQ_WAIT(WaitClks/2));

  AD5940_AFECtrlS(AFECTRL_ADCCNV|AFECTRL_DFT, bFALSE); 
  AD5940_SEQGenInsert(SEQ_WAIT(16*10)); 
  AD5940_AFECtrlS(AFECTRL_WG, bFALSE);

  /* ----------------------------------------------------------------------- */
  /* Step 3: Measure Sensor Voltage (V)                                      */
  /* ----------------------------------------------------------------------- */
  // Switches remain same (Force path active)
  
  // ADC Mux for Voltage (AIN2/AIN3)
  AD5940_ADCMuxCfgS(ADCMUXP_AIN2, ADCMUXN_AIN3);

  AD5940_AFECtrlS(AFECTRL_ADCPWR|AFECTRL_WG, bTRUE); 
  AD5940_SEQGenInsert(SEQ_WAIT(16*2000)); 
  AD5940_AFECtrlS(AFECTRL_ADCCNV|AFECTRL_DFT, bTRUE); 
  
  AD5940_SEQGenFetchSeq(NULL, &AppIMPCfg.SeqWaitAddr[2]); 
  AD5940_SEQGenInsert(SEQ_WAIT(WaitClks/2));
  AD5940_SEQGenInsert(SEQ_WAIT(WaitClks/2));

  // Stop ADC/DFT, wait 10us, then stop WG and ADC Power
  AD5940_AFECtrlS(AFECTRL_ADCCNV|AFECTRL_DFT, bFALSE); 
  AD5940_SEQGenInsert(SEQ_WAIT(16*10)); 
  AD5940_AFECtrlS(AFECTRL_WG|AFECTRL_ADCPWR, bFALSE); 

  // Power down AFE blocks
  AD5940_AFECtrlS(AFECTRL_HSTIAPWR|AFECTRL_INAMPPWR|AFECTRL_EXTBUFPWR|\
                AFECTRL_WG|AFECTRL_DACREFPWR|AFECTRL_HSDACPWR|\
                AFECTRL_SINC2NOTCH, bFALSE);
  AD5940_SEQGpioCtrlS(0); 

  AD5940_SEQGenInsert(SEQ_STOP()); 

  error = AD5940_SEQGenFetchSeq(&pSeqCmd, &SeqLen);
  AD5940_SEQGenCtrl(bFALSE); 

  if(error == AD5940ERR_OK)
  {
    AppIMPCfg.MeasureSeqInfo.SeqId = SEQID_0;
    AppIMPCfg.MeasureSeqInfo.SeqRamAddr = AppIMPCfg.InitSeqInfo.SeqRamAddr + AppIMPCfg.InitSeqInfo.SeqLen ;
    AppIMPCfg.MeasureSeqInfo.pSeqCmd = pSeqCmd;
    AppIMPCfg.MeasureSeqInfo.SeqLen = SeqLen;
    AD5940_SEQCmdWrite(AppIMPCfg.MeasureSeqInfo.SeqRamAddr, pSeqCmd, SeqLen);
  }
  else
    return error; 
  return AD5940ERR_OK;
}

AD5940Err AppIMPCheckFreq(float freq)
{
  ADCFilterCfg_Type filter_cfg;
  DFTCfg_Type dft_cfg;
  uint32_t WaitClks;
  ClksCalInfo_Type clks_cal;
  FreqParams_Type freq_params;
  uint32_t SeqCmdBuff[32];
  uint32_t SRAMAddr = 0;
  
  freq_params = AD5940_GetFreqParameters(freq);
  
  // Update ADC Filter and DFT settings based on frequency
  filter_cfg.ADCAvgNum = ADCAVGNUM_16; 
  filter_cfg.ADCSinc2Osr = freq_params.ADCSinc2Osr;
  filter_cfg.ADCSinc3Osr = freq_params.ADCSinc3Osr;
  filter_cfg.BpSinc3 = bFALSE;
  filter_cfg.BpNotch = bTRUE;
  filter_cfg.Sinc2NotchEnable = bTRUE;
  filter_cfg.ADCRate = ADCRATE_800KHZ; // Fixed ADC Rate for stability
  
  dft_cfg.DftNum = freq_params.DftNum;
  dft_cfg.DftSrc = freq_params.DftSrc;
  dft_cfg.HanWinEn = AppIMPCfg.HanWinEn;
  
  AD5940_ADCFilterCfgS(&filter_cfg);
  AD5940_DFTCfgS(&dft_cfg);
  
  // Recalculate settling times (WaitClks) for the new frequency
  clks_cal.DataType = DATATYPE_DFT;
  clks_cal.DftSrc = freq_params.DftSrc;
  clks_cal.DataCount = 1L<<(freq_params.DftNum+2); 
  clks_cal.ADCSinc2Osr = freq_params.ADCSinc2Osr;
  clks_cal.ADCSinc3Osr = freq_params.ADCSinc3Osr;
  clks_cal.ADCAvgNum = 0;
  clks_cal.RatioSys2AdcClk = AppIMPCfg.SysClkFreq/AppIMPCfg.AdcClkFreq;
  AD5940_ClksCalculate(&clks_cal, &WaitClks);		
  
  // Add safety margin
  WaitClks += 200;
	
  // Update Wait Times in SRAM for all 3 measurements
  for (int i = 0; i < 3; i++) {
      if (AppIMPCfg.SeqWaitAddr[i] == 0) continue;

      SRAMAddr = AppIMPCfg.MeasureSeqInfo.SeqRamAddr + AppIMPCfg.SeqWaitAddr[i];
      
      // Split wait time into two commands to handle large values
      uint32_t finalWait = WaitClks/2;
      
      SeqCmdBuff[0] = SEQ_WAIT(finalWait);
      SeqCmdBuff[1] = SEQ_WAIT(finalWait);
      AD5940_SEQCmdWrite(SRAMAddr, SeqCmdBuff, 2);
  }
		
  return AD5940ERR_OK;
}

int32_t AppIMPInit(uint32_t *pBuffer, uint32_t BufferSize)
{
  AD5940Err error = AD5940ERR_OK;  
  SEQCfg_Type seq_cfg;
  FIFOCfg_Type fifo_cfg;

  if(AD5940_WakeUp(10) > 10) return AD5940ERR_WAKEUP;

  // Stop timers and sequencer before reconfiguration
  AD5940_WUPTCtrl(bFALSE);
  AD5940_SEQCtrlS(bFALSE);

  seq_cfg.SeqMemSize = SEQMEMSIZE_2KB; 
  seq_cfg.SeqBreakEn = bFALSE;
  seq_cfg.SeqIgnoreEn = bTRUE;
  seq_cfg.SeqCntCRCClr = bTRUE;
  seq_cfg.SeqEnable = bFALSE;
  seq_cfg.SeqWrTimer = 0;
  AD5940_SEQCfg(&seq_cfg);
  
  AD5940_FIFOCtrlS(FIFOSRC_DFT, bFALSE);
  fifo_cfg.FIFOEn = bTRUE;
  fifo_cfg.FIFOMode = FIFOMODE_FIFO;
  fifo_cfg.FIFOSize = FIFOSIZE_4KB; 
  fifo_cfg.FIFOSrc = FIFOSRC_DFT;
  fifo_cfg.FIFOThresh = AppIMPCfg.FifoThresh; 
  AD5940_FIFOCfg(&fifo_cfg);
  AD5940_INTCClrFlag(AFEINTSRC_ALLINT);

  if((AppIMPCfg.IMPInited == bFALSE)||(AppIMPCfg.bParaChanged == bTRUE))
  {
    if(pBuffer == 0)  return AD5940ERR_PARA;
    if(BufferSize == 0) return AD5940ERR_PARA;   
    AD5940_SEQGenInit(pBuffer, BufferSize);

    error = AppIMPSeqCfgGen(); 
    if(error != AD5940ERR_OK) return error;

    error = AppIMPSeqMeasureGen();
    if(error != AD5940ERR_OK) return error;

    AppIMPCfg.bParaChanged = bFALSE; 
  }

  AppIMPCfg.InitSeqInfo.WriteSRAM = bFALSE;
  AD5940_SEQInfoCfg(&AppIMPCfg.InitSeqInfo);
  seq_cfg.SeqEnable = bTRUE;
  AD5940_SEQCfg(&seq_cfg); 
  AD5940_SEQMmrTrig(AppIMPCfg.InitSeqInfo.SeqId);
  
  AD5940_INTCCfg(AFEINTC_1, AFEINTSRC_ENDSEQ, bTRUE);
  
  // Wait for initialization sequence to complete
  int timeout = 100000;
  while(AD5940_INTCTestFlag(AFEINTC_1, AFEINTSRC_ENDSEQ) == bFALSE) {
      if(--timeout <= 0) {
          return AD5940ERR_TIMEOUT; 
      }
  }
  AD5940_INTCClrFlag(AFEINTSRC_ALLINT);

  AppIMPCfg.MeasureSeqInfo.WriteSRAM = bFALSE;
  AD5940_SEQInfoCfg(&AppIMPCfg.MeasureSeqInfo);
  
  // Set initial frequency parameters
  AppIMPCheckFreq(AppIMPCfg.FreqofData);

  seq_cfg.SeqEnable = bTRUE;
  AD5940_SEQCfg(&seq_cfg); 
  AD5940_ClrMCUIntFlag(); 

  AppIMPCfg.IMPInited = bTRUE; 
  return AD5940ERR_OK;
}

int32_t AppIMPRegModify(int32_t * const pData, uint32_t *pDataCount)
{
  if(AppIMPCfg.NumOfData > 0)
  {
    AppIMPCfg.FifoDataCount += *pDataCount/6; 
    if(AppIMPCfg.FifoDataCount >= AppIMPCfg.NumOfData)
    {
      AD5940_WUPTCtrl(bFALSE);
      AD5940_SEQCtrlS(bFALSE); 
      return AD5940ERR_STOP; // Return STOP code
    }
  }
  if(AppIMPCfg.StopRequired == bTRUE)
  {
    AD5940_WUPTCtrl(bFALSE);
    return AD5940ERR_OK;
  }
  if(AppIMPCfg.SweepCfg.SweepEn) 
  {
    // Update Filters and Wait Times in SRAM
    AppIMPCheckFreq(AppIMPCfg.SweepNextFreq);
    
    // Update WG Frequency
    AD5940_WGFreqCtrlS(AppIMPCfg.SweepNextFreq, AppIMPCfg.SysClkFreq);
  }
  return AD5940ERR_OK;
}

int32_t AppIMPDataProcess(int32_t * const pData, uint32_t *pDataCount)
{
  uint32_t DataCount = *pDataCount;
  uint32_t ImpResCount = DataCount/6; 

  fImpPol_Type * const pOut = (fImpPol_Type*)pData;
  iImpCar_Type * pSrcData = (iImpCar_Type*)pData;

  *pDataCount = 0;

  for(uint32_t i=0; i<DataCount; i++)
  {
    pData[i] &= 0x3ffff; 
    if(pData[i]&(1L<<17)) 
    {
      pData[i] |= 0xfffc0000; 
    }
  }
  
  for(uint32_t i=0; i<ImpResCount; i++)
  {
    iImpCar_Type *pDftRcal, *pDftI, *pDftV;

    pDftRcal = pSrcData++;
    pDftI = pSrcData++;
    pDftV = pSrcData++;
    
    float MagI, PhaseI;
    float MagV, PhaseV;
    float ZMag, ZPhase;
    
    MagI = sqrt((float)pDftI->Real*pDftI->Real + (float)pDftI->Image*pDftI->Image);
    PhaseI = atan2(-pDftI->Image, pDftI->Real);

    MagV = sqrt((float)pDftV->Real*pDftV->Real + (float)pDftV->Image*pDftV->Image);
    PhaseV = atan2(-pDftV->Image, pDftV->Real);

    // Calculate Impedance: Z = (V / I) * Rtia
    if(MagI > 1e-9) 
        ZMag = (MagV / MagI) * AppIMPCfg.RtiaVal;
    else
        ZMag = 0;

    ZPhase = PhaseV - PhaseI + MATH_PI;
    
    while(ZPhase > MATH_PI) ZPhase -= 2*MATH_PI;
    while(ZPhase < -MATH_PI) ZPhase += 2*MATH_PI;

    pOut[i].Magnitude = ZMag;
    pOut[i].Phase = ZPhase;
  }
  *pDataCount = ImpResCount; 
  AppIMPCfg.FreqofData = AppIMPCfg.SweepCurrFreq;
  
  if(AppIMPCfg.SweepCfg.SweepEn == bTRUE)
  {
    AppIMPCfg.FreqofData = AppIMPCfg.SweepCurrFreq;
    AppIMPCfg.SweepCurrFreq = AppIMPCfg.SweepNextFreq;
    AD5940_SweepNext(&AppIMPCfg.SweepCfg, &AppIMPCfg.SweepNextFreq);
  }

  return 0;
}

int32_t AppIMPISR(void *pBuff, uint32_t *pCount)
{
  uint32_t BuffCount;
  uint32_t FifoCnt;
  BuffCount = *pCount;
  
  *pCount = 0;
  
  if(AD5940_WakeUp(10) > 10) return AD5940ERR_WAKEUP;
  AD5940_SleepKeyCtrlS(SLPKEY_LOCK); 

  uint32_t IntFlag = AD5940_INTCGetFlag(AFEINTC_1);

  if (IntFlag & (AFEINTSRC_DATAFIFOOF | AFEINTSRC_DATAFIFOUF)) {
      AD5940_INTCClrFlag(AFEINTSRC_DATAFIFOOF | AFEINTSRC_DATAFIFOUF);
      FIFOCfg_Type fifo_cfg;
      fifo_cfg.FIFOEn = bFALSE;
      AD5940_FIFOCfg(&fifo_cfg);
      fifo_cfg.FIFOEn = bTRUE;
      fifo_cfg.FIFOMode = FIFOMODE_FIFO;
      fifo_cfg.FIFOSize = FIFOSIZE_4KB;
      fifo_cfg.FIFOSrc = FIFOSRC_DFT;
      fifo_cfg.FIFOThresh = AppIMPCfg.FifoThresh;
      AD5940_FIFOCfg(&fifo_cfg);
      
      AD5940_SleepKeyCtrlS(SLPKEY_UNLOCK);
      return AD5940ERR_FIFO;
  }

  if(IntFlag & AFEINTSRC_ENDSEQ)
  {
      int timeout = 200; 
      while(AD5940_FIFOGetCnt() < 6 && timeout-- > 0)
      {
          AD5940_Delay10us(10); 
      }

      FifoCnt = (AD5940_FIFOGetCnt()/6)*6;
      if(FifoCnt > BuffCount) FifoCnt = BuffCount;
      
      AD5940_FIFORd((uint32_t *)pBuff, FifoCnt);
      AD5940_INTCClrFlag(AFEINTSRC_ENDSEQ);
      
      if (FifoCnt > 0) 
      {
          AD5940_SEQCtrlS(bFALSE);

          int32_t status = AppIMPRegModify(pBuff, &FifoCnt);
          if (status == AD5940ERR_OK) 
          {
              if(AppIMPCfg.FifoDataCount < AppIMPCfg.NumOfData || AppIMPCfg.NumOfData == -1)
              {
                  if(AppIMPCfg.StopRequired == bFALSE)
                  {
                      AD5940_Delay10us(20); 

                      AD5940_WriteReg(REG_AFE_SEQCNT, 0);
                      AD5940_SEQCtrlS(bTRUE);
                      AD5940_SEQMmrTrig(SEQID_0);
                  }
              }
          }
          
          AD5940_SleepKeyCtrlS(SLPKEY_UNLOCK); 
          AppIMPDataProcess((int32_t*)pBuff, &FifoCnt); 
          
          // Discard extra data points if they exceed the requested count
          if (AppIMPCfg.SweepCfg.SweepEn && AppIMPCfg.RealDataCount > 0) {
              if (AppIMPCfg.FifoDataCount > AppIMPCfg.RealDataCount) {
                  *pCount = 0; // Discard
              } else {
                  *pCount = FifoCnt;
              }
          } else {
              *pCount = FifoCnt;
          }
          
          if (status == AD5940ERR_STOP) return AD5940ERR_STOP;
      }
      else
      {
          AD5940_SleepKeyCtrlS(SLPKEY_UNLOCK);
          *pCount = 0;
      }
      return 0;
  }
  
  AD5940_INTCClrFlag(AFEINTSRC_ALLINT);
  AD5940_SleepKeyCtrlS(SLPKEY_UNLOCK);
  return 0;
}