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

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

void Routine_Measure(float freq) {
    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();
    
    AD5940ImpedanceStructInit(); // Reload config with current HP settings
    
    pCfg->WuptClkFreq = LFOSCFreq;
    pCfg->SweepCfg.SweepEn = bFALSE;
    pCfg->SinFreq = freq;
    pCfg->NumOfData = -1; 
    pCfg->RealDataCount = -1; 
    pCfg->bParaChanged = bTRUE; 
    
    if(AppIMPInit(AppBuff, APPBUFF_SIZE) == AD5940ERR_OK) {
        AppIMPCtrl(IMPCTRL_START, 0);
    } else {
        printf("ERROR: Init Failed\n");
    }
}

void Routine_Sweep(float start, float end, int steps) {
    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();
    
    AD5940ImpedanceStructInit(); // Reload config with current HP settings
    
    pCfg->WuptClkFreq = LFOSCFreq;
    pCfg->SweepCfg.SweepEn = bTRUE;
    pCfg->SweepCfg.SweepStart = start;
    pCfg->SweepCfg.SweepStop = end;
    pCfg->SweepCfg.SweepPoints = steps + 1;
    pCfg->NumOfData = steps + 1; 
    pCfg->RealDataCount = steps; 
    pCfg->SweepCfg.SweepLog = bTRUE;
    pCfg->bParaChanged = bTRUE; 
    
    if(AppIMPInit(AppBuff, APPBUFF_SIZE) == AD5940ERR_OK) {
        AppIMPCtrl(IMPCTRL_START, 0);
    } else {
        printf("ERROR: Init Failed\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");
    }
}

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 = 100.0;
    lprtia_cal.LpTiaRtia = GetLPTIARtia(ConfigLptiaVal);
    lprtia_cal.LpAmpPwrMod = LPAMPPWR_NORM;
    lprtia_cal.bWithCtia = bFALSE;
    // Use a low frequency for LPTIA calibration
    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...\n", ConfigLptiaVal);
    if (AD5940_LPRtiaCal(&lprtia_cal, &LpRes) == AD5940ERR_OK) {
        printf("Calibrated LPTIA: Mag = %f Ohm, Phase = %f\n", LpRes.Magnitude, LpRes.Phase);
        CalibratedLptiaVal = LpRes.Magnitude;
    } else {
        printf("LPTIA Calibration Failed\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;
    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("Calibrated HSTIA: Mag = %f Ohm, Phase = %f\n", HsRes.Magnitude, HsRes.Phase);
        CalibratedHstiaVal = HsRes.Magnitude;
    } else {
        printf("HSTIA Calibration Failed\n");
    }
}