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

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

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

  .ImpODR = 20.0,             /* 20.0 Hz = 50ms period */
  .NumOfData = -1,
  .SysClkFreq = 16000000.0,
  .WuptClkFreq = 32000.0,
  .AdcClkFreq = 16000000.0,
  .RcalVal = 10000.0,
  .RtiaVal = 200.0,

  .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,
  .BiasVolt = -0.0f,

  .SinFreq = 1000.0,

  .DftNum = DFTNUM_16384,
  .DftSrc = DFTSRC_SINC3,
  .HanWinEn = bTRUE,

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

  .ADCAvgNum = ADCAVGNUM_16,

  .SweepCfg.SweepEn = bTRUE,
  .SweepCfg.SweepStart = 1000,
  .SweepCfg.SweepStop = 100000.0,
  .SweepCfg.SweepPoints = 101,
  .SweepCfg.SweepLog = bFALSE,
  .SweepCfg.SweepIndex = 0,

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

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:
    {
      WUPTCfg_Type wupt_cfg;
      if(AD5940_WakeUp(10) > 10) return AD5940ERR_WAKEUP;
      if(AppIMPCfg.IMPInited == bFALSE) return AD5940ERR_APPERROR;
      
      wupt_cfg.WuptEn = bTRUE;
      wupt_cfg.WuptEndSeq = WUPTENDSEQ_A;
      wupt_cfg.WuptOrder[0] = SEQID_0;
      wupt_cfg.SeqxSleepTime[SEQID_0] = 4;
      
      // Calculate Wakeup Time based on ODR
      // 32kHz clock. For 20Hz: 32000/20 = 1600 ticks.
      uint32_t wait_cycles = (uint32_t)(AppIMPCfg.WuptClkFreq/AppIMPCfg.ImpODR);
      if(wait_cycles < 20) wait_cycles = 20; // Minimum safety guard
      wupt_cfg.SeqxWakeupTime[SEQID_0] = wait_cycles - 4;
      
      AD5940_WUPTCfg(&wupt_cfg);
      
      AppIMPCfg.FifoDataCount = 0;
      break;
    }
    case IMPCTRL_STOPNOW:
    {
      if(AD5940_WakeUp(10) > 10) return AD5940ERR_WAKEUP;
      AD5940_WUPTCtrl(bFALSE);
      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;
}

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

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

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

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

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

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;

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

  /* ----------------------------------------------------------------------- */
  /* Measurement 1: RCAL (Current)                                           */
  /* ----------------------------------------------------------------------- */
  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);

  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*10)); // Settling
  AD5940_AFECtrlS(AFECTRL_ADCCNV|AFECTRL_DFT, bTRUE); 
  
  AD5940_SEQGenFetchSeq(NULL, &AppIMPCfg.SeqWaitAddr[0]); 
  AD5940_SEQGenInsert(SEQ_WAIT(WaitClks/2));
  AD5940_SEQGenInsert(SEQ_WAIT(WaitClks/2));
  
  AD5940_AFECtrlS(AFECTRL_ADCCNV|AFECTRL_DFT|AFECTRL_WG, bFALSE); 

  /* ----------------------------------------------------------------------- */
  /* Measurement 2: 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*10)); 
  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|AFECTRL_WG, bFALSE); 

  /* ----------------------------------------------------------------------- */
  /* Measurement 3: 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*10)); 
  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));

  AD5940_AFECtrlS(AFECTRL_ADCCNV|AFECTRL_DFT|AFECTRL_WG|AFECTRL_ADCPWR, bFALSE); 
  AD5940_AFECtrlS(AFECTRL_HSTIAPWR|AFECTRL_INAMPPWR|AFECTRL_EXTBUFPWR|\
                AFECTRL_WG|AFECTRL_DACREFPWR|AFECTRL_HSDACPWR|\
                AFECTRL_SINC2NOTCH, bFALSE);
  AD5940_SEQGpioCtrlS(0); 

  AD5940_EnterSleepS();

  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;
  HSDACCfg_Type hsdac_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);
  
  if(freq < 0.51)
  {
    hsdac_cfg.ExcitBufGain = EXCITBUFGAIN_2;
    hsdac_cfg.HsDacGain = HSDACGAIN_1;
    hsdac_cfg.HsDacUpdateRate = 0x1B;
    AD5940_HSDacCfgS(&hsdac_cfg);
    AD5940_HSRTIACfgS(HSTIARTIA_40K); 
    filter_cfg.ADCRate = ADCRATE_800KHZ;
    AppIMPCfg.AdcClkFreq = 16e6;
    AD5940_HPModeEn(bFALSE);
  }
  else if(freq < 5 )
  {
    hsdac_cfg.ExcitBufGain = EXCITBUFGAIN_2;
    hsdac_cfg.HsDacGain = HSDACGAIN_1;
    hsdac_cfg.HsDacUpdateRate = 0x1B;
    AD5940_HSDacCfgS(&hsdac_cfg);
    AD5940_HSRTIACfgS(HSTIARTIA_40K); 
    filter_cfg.ADCRate = ADCRATE_800KHZ;
    AppIMPCfg.AdcClkFreq = 16e6;
    AD5940_HPModeEn(bFALSE);
  }
  else if(freq < 450)
  {
    hsdac_cfg.ExcitBufGain = AppIMPCfg.ExcitBufGain;
    hsdac_cfg.HsDacGain = AppIMPCfg.HsDacGain;  
    hsdac_cfg.HsDacUpdateRate = 0x1B;
    AD5940_HSDacCfgS(&hsdac_cfg);
    AD5940_HSRTIACfgS(HSTIARTIA_5K); 
    filter_cfg.ADCRate = ADCRATE_800KHZ;
    AppIMPCfg.AdcClkFreq = 16e6;
    AD5940_HPModeEn(bFALSE);
  }
  else if(freq<80000)
  {
    hsdac_cfg.ExcitBufGain = AppIMPCfg.ExcitBufGain;
    hsdac_cfg.HsDacGain = AppIMPCfg.HsDacGain;
    hsdac_cfg.HsDacUpdateRate = 0x1B;
    AD5940_HSDacCfgS(&hsdac_cfg);
    AD5940_HSRTIACfgS(HSTIARTIA_5K); 
    filter_cfg.ADCRate = ADCRATE_800KHZ;
    AppIMPCfg.AdcClkFreq = 16e6;
    AD5940_HPModeEn(bFALSE);
  }
  if(freq >= 80000)
  {
    hsdac_cfg.ExcitBufGain = AppIMPCfg.ExcitBufGain;
    hsdac_cfg.HsDacGain = AppIMPCfg.HsDacGain;
    hsdac_cfg.HsDacUpdateRate = 0x07;
    AD5940_HSDacCfgS(&hsdac_cfg);
    AD5940_HSRTIACfgS(HSTIARTIA_5K); 
    filter_cfg.ADCRate = ADCRATE_1P6MHZ;
    AppIMPCfg.AdcClkFreq = 32e6;
    AD5940_HPModeEn(bTRUE);
  }
  
  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;
  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);
  
  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);		
	
  // Update Wait Times for all 3 measurements
  for (int i = 0; i < 3; i++) {
      SRAMAddr = AppIMPCfg.MeasureSeqInfo.SeqRamAddr + AppIMPCfg.SeqWaitAddr[i];
      SeqCmdBuff[0] = SEQ_WAIT(WaitClks/2);
      SeqCmdBuff[1] = SEQ_WAIT(WaitClks/2);
      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;

  // CRITICAL: Stop WUPT 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);
  
  // Safety timeout for Init Sequence
  int timeout = 100000;
  while(AD5940_INTCTestFlag(AFEINTC_1, AFEINTSRC_ENDSEQ) == bFALSE) {
      if(--timeout <= 0) {
          return AD5940ERR_TIMEOUT; // Return error if init fails
      }
  }
  AD5940_INTCClrFlag(AFEINTSRC_ALLINT);

  AppIMPCfg.MeasureSeqInfo.WriteSRAM = bFALSE;
  AD5940_SEQInfoCfg(&AppIMPCfg.MeasureSeqInfo);
  
  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; // 6 words per point
    if(AppIMPCfg.FifoDataCount >= AppIMPCfg.NumOfData)
    {
      AD5940_WUPTCtrl(bFALSE);
      return AD5940ERR_OK;
    }
  }
  if(AppIMPCfg.StopRequired == bTRUE)
  {
    AD5940_WUPTCtrl(bFALSE);
    return AD5940ERR_OK;
  }
  if(AppIMPCfg.SweepCfg.SweepEn) 
  {
    AD5940_WGFreqCtrlS(AppIMPCfg.SweepNextFreq, AppIMPCfg.SysClkFreq);
    AppIMPCheckFreq(AppIMPCfg.SweepNextFreq);
  }
  return AD5940ERR_OK;
}

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

  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;
    
    // Calculate Magnitude and Phase for Current (I)
    MagI = sqrt((float)pDftI->Real*pDftI->Real + (float)pDftI->Image*pDftI->Image);
    PhaseI = atan2(-pDftI->Image, pDftI->Real);

    // Calculate Magnitude and Phase for Voltage (V)
    MagV = sqrt((float)pDftV->Real*pDftV->Real + (float)pDftV->Image*pDftV->Image);
    PhaseV = atan2(-pDftV->Image, pDftV->Real);

    // Z = V / I * Rtia
    if(MagI > 1e-9) // Avoid division by zero
        ZMag = (MagV / MagI) * AppIMPCfg.RtiaVal;
    else
        ZMag = 0;

    ZPhase = PhaseV - PhaseI;
    
    // Normalize Phase to -PI to +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); 

  if(AD5940_INTCTestFlag(AFEINTC_0, AFEINTSRC_DATAFIFOTHRESH) == bTRUE)
  {
    FifoCnt = (AD5940_FIFOGetCnt()/6)*6; // Align to 6 words
    
    if (FifoCnt == 0) {
        // Safety: If interrupt fired but no data, clear flag and exit
        AD5940_INTCClrFlag(AFEINTSRC_DATAFIFOTHRESH);
        AD5940_SleepKeyCtrlS(SLPKEY_UNLOCK);
        return 0;
    }

    if(FifoCnt > BuffCount)
    {
       FifoCnt = BuffCount;
    }
    AD5940_FIFORd((uint32_t *)pBuff, FifoCnt);
    AD5940_INTCClrFlag(AFEINTSRC_DATAFIFOTHRESH);
    AppIMPRegModify(pBuff, &FifoCnt);   
    AD5940_SleepKeyCtrlS(SLPKEY_UNLOCK); 
    AppIMPDataProcess((int32_t*)pBuff, &FifoCnt); 
    *pCount = FifoCnt;
    return 0;
  }
  
  return 0;
}