// File: main.c
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include "pico/stdlib.h"
#include "hardware/spi.h"
#include "hardware/gpio.h"
#include "hardware/watchdog.h"
#include "ad5940.h"
#include "Impedance.h"
#include "Amperometric.h"
#include "RampTest.h"

#define AD5940ERR_STOP 10

// Fix for missing definition in some SDK versions
#ifndef LPTIARF_BYPASS
#define LPTIARF_BYPASS 0x2000
#endif

// ---------------------------------------------------------------------------
// Hardware Definitions
// ---------------------------------------------------------------------------
#define PIN_MISO 0
#define PIN_CS   1
#define PIN_SCK  2
#define PIN_MOSI 3
#define PIN_RST  9
#define PIN_INT  29 

#define APPBUFF_SIZE 512
uint32_t AppBuff[APPBUFF_SIZE];

// Application State
typedef enum {
    MODE_IDLE,
    MODE_IMPEDANCE,
    MODE_AMPEROMETRIC,
    MODE_RAMP
} AppMode;

AppMode CurrentMode = MODE_IDLE;
float LFOSCFreq = 32000.0; // Default, updated by calibration

// ---------------------------------------------------------------------------
// Range / RTIA Management
// ---------------------------------------------------------------------------

// Nominal values for the 8 supported ranges
float RtiaCalibrationTable[8] = {200.0, 1000.0, 5000.0, 10000.0, 20000.0, 40000.0, 80000.0, 160000.0};
uint32_t CurrentRtiaIndex = 0; // Default to 200 Ohm (Index 0)
uint32_t ConfigRtiaVal = 200;  // Default Value
uint32_t CurrentLpTiaRf = LPTIARF_20K; // Default LPF

// Map integer value to HSTIA Enum (Impedance)
uint32_t GetHSTIARtia(uint32_t val) {
    switch(val) {
        case 200: return HSTIARTIA_200;
        case 1000: return HSTIARTIA_1K;
        case 5000: return HSTIARTIA_5K;
        case 10000: return HSTIARTIA_10K;
        case 20000: return HSTIARTIA_20K;
        case 40000: return HSTIARTIA_40K;
        case 80000: return HSTIARTIA_80K;
        case 160000: return HSTIARTIA_160K;
        default: return HSTIARTIA_200;
    }
}

// Map integer value to LPTIA Enum (Amperometric/Ramp)
uint32_t GetLPTIARtia(uint32_t val) {
    switch(val) {
        case 200: return LPTIARTIA_200R;
        case 1000: return LPTIARTIA_1K;
        case 5000: return LPTIARTIA_4K; 
        case 10000: return LPTIARTIA_10K;
        case 20000: return LPTIARTIA_20K;
        case 40000: return LPTIARTIA_40K;
        case 80000: return LPTIARTIA_85K; 
        case 160000: return LPTIARTIA_160K;
        default: return LPTIARTIA_1K;
    }
}

// Helper to find index for calibration table
int GetRtiaIndex(uint32_t val) {
    switch(val) {
        case 200: return 0;
        case 1000: return 1;
        case 5000: return 2;
        case 10000: return 3;
        case 20000: return 4;
        case 40000: return 5;
        case 80000: return 6;
        case 160000: return 7;
        default: return 0;
    }
}

// ---------------------------------------------------------------------------
// Platform Interface Implementation
// ---------------------------------------------------------------------------
void AD5940_CsClr(void) { gpio_put(PIN_CS, 0); }
void AD5940_CsSet(void) { gpio_put(PIN_CS, 1); }
void AD5940_RstClr(void) { gpio_put(PIN_RST, 0); }
void AD5940_RstSet(void) { gpio_put(PIN_RST, 1); }
void AD5940_Delay10us(uint32_t time) { sleep_us(time * 10); }
void AD5940_ReadWriteNBytes(unsigned char *pSendBuffer, unsigned char *pRecvBuff, unsigned long length) {
    spi_write_read_blocking(spi0, pSendBuffer, pRecvBuff, length);
}

uint32_t AD5940_GetMCUIntFlag(void) {
    return (gpio_get(PIN_INT) == 0);
}

uint32_t AD5940_ClrMCUIntFlag(void) {
    return 1;
}

uint32_t AD5940_MCUResourceInit(void *pCfg) {
    return 0;
}

void AD5940_MCUGpioWrite(uint32_t data) { (void)data; }
uint32_t AD5940_MCUGpioRead(uint32_t pin) { (void)pin; return 0; }
void AD5940_MCUGpioCtrl(uint32_t pin, BoolFlag enable) { (void)pin; (void)enable; }

// ---------------------------------------------------------------------------
// Application Logic
// ---------------------------------------------------------------------------

void setup_pins(void) {
    spi_init(spi0, 4000000); 
    gpio_set_function(PIN_MISO, GPIO_FUNC_SPI);
    gpio_set_function(PIN_SCK, GPIO_FUNC_SPI);
    gpio_set_function(PIN_MOSI, GPIO_FUNC_SPI);

    gpio_init(PIN_CS);
    gpio_set_dir(PIN_CS, GPIO_OUT);
    gpio_put(PIN_CS, 1);

    gpio_init(PIN_RST);
    gpio_set_dir(PIN_RST, GPIO_OUT);
    gpio_put(PIN_RST, 1);

    gpio_init(PIN_INT);
    gpio_set_dir(PIN_INT, GPIO_IN);
    gpio_pull_up(PIN_INT); 
}

void AD5940ImpedanceStructInit(void)
{
  AppIMPCfg_Type *pImpedanceCfg;
  AppIMPGetCfg(&pImpedanceCfg);
  
  pImpedanceCfg->SeqStartAddr = 0;
  pImpedanceCfg->MaxSeqLen = 512; 
  pImpedanceCfg->RcalVal = 100.0; 
  pImpedanceCfg->RtiaVal = RtiaCalibrationTable[CurrentRtiaIndex]; 
  pImpedanceCfg->SinFreq = 1000.0;
  pImpedanceCfg->FifoThresh = 6; 
  pImpedanceCfg->DacVoltPP = 600.0;
  pImpedanceCfg->ExcitBufGain = EXCITBUFGAIN_0P25;
  pImpedanceCfg->HsDacGain = HSDACGAIN_0P2;
  pImpedanceCfg->DswitchSel = SWD_CE0;
  pImpedanceCfg->PswitchSel = SWP_CE0;
  pImpedanceCfg->NswitchSel = SWN_SE0;
  pImpedanceCfg->TswitchSel = SWT_SE0LOAD; 
  pImpedanceCfg->HstiaRtiaSel = GetHSTIARtia(ConfigRtiaVal);	
  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;
}

void AD5940AMPStructInit(void)
{
  AppAMPCfg_Type *pAMPCfg;
  AppAMPGetCfg(&pAMPCfg);
  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(ConfigRtiaVal); 
  pAMPCfg->LpTiaRl = LPTIARLOAD_10R;
  pAMPCfg->LpTiaRf = CurrentLpTiaRf; // Use configured LPF
  pAMPCfg->Vzero = 1100;
  pAMPCfg->ADCRefVolt = 1.82;
}

void AD5940RampStructInit(void)
{
  AppRAMPCfg_Type *pRampCfg;
  AppRAMPGetCfg(&pRampCfg);
  
  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(ConfigRtiaVal);
  pRampCfg->LPTIARloadSel = LPTIARLOAD_10R;
  pRampCfg->LpTiaRf = CurrentLpTiaRf; // Use configured LPF
  pRampCfg->AdcPgaGain = ADCPGA_1P5;
}

static int32_t AD5940PlatformCfg(void)
{
  CLKCfg_Type clk_cfg;
  FIFOCfg_Type fifo_cfg;
  AGPIOCfg_Type gpio_cfg;

  AD5940_HWReset();
  AD5940_Initialize();
  
  clk_cfg.ADCClkDiv = ADCCLKDIV_1;
  clk_cfg.ADCCLkSrc = ADCCLKSRC_HFOSC;
  clk_cfg.SysClkDiv = SYSCLKDIV_1;
  clk_cfg.SysClkSrc = SYSCLKSRC_HFOSC;
  clk_cfg.HfOSC32MHzMode = bFALSE;
  clk_cfg.HFOSCEn = bTRUE;
  clk_cfg.HFXTALEn = bFALSE; 
  clk_cfg.LFOSCEn = bTRUE;
  AD5940_CLKCfg(&clk_cfg);
  printf("Clock Configured (HFOSC 16MHz).\n");
  
  fifo_cfg.FIFOEn = bFALSE;
  fifo_cfg.FIFOMode = FIFOMODE_FIFO;
  fifo_cfg.FIFOSize = FIFOSIZE_4KB;
  fifo_cfg.FIFOSrc = FIFOSRC_DFT;
  fifo_cfg.FIFOThresh = 6;		
  AD5940_FIFOCfg(&fifo_cfg);
  fifo_cfg.FIFOEn = bTRUE;
  AD5940_FIFOCfg(&fifo_cfg);
  
  AD5940_INTCCfg(AFEINTC_1, AFEINTSRC_ALLINT, bTRUE); 
  AD5940_INTCClrFlag(AFEINTSRC_ALLINT);
  AD5940_INTCCfg(AFEINTC_0, AFEINTSRC_DATAFIFOTHRESH|AFEINTSRC_CUSTOMINT0, bTRUE); 
  AD5940_INTCClrFlag(AFEINTSRC_ALLINT);

  gpio_cfg.FuncSet = GP0_INT;
  gpio_cfg.InputEnSet = 0;
  gpio_cfg.OutputEnSet = AGPIO_Pin0;
  gpio_cfg.OutVal = 0;
  gpio_cfg.PullEnSet = 0;
  AD5940_AGPIOCfg(&gpio_cfg);
  
  AD5940_SleepKeyCtrlS(SLPKEY_UNLOCK); 
  return 0;
}

void ImpedanceShowResult(uint32_t *pData, uint32_t DataCount)
{
  float freq;
  fImpPol_Type *pImp = (fImpPol_Type*)pData;
  AppIMPCtrl(IMPCTRL_GETFREQ, &freq);

  for(int i=0;i<DataCount;i++)
  {
    float mag = pImp[i].Magnitude;
    float phase = pImp[i].Phase;
    float real = mag * cosf(phase);
    float imag = mag * sinf(phase);

    printf("DATA,%.2f,%.4f,%.4f,%.4f,%.4f\n", freq, mag, phase * 180.0f / MATH_PI, real, imag);
  }
}

void AmperometricShowResult(float *pData, uint32_t DataCount)
{
  static int index = 0;
  for(int i=0;i<DataCount;i++)
  {
    printf("AMP,%d,%.4f\n", index++, pData[i]);
  }
}

void RampShowResult(float *pData, uint32_t DataCount)
{
  static int index = 0;
  for(int i=0;i<DataCount;i++)
  {
    printf("RAMP,%d,%.4f\n", index++, pData[i]);
  }
}

// ---------------------------------------------------------------------------
// High-Level Routines
// ---------------------------------------------------------------------------

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();
    pCfg->WuptClkFreq = LFOSCFreq;
    pCfg->HstiaRtiaSel = GetHSTIARtia(ConfigRtiaVal);
    pCfg->RtiaVal = RtiaCalibrationTable[CurrentRtiaIndex];
    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();
    pCfg->WuptClkFreq = LFOSCFreq;
    pCfg->HstiaRtiaSel = GetHSTIARtia(ConfigRtiaVal);
    pCfg->RtiaVal = RtiaCalibrationTable[CurrentRtiaIndex];
    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, Range: %d)...\n", bias_mv, ConfigRtiaVal);

    AppAMPCfg_Type *pCfg;
    AppAMPGetCfg(&pCfg);
    AD5940AMPStructInit(); 
    pCfg->SensorBias = bias_mv;
    pCfg->WuptClkFreq = LFOSCFreq;
    pCfg->LptiaRtiaSel = GetLPTIARtia(ConfigRtiaVal);
    pCfg->ReDoRtiaCal = bTRUE; 
    
    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)...\n", start_mv, end_mv, steps, duration_ms);

    AppRAMPCfg_Type *pCfg;
    AppRAMPGetCfg(&pCfg);
    AD5940RampStructInit();

    pCfg->RampStartVolt = start_mv;
    pCfg->RampPeakVolt = end_mv;
    pCfg->StepNumber = steps;
    pCfg->RampDuration = duration_ms;
    pCfg->LFOSCClkFreq = LFOSCFreq;
    pCfg->LPTIARtiaSel = GetLPTIARtia(ConfigRtiaVal);
    
    // Use global calibration value
    pCfg->RtiaValue.Magnitude = RtiaCalibrationTable[CurrentRtiaIndex];
    pCfg->RtiaValue.Phase = 0;
    
    pCfg->bRampOneDir = bTRUE; // Linear Sweep (not Cyclic)
    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);

    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 Res;
    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(ConfigRtiaVal); 
    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 RTIA %d Ohm...\n", ConfigRtiaVal);
    if (AD5940_HSRtiaCal(&hsrtia_cal, &Res) == AD5940ERR_OK) {
        printf("Calibrated Rtia: Mag = %f Ohm, Phase = %f\n", Res.Magnitude, Res.Phase);
        RtiaCalibrationTable[CurrentRtiaIndex] = Res.Magnitude;
        pCfg->RtiaVal = Res.Magnitude; 
    } else {
        printf("Calibration Failed\n");
    }
}

// ---------------------------------------------------------------------------
// Main Loop
// ---------------------------------------------------------------------------

char input_buffer[64];
int input_pos = 0;

void process_command() {
    char cmd = input_buffer[0];
    sleep_ms(10); 

    if (cmd == 'v') {
        uint32_t id = AD5940_ReadReg(REG_AFECON_CHIPID);
        printf("CHIP_ID:0x%04X\n", id);
    } 
    else if (cmd == 'r') {
        if (strlen(input_buffer) > 2) {
            uint32_t val = atoi(input_buffer + 2);
            ConfigRtiaVal = val;
            CurrentRtiaIndex = GetRtiaIndex(val);
            printf("RANGE_SET:%d\n", ConfigRtiaVal);
        }
    }
    else if (cmd == 'f') {
        if (strlen(input_buffer) > 2) {
            int idx = atoi(input_buffer + 2);
            switch(idx) {
                case 0: CurrentLpTiaRf = LPTIARF_BYPASS; break;
                case 1: CurrentLpTiaRf = LPTIARF_20K; break;
                case 2: CurrentLpTiaRf = LPTIARF_100K; break;
                case 3: CurrentLpTiaRf = LPTIARF_200K; break;
                case 4: CurrentLpTiaRf = LPTIARF_400K; break;
                case 5: CurrentLpTiaRf = LPTIARF_600K; break;
                case 6: CurrentLpTiaRf = LPTIARF_1M; break;
                default: CurrentLpTiaRf = LPTIARF_20K; break;
            }
            printf("LPF_SET:%d\n", idx);
        }
    }
    else if (cmd == 'c') {
        Routine_CalibrateLFO();
        Routine_CalibrateSystem();
    }
    else if (cmd == 'm') {
        float freq = 1000.0f;
        if (strlen(input_buffer) > 2) freq = atof(input_buffer + 2);
        Routine_Measure(freq);
    }
    else if (cmd == 's') {
        float start = 100.0f, end = 100000.0f;
        int steps = 50;
        if (strlen(input_buffer) > 2) sscanf(input_buffer + 2, "%f %f %d", &start, &end, &steps);
        Routine_Sweep(start, end, steps);
    }
    else if (cmd == 'a') {
        float bias = 0.0f;
        if (strlen(input_buffer) > 2) bias = atof(input_buffer + 2);
        Routine_Amperometric(bias);
    }
    else if (cmd == 'l') {
        // l <start> <end> <steps> <duration>
        float start = -500.0f, end = 500.0f;
        int steps = 100, duration = 10000;
        if (strlen(input_buffer) > 2) sscanf(input_buffer + 2, "%f %f %d %d", &start, &end, &steps, &duration);
        Routine_LSV(start, end, steps, duration);
    }
    else if (cmd == 'x') {
        if (CurrentMode == MODE_IMPEDANCE) AppIMPCleanup();
        else if (CurrentMode == MODE_AMPEROMETRIC) AppAMPCtrl(AMPCTRL_SHUTDOWN, 0);
        else if (CurrentMode == MODE_RAMP) AppRAMPCtrl(APPCTRL_SHUTDOWN, 0);
        CurrentMode = MODE_IDLE;
        printf("STOPPED\n");
    }
    else if (cmd == 'z') {
        watchdog_reboot(0, 0, 0);
    }
}

int main() {
    stdio_init_all();
    sleep_ms(2000);
    
    setup_pins();
    AD5940PlatformCfg();
    AD5940ImpedanceStructInit();
    AD5940AMPStructInit();
    AD5940RampStructInit();
    
    Routine_CalibrateLFO();
    
    printf("SYSTEM_READY\n");

    while (true) {
        int c = getchar_timeout_us(0);
        if (c != PICO_ERROR_TIMEOUT) {
            if (c == '\n' || c == '\r') {
                input_buffer[input_pos] = 0;
                if (input_pos > 0) process_command();
                input_pos = 0;
            } else if (input_pos < 63) {
                input_buffer[input_pos++] = (char)c;
            }
        }

        if (gpio_get(PIN_INT) == 0) {
            uint32_t temp = APPBUFF_SIZE;
            int32_t status = 0;
            
            if (CurrentMode == MODE_IMPEDANCE) {
                status = AppIMPISR(AppBuff, &temp);
                if (status == AD5940ERR_FIFO) {
                    printf("ERROR: FIFO Overflow/Underflow. Stopping.\n");
                    AppIMPCleanup();
                    CurrentMode = MODE_IDLE;
                } else if(temp > 0) {
                    ImpedanceShowResult(AppBuff, temp);
                }
            } 
            else if (CurrentMode == MODE_AMPEROMETRIC) {
                status = AppAMPISR(AppBuff, &temp);
                if (status == AD5940ERR_FIFO) {
                    printf("ERROR: FIFO Overflow/Underflow. Stopping.\n");
                    AppAMPCtrl(AMPCTRL_SHUTDOWN, 0);
                    CurrentMode = MODE_IDLE;
                } else if(temp > 0) {
                    AmperometricShowResult((float*)AppBuff, temp);
                }
            }
            else if (CurrentMode == MODE_RAMP) {
                status = AppRAMPISR(AppBuff, &temp);
                if (status == AD5940ERR_FIFO) {
                    printf("ERROR: FIFO Overflow/Underflow. Stopping.\n");
                    AppRAMPCtrl(APPCTRL_SHUTDOWN, 0);
                    CurrentMode = MODE_IDLE;
                } else if(temp > 0) {
                    RampShowResult((float*)AppBuff, temp);
                }
            }
            
            if (status == AD5940ERR_STOP) {
                printf("STOPPED\n");
                if (CurrentMode == MODE_IMPEDANCE) AppIMPCleanup();
                else if (CurrentMode == MODE_AMPEROMETRIC) AppAMPCtrl(AMPCTRL_SHUTDOWN, 0);
                else if (CurrentMode == MODE_RAMP) AppRAMPCtrl(APPCTRL_SHUTDOWN, 0);
                CurrentMode = MODE_IDLE;
            }
        }
    }
    return 0;
}