/**
 * @file ad5941_library_extension.cpp
 * @brief For connecting teensy microcontroller to ad5941 via SPI with interrupt functionality
 * @author MK, Aly Development
 * @date 09/30/2025, last modified
 *
 * @details
 *  Teensy 4.0 extension for the AD5941/AD5940 AFE (Analog Front End) library
 *         from Analog Devices Inc.
 *
 *         Provides hardware interface functions and
 *         application-specific measurement routines for conductivity
 *         and RTD temperature sensing.
 *
 * **** Note: Any application of this software should be thoroughly tested and validated ****
 *
 * Adapted and expanded from: https://github.com/analogdevicesinc/ad5940-examples
 * Note impedance.c modified for jitter
 */

// Include guard
#ifndef ad5940_library_extension_CPP
#define ad5940_library_extension_CPP

// Include dependencies
#include "ad5941_library_extension.h"
#include "DDTeensyDefs.h"

extern "C"
{
#include "ad5940.h"
#include "impedance.h"
}
#include <SPI.h>

// Stores the instance of currently connected / detected Serial port
Stream *activeSerial = nullptr;

Stream *serialPorts[] = 
{
    &Serial,  // USB virtual serial
    &Serial1, // pins 0, 1
    &Serial2, // pins 7, 8
    &Serial3, // pins 14, 15
    &Serial4, // pins 16, 17
    &Serial5, // pins 21, 20
    &Serial6, // pins 24, 25
    &Serial7  // pins 28, 29
};

const char *serialNames[] = 
{
    "Serial (USB)",
    "Serial1 (pins 0,1)",
    "Serial2 (pins 7,8)",
    "Serial3 (pins 14,15)",
    "Serial4 (pins 16,17)",
    "Serial5 (pins 21,20)",
    "Serial6 (pins 24,25)",
    "Serial7 (pins 28,29)"
  };

static const char *cfgSettingsParam[] =
 {
    "sinfreq",
    "dacpp",
    "bias",
    "rtia",
    "pga",
    "dftnum",
    "avgnum"
  };

uint32_t AppBuff[APPBUFF_SIZE];

// Arrays to store up to 5 measurements
float magnitudeArray[repeatNumber]; // Array to hold magnitude values (in Ohms)
float phaseArray[repeatNumber];     // Array to hold phase values (in degrees)
float storedFrequency;              // Variable to store the measurement frequency

volatile static uint32_t ucInterrupted = 0; /* Flag to indicate interrupt occurred */

int currentTIA = DEFAULT_RTIA;

int verboseMode = 0;

unsigned long previousMillis = 0; // Store last measurement time
const long jitterTimeout = 1500;  // 1.5 seconds in milliseconds if it's been longer than this many mSec since last measurement, we assume discard first {numSettlingMeasurements} measurement as it may have startup jitter.

// SPI Settings for Teensy 4.0
static const SPISettings SPISettings(SPI_SPEED, MSBFIRST, SPI_MODE0);

static bool sendSensorData = false;

// Configuration storage structure
struct SavedConfig
{
  float SinFreq;
  float DacVoltPP;
  float BiasVolt;
  uint32_t HstiaRtiaSel;
  uint32_t AdcPgaGain;
  uint32_t DftNum;
  uint32_t ADCAvgNum;
  struct
  {
    BoolFlag SweepEn;
    float SweepStart;
    float SweepStop;
    int SweepPoints;
    BoolFlag SweepLog;
  } SweepCfg;
};

// Enum to track unit status
enum UnitStatus
{
  STATUS_SKIPPED = 0, // Unit was never attempted to be initialized
  STATUS_FAILED = 1,  // Unit was attempted but failed initialization
  STATUS_SUCCESS = 2  // Unit was successfully initialized
};

// Array to keep track of unit status (indexed 0-5 for units 1-6)
// Initialize all to skipped by default
UnitStatus unitStatus[6] = {
    STATUS_SKIPPED, STATUS_SKIPPED, STATUS_SKIPPED,
    STATUS_SKIPPED, STATUS_SKIPPED, STATUS_SKIPPED};

static bool isInitialized;                                      // Set to true if the initialization function has been completely executed atleast once.
static Init_Status initStatus;                                  // Stores the phase of initialization
static sensorDataPacketStruct sensorPacket[MAX_NUM_OF_SENSORS]; // Stores Impedance and RTD measurements for all the sensors.
static sensorDataPacketStruct singleSensorPacket;               // Stores Impedance and RTD measurements for only singularly selected sensor.
static eepromDataPacketStruct eepromDataPacket;                 // Stores all the eeprom data
static measurementSettingsStruct measurementSettingsPacket;     //  Stores all the conductivity measurement settings
static measurementSettingsStruct tempSettings;                  // Stores received measurement settings to be updated.
static Update_cfg_Status updateCfgStatus[MAX_CONDUCTIVITY_MST_PARAM_IDX];
static Update_EEPROM_Status updateEepromStatus = UPDATE_EEPROM_STATUS_INVALID_CMD;

// Global variable to store the last known good configuration
SavedConfig lastConfig;
int propagateSettingsChanges = 0;
int TEENSY_RESET_PIN = 17; // Reset pin for AD5940
int AD5940_INT_PIN = 18;   // Interrupt pin from AD5940
float rtdVoltageValue = DEFAULT_RTD_VALUE;
int outputNow = 0;

/******************************************************************************
 * @brief Method to deselect the chip select function of the AD5940
 *****************************************************************************/
void AD5940_CsSet(void)
{
  digitalWrite(TEENSY_SPI_CS_PIN, HIGH);
  delayMicroseconds(10); // this is necessary to keep things from switching too fast -MK
}

/******************************************************************************
 * @brief Method to select the chip select function of the AD5940
 *****************************************************************************/
void AD5940_CsClr(void)
{
  digitalWrite(TEENSY_SPI_CS_PIN, LOW);
  delayMicroseconds(10); // this is necessary to keep things from switching too fast -MK
}

/******************************************************************************
 * @brief Method to reset the AD5940
 *****************************************************************************/
void AD5940_RstSet(void)
{
  digitalWrite(TEENSY_RESET_PIN, HIGH);
}

/******************************************************************************
 * @brief Method to clear the reset on the AD5940
 *****************************************************************************/
void AD5940_RstClr(void)
{
  digitalWrite(TEENSY_RESET_PIN, LOW);
}

/******************************************************************************
 * @brief Method for delaying the AD5940 for a multiple time of 10 micro seconds
 *
 * @param time: multiplier which defines the amount of times the AD5940 should
 * be delayed by 10 micro seconds
 *****************************************************************************/
void AD5940_Delay10us(uint32_t iTime)
{
  if (iTime < 1638)
  {
    delayMicroseconds(iTime * 10);
  }
  else
  {
    uint32_t iTimeDelayMicro = iTime % 1000;
    uint32_t iTimeDelay = iTime - iTimeDelayMicro;
    delay(iTimeDelay / 100);
    delayMicroseconds(iTimeDelayMicro * 10);
  }
}

/******************************************************************************
 * @brief Method to write and read data from the SPI port
 * @details: Modified for Teensy 4.0's SPI implementation
 * System clock frequency for the AD5940 remains at 16 MHz.
 * System clock frequency for Teensy 4.0 is 600 MHz.
 *
 * @param pSendBuffer: Send buffer holding all data that should be sent via SPI
 * @param pRecvBuff: Receive buffer storing all data transmitted by the AD5940
 * @param length: Length of transmitted data
 *****************************************************************************/
void AD5940_ReadWriteNBytes(unsigned char *pSendBuffer,
                            unsigned char *pRecvBuff,
                            unsigned long length)
{
  SPI.beginTransaction(SPISettings);

  for (unsigned long i = 0; i < length; i++)
  {
    *pRecvBuff++ = SPI.transfer(*pSendBuffer++);
    delayMicroseconds(10); // Add small delay between bytes
  }

  SPI.endTransaction();
  delayMicroseconds(10); // Add delay after transaction
}

/******************************************************************************
 * @brief Method to get the MCU interrupt flag status
 * @return uint32_t Returns the current state of the interrupt flag
 *****************************************************************************/
uint32_t AD5940_GetMCUIntFlag(void)
{
  return ucInterrupted;
}

/******************************************************************************
 * @brief Method to clear the MCU interrupt flag
 * @details Clears both the interrupt flag variable and any pending hardware
 * interrupt flags on the Teensy 4.0
 * @return uint32_t Returns 1 after successfully clearing the flag
 *****************************************************************************/
uint32_t AD5940_ClrMCUIntFlag(void)
{
  ucInterrupted = 0;
#ifdef triggDiag
  activeSerial->println("clear trigger");
#endif

  return 1;
}

/******************************************************************************
 * @brief error reporting function; only if ADI_DEBUG enabled in ad5940.h`
 * @details note must update in ad5940.h to set debug method output
 * see:
 * #define ADI_DEBUG
 * #ifdef ADI_DEBUG
 * #define ADI_Print canary
 * #endif
 *****************************************************************************/
void canary(const char *format, ...)
{
  if (verboseMode)
  {                   // only print in verbose mode
    char buffer[256]; // Buffer for formatted string
    va_list args;
    va_start(args, format);

    // Format the string
    vsnprintf(buffer, sizeof(buffer), format, args);

    // Print to serial with canary prefix
    // activeSerial->print("🐤 "); // I commented this out, but it was funnier to have the canary function with this -MK
    activeSerial->println(buffer);

    va_end(args);

    // Force flush the serial buffer
    activeSerial->flush();
  }
}

/******************************************************************************
 * @brief simple interupt handler
 *****************************************************************************/
void AD5940_InterruptHandler()
{
  ucInterrupted = 1;
#ifdef triggDiag
  activeSerial->println("triggered");
#endif
}

/******************************************************************************
 * APPLICATION SPECIFIC FUNCTIONS
 *
 * The following functions extend the base AD5940 library with application-
 * specific implementations for impedance measurement, RTD sensing, and
 * system configuration management.
 *****************************************************************************/

/**
 * @brief convert mV to WgCode for setting on HSDAC
 *
 * Double check this function before any critical deployment. It was tested and matched emperically for 25mV, but should be verified for more complex usages as needed.
 *
 * @return uint16_t - WgCode for HSDAC
 */
uint16_t mV_to_WgCode(float mV) //
{
  const float VDAC_MIN = 0.0f;  // 0 V
  const float VDAC_MAX = 1.82f; // full-scale HSDAC
  const float VBIAS = 0;

  float Vs = mV * 1e-3f;            // V
  float Vdac = (Vs + VBIAS) * 0.5f; // V

  if (Vdac < VDAC_MIN)
    Vdac = VDAC_MIN;
  if (Vdac > VDAC_MAX)
    Vdac = VDAC_MAX;

  return (uint16_t)(Vdac * 4095.0f / 1.82f + 0.5f);
}

uint16_t mV_to_WgCodeUpdate(float mV)
{
  const float VDAC_MIN = 0.0f;  // 0 V
  const float VDAC_MAX = 1.82f; // full-scale HSDAC (1.82V reference)

  // Convert mV to volts
  float Vs = mV * 1e-3f;

  // For RTD excitation, we typically want the DAC voltage to directly
  // correspond to the excitation voltage (no 0.5x factor)
  float Vdac = Vs;

  // Handle negative voltages by using the bipolar capability
  // The AD5940 can do bipolar output around a bias point
  if (Vs < 0)
  {
    // For negative voltages, we might need to use a different approach
    // depending on your circuit configuration
    Vdac = 0.91f + Vs; // 0.91V is mid-scale for 1.82V reference
  }
  else
  {
    Vdac = 0.91f + Vs; // Positive voltages above mid-scale
  }

  // Clamp to valid DAC range
  if (Vdac < VDAC_MIN)
    Vdac = VDAC_MIN;
  if (Vdac > VDAC_MAX)
    Vdac = VDAC_MAX;

  // Convert to 12-bit DAC code (0-4095)
  uint16_t code = (uint16_t)(Vdac * 4095.0f / 1.82f + 0.5f);

  return code;
}

/**
 * @brief sample from ADC with delay
 *
 * Double check this function before any critical deployment. It was tested and matched emperically for 25mV, but should be verified for more complex usages as needed.
 *
 * @return uint32_t code, result code from ADC
 */
static inline uint32_t sampleADC(uint32_t t10us)
{
  AD5940_AFECtrlS(AFECTRL_ADCCNV, bTRUE);
  AD5940_Delay10us(t10us);
  uint32_t code = AD5940_ReadAfeResult(AFERESULT_SINC2);
  AD5940_AFECtrlS(AFECTRL_ADCCNV, bFALSE);
  // activeSerial->println(code);
  return code;
}

/**
 * @brief Improved RTD measurement function with better error handling and stability
 *
 * Key improvements:
 * - Better settling time management
 * - More robust ADC sampling
 * - Improved error handling
 * - Consistent switch matrix configuration
 */
float AppRTDMeasure(float sensor_mV)
{
  AFERefCfg_Type aferef_cfg;
  HSLoopCfg_Type HsLoopCfg;
  DSPCfg_Type dsp_cfg;

  uint32_t adcCode_rtd, adcCode_ref;
  float volt_rtd, volt_ref;
  float rtd_resistance;
  const float RtdRefRes = REF_RESISTOR_VALUE;

  // Wake up the AD5940 from low power mode
  if (AD5940_WakeUp(10) > 10)
  {
    // activeSerial->println("ERROR: AD5940 wakeup timeout");
    return -1000.0f;
  }

  // Reset LPDAC configuration before RTD measurement
  LPDACCfg_Type lpdac_reset = {0};
  lpdac_reset.PowerEn = bFALSE;
  AD5940_LPDACCfgS(&lpdac_reset);
  // no longer change ref subsystem between RTD and conductivity measurements -MK
  // // Configure reference subsystem
  // 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 = bTRUE;
  // aferef_cfg.Lp1V8BuffEn = bTRUE;
  // aferef_cfg.LpBandgapEn = bFALSE;
  // aferef_cfg.LpRefBufEn = bFALSE;
  // aferef_cfg.LpRefBoostEn = bFALSE;
  // AD5940_REFCfgS(&aferef_cfg);

  // Configure High-Speed Loop
  HsLoopCfg.HsDacCfg.ExcitBufGain = EXCITBUFGAIN_2;
  HsLoopCfg.HsDacCfg.HsDacGain = HSDACGAIN_1;
  HsLoopCfg.HsDacCfg.HsDacUpdateRate = 7;

  // TIA settings
  HsLoopCfg.HsTiaCfg.DiodeClose = bFALSE;
  HsLoopCfg.HsTiaCfg.HstiaBias = HSTIABIAS_1P1;
  HsLoopCfg.HsTiaCfg.HstiaCtia = 32;
  HsLoopCfg.HsTiaCfg.HstiaDeRload = HSTIADERLOAD_OPEN;
  HsLoopCfg.HsTiaCfg.HstiaDeRtia = HSTIADERTIA_OPEN;
  HsLoopCfg.HsTiaCfg.HstiaRtiaSel = RTD_RTIA;

  // Switch matrix for RTD measurement
  HsLoopCfg.SWMatCfg.Dswitch = RTD_DSWITCH;
  HsLoopCfg.SWMatCfg.Pswitch = RTD_PSWITCH;
  HsLoopCfg.SWMatCfg.Nswitch = RTD_NSWITCH;
  HsLoopCfg.SWMatCfg.Tswitch = RTD_TSWITCH;

  // Configure waveform generator
  HsLoopCfg.WgCfg.WgType = WGTYPE_MMR;
  HsLoopCfg.WgCfg.GainCalEn = bFALSE;
  HsLoopCfg.WgCfg.OffsetCalEn = bFALSE;
  uint32_t code = mV_to_WgCode(sensor_mV);
  HsLoopCfg.WgCfg.WgCode = code;

  // Apply configuration twice (didn't seem to stick unless we did)
  AD5940_HSLoopCfgS(&HsLoopCfg);
  AD5940_HSLoopCfgS(&HsLoopCfg);

  // Configure ADC
  dsp_cfg.ADCBaseCfg.ADCMuxN = ADCMUXN_HSTIA_N;
  dsp_cfg.ADCBaseCfg.ADCMuxP = ADCMUXP_HSTIA_P;
  dsp_cfg.ADCBaseCfg.ADCPga = ADCPGA_1;

  dsp_cfg.ADCFilterCfg.ADCAvgNum = ADCAVGNUM_16;
  dsp_cfg.ADCFilterCfg.ADCRate = ADCRATE_800KHZ;
  dsp_cfg.ADCFilterCfg.ADCSinc2Osr = ADCSINC2OSR_44;
  dsp_cfg.ADCFilterCfg.ADCSinc3Osr = ADCSINC3OSR_4;
  dsp_cfg.ADCFilterCfg.BpNotch = bTRUE;
  dsp_cfg.ADCFilterCfg.BpSinc3 = bTRUE;
  dsp_cfg.ADCFilterCfg.Sinc2NotchEnable = bTRUE;

  AD5940_DSPCfgS(&dsp_cfg);

  // Enable AFE blocks
  AD5940_AFECtrlS(AFECTRL_HSTIAPWR | AFECTRL_INAMPPWR | AFECTRL_EXTBUFPWR | AFECTRL_WG | AFECTRL_DACREFPWR | AFECTRL_HSDACPWR | AFECTRL_SINC2NOTCH | AFECTRL_ADCPWR,
                  bTRUE);

  // Restart waveform generator with proper sequence
  AD5940_AFECtrlS(AFECTRL_WG, bFALSE);
  AD5940_Delay10us(1000); // 10ms settling
  AD5940_AFECtrlS(AFECTRL_WG, bTRUE);

  // Extended settling time for RTD measurement
  AD5940_Delay10us(500); // 5ms settling time

  // Sample RTD with improved averaging
  sampleADC(1);
  uint64_t accRTD = 0;
  for (uint32_t i = 0; i < NUM_RTD_SAMPLES; ++i)
  {
    accRTD += sampleADC(1); // Increased sampling delay
    // activeSerial->println(accRTD);
    AD5940_Delay10us(10); // Small delay between samples
  }
  adcCode_rtd = accRTD / NUM_RTD_SAMPLES;

  // Reconfigure switch matrix for reference resistor
  HsLoopCfg.SWMatCfg.Dswitch = SWD_RCAL0;
  HsLoopCfg.SWMatCfg.Pswitch = SWP_RCAL0;
  HsLoopCfg.SWMatCfg.Nswitch = SWN_RCAL1;
  HsLoopCfg.SWMatCfg.Tswitch = SWT_RCAL1 | SWT_TRTIA;

  // Apply new switch configuration
  AD5940_SWMatrixCfgS(&HsLoopCfg.SWMatCfg);

  // Additional settling time after switch change
  AD5940_Delay10us(500); // 5ms settling
  sampleADC(1);
  // Sample reference resistor
  uint64_t accREF = 0;
  for (uint32_t i = 0; i < NUM_RTD_SAMPLES; ++i)
  {
    accREF += sampleADC(1);
    AD5940_Delay10us(10);
  }
  adcCode_ref = accREF / NUM_RTD_SAMPLES;

  // reset switch matrix
  resetSwitchMatrix();

  // Disable all AFE blocks that were enabled
  AD5940_AFECtrlS(AFECTRL_HSTIAPWR | AFECTRL_INAMPPWR | AFECTRL_EXTBUFPWR | AFECTRL_WG | AFECTRL_DACREFPWR | AFECTRL_HSDACPWR | AFECTRL_SINC2NOTCH | AFECTRL_ADCPWR, bFALSE);

  // fullAD5940Reset();

  // Convert to voltages
  volt_ref = AD5940_ADCCode2Volt(adcCode_ref, ADCPGA_1, 1.82);
  volt_rtd = AD5940_ADCCode2Volt(adcCode_rtd, ADCPGA_1, 1.82);

  // // Validate measurements
  // if (volt_ref <= 0.0f || volt_rtd <= 0.0f) {
  //   activeSerial->println("ERROR: Invalid voltage measurements");
  //   return -1000.0f;
  // }

  // Calculate resistance
  rtd_resistance = RtdRefRes * (volt_ref / volt_rtd);

  // Verbose output
  if (verboseMode)
  { // shrinking this output to avoid being too long
    // activeSerial->println("");
    // activeSerial->println("verboseMode output");
    // activeSerial->print("sensor_mV");
    // activeSerial->println(sensor_mV);
    activeSerial->println(" ");

    activeSerial->println("RTD Measurement");
    activeSerial->print("  RCal Averaged Measurement: ");
    activeSerial->println(adcCode_ref);
    activeSerial->print("  RTD Averaged Measurement: ");
    activeSerial->println(adcCode_rtd);
    activeSerial->printf("  NUM_SAMPLES: %u", NUM_RTD_SAMPLES);
    activeSerial->println(" ");
    activeSerial->print("  RTD TIA Resistor: ");
    switch (RTD_RTIA)
    {
    case HSTIARTIA_200:
      activeSerial->println("200 Ohm");
      break;
    case HSTIARTIA_1K:
      activeSerial->println("1K Ohm");
      break;
    case HSTIARTIA_5K:
      activeSerial->println("5K Ohm");
      break;
    case HSTIARTIA_10K:
      activeSerial->println("10K Ohm");
      break;
    case HSTIARTIA_20K:
      activeSerial->println("20K Ohm");
      break;
    case HSTIARTIA_40K:
      activeSerial->println("40K Ohm");
      break;
    case HSTIARTIA_80K:
      activeSerial->println("80K Ohm");
      break;
    case HSTIARTIA_160K:
      activeSerial->println("160K Ohm");
      break;
    default:
      activeSerial->println("Unknown");
      break;
    }
    // activeSerial->print("volt_ref: ");
    // activeSerial->println(volt_ref, 6);  // More precision
    // activeSerial->print("volt_rtd: ");
    // activeSerial->println(volt_rtd, 6);
    // activeSerial->print("volt_ref / volt_rtd: ");
    // activeSerial->println(volt_ref / volt_rtd, 6);
    // activeSerial->print("Calculated RTD resistance: ");
    // activeSerial->println(rtd_resistance, 6);
  }

  // Display result
  activeSerial->print("Freq:0.0  1 RzMag: ");
  activeSerial->print(rtd_resistance, 5);
  activeSerial->println(" Ohm, RzPhase: 0.0");

  return rtd_resistance;
}

/**
 * @brief Resets the AD5940 switch matrix to its default state
 *
 * This function creates a clean configuration for the AD5940 switch matrix
 * by clearing all switch selections. It ensures that all internal connections
 * are opened before setting up a new configuration, preventing potential
 * conflicts or shorts between different signal paths.
 */
void resetSwitchMatrix()
{
  // Create a switch matrix configuration structure
  SWMatrixCfg_Type sw_cfg;

  // Clear all switch selections by setting them to 0
  sw_cfg.Dswitch = 0; // D switches (typically connected to excitation source)
  sw_cfg.Pswitch = 0; // P switches (typically connected to positive input)
  sw_cfg.Nswitch = 0; // N switches (typically connected to negative input)
  sw_cfg.Tswitch = 0; // T switches (typically connected to TIA feedback network)

  // Apply the configuration to the AD5940 hardware
  AD5940_SWMatrixCfgS(&sw_cfg);
}

// Function to save current configuration
void saveCurrentConfig()
{
  AppIMPCfg_Type *pImpedanceCfg;
  AppIMPGetCfg(&pImpedanceCfg);
  lastConfig.SinFreq = pImpedanceCfg->SinFreq;
  lastConfig.DacVoltPP = pImpedanceCfg->DacVoltPP;
  lastConfig.BiasVolt = pImpedanceCfg->BiasVolt;
  lastConfig.HstiaRtiaSel = pImpedanceCfg->HstiaRtiaSel;
  lastConfig.AdcPgaGain = pImpedanceCfg->AdcPgaGain;
  lastConfig.DftNum = pImpedanceCfg->DftNum;
  lastConfig.ADCAvgNum = pImpedanceCfg->ADCAvgNum;
  lastConfig.SweepCfg.SweepEn = pImpedanceCfg->SweepCfg.SweepEn;
  lastConfig.SweepCfg.SweepStart = pImpedanceCfg->SweepCfg.SweepStart;
  lastConfig.SweepCfg.SweepStop = pImpedanceCfg->SweepCfg.SweepStop;
  lastConfig.SweepCfg.SweepPoints = pImpedanceCfg->SweepCfg.SweepPoints;
  lastConfig.SweepCfg.SweepLog = pImpedanceCfg->SweepCfg.SweepLog;
}

// Function to recall saved configuration
void recallSavedConfig()
{
  AppIMPCfg_Type *pImpedanceCfg;
  AppIMPGetCfg(&pImpedanceCfg);
  pImpedanceCfg->SinFreq = lastConfig.SinFreq;
  pImpedanceCfg->DacVoltPP = lastConfig.DacVoltPP;
  pImpedanceCfg->BiasVolt = lastConfig.BiasVolt;
  pImpedanceCfg->HstiaRtiaSel = lastConfig.HstiaRtiaSel;
  pImpedanceCfg->AdcPgaGain = lastConfig.AdcPgaGain;
  pImpedanceCfg->DftNum = lastConfig.DftNum;
  pImpedanceCfg->ADCAvgNum = lastConfig.ADCAvgNum;
  pImpedanceCfg->SweepCfg.SweepEn = lastConfig.SweepCfg.SweepEn;
  pImpedanceCfg->SweepCfg.SweepStart = lastConfig.SweepCfg.SweepStart;
  pImpedanceCfg->SweepCfg.SweepStop = lastConfig.SweepCfg.SweepStop;
  pImpedanceCfg->SweepCfg.SweepPoints = lastConfig.SweepCfg.SweepPoints;
  pImpedanceCfg->SweepCfg.SweepLog = lastConfig.SweepCfg.SweepLog;
  pImpedanceCfg->bParaChanged = bTRUE;
}

/**
 * @brief Handles configuration commands sent via serial interface
 *
 * This function processes configuration commands in CSV format:
 * cfg,parameter,value
 *
 * It allows users to modify impedance measurement parameters,
 * save configurations, and recall saved configurations.
 *
 * @param cmd String containing the configuration command
 */
void handleConfigCommand(String cmd)
{
  // Command format: cfg,parameter,value
  // Examples:
  // - cfg,sinfreq,1000.0    (Set sine wave frequency to 1000 Hz)

  // Array to hold parsed command parts
  String params[3];
  int paramIndex = 0;
  int startIndex = 0;

  // Parse command string using comma delimiter
  for (uint i = 0; i < cmd.length(); i++)
  {
    if (cmd.charAt(i) == ',')
    {
      params[paramIndex] = cmd.substring(startIndex, i);
      startIndex = i + 1;
      paramIndex++;

      // Safety check to prevent array overflow
      if (paramIndex >= 3)
        break;
    }
  }

  // Get last parameter (after the last comma)
  if (paramIndex < 3)
  {
    params[paramIndex] = cmd.substring(startIndex);
  }

  // Get configuration structure pointer
  AppIMPCfg_Type *pImpedanceCfg;
  AppIMPGetCfg(&pImpedanceCfg);

  // Extract parameter name and value
  String param = params[1];
  String value = params[2];

  // Handle special commands for saving/recalling configurations
  if (param == "save")
  {
    saveCurrentConfig();
    activeSerial->println("Configuration saved");
    return;
  }
  else if (param == "recall")
  {
    recallSavedConfig();
    activeSerial->println("Configuration recalled");
    return;
  }

  // Process different configuration parameters
  if (param == "sinfreq")
  {
    // Set sine wave frequency (Hz)
    pImpedanceCfg->SinFreq = value.toFloat();
    activeSerial->printf("Set SinFreq to: %.2f Hz\n", pImpedanceCfg->SinFreq);
  }
  else if (param == "dacpp")
  {
    // Set DAC peak-to-peak voltage (mV)
    pImpedanceCfg->DacVoltPP = value.toFloat();
    activeSerial->printf("Set DacVoltPP to: %.2f mV\n", pImpedanceCfg->DacVoltPP);
  }
  else if (param == "bias")
  {
    // Set DC bias voltage (mV)
    pImpedanceCfg->BiasVolt = value.toFloat();
    activeSerial->printf("Set BiasVolt to: %.2f mV\n", pImpedanceCfg->BiasVolt);
  }
  else if (param == "rtia")
  {
    // Set TIA (Transimpedance Amplifier) feedback resistor value
    uint32_t rtia = DEFAULT_RTIA; // default value
    int valueTest = value.toInt();
    activeSerial->print("Value:");
    activeSerial->println(value);
    // Map string values to corresponding constants
    if (valueTest == 0)
      rtia = HSTIARTIA_200;
    else if (valueTest == 1)
      rtia = HSTIARTIA_1K;
    else if (valueTest == 2)
      rtia = HSTIARTIA_5K;
    else if (valueTest == 3)
      rtia = HSTIARTIA_10K;
    else if (valueTest == 4)
      rtia = HSTIARTIA_20K;
    else if (valueTest == 5)
      rtia = HSTIARTIA_40K;
    else if (valueTest == 6)
      rtia = HSTIARTIA_80K;
    else if (valueTest == 7)
      rtia = HSTIARTIA_160K;
    else
      activeSerial->print("no change to tia");

    pImpedanceCfg->HstiaRtiaSel = rtia;
    activeSerial->printf("Set RTIA to: %s\n", value.c_str());
    currentTIA = rtia;
    activeSerial->print("currentTIA: ");
    activeSerial->println(currentTIA);
  }
  else if (param == "pga")
  {
    // Set ADC Programmable Gain Amplifier gain
    uint32_t pga = ADCPGA_1; // default value (1x gain)

    // Map string values to corresponding constants
    if (value == "1")
      pga = ADCPGA_1;
    else if (value == "1.5")
      pga = ADCPGA_1P5;
    else if (value == "2")
      pga = ADCPGA_2;
    else if (value == "4")
      pga = ADCPGA_4;
    else if (value == "9")
      pga = ADCPGA_9;

    pImpedanceCfg->AdcPgaGain = pga;
    activeSerial->printf("Set PGA gain to: %s\n", value.c_str());
  }
  else if (param == "dftnum")
  {
    // Set DFT (Discrete Fourier Transform) number of points
    uint32_t dft = DFTNUM_4096; // default value (4096 points)

    // Map string values to corresponding constants
    if (value == "4096")
      dft = DFTNUM_4096;
    else if (value == "2048")
      dft = DFTNUM_2048;
    else if (value == "1024")
      dft = DFTNUM_1024;
    else if (value == "512")
      dft = DFTNUM_512;
    else if (value == "256")
      dft = DFTNUM_256;

    pImpedanceCfg->DftNum = dft;
    activeSerial->printf("Set DFT number to: %s\n", value.c_str());
  }
  else if (param == "avgnum")
  {
    // Set ADC averaging number (how many samples to average)
    uint32_t avg = ADCAVGNUM_16; // default value (16 samples)

    // Map string values to corresponding constants
    if (value == "2")
      avg = ADCAVGNUM_2;
    else if (value == "4")
      avg = ADCAVGNUM_4;
    else if (value == "8")
      avg = ADCAVGNUM_8;
    else if (value == "16")
      avg = ADCAVGNUM_16;

    pImpedanceCfg->ADCAvgNum = avg;
    activeSerial->printf("Set ADC average number to: %s\n", value.c_str());
  }
  else if (param == "sweep")
  {
    // Enable/disable frequency sweep mode
    bool enableSweep = (value == "1" || value.equalsIgnoreCase("true"));
    pImpedanceCfg->SweepCfg.SweepEn = enableSweep ? bTRUE : bFALSE;
    activeSerial->printf("Set sweep enable to: %d\n", pImpedanceCfg->SweepCfg.SweepEn);
  }
  else if (param == "sweepstart")
  {
    // Set sweep start frequency (Hz)
    pImpedanceCfg->SweepCfg.SweepStart = value.toFloat();
    activeSerial->printf("Set sweep start frequency to: %.2f Hz\n", pImpedanceCfg->SweepCfg.SweepStart);
  }
  else if (param == "sweepstop")
  {
    // Set sweep stop frequency (Hz)
    pImpedanceCfg->SweepCfg.SweepStop = value.toFloat();
    activeSerial->printf("Set sweep stop frequency to: %.2f Hz\n", pImpedanceCfg->SweepCfg.SweepStop);
  }
  else if (param == "sweeppoints")
  {
    // Set number of frequency points in the sweep
    pImpedanceCfg->SweepCfg.SweepPoints = value.toInt();
    activeSerial->printf("Set sweep points to: %d\n", pImpedanceCfg->SweepCfg.SweepPoints);
  }
  else if (param == "sweeplog")
  {
    // Set logarithmic (true) or linear (false) frequency spacing
    bool logSpacing = (value == "1" || value.equalsIgnoreCase("true"));
    pImpedanceCfg->SweepCfg.SweepLog = logSpacing ? bTRUE : bFALSE;
    activeSerial->printf("Set sweep log mode to: %d\n", pImpedanceCfg->SweepCfg.SweepLog);
  }
  else
  {
    // Handle unrecognized parameter
    activeSerial->println("Malformed command: unrecognized parameter");
    return;
  }

  // Mark that parameters have been changed and need to be applied
  pImpedanceCfg->bParaChanged = bTRUE;

  // Automatically save the configuration after any successful change
  saveCurrentConfig();
}

/**
 * @brief Displays impedance measurement results
 *
 * This function formats and outputs impedance measurement data to the serial port.
 * It retrieves the current measurement frequency and prints the magnitude and phase
 * of each impedance measurement in the data array.
 * changed to enable just outputting every Nth result for debugging
 *
 * @param pData Pointer to array of impedance data (as fImpPol_Type structures)
 * @param DataCount Number of impedance measurements in the array
 */
void ImpedanceShowResult(uint32_t *pData, uint32_t DataCount, int readingCount)
{
  // Variable to store measurement frequency
  float freq;
  // Cast raw data pointer to impedance polar format type
  fImpPol_Type *pImp = (fImpPol_Type *)pData;

  // Get the current measurement frequency
  AppIMPCtrl(IMPCTRL_GETFREQ, &freq);

  // Print measurement frequency
  // activeSerial->printf("Freq: %.2f Hz ", freq);

  // Print the number of data points
  // activeSerial->printf("DataPoints: %lu ", DataCount);

  // // Process and print each impedance measurement
  // for (uint32_t i = 0; i < DataCount; i++) {
  //   // Print magnitude in ohms and phase in degrees
  //   // Note: Converting phase from radians to degrees (phase * 180 / π)
  //   activeSerial->printf("RzMag: %.2f Ohm, RzPhase: %.2f deg",
  //                 pImp[i].Magnitude,
  //                 pImp[i].Phase * 180 / MATH_PI);

  //   // Add separator between multiple measurements if needed
  //   if (i < DataCount - 1) {
  //     activeSerial->print(", ");
  //   }
  // }
  storedFrequency = freq;
  magnitudeArray[readingCount] = pImp[0].Magnitude;
  phaseArray[readingCount] = pImp[0].Phase * 180 / MATH_PI;
  // Add newline for better readability in serial monitor
  // activeSerial->println();
}

/**
 * @brief Initializes the AD5940 impedance measurement configuration structure
 *
 * This function sets default values for the impedance measurement configuration
 * structure including sequence settings, frequency, switch matrix connections,
 * gain settings, filtering, and DFT parameters. It configures the AD5940 for
 * basic impedance measurements with appropriate default settings.
 */
void AD5940ImpedanceStructInit(void)
{
  // Pointer to the impedance configuration structure
  AppIMPCfg_Type *pImpedanceCfg;

  // Reset the switch matrix to default state
  resetSwitchMatrix();

  // Get pointer to the impedance configuration structure
  AppIMPGetCfg(&pImpedanceCfg);

  //---------- Sequence Configuration ----------
  // Set sequence memory allocation
  pImpedanceCfg->SeqStartAddr = 0; // Start address in SRAM
  pImpedanceCfg->MaxSeqLen = 512;  // Maximum sequence length

  //---------- Measurement Parameters ----------
  pImpedanceCfg->RcalVal = REF_RESISTOR_VALUE;     // calibration resistor
  pImpedanceCfg->SinFreq = DEFAULT_FREQ;           // 10kHz sine wave frequency
  pImpedanceCfg->FifoThresh = DEFAULT_FIFO_THRESH; // FIFO threshold for interrupt
  //---------- Switch Matrix Configuration ----------
  // Configure switch matrix connections for the measurement path
  // Connect AIN2 and AIN3 pins to the HSTIA (High-Speed TIA) inputs
  pImpedanceCfg->DswitchSel = DEFAULT_DSWITCH_CON; // D switch to AIN2
  pImpedanceCfg->PswitchSel = DEFAULT_PSWITCH_CON; // P switch to AIN2
  pImpedanceCfg->NswitchSel = DEFAULT_NSWITCH_CON; // N switch to AIN3
  pImpedanceCfg->TswitchSel = DEFAULT_TSWITCH_CON; // T switch to AIN3 and RTIA

  //---------- Gain and Filtering Configuration ----------
  // Set gain and filtering parameters for accurate measurements
  // Note: Using 200Ω RTIA to handle low impedance sensors without saturation
  pImpedanceCfg->HstiaRtiaSel = currentTIA; // 200Ω TIA feedback resistor
  pImpedanceCfg->ADCAvgNum = ADCAVGNUM_16;  // Average 16 ADC samples

  //---------- Sweep Configuration ----------
  // Disable frequency sweep by default (single frequency measurement)
  pImpedanceCfg->SweepCfg.SweepEn = bFALSE;

  //---------- Power Mode Configuration ----------
  // Set high-power mode for optimal performance at frequencies > 80kHz
  pImpedanceCfg->PwrMod = AFEPWR_HP; // High power mode

  //---------- Signal Processing Configuration ----------
  // Configure decimation filters and DFT parameters
  pImpedanceCfg->ADCSinc3Osr = ADCSINC3OSR_2; // Sinc3 filter decimation ratio: 2
                                              // (ADC sampling rate: 800kSPS/2 = 400kSPS)
  pImpedanceCfg->DftNum = DEFAULT_DFTNUM;     // 4096-point DFT
  pImpedanceCfg->DftSrc = DFTSRC_SINC3;       // DFT input from Sinc3 filter

  // Mark configuration as changed so it will be applied
  pImpedanceCfg->bParaChanged = bTRUE;

  // Debugging functions
  // printCurrentConfig(pImpedanceCfg);
  // printImpConfig();
}
/**
 * @brief Configures the AD5940 platform settings including clock, FIFO, interrupts, and GPIO
 *
 * This function performs the low-level platform configuration of the AD5940 AFE.
 * It sets up the system clocks, FIFO buffer, interrupt controller, and GPIO pins
 * required for proper operation of the measurement system.
 *
 * @return int32_t Returns 0 on success
 */
static int32_t AD5940PlatformCfg(void)
{
  // Configuration structures
  CLKCfg_Type clk_cfg;    // Clock configuration
  FIFOCfg_Type fifo_cfg;  // FIFO configuration
  AGPIOCfg_Type gpio_cfg; // GPIO configuration

  // Hardware reset is commented out as it's handled elsewhere
  // AD5940_HWReset();

  // Allow time for system stabilization
  delay(100);

  // Initialize SPI communication with AD5940
  SPI.begin();
  // activeSerial->println("chipRead:");
  uint32_t testResult = AD5940_ReadReg(REG_AFECON_CHIPID);
  // activeSerial->println(testResult);

  if (testResult == 0 || testResult == 65535)
  {
    // activeSerial->println("System Cannot detect AD5940/1");
    // activeSerial->println(testResult);
    SPI.end();
    return 1; // chip not present
  }
  // activeSerial->println(AD5940_ReadReg(REG_AFECON_CHIPID));
  // activeSerial->println("ENDchipRead");
  //  Initialize AD5940 core functions
  AD5940_Initialize();

  //---------- Clock Configuration ----------
  // Set up system and ADC clocks
  clk_cfg.ADCClkDiv = ADCCLKDIV_1;     // No division for ADC clock
  clk_cfg.ADCCLkSrc = ADCCLKSRC_HFOSC; // ADC clock from high-frequency oscillator
  clk_cfg.SysClkDiv = SYSCLKDIV_1;     // No division for system clock
  clk_cfg.SysClkSrc = SYSCLKSRC_HFOSC; // System clock from high-frequency oscillator
  clk_cfg.HfOSC32MHzMode = bFALSE;     // Use 16MHz mode for HFOSC
  clk_cfg.HFOSCEn = bTRUE;             // Enable high-frequency oscillator
  clk_cfg.HFXTALEn = bFALSE;           // Disable high-frequency crystal
  clk_cfg.LFOSCEn = bTRUE;             // Enable low-frequency oscillator

  // Apply clock configuration
  AD5940_CLKCfg(&clk_cfg);

  //---------- FIFO Configuration ----------
  // Configure FIFO buffer for measurement data
  fifo_cfg.FIFOEn = bFALSE;          // Disable FIFO during configuration
  fifo_cfg.FIFOMode = FIFOMODE_FIFO; // Use standard FIFO mode (not stream)
  fifo_cfg.FIFOSize = FIFOSIZE_4KB;  // Allocate 4KB for FIFO (2KB for sequencer)
  fifo_cfg.FIFOSrc = FIFOSRC_DFT;    // FIFO source is DFT results
  fifo_cfg.FIFOThresh = 4;           // Threshold for FIFO interrupt

  // Apply FIFO configuration
  AD5940_FIFOCfg(&fifo_cfg);

  // Enable FIFO after configuration
  fifo_cfg.FIFOEn = bTRUE;
  AD5940_FIFOCfg(&fifo_cfg);

  //---------- Interrupt Controller Configuration ----------
  // Configure interrupt controller for data acquisition

  // Enable all interrupts in INTC1 for debugging/status checking
  AD5940_INTCCfg(AFEINTC_1, AFEINTSRC_ADCMAXERR, bTRUE); // change to only adc max testing on intc1
  AD5940_INTCClrFlag(AFEINTSRC_ALLINT);

  // Configure INTC0 for FIFO threshold interrupt only
  AD5940_INTCCfg(AFEINTC_0, AFEINTSRC_DATAFIFOTHRESH, bTRUE);
  AD5940_INTCClrFlag(AFEINTSRC_ALLINT);

  //---------- GPIO Configuration ----------
  // Configure AD5940 GPIO pins for specific functions
  gpio_cfg.FuncSet = GP0_INT | GP1_SLEEP | GP2_SYNC;           // GP0: Interrupt, GP1: Sleep, GP2: Sync
  gpio_cfg.InputEnSet = 0;                                     // No GPIO as inputs
  gpio_cfg.OutputEnSet = AGPIO_Pin0 | AGPIO_Pin1 | AGPIO_Pin2; // Enable output on GP0-GP2
  gpio_cfg.OutVal = 0;                                         // Initial output values low
  gpio_cfg.PullEnSet = 0;                                      // No pull-up/down resistors

  // Apply GPIO configuration
  AD5940_AGPIOCfg(&gpio_cfg);

  // Unlock sleep mode to allow power management
  AD5940_SleepKeyCtrlS(SLPKEY_UNLOCK);

  return 0; // Return success
}

/**
 * @brief Initializes the hardware and software components of the measurement system
 *
 * This function performs the complete startup sequence required to initialize
 * the AD5940 AFE and the measurement system. It configures the GPIO pins,
 * resets the AD5940, sets up the interrupt handling, and initializes the
 * impedance measurement application.
 */
bool startupAD5941()
{
  // Initial delay to ensure power stabilization
  delay(100);

  //---------- GPIO Configuration ----------
  // Configure SPI chip select pin
  pinMode(TEENSY_SPI_CS_PIN, OUTPUT);
  digitalWrite(TEENSY_SPI_CS_PIN, HIGH); // Deselect AD5940 initially

  //---------- AD5940 Reset Sequence ----------
  // Configure reset pin
  pinMode(TEENSY_RESET_PIN, OUTPUT);
  digitalWrite(TEENSY_RESET_PIN, HIGH);

  // Hardware reset sequence for AD5940
  digitalWriteFast(TEENSY_RESET_PIN, HIGH);
  delay(1); // Wait for reset line to stabilize
  digitalWriteFast(TEENSY_RESET_PIN, LOW);
  delay(1); // Hold reset active
  digitalWriteFast(TEENSY_RESET_PIN, HIGH);
  delay(1); // Allow time for AD5940 to initialize

  // Ensure SPI chip select is properly configured
  pinMode(TEENSY_SPI_CS_PIN, OUTPUT);
  digitalWrite(TEENSY_SPI_CS_PIN, HIGH); // Deselect AD5940

  //---------- Interrupt Configuration ----------
  // Configure AD5940 interrupt pin with internal pull-up
  // (AD5940 interrupt is active-low)
  pinMode(AD5940_INT_PIN, INPUT_PULLUP);

  // Attach interrupt handler to respond to AD5940 interrupts
  attachInterrupt(digitalPinToInterrupt(AD5940_INT_PIN),
                  AD5940_InterruptHandler,
                  FALLING); // Trigger on falling edge

  //---------- AD5940 Initialization ----------
  // Initialize AD5940 platform-specific configurations
  // (SPI communication, timing, etc.)

  if (AD5940PlatformCfg())
  {
    return true; // startupfailed
  }

  // Initialize impedance measurement configuration structure
  // with default settings
  AD5940ImpedanceStructInit();
  // Initialize impedance measurement application
  // Provide buffer for storing sequencer commands
  AppIMPInit(AppBuff, APPBUFF_SIZE);

  // Ensure measurement system is in a known stopped state
  AppIMPCtrl(IMPCTRL_STOPSYNC, 0);
  resetSwitchMatrix(); // Disconnect all switches
  // Save the initial configuration
  saveCurrentConfig();
  return false;
  // Confirm initialization complete and display user information
  // activeSerial->println("Startup Complete");
  // printHelp();                // Show available commands
  // printCurrentConfigShort();  // Display current measurement settings
  // System is now ready for measurements
  // activeSerial->println("System ready; enter b to perform reading");
}

/**
 * @brief Performs a single impedance measurement and displays the result
 *
 * This function conducts a complete impedance measurement cycle using the AD5940.
 * It initializes the measurement buffer, starts the measurement, waits for
 * completion, processes the data, and displays the results.
 */
bool AppIMPMeasure(int readingCount)
{
  uint32_t bufferSize;                   // Variable to track valid data in buffer
  int trigCount = 0;                     // Counter for completed measurements
  unsigned long startTime;               // For timeout tracking
  const unsigned long TIMEOUT_MS = 5000; // 5 second timeout (adjust as needed)
  bool timeoutOccurred = false;
  resetSwitchMatrix();
  AppIMPInit(AppBuff, APPBUFF_SIZE);
  AD5940_EnableAdcMaxSaturationIRQ(/*max_code=*/maxValueThresholdADC, /*hysteresis=*/0x0080);

  // Initialize the buffer for impedance measurement

  // AppIMPCtrl(IMPCTRL_STOPNOW, 0);

  // // Stop any ongoing measurement before starting a new one
  // AppIMPCtrl(IMPCTRL_STOPSYNC, 0);

  // AD5940_Delay10us(5000);  // 50ms settling

  // Start the impedance measurement
  AppIMPCtrl(IMPCTRL_START, 0);

  // Record start time for timeout
  startTime = millis();
  // Wait for one measurement cycle to complete or timeout
  trigCount = 0;
  while (trigCount < 1)
  {
    // Check for timeout
    if (millis() - startTime > TIMEOUT_MS)
    {
      timeoutOccurred = true;
      break;
    }

    // Check if interrupt flag is set (measurement data ready)
    if (AD5940_GetMCUIntFlag() == 1)
    {
      // Clear the interrupt flag
      AD5940_ClrMCUIntFlag();

      // Set up buffer for data collection and process the measurement
      bufferSize = APPBUFF_SIZE;
      AppIMPISR(AppBuff, &bufferSize);
      // activeSerial->println("tset");

      // Display the impedance measurement results
      // bufferSize now contains the actual number of valid data points
      ImpedanceShowResult(AppBuff, bufferSize, readingCount);

      // Increment measurement counter
      trigCount++;
    }
  }

  // Stop the impedance measurement cleanly
  AppIMPCtrl(IMPCTRL_STOPSYNC, 0);

  // Handle timeout situation
  if (timeoutOccurred)
  {
    // activeSerial->println("ERROR: Impedance measurement timeout occurred!");
  }
  return timeoutOccurred;
}

/**
 * @brief Displays the current conductivity measurement configuration settings
 *
 * This function retrieves the current configuration settings from the
 * impedance measurement module and prints a summary of the key parameters
 * to the serial monitor. It shows the essential parameters related to
 * excitation signal and bias voltage.
 */
void printCurrentConfigShort()
{
  // Pointer to store configuration structure
  AppIMPCfg_Type *cfg;

  // Get the current impedance measurement configuration
  AppIMPGetCfg(&cfg);

  // Print header with separator
  activeSerial->println("------------------------");
  activeSerial->println("Current Conductivity Settings");

  // Print essential measurement parameters with units
  activeSerial->printf("SinFreq: %.2f Hz\n", cfg->SinFreq);     // Sine wave frequency
  activeSerial->printf("DacVoltPP: %.2f mV\n", cfg->DacVoltPP); // DAC peak-to-peak voltage
  activeSerial->printf("BiasVolt: %.2f mV\n", cfg->BiasVolt);   // DC bias voltage

  // Additional parameters (commented out for simplified display)
  // Uncomment these lines to display more detailed configuration
  activeSerial->printf("RTIA: %lu\n", cfg->HstiaRtiaSel);   // Transimpedance amplifier setting
  activeSerial->printf("PGA Gain: %lu\n", cfg->AdcPgaGain); // Programmable gain amplifier setting
  activeSerial->printf("DFT Points: %lu\n", cfg->DftNum);   // Number of DFT points
  activeSerial->printf("ADC Avg: %u\n", cfg->ADCAvgNum);    // ADC averaging setting

  // Print footer with separator
  activeSerial->println("------------------------");
}

/**
 * @brief Displays available commands and parameters to the serial monitor
 *
 * This function prints a formatted help menu with all available commands,
 * their descriptions, and configuration parameters to guide users on how
 * to control the measurement system via the serial interface.
 */
void printHelp()
{

  activeSerial->println("------------------");
  activeSerial->println("Version:");
  activeSerial->println(VERSION);
  activeSerial->println(AUTHOR);

  activeSerial->println("------------------");
  activeSerial->println("Available commands:");

  // Measurement and system control commands
  activeSerial->println("d - Perform measurement (Outputs: conductivity followed by temperature data,");
  activeSerial->println("    format: freq rzmag rzphase for each measurement separated by ;)");
  activeSerial->println("a - rerun initialize on all units");

  activeSerial->println("b - Perform measurement on all previously initialized units");

  activeSerial->println("");
  activeSerial->println("s - Rerun system setup and initialization");
  activeSerial->println("y - Print current conductivity measurement settings");
  activeSerial->println("r - Restart microcontroller");
  activeSerial->println("i - Blink LED on teensy to id unit");
  activeSerial->println("j # - select unit 1 through 6; ie 'j 1' selected unit 1");
  activeSerial->println("v - toggle verbose mode for rtd measurement");
  activeSerial->println("load - read eeprom values");
  activeSerial->println("save - save values to eeprom (will prompt for confirmation) ie: 'save, 45.34,3634.344,63232.32,6232.2' will save those values to eeprom for currently selected unit upon confirmation");
  activeSerial->println();
  activeSerial->println();
  // blink LED on teensy to id unit

  // Configuration command format and parameters
  activeSerial->println("cfg - Change settings for conductivity measurement");
  activeSerial->println("  cfg,<param>,<value> - Configure parameters for conductivity measurement");
  activeSerial->println("  Parameters:");
  activeSerial->println("    sinfreq - Sine wave frequency (Hz)");
  activeSerial->println("    dacpp   - DAC peak-to-peak voltage (mV, maximum: 800mVpp)");
  activeSerial->println("    bias    - DC bias voltage (mV, maximum: 2200mV)");

  activeSerial->println("------------------");
}

/**
 * @brief Reads and processes commands from the serial interface
 *
 * This function reads incoming commands from the serial port and processes them based
 * on the first character or prefix. It handles configuration commands, measurement
 * requests, help display, and system control functions.
 */
void readCommand()
{

  // Read a line from the serial port until newline character
  String command = activeSerial->readStringUntil('\n');

  // Handle configuration commands (format: cfg,parameter,value)
  if (command.startsWith("cfg,"))
  {
    handleConfigCommand(command);
    // activeSerial->println(command);
    // activeSerial->print("*");
    propagateSettingsChanges = 1;
    return;
  }

  if (command.startsWith("mst,"))
  {
    handleMstCommand(command);
    propagateSettingsChanges = 1;
    activeSerial->write((uint8_t *)&updateCfgStatus, sizeof(updateCfgStatus));
    return;
  }

  // Handle save command for doubles (format: save,value1,value2,value3,value4)
  if (command.startsWith("save,"))
  {
    handleSaveCommand(command);
    return;
  }

  // Handle save command for doubles (format: save,value1,value2,value3,value4)
  if (command.startsWith("upe,"))
  {
    updateEEPROMdata(command);
    activeSerial->write((uint8_t *)&updateEepromStatus, sizeof(updateEepromStatus));
    return;
  }

  // Handle load command for doubles (format: load)
  if (command.startsWith("load"))
  {
    handleLoadCommand();
    return;
  }

  // Get Single measurement for given sensor
  if (command.startsWith("h,"))
  {
    int firstComma = command.indexOf(',');
    if (firstComma > 0)
    {
      int unit = command.substring(firstComma + 1).toInt();

      if ((isSensorValid (unit)) && (isSensorInitialized (unit)))
      {
        // Select the sensor
        selectUnit(unit);
        //  Get single sensor data in binary format.
        getSelectedSensorMeasurements();
      }
      else
      {
        initSinglePacketToDefault();
        activeSerial->write((uint8_t *)&singleSensorPacket, sizeof(singleSensorPacket));
      }
    }
    return;
  }

  // Process commands based on the first character
  switch (command.charAt(0))
  {
  case '?': // Help command - display available commands
  {
    printHelp();
  }
  break;
  case 'a':
  {
    isInitialized = false;
    while (!initializeSelectedUnits())
      ;
    if (true == isInitialized)
    {
      getInitStatus();
    }
    selectFirstWorkingUnit();
    // activeSerial->print("*");
  }
  break;
  case '-':
  {
    // if (AD5940_IsAdcMaxSaturatedAndClear()) {
    //   activeSerial->println("SaturationFlag");
    // } else {
    //   activeSerial->println("noSat");
    // }
  }
  break;
  case '+':
  {
    // AD5940_EnableAdcMaxSaturationIRQ(/*max_code=*/0xFFFF, /*hysteresis=*/0x0080);
    activeSerial->println("rearm");
  }
  break;
  case 'b':
  {
    measureAllSuccessfulUnits();
    activeSerial->print("*");
  }
  break;
  case 'c':
  {
    // test case only
    // AppRTDMeasure(rtdVoltageValue);
    // AppIMPMeasure(0);
  }
  break;
  case 'd': // Perform impedance measurement followed by RTD measurement
  {

    // quick patch for fixing spontaneous measurements
    unsigned long currentMillis = millis();
    if (currentMillis - previousMillis >= jitterTimeout)
    {

      // Run multiple settling measurements in a loop
      for (int settling = 0; settling < numSettlingMeasurements; settling++)
      {
        if (settling == 0)
        {
          activeSerial->println("Performing settling measurements...");
        }
        if (verboseMode)
        {
          activeSerial->printf(" Settling Measurement#: %u ", settling);
          activeSerial->println(" ");
        }
        // Run one complete measurement cycle but discard results
        AppIMPInit(AppBuff, APPBUFF_SIZE);
        AppIMPCtrl(IMPCTRL_START, 0);

        // Wait for completion (simplified polling)
        int timeout_count = 0;
        while (AD5940_GetMCUIntFlag() != 1 && timeout_count < 1000)
        {
          delay(1);
          timeout_count++;
        }

        if (AD5940_GetMCUIntFlag() == 1)
        {
          AD5940_ClrMCUIntFlag();
          uint32_t dummy_size = APPBUFF_SIZE;
          AppIMPISR(AppBuff, &dummy_size); // Discard this data
        }

        AppIMPCtrl(IMPCTRL_STOPSYNC, 0);

        // Small delay between settling measurements
        if (settling < numSettlingMeasurements - 1)
        {
          AD5940_Delay10us(1000); // 10ms between measurements
        }
      }
      if (verboseMode)
      {
        activeSerial->println("  End Settling Measurements");
      }
    }

    if (verboseMode)
    {
      activeSerial->println("Conductivity");
      activeSerial->print("  Conductivity TIA Resistor: ");
      switch (DEFAULT_RTIA)
      {
      case HSTIARTIA_200:
        activeSerial->println("200 Ohm");
        break;
      case HSTIARTIA_1K:
        activeSerial->println("1K Ohm");
        break;
      case HSTIARTIA_5K:
        activeSerial->println("5K Ohm");
        break;
      case HSTIARTIA_10K:
        activeSerial->println("10K Ohm");
        break;
      case HSTIARTIA_20K:
        activeSerial->println("20K Ohm");
        break;
      case HSTIARTIA_40K:
        activeSerial->println("40K Ohm");
        break;
      case HSTIARTIA_80K:
        activeSerial->println("80K Ohm");
        break;
      case HSTIARTIA_160K:
        activeSerial->println("160K Ohm");
        break;
      default:
        activeSerial->println("Unknown");
        break;
      }
    }
    // Perform impedance measurement
    for (int i = 0; i < repeatNumber; i++)
    {
      AppIMPMeasure(i);
    }
    if (verboseMode)
    {
      if (AD5940_IsAdcMaxSaturatedAndClear())
      {
        activeSerial->println("SaturationFlag tripped!");
      }
      else
      {
        activeSerial->println("SaturationFlag NOT tripped!");
      }
    }
    float medianMag;
    float medianPhase;
    // take median and output results
    if (repeatNumber == 1)
    {
      medianMag = magnitudeArray[0];
      medianPhase = phaseArray[0];
    }
    else
    {
      medianMag = calculateMedian(magnitudeArray, repeatNumber);
      medianPhase = calculateMedian(phaseArray, repeatNumber);
    }

    activeSerial->printf("Freq: %.2f Hz ", storedFrequency);

    // Print the number of data points
    activeSerial->printf("DataPoints: %lu ", 1);

    // Print magnitude in ohms and phase in degrees
    // Note: Converting phase from radians to degrees (phase * 180 / π)
    activeSerial->printf("RzMag: %.2f Ohm, RzPhase: %.2f deg",
                         medianMag,
                         medianPhase);

    // Add separator and perform RTD (temperature) measurement
    activeSerial->print(";");
    AppRTDMeasure(rtdVoltageValue);
    activeSerial->print("*");
    previousMillis = millis();
  }
  break;
  case 'f':
  {
    // Anything after the initial 't' (e.g. ",123.4" or " -12.3")?
    if (command.length() > 1)
    {
      // Strip the leading 't' plus optional comma or space
      String arg = command.substring(1);
      arg.trim(); // remove whitespace
      if (arg.startsWith(","))
        arg.remove(0, 1);
      arg.trim();

      // Convert to float; String::toFloat() returns 0.0 if conversion fails
      float parsed = arg.toFloat();
      if (arg.length() && !(isnan(parsed) || isinf(parsed)))
      {
        rtdVoltageValue = parsed; // valid number received
      }
      else
      {
        activeSerial->println("bad value, not changing rtdVoltageValue");
      }
    }
    activeSerial->println("rtdVoltageValue");
    activeSerial->println(rtdVoltageValue);
  }
  break;

  case 't': // Read temperature at a user-defined RTD drive level (mV)
  {

    AppRTDMeasure(rtdVoltageValue);
    activeSerial->print("*"); // optional “command complete” asterisk
  }
  case 's': // Perform system startup and configuration
  {
    // Run startup initialization
    startupAD5941();

    // Save the configuration and stop any ongoing measurement
    saveCurrentConfig();
    AppIMPCtrl(IMPCTRL_STOPSYNC, 0);
    activeSerial->println("Startup Complete");
  }
  break;
  case 'p':
  {
    // propagate settings change through all units
    activeSerial->println("function removed");
    activeSerial->println("*");
  }
  case 'j': // Perform system startup and configuration
  {
    selectUnit(command.substring(2).toInt());
    activeSerial->write((uint8_t*)&singleSensorPacket.sensorNum, sizeof(uint32_t));
  }
  break;

  case 'r': // Restart the microcontroller
  {
    _reboot_Teensyduino_();
  }
  break;

  case 'x': // Echo test command
  {
    activeSerial->println("z");
    activeSerial->println("*");
  }
  break;
  case 'v': // Toggle verbose mode on/off
  {
    verboseMode = !verboseMode;
    activeSerial->print("Verbose mode ");
    activeSerial->println(verboseMode ? "ON" : "OFF");
  }
  break;
  case 'y': // Print current configuration in short format
  {
    printCurrentConfigShort();
  }
  break;
  case 'z': // Print current configuration in short format
  {
    activeSerial->print("AlyIDNumber: ");
    // Read the 4 words (32-bits each) that make up the 128-bit serial number
    uint32_t serNum[4];

    // OCOTP serial number registers
    serNum[0] = HW_OCOTP_CFG0; // First 32 bits
    serNum[1] = HW_OCOTP_CFG1; // Second 32 bits
    serNum[2] = HW_OCOTP_CFG2; // Third 32 bits
    serNum[3] = HW_OCOTP_CFG3; // Fourth 32 bits

    // Print the serial number in hexadecimal format
    for (int i = 0; i < 4; i++)
    {
      // Print each 32-bit word with leading zeros
      char buf[9];
      snprintf(buf, sizeof(buf), "%08lX", serNum[i]);
      activeSerial->print(buf);

      // Add a separator between words (except after the last one)
      if (i < 3)
      {
        activeSerial->print("-");
      }
    }
    activeSerial->println();
  }
  break;

  case 'i': // blink LED on teensy to id unit
  {
    pinMode(LED_BUILTIN, OUTPUT);
    digitalWrite(LED_BUILTIN, HIGH);
    delay(500);
    digitalWrite(LED_BUILTIN, LOW);
    delay(500);
    digitalWrite(LED_BUILTIN, HIGH);
    delay(500);
    digitalWrite(LED_BUILTIN, LOW);
    delay(500);
    SPI.begin(); // rerun SPI to initialize CLK pin (which is shared with LED_BUILTIN)
  }
  break;

  case 'e':
  {
    // Transmit all the data stored in EEPROM
    getEEPROMdata();
  }
  break;

  case 'g':
  {
    sendSensorData = false;
    // Mesaure and transmit for all the initialized sensors
    if (true == getAllMesaurements())
    {
      // Send all the sensor data to DD
      activeSerial->write((uint8_t *)&sensorPacket, sizeof(sensorPacket));
    }
    else
    {
      activeSerial->write((uint8_t)0);
    }
  }
  break;

  case 'k':
  {
    //  Get conductivity mesaurement settings in binary format.
    getMesaurementSettings();
  }
  break;

  case 'l':
  {
    //  Get Initialization Status
    getInitStatus();
  }
  break;

  case 'm':
  {
    sendSensorData = true;
    sendAllSensorData();
  }
  break;
  case 'n':
  {
    sendSensorData = false;
    activeSerial->write((uint8_t)1);
    // sendAllSensorData();
  }
  break;

  default: // Handle unknown commands
  {
    activeSerial->println("Unknown command. Type ? for help.");
  }
  break;
  }
}

// Function to select a unit and update pins
void selectUnit(int unitIn)
{
  // Validate unit input (1-6)
  if (unitIn < 1 || unitIn > 6)
  {
    // activeSerial->println("Error: Unit must be between 1 and 6");
    return;
  }

  // Initialize all mux control pins
  pinMode(EN_A, OUTPUT);
  pinMode(EN_B, OUTPUT);
  pinMode(EN_C, OUTPUT);
  pinMode(SELECT_A, OUTPUT);
  pinMode(SELECT_B, OUTPUT);
  pinMode(SELECT_C, OUTPUT);

  // Default state - disable all units
  digitalWrite(EN_A, HIGH); // Disabled (active LOW)
  digitalWrite(EN_B, HIGH); // Disabled
  digitalWrite(EN_C, HIGH); // Disabled
  digitalWrite(SELECT_A, LOW);
  digitalWrite(SELECT_B, LOW);
  digitalWrite(SELECT_C, LOW);

  // Update reset and interrupt pins based on selected unit
  switch (unitIn)
  {
  case 1:
    TEENSY_RESET_PIN = 17;
    AD5940_INT_PIN = 18;
    digitalWrite(EN_A, LOW); // Enable unit 1 (A)
    break;
  case 2:
    TEENSY_RESET_PIN = 22;
    AD5940_INT_PIN = 21;
    digitalWrite(EN_A, LOW);      // Enable unit 2 (A)
    digitalWrite(SELECT_A, HIGH); // Select unit 2 on mux A
    break;
  case 3:
    TEENSY_RESET_PIN = 14;
    AD5940_INT_PIN = 15;
    digitalWrite(EN_B, LOW); // Enable unit 3 (B)
    break;
  case 4:
    TEENSY_RESET_PIN = 3;
    AD5940_INT_PIN = 4;
    digitalWrite(EN_B, LOW);      // Enable unit 4 (B)
    digitalWrite(SELECT_B, HIGH); // Select unit 4 on mux B
    break;
  case 5:
    TEENSY_RESET_PIN = 9;
    AD5940_INT_PIN = 8;
    digitalWrite(EN_C, LOW); // Enable unit 5 (C)
    break;
  case 6:
    TEENSY_RESET_PIN = 0;
    AD5940_INT_PIN = 1;
    digitalWrite(EN_C, LOW);      // Enable unit 6 (C)
    digitalWrite(SELECT_C, HIGH); // Select unit 6 on mux C
    break;
  }

  // Configure the new reset and interrupt pins
  pinMode(TEENSY_RESET_PIN, OUTPUT);
  pinMode(AD5940_INT_PIN, INPUT);

  singleSensorPacket.sensorNum = unitIn; // Conductivity Sensor number i.e unit 1 to 6.

  // activeSerial->print("Selected Unit: ");
  // activeSerial->print(unitIn);
  // activeSerial->print(" | Reset Pin: ");
  // activeSerial->print(TEENSY_RESET_PIN);
  // activeSerial->print(" | Interrupt Pin: ");
  // activeSerial->println(AD5940_INT_PIN);
}

// Write a mixed array (doubles first, then floats) to the EEPROM
void writeMixedValues(unsigned int baseAddress, double *doubleData, int doubleCount, float *floatData, int floatCount)
{
  // Write doubles first
  for (int i = 0; i < doubleCount; i++)
  {
    unsigned int address = baseAddress + (i * sizeof(double));
    writeDouble(address, doubleData[i]);
    delay(10);
  }

  // Write floats after the doubles
  unsigned int floatBaseAddress = baseAddress + (doubleCount * sizeof(double));
  for (int i = 0; i < floatCount; i++)
  {
    unsigned int address = floatBaseAddress + (i * sizeof(float));
    writeFloat(address, floatData[i]);
    delay(10);
  }
}

// Read a mixed array (doubles first, then floats) from the EEPROM
void readMixedValues(unsigned int baseAddress, double *doubleData, int doubleCount, float *floatData, int floatCount)
{
  // Read doubles first
  for (int i = 0; i < doubleCount; i++)
  {
    unsigned int address = baseAddress + (i * sizeof(double));
    doubleData[i] = readDouble(address);
  }

  // Read floats after the doubles
  unsigned int floatBaseAddress = baseAddress + (doubleCount * sizeof(double));
  for (int i = 0; i < floatCount; i++)
  {
    unsigned int address = floatBaseAddress + (i * sizeof(float));
    floatData[i] = readFloat(address);
  }
}

// Write a single float to the EEPROM
void writeFloat(unsigned int address, float data)
{
  // Create a union to access the bytes of the float
  union
  {
    float f;
    byte b[4];
  } floatBytes;

  floatBytes.f = data;

  // Write each byte of the float
  for (uint i = 0; i < sizeof(float); i++)
  {
    writeByte(address + i, floatBytes.b[i]);
    delay(5); // Small delay between byte writes
  }
}

// Read a single float from the EEPROM
float readFloat(unsigned int address)
{
  // Create a union to build the float from bytes
  union
  {
    float f;
    byte b[4];
  } floatBytes;

  // Read each byte of the float
  for (uint i = 0; i < sizeof(float); i++)
  {
    floatBytes.b[i] = readByte(address + i);
  }

  return floatBytes.f;
}

// Write an array of doubles to the EEPROM
void writeDoubles(unsigned int baseAddress, double *data, int count)
{
  for (int i = 0; i < count; i++)
  {
    // Calculate the address for this double
    // Each double is 8 bytes
    unsigned int address = baseAddress + (i * sizeof(double));

    // Write this double
    writeDouble(address, data[i]);

    // Small delay between writes to ensure EEPROM has time to complete
    delay(10);
  }
}

// Read an array of doubles from the EEPROM
void readDoubles(unsigned int baseAddress, double *data, int count)
{
  for (int i = 0; i < count; i++)
  {
    // Calculate the address for this double
    // Each double is 8 bytes
    unsigned int address = baseAddress + (i * sizeof(double));

    // Read this double
    data[i] = readDouble(address);
  }
}

// Write a single double to the EEPROM
void writeDouble(unsigned int address, double data)
{
  // Create a union to access the bytes of the double
  union
  {
    double d;
    byte b[8];
  } doubleBytes;

  doubleBytes.d = data;

  // Write each byte of the double
  for (uint i = 0; i < sizeof(double); i++)
  {
    writeByte(address + i, doubleBytes.b[i]);
    delay(5); // Small delay between byte writes
  }
}

// Read a single double from the EEPROM
double readDouble(unsigned int address)
{
  // Create a union to build the double from bytes
  union
  {
    double d;
    byte b[8];
  } doubleBytes;

  // Read each byte of the double
  for (uint i = 0; i < sizeof(double); i++)
  {
    doubleBytes.b[i] = readByte(address + i);
  }

  return doubleBytes.d;
}

// Write a single byte to the EEPROM
void writeByte(unsigned int address, byte data)
{
  // Check if address is valid
  if (address >= EEPROM_SIZE)
  {
    activeSerial->print("Error: Address out of range: ");
    activeSerial->println(address);
    return;
  }

  // Enable writing
  writeEnable();

  // Select the EEPROM
  digitalWrite(TEENSY_SPI_CS_PIN, HIGH);

  // Send write command
  SPI.transfer(EEPROM_WRITE);

  // Send address
  if (EEPROM_SIZE > 256)
  {
    // For AT25040B (4Kbit), use 9-bit address (A8-A0)
    SPI.transfer((address >> 8) & 0x01); // Only 1 bit for A8
  }
  SPI.transfer(address & 0xFF);

  // Send data
  SPI.transfer(data);

  // Deselect the EEPROM
  digitalWrite(TEENSY_SPI_CS_PIN, LOW);

  // Wait for the write to complete
  delay(5); // 5ms max write time according to datasheet
}

// Read a single byte from the EEPROM
byte readByte(unsigned int address)
{
  // Check if address is valid
  if (address >= EEPROM_SIZE)
  {
    activeSerial->print("Error: Address out of range: ");
    activeSerial->println(address);
    return 0;
  }

  byte data;

  // Select the EEPROM
  digitalWrite(TEENSY_SPI_CS_PIN, HIGH);

  // Send read command
  SPI.transfer(EEPROM_READ);

  // Send address
  if (EEPROM_SIZE > 256)
  {
    // For AT25040B (4Kbit), use 9-bit address (A8-A0)
    SPI.transfer((address >> 8) & 0x01); // Only 1 bit for A8
  }
  SPI.transfer(address & 0xFF);

  // Read data
  data = SPI.transfer(0x00);

  // Deselect the EEPROM
  digitalWrite(TEENSY_SPI_CS_PIN, LOW);

  return data;
}

void writeEnable()
{
  digitalWrite(TEENSY_SPI_CS_PIN, HIGH);
  SPI.transfer(EEPROM_WREN);
  digitalWrite(TEENSY_SPI_CS_PIN, LOW);
  delay(1); // Small delay to ensure the command is processed
}

// Modified command handler for saving mixed precision values to EEPROM
// Format: "save,value1,value2,value3,..." (first DOUBLE_COUNT are doubles, rest are floats)
void handleSaveCommand(String command)
{
  int firstComma = command.indexOf(',');
  if (firstComma < 0)
  {
    activeSerial->println("Error: Invalid save command format");
    return;
  }
  String dataStr = command.substring(firstComma + 1);

  // Calculate maximum values we can store in AT25010B (128 bytes)
  // DOUBLE_COUNT * 8 + float_count * 4 <= 128
  int maxFloats = (128 - (DOUBLE_COUNT * 8)) / 4;
  int maxTotal = DOUBLE_COUNT + maxFloats;

  double doubleValues[DOUBLE_COUNT];
  float floatValues[maxFloats];
  int valueIndex = 0;

  // Parse values
  while (valueIndex < maxTotal && dataStr.length() > 0)
  {
    int commaPos = dataStr.indexOf(',');
    String valueStr;
    if (commaPos >= 0)
    {
      valueStr = dataStr.substring(0, commaPos);
      dataStr = dataStr.substring(commaPos + 1);
    }
    else
    {
      // Last value
      valueStr = dataStr;
      dataStr = ""; // Clear dataStr to end the loop
    }

    // Convert string and store in appropriate array
    if (valueStr.length() > 0)
    {
      if (valueIndex < DOUBLE_COUNT)
      {
        // Store as double
        doubleValues[valueIndex] = strtod(valueStr.c_str(), NULL);
      }
      else
      {
        // Store as float
        floatValues[valueIndex - DOUBLE_COUNT] = strtof(valueStr.c_str(), NULL);
      }
      valueIndex++;
    }
  }

  // Check if we got at least 1 value
  if (valueIndex == 0)
  {
    activeSerial->println("Error: No valid values found");
    return;
  }
  if (valueIndex > maxTotal)
  {
    activeSerial->print("Error: Too many values, maximum is ");
    activeSerial->print(maxTotal);
    activeSerial->print(" (");
    activeSerial->print(DOUBLE_COUNT);
    activeSerial->print(" doubles + ");
    activeSerial->print(maxFloats);
    activeSerial->println(" floats)");
    return;
  }

  int doubleCount = min(valueIndex, DOUBLE_COUNT);
  int floatCount = max(0, valueIndex - DOUBLE_COUNT);

  // Display the values and ask for confirmation
  activeSerial->print("Preparing to save ");
  activeSerial->print(valueIndex);
  activeSerial->print(" values to EEPROM (");
  activeSerial->print(doubleCount);
  activeSerial->print(" doubles, ");
  activeSerial->print(floatCount);
  activeSerial->println(" floats):");

  // Display double values
  for (int i = 0; i < doubleCount; i++)
  {
    activeSerial->print(i);
    activeSerial->print(" (double): ");
    activeSerial->println(doubleValues[i], 10);
  }

  // Display float values
  for (int i = 0; i < floatCount; i++)
  {
    activeSerial->print(i + DOUBLE_COUNT);
    activeSerial->print(" (float): ");
    activeSerial->println(floatValues[i], 6); // Floats typically have 6-7 significant digits
  }

  activeSerial->println("Confirm write to EEPROM? (y/n)");

  // Wait for user confirmation
  unsigned long startTime = millis();
  const unsigned long timeout = 30000; // 30 seconds timeout
  while (true)
  {
    // Check for timeout
    if (millis() - startTime > timeout)
    {
      activeSerial->println("Timed out waiting for confirmation. Operation aborted.");
      return;
    }
    // Check if data is available to read
    if (activeSerial->available() > 0)
    {
      char response = activeSerial->read();
      // Clear any remaining characters in buffer (like newline)
      while (activeSerial->available() > 0)
      {
        activeSerial->read();
      }
      // Process response
      if (response == 'y' || response == 'Y')
      {
        // User confirmed, write to EEPROM
        unsigned int baseAddress = 0;
        writeMixedValues(baseAddress, doubleValues, doubleCount, floatValues, floatCount);
        activeSerial->println("save attempted");

        // Read back the values to confirm they were written correctly
        double readDoubles[DOUBLE_COUNT];
        float readFloats[maxFloats];
        readMixedValues(baseAddress, readDoubles, doubleCount, readFloats, floatCount);

        activeSerial->println("Verification - Values read back from EEPROM:");
        bool verificationSuccess = true;

        // Verify doubles
        for (int i = 0; i < doubleCount; i++)
        {
          activeSerial->print(i);
          activeSerial->print(" (double): ");
          activeSerial->println(readDoubles[i], 10);
          if (abs(doubleValues[i] - readDoubles[i]) > 0.000000001)
          {
            verificationSuccess = false;
            activeSerial->print("Warning: Double value ");
            activeSerial->print(i);
            activeSerial->println(" does not match exactly what was written");
          }
        }

        // Verify floats
        for (int i = 0; i < floatCount; i++)
        {
          activeSerial->print(i + DOUBLE_COUNT);
          activeSerial->print(" (float): ");
          activeSerial->println(readFloats[i], 6);
          if (abs(floatValues[i] - readFloats[i]) > 0.000001)
          {
            verificationSuccess = false;
            activeSerial->print("Warning: Float value ");
            activeSerial->print(i + DOUBLE_COUNT);
            activeSerial->println(" does not match exactly what was written");
          }
        }

        if (verificationSuccess)
        {
          activeSerial->println("Verification successful: All values match!");
        }
        else
        {
          activeSerial->println("Verification Failed, confirm device attached and powered.");
        }
        return;
      }
      else if (response == 'n' || response == 'N')
      {
        // User aborted
        activeSerial->println("Operation aborted by user.");
        return;
      }
      else
      {
        // Invalid response, ask again
        activeSerial->println("Please enter 'y' to confirm or 'n' to abort:");
      }
    }
    // Small delay to prevent CPU hogging
    delay(10);
  }
}

void handleLoadCommand()
{
  unsigned int baseAddress = 0; // Same address used in save

  // Calculate maximum values we can store
  int maxFloats = (128 - (DOUBLE_COUNT * 8)) / 4;

  double doubleValues[DOUBLE_COUNT];
  float floatValues[maxFloats];

  readMixedValues(baseAddress, doubleValues, DOUBLE_COUNT, floatValues, maxFloats);

  activeSerial->println("Loaded values from EEPROM:");

  // Display doubles
  for (int i = 0; i < DOUBLE_COUNT; i++)
  {
    activeSerial->print(i);
    activeSerial->print(" (double): ");
    activeSerial->println(doubleValues[i], 10);
  }

  // Display floats
  for (int i = 0; i < maxFloats; i++)
  {
    activeSerial->print(i + DOUBLE_COUNT);
    activeSerial->print(" (float): ");
    activeSerial->println(floatValues[i], 6);
  }
}

// Function to initialize all units sequentially
void initializeAllUnits()
{
  activeSerial->println("Initializing all units (1-6)...");

  for (int unit = 1; unit <= 6; unit++)
  {
    activeSerial->print("Initializing Unit ");
    activeSerial->println(unit);

    // Select the current unit
    selectUnit(unit);

    // Allow some settling time after switching units
    delay(50);

    // Initialize the AD5940 analog front-end for this unit
    startupAD5941();

    // Give some time for the initialization to complete
    delay(200);

    activeSerial->print("Unit ");
    activeSerial->print(unit);
    activeSerial->println(" initialized successfully");
  }

  activeSerial->println("All units initialized successfully");
}

// Function to ask the user which units to initialize and then initialize them
bool initializeSelectedUnits()
{
  initStatus = INIT_STATUS_IN_PROGRESS;
  activeSerial->write((uint8_t *)&initStatus, sizeof(initStatus));

  // activeSerial->println("\nUnit Initialization");
  // activeSerial->println("Enter 'all' to initialize all units (1-6)");
  // activeSerial->println("Or enter specific units separated by commas (e.g., '1,3,5')");
  // activeSerial->println("Then press Enter:");

  // // Wait for user input
  // while (!activeSerial->available()) {
  //   // Wait for input
  //   delay(100);
  // }

  // Read the input
  String input = activeSerial->readStringUntil('\n');
  input.trim(); // Remove any whitespace

  // Initialize all units
  // activeSerial->println("Initializing all units (1-6)...");

  for (int unit = 1; unit <= 6; unit++)
  {
    initializeSingleUnit(unit);
  }
  isInitialized = true;
  // Print summary of unit status
  // printUnitStatusSummary();

  // else {
  //   // Parse the comma-separated list
  //   activeSerial->print("Initializing selected units: ");

  //   // Start at the beginning of the string
  //   int startPos = 0;
  //   int endPos;

  //   // Parse each number in the comma-separated list
  //   while (startPos < input.length()) {
  //     // Find the next comma or the end of the string
  //     endPos = input.indexOf(',', startPos);
  //     if (endPos == -1) {
  //       endPos = input.length();
  //     }

  //     // Extract the unit number
  //     String unitStr = input.substring(startPos, endPos);
  //     unitStr.trim();

  //     // Convert to integer
  //     int unit = unitStr.toInt();

  //     // Check if it's a valid unit number
  //     if (unit >= 1 && unit <= 6) {
  //       activeSerial->print(unit);
  //       activeSerial->print(" ");

  //       // Initialize this unit
  //       initializeSingleUnit(unit);
  //     } else {
  //       activeSerial->print("(Skipping invalid unit ");
  //       activeSerial->print(unitStr);
  //       activeSerial->print(") ");
  //     }

  //     // Move to the position after the comma
  //     startPos = endPos + 1;
  //   }

  //   activeSerial->println("\nSelected units initialization complete");

  // Print summary of unit status
  return isInitialized;
}

// Function to select the first working unit
void selectFirstWorkingUnit()
{
  for (int i = 0; i < 6; i++)
  {
    if (unitStatus[i] == STATUS_SUCCESS)
    {
      // Found a working unit, select it (adding 1 because units are 1-indexed)
      int workingUnit = i + 1;
      selectUnit(workingUnit);

      // activeSerial->print("\nAutomatically selected first working unit: Unit ");
      // activeSerial->println(workingUnit);
      startupAD5941();
      return; // Exit after selecting the first working unit
    }
  }

  // If we get here, no working units were found
  // activeSerial->println("\nNo working units available to select!");
}
// Helper function to initialize a single unit
void initializeSingleUnit(int unit)
{
  // activeSerial->print("\nInitializing Unit ");
  // activeSerial->println(unit);

  // Select the unit
  selectUnit(unit);

  // Allow some settling time after switching units
  delay(50);

  // Initialize the AD5940 analog front-end for this unit
  if (startupAD5941())
  {
    // Startup failed
    // activeSerial->print("Unit ");
    // activeSerial->print(unit);
    // activeSerial->println(" startup FAILED");

    // Record the failure in our status array (0-indexed)
    unitStatus[unit - 1] = STATUS_FAILED;
  }
  else
  {
    // Startup worked
    // activeSerial->print("Unit ");
    // activeSerial->print(unit);
    // activeSerial->println(" startup SUCCESS");

    // Record the success in our status array
    unitStatus[unit - 1] = STATUS_SUCCESS;
  }
}

// Function to print a summary of the status of all units
void printUnitStatusSummary()
{
  // Count units by status
  int skippedCount = 0;
  int failedCount = 0;
  int successCount = 0;

  for (int i = 0; i < 6; i++)
  {
    if (unitStatus[i] == STATUS_SKIPPED)
    {
      skippedCount++;
    }
    else if (unitStatus[i] == STATUS_FAILED)
    {
      failedCount++;
    }
    else
    { // STATUS_SUCCESS
      successCount++;
    }
  }

  // Print summary
  activeSerial->println("\n----- Initialization Results -----");

  // Report units by status
  activeSerial->print("Successful: ");
  activeSerial->print(successCount);
  activeSerial->print(", Failed: ");
  activeSerial->print(failedCount);
  activeSerial->print(", Skipped: ");
  activeSerial->println(skippedCount);

  // List failed units
  if (failedCount > 0)
  {
    activeSerial->print("Failed units: ");
    bool firstPrinted = false;
    for (int i = 0; i < 6; i++)
    {
      if (unitStatus[i] == STATUS_FAILED)
      {
        if (firstPrinted)
        {
          activeSerial->print(", ");
        }
        activeSerial->print(i + 1);
        firstPrinted = true;
      }
    }
    activeSerial->println();
  }

  // List skipped units
  if (skippedCount > 0)
  {
    activeSerial->print("Skipped units: ");
    bool firstPrinted = false;
    for (int i = 0; i < 6; i++)
    {
      if (unitStatus[i] == STATUS_SKIPPED)
      {
        if (firstPrinted)
        {
          activeSerial->print(", ");
        }
        activeSerial->print(i + 1);
        firstPrinted = true;
      }
    }
    activeSerial->println();
  }

  // Report available units
  activeSerial->print("\nAvailable units: ");
  if (successCount == 0)
  {
    activeSerial->println("NONE - No successfully initialized units!");
  }
  else
  {
    // List available units
    bool firstPrinted = false;
    for (int i = 0; i < 6; i++)
    {
      if (unitStatus[i] == STATUS_SUCCESS)
      {
        // Add comma if not the first available unit
        if (firstPrinted)
        {
          activeSerial->print(", ");
        }
        activeSerial->print(i + 1);
        firstPrinted = true;
      }
    }
    activeSerial->println();
  }

  activeSerial->println("---------------------------------");
}

// Function to measure all successfully initialized units
void measureAllSuccessfulUnits()
{
  int successCount = 0;

  // Count successful units first
  for (int i = 0; i < 6; i++)
  {
    if (unitStatus[i] == STATUS_SUCCESS)
    {
      successCount++;
    }
  }

  // If no successful units, report and return
  if (successCount == 0)
  {
    activeSerial->println("No successfully initialized units to measure!");
    return;
  }

  activeSerial->print("Measuring all ");
  activeSerial->print(successCount);
  activeSerial->println(" successful units...");
  AppIMPCfg_Type *pImpedanceCfg;

  // Loop through and measure each successful unit
  for (int unit = 1; unit <= 6; unit++)
  {
    // Skip units that aren't successfully initialized
    if (unitStatus[unit - 1] != STATUS_SUCCESS)
    {
      continue;
    }

    // Select the unit
    selectUnit(unit);
    if (propagateSettingsChanges)
    {
      AppIMPGetCfg(&pImpedanceCfg);
      pImpedanceCfg->bParaChanged = bTRUE;
    }
    // Allow some settling time after switching units
    delay(50);

    // Print unit identifier before measurement
    activeSerial->print("Unit ");
    activeSerial->print(unit);
    activeSerial->print(": ");

    // quick patch for fixing spontaneous measurements
    unsigned long currentMillis = millis();
    if (currentMillis - previousMillis >= jitterTimeout)
    {
      activeSerial->println("Performing settling measurements...");

      // Run multiple settling measurements in a loop
      for (int settling = 0; settling < numSettlingMeasurements; settling++)
      {
        // Run one complete measurement cycle but discard results
        AppIMPInit(AppBuff, APPBUFF_SIZE);
        AppIMPCtrl(IMPCTRL_START, 0);

        // Wait for completion (simplified polling)
        int timeout_count = 0;
        while (AD5940_GetMCUIntFlag() != 1 && timeout_count < 1000)
        {
          delay(1);
          timeout_count++;
        }

        if (AD5940_GetMCUIntFlag() == 1)
        {
          AD5940_ClrMCUIntFlag();
          uint32_t dummy_size = APPBUFF_SIZE;
          AppIMPISR(AppBuff, &dummy_size); // Discard this data
        }

        AppIMPCtrl(IMPCTRL_STOPSYNC, 0);

        // Small delay between settling measurements
        if (settling < numSettlingMeasurements - 1)
        {
          AD5940_Delay10us(1000); // 10ms between measurements
        }
      }
    }

    // Perform impedance measurement
    for (int i = 0; i < repeatNumber; i++)
    {
      AppIMPMeasure(i);
    }
    float medianMag;
    float medianPhase;

    // take median and output results
    if (repeatNumber == 1)
    {
      medianMag = magnitudeArray[0];
      medianPhase = phaseArray[0];
    }
    else
    {
      medianMag = calculateMedian(magnitudeArray, repeatNumber);
      medianPhase = calculateMedian(phaseArray, repeatNumber);
    }
    activeSerial->printf("Freq: %.2f Hz ", storedFrequency);

    // Print the number of data points
    activeSerial->printf("DataPoints: %lu ", 1);

    // Print magnitude in ohms and phase in degrees
    // Note: Converting phase from radians to degrees (phase * 180 / π)
    activeSerial->printf("RzMag: %.2f Ohm, RzPhase: %.2f deg",
                         medianMag,
                         medianPhase);

    // Add separator
    activeSerial->print(";");

    // Perform RTD (temperature) measurement
    AppRTDMeasure(rtdVoltageValue);

    // Line break between units
    activeSerial->println();
  }
  previousMillis = millis();
  activeSerial->println("All measurements complete.");
  propagateSettingsChanges = 0;
}

float calculateMedian(float arr[], int size)
{
  // Create a temporary copy of the array
  float tempArr[size];
  for (int i = 0; i < size; i++)
  {
    tempArr[i] = arr[i];
  }

  // Sort the temporary array
  for (int i = 0; i < size - 1; i++)
  {
    for (int j = 0; j < size - i - 1; j++)
    {
      if (tempArr[j] > tempArr[j + 1])
      {
        float temp = tempArr[j];
        tempArr[j] = tempArr[j + 1];
        tempArr[j + 1] = temp;
      }
    }
  }

  // Return median value
  if (size % 2 == 0)
  {
    return (tempArr[size / 2 - 1] + tempArr[size / 2]) / 2.0;
  }
  else
  {
    return tempArr[size / 2];
  }
}

static void AD5940_EnableAdcMaxSaturationIRQ(uint32_t max_code, uint16_t hysteresis)
{
  // 1) Program the max comparator + hysteresis
  AD5940_WriteReg(0x000020B0, max_code);   // ADCMAX
  AD5940_WriteReg(0x000020B4, hysteresis); // ADCMAXSMEN

  // 2) Enable FLAG5 in INTC1 (had to move INTC1 off debug, but we weren't really using it -MK)
  uint32_t sel1 = AD5940_ReadReg(0x0000300C); // INTCSEL1
  sel1 |= (1u << 5);                          // INTSEL5: enable ADCMAX fail IRQ on this controller
  AD5940_WriteReg(0x0000300C, sel1);

  // 3) Clear any stale flags (write-1-to-clear)
  AD5940_WriteReg(0x00003004, (1u << 5)); // INTCCLR: clear ADCMAX fail
  if (verboseMode)
  {
    activeSerial->println("saturationFlagSet");
  }
}

static inline bool AD5940_IsAdcMaxSaturatedAndClear()
{
  // Read flags (either INTCFLAG0 or INTCFLAG1 is fine since we enabled in INTC1)
  uint32_t f1 = AD5940_ReadReg(0x00003014); // INTCFLAG1
  bool saturated = (f1 & (1u << 5)) != 0;   // FLAG5: ADC max fail

  if (saturated)
  {
    // Clear it (write-1-to-clear)
    AD5940_WriteReg(0x00003004, (1u << 5)); // INTCCLR: clear ADCMAX fail
  }
  return saturated;
}

// ************************************ Diality **********************************************

void detectSerial()
{
  unsigned long startTime = 0;
  // First check Serial7 explicitly

  beginSerial(&Serial7, 115200);
  startTime = millis();

  while ((millis() - startTime) < 3000)
  { // Wait up to 3 seconds
    if (Serial7.available()) 
    {
      activeSerial = &Serial7;
      // activeSerial->println("Using Serial7 (pins 28,29)");
      return; // Done, since Serial7 is preferred
    }
  }
  delay(50);

  for (int i = 0; i < 8; i++)
  {
    beginSerial(serialPorts[i], 115200);
    delay(50); // Give time for init
    // For USB Serial, wait for connection
    if (serialPorts[i] == &Serial)
    {
      startTime = millis();
      while (!Serial && millis() - startTime < 2000)
      {
        // Wait up to 2s for USB
      }
      activeSerial = serialPorts[i];
      break;
    }
  }
}

  void beginSerial(Stream * s, unsigned long baud)
  {
    if (s == &Serial)
    {
      Serial.begin(baud);
    }
    else if (s == &Serial1)
    {
      Serial1.begin(baud);
    }
    else if (s == &Serial2)
    {
      Serial2.begin(baud);
    }
    else if (s == &Serial3)
    {
      Serial3.begin(baud);
    }
    else if (s == &Serial4)
    {
      Serial4.begin(baud);
    }
    else if (s == &Serial5)
    {
      Serial5.begin(baud);
    }
    else if (s == &Serial6)
    {
      Serial6.begin(baud);
    }
    else if (s == &Serial7)
    {
      Serial7.begin(baud);
    }
  }

  // Function to measure all successfully initialized units
  bool getAllMesaurements(void)
  {
    int successCount = 0;
    bool retVal = true; // Success

    // Count successful units first
    for (int i = 0; i < 6; i++)
    {
      if (unitStatus[i] == STATUS_SUCCESS)
      {
        successCount++;
      }
    }

    // If no successful units, report and return
    if (successCount == 0)
    {
      retVal = false; // Error
      // activeSerial->println("No successfully initialized units to measure!");
      return retVal;
    }

    AppIMPCfg_Type *pImpedanceCfg;

    // Loop through and measure each successful unit
    for (int unit = 1; unit <= 6; unit++)
    {
      // Skip units that aren't successfully initialized
      if (unitStatus[unit - 1] != STATUS_SUCCESS)
      {
        continue;
      }

      // Select the unit
      selectUnit(unit);
      if (propagateSettingsChanges)
      {
        AppIMPGetCfg(&pImpedanceCfg);
        pImpedanceCfg->bParaChanged = bTRUE;
      }
      // Allow some settling time after switching units
      delay(50);

      // quick patch for fixing spontaneous measurements
      unsigned long currentMillis = millis();
      if (currentMillis - previousMillis >= jitterTimeout)
      {
        // Run multiple settling measurements in a loop
        for (int settling = 0; settling < numSettlingMeasurements; settling++)
        {
          // Run one complete measurement cycle but discard results
          AppIMPInit(AppBuff, APPBUFF_SIZE);
          AppIMPCtrl(IMPCTRL_START, 0);

          // Wait for completion (simplified polling)
          int timeout_count = 0;
          while (AD5940_GetMCUIntFlag() != 1 && timeout_count < 1000)
          {
            delay(1);
            timeout_count++;
          }

          if (AD5940_GetMCUIntFlag() == 1)
          {
            AD5940_ClrMCUIntFlag();
            uint32_t dummy_size = APPBUFF_SIZE;
            AppIMPISR(AppBuff, &dummy_size); // Discard this data
          }

          AppIMPCtrl(IMPCTRL_STOPSYNC, 0);

          // Small delay between settling measurements
          if (settling < numSettlingMeasurements - 1)
          {
            AD5940_Delay10us(1000); // 10ms between measurements
          }
        }
      }

      // Perform impedance measurement
      for (int i = 0; i < repeatNumber; i++)
      {
        AppIMPMeasure(i);
      }
      float medianMag;
      float medianPhase;

      // take median and output results
      if (repeatNumber == 1)
      {
        medianMag = magnitudeArray[0];
        medianPhase = phaseArray[0];
      }
      else
      {
        medianMag = calculateMedian(magnitudeArray, repeatNumber);
        medianPhase = calculateMedian(phaseArray, repeatNumber);
      }

      // Perform RTD (temperature) measurement
      float rtd_res = getRTDMeasurements(rtdVoltageValue);

      previousMillis = millis();

      // Check for "ERROR: AD5940 wakeup timeout"
      if (-1000.0f == rtd_res)
      {
        // In case of an error, send all 0's
        initPacketToDefault(unit);
      }
      if (false == isSensorInitialized(unit))
      {
        // In case the unit is not initialized, send all 0's
        initPacketToDefault(unit);
      }
      // Pack data for case 'g'
      else
      {
        // Pack the sensor data
        // The ALY code loops from 1 to 6. Our packet index from 0 to 5. So, index at i-1.
        int sensorIdx = unit - 1;
        sensorPacket[sensorIdx].sensorNum = unit;          // Conductivity Sensor number i.e unit 1 to 6.
        sensorPacket[sensorIdx].impFreq = storedFrequency; // Impedance Frequency
        sensorPacket[sensorIdx].impDataPoints = 1;         // Impedance Data Points. Hard coded to 1
        sensorPacket[sensorIdx].impRzMag = medianMag;      // Value of medianMag
        sensorPacket[sensorIdx].impRzPhase = medianPhase;  // Value of medianPhase
        sensorPacket[sensorIdx].rtdFreq = 0;               // RTD Frequency. Hard coded to 0.0
        sensorPacket[sensorIdx].rtdDataPoints = 1;         // Impedance Data Points. Hard coded to 1
        sensorPacket[sensorIdx].rtdRzMag = rtd_res;        // Value of rtd_resistance
        sensorPacket[sensorIdx].rtdRzPhase = 0.0;          // RTD Rz Phase. Hard coded to 0.0

        // Reusing this function for case 'm'
        if (sendSensorData)
        {
          singleSensorPacket.sensorNum = sensorPacket[sensorIdx].sensorNum;
          singleSensorPacket.impFreq = sensorPacket[sensorIdx].impFreq;             // Impedance Frequency
          singleSensorPacket.impDataPoints = sensorPacket[sensorIdx].impDataPoints; // Impedance Data Points. Hard coded to 1
          singleSensorPacket.impRzMag = sensorPacket[sensorIdx].impRzMag;           // Value of medianMag
          singleSensorPacket.impRzPhase = sensorPacket[sensorIdx].impRzPhase;       // Value of medianPhase
          singleSensorPacket.rtdFreq = sensorPacket[sensorIdx].rtdFreq;             // RTD Frequency. Hard coded to 0.0
          singleSensorPacket.rtdDataPoints = sensorPacket[sensorIdx].rtdDataPoints; // Impedance Data Points. Hard coded to 1
          singleSensorPacket.rtdRzMag = sensorPacket[sensorIdx].rtdRzMag;           // Value of rtd_resistance
          singleSensorPacket.rtdRzPhase = sensorPacket[sensorIdx].rtdRzPhase;       // RTD Rz Phase. Hard coded to 0.0
        }
      }
    }

    propagateSettingsChanges = 0;
    return retVal;
  }

  void getSelectedSensorMeasurements(void)
  {
    // quick patch for fixing spontaneous measurements
    unsigned long currentMillis = millis();
    if (currentMillis - previousMillis >= jitterTimeout)
    {

      // Run multiple settling measurements in a loop
      for (int settling = 0; settling < numSettlingMeasurements; settling++)
      {
        if (settling == 0)
        {
          // activeSerial->println("Performing settling measurements...");
        }
        if (verboseMode)
        {
          // activeSerial->printf(" Settling Measurement#: %u ", settling);
          // activeSerial->println(" ");
        }
        // Run one complete measurement cycle but discard results
        AppIMPInit(AppBuff, APPBUFF_SIZE);
        AppIMPCtrl(IMPCTRL_START, 0);

        // Wait for completion (simplified polling)
        int timeout_count = 0;
        while (AD5940_GetMCUIntFlag() != 1 && timeout_count < 1000)
        {
          delay(1);
          timeout_count++;
        }

        if (AD5940_GetMCUIntFlag() == 1)
        {
          AD5940_ClrMCUIntFlag();
          uint32_t dummy_size = APPBUFF_SIZE;
          AppIMPISR(AppBuff, &dummy_size); // Discard this data
        }

        AppIMPCtrl(IMPCTRL_STOPSYNC, 0);

        // Small delay between settling measurements
        if (settling < numSettlingMeasurements - 1)
        {
          AD5940_Delay10us(1000); // 10ms between measurements
        }
      }
      if (verboseMode)
      {
        // activeSerial->println("  End Settling Measurements");
      }
    }

    if (verboseMode)
    {
      // activeSerial->println("Conductivity");
      // activeSerial->print("  Conductivity TIA Resistor: ");
      switch (DEFAULT_RTIA)
      {
      case HSTIARTIA_200:
        // activeSerial->println("200 Ohm");
        break;
      case HSTIARTIA_1K:
        // activeSerial->println("1K Ohm");
        break;
      case HSTIARTIA_5K:
        // activeSerial->println("5K Ohm");
        break;
      case HSTIARTIA_10K:
        // activeSerial->println("10K Ohm");
        break;
      case HSTIARTIA_20K:
        // activeSerial->println("20K Ohm");
        break;
      case HSTIARTIA_40K:
        // activeSerial->println("40K Ohm");
        break;
      case HSTIARTIA_80K:
        // activeSerial->println("80K Ohm");
        break;
      case HSTIARTIA_160K:
        // activeSerial->println("160K Ohm");
        break;
      default:
        // activeSerial->println("Unknown");
        break;
      }
    }
    // Perform impedance measurement
    for (int i = 0; i < repeatNumber; i++)
    {
      AppIMPMeasure(i);
    }
    if (verboseMode)
    {
      if (AD5940_IsAdcMaxSaturatedAndClear())
      {
        // activeSerial->println("SaturationFlag tripped!");
      }
      else
      {
        // activeSerial->println("SaturationFlag NOT tripped!");
      }
    }
    float medianMag;
    float medianPhase;
    // take median and output results
    if (repeatNumber == 1)
    {
      medianMag = magnitudeArray[0];
      medianPhase = phaseArray[0];
    }
    else
    {
      medianMag = calculateMedian(magnitudeArray, repeatNumber);
      medianPhase = calculateMedian(phaseArray, repeatNumber);
    }

    // activeSerial->printf("Freq: %.2f Hz ", storedFrequency);

    // Print the number of data points
    // activeSerial->printf("DataPoints: %lu ", 1);

    // Print magnitude in ohms and phase in degrees
    // Note: Converting phase from radians to degrees (phase * 180 / π)
    // activeSerial->printf("RzMag: %.2f Ohm, RzPhase: %.2f deg",
    // medianMag,
    // medianPhase);

    // Add separator and perform RTD (temperature) measurement
    // activeSerial->print(";");
    float rtd_res = getRTDMeasurements(rtdVoltageValue);
    // activeSerial->print("*");
    previousMillis = millis();

    // Check for "ERROR: AD5940 wakeup timeout"
    if (-1000.0f == rtd_res)
    {
      // In case of an error, send all 0's
      initSinglePacketToDefault();
    }
    else
    {
      singleSensorPacket.impFreq = storedFrequency; // Impedance Frequency
      singleSensorPacket.impDataPoints = 1;         // Impedance Data Points. Hard coded to 1
      singleSensorPacket.impRzMag = medianMag;      // Value of medianMag
      singleSensorPacket.impRzPhase = medianPhase;  // Value of medianPhase
      singleSensorPacket.rtdFreq = 0;               // RTD Frequency. Hard coded to 0.0
      singleSensorPacket.rtdDataPoints = 1;         // Impedance Data Points. Hard coded to 1
      singleSensorPacket.rtdRzMag = rtd_res;        // Value of rtd_resistance
      singleSensorPacket.rtdRzPhase = 0.0;          // RTD Rz Phase. Hard coded to 0.0
    }
    activeSerial->write((uint8_t *)&singleSensorPacket, sizeof(singleSensorPacket));
  }

  float getRTDMeasurements(float sensor_mV)
  {
    AFERefCfg_Type aferef_cfg;
    HSLoopCfg_Type HsLoopCfg;
    DSPCfg_Type dsp_cfg;

    uint32_t adcCode_rtd, adcCode_ref;
    float volt_rtd, volt_ref;
    float rtd_resistance;
    const float RtdRefRes = REF_RESISTOR_VALUE;

    // Wake up the AD5940 from low power mode
    if (AD5940_WakeUp(10) > 10)
    {
      // activeSerial->println("ERROR: AD5940 wakeup timeout");
      return -1000.0f;
    }

    // Reset LPDAC configuration before RTD measurement
    LPDACCfg_Type lpdac_reset = {0};
    lpdac_reset.PowerEn = bFALSE;
    AD5940_LPDACCfgS(&lpdac_reset);

    // Configure High-Speed Loop
    HsLoopCfg.HsDacCfg.ExcitBufGain = EXCITBUFGAIN_2;
    HsLoopCfg.HsDacCfg.HsDacGain = HSDACGAIN_1;
    HsLoopCfg.HsDacCfg.HsDacUpdateRate = 7;

    // TIA settings
    HsLoopCfg.HsTiaCfg.DiodeClose = bFALSE;
    HsLoopCfg.HsTiaCfg.HstiaBias = HSTIABIAS_1P1;
    HsLoopCfg.HsTiaCfg.HstiaCtia = 32;
    HsLoopCfg.HsTiaCfg.HstiaDeRload = HSTIADERLOAD_OPEN;
    HsLoopCfg.HsTiaCfg.HstiaDeRtia = HSTIADERTIA_OPEN;
    HsLoopCfg.HsTiaCfg.HstiaRtiaSel = RTD_RTIA;

    // Switch matrix for RTD measurement
    HsLoopCfg.SWMatCfg.Dswitch = RTD_DSWITCH;
    HsLoopCfg.SWMatCfg.Pswitch = RTD_PSWITCH;
    HsLoopCfg.SWMatCfg.Nswitch = RTD_NSWITCH;
    HsLoopCfg.SWMatCfg.Tswitch = RTD_TSWITCH;

    // Configure waveform generator
    HsLoopCfg.WgCfg.WgType = WGTYPE_MMR;
    HsLoopCfg.WgCfg.GainCalEn = bFALSE;
    HsLoopCfg.WgCfg.OffsetCalEn = bFALSE;
    uint32_t code = mV_to_WgCode(sensor_mV);
    HsLoopCfg.WgCfg.WgCode = code;

    // Apply configuration twice (didn't seem to stick unless we did)
    AD5940_HSLoopCfgS(&HsLoopCfg);
    AD5940_HSLoopCfgS(&HsLoopCfg);

    // Configure ADC
    dsp_cfg.ADCBaseCfg.ADCMuxN = ADCMUXN_HSTIA_N;
    dsp_cfg.ADCBaseCfg.ADCMuxP = ADCMUXP_HSTIA_P;
    dsp_cfg.ADCBaseCfg.ADCPga = ADCPGA_1;

    dsp_cfg.ADCFilterCfg.ADCAvgNum = ADCAVGNUM_16;
    dsp_cfg.ADCFilterCfg.ADCRate = ADCRATE_800KHZ;
    dsp_cfg.ADCFilterCfg.ADCSinc2Osr = ADCSINC2OSR_44;
    dsp_cfg.ADCFilterCfg.ADCSinc3Osr = ADCSINC3OSR_4;
    dsp_cfg.ADCFilterCfg.BpNotch = bTRUE;
    dsp_cfg.ADCFilterCfg.BpSinc3 = bTRUE;
    dsp_cfg.ADCFilterCfg.Sinc2NotchEnable = bTRUE;

    AD5940_DSPCfgS(&dsp_cfg);

    // Enable AFE blocks
    AD5940_AFECtrlS(AFECTRL_HSTIAPWR | AFECTRL_INAMPPWR | AFECTRL_EXTBUFPWR | AFECTRL_WG | AFECTRL_DACREFPWR | AFECTRL_HSDACPWR | AFECTRL_SINC2NOTCH | AFECTRL_ADCPWR,
                    bTRUE);

    // Restart waveform generator with proper sequence
    AD5940_AFECtrlS(AFECTRL_WG, bFALSE);
    AD5940_Delay10us(1000); // 10ms settling
    AD5940_AFECtrlS(AFECTRL_WG, bTRUE);

    // Extended settling time for RTD measurement
    AD5940_Delay10us(500); // 5ms settling time

    // Sample RTD with improved averaging
    sampleADC(1);
    uint64_t accRTD = 0;
    for (uint32_t i = 0; i < NUM_RTD_SAMPLES; ++i)
    {
      accRTD += sampleADC(1); // Increased sampling delay

      AD5940_Delay10us(10); // Small delay between samples
    }
    adcCode_rtd = accRTD / NUM_RTD_SAMPLES;

    // Reconfigure switch matrix for reference resistor
    HsLoopCfg.SWMatCfg.Dswitch = SWD_RCAL0;
    HsLoopCfg.SWMatCfg.Pswitch = SWP_RCAL0;
    HsLoopCfg.SWMatCfg.Nswitch = SWN_RCAL1;
    HsLoopCfg.SWMatCfg.Tswitch = SWT_RCAL1 | SWT_TRTIA;

    // Apply new switch configuration
    AD5940_SWMatrixCfgS(&HsLoopCfg.SWMatCfg);

    // Additional settling time after switch change
    AD5940_Delay10us(500); // 5ms settling
    sampleADC(1);
    // Sample reference resistor
    uint64_t accREF = 0;
    for (uint32_t i = 0; i < NUM_RTD_SAMPLES; ++i)
    {
      accREF += sampleADC(1);
      AD5940_Delay10us(10);
    }
    adcCode_ref = accREF / NUM_RTD_SAMPLES;

    // reset switch matrix
    resetSwitchMatrix();

    // Disable all AFE blocks that were enabled
    AD5940_AFECtrlS(AFECTRL_HSTIAPWR | AFECTRL_INAMPPWR | AFECTRL_EXTBUFPWR | AFECTRL_WG | AFECTRL_DACREFPWR | AFECTRL_HSDACPWR | AFECTRL_SINC2NOTCH | AFECTRL_ADCPWR, bFALSE);

    // Convert to voltages
    volt_ref = AD5940_ADCCode2Volt(adcCode_ref, ADCPGA_1, 1.82);
    volt_rtd = AD5940_ADCCode2Volt(adcCode_rtd, ADCPGA_1, 1.82);

    // Calculate resistance
    rtd_resistance = RtdRefRes * (volt_ref / volt_rtd);

    // Verbose output
    if (verboseMode)
    {
      activeSerial->println(" ");

      // activeSerial->println("RTD Measurement");
      // activeSerial->print("  RCal Averaged Measurement: ");
      activeSerial->println(adcCode_ref);
      // activeSerial->print("  RTD Averaged Measurement: ");
      activeSerial->println(adcCode_rtd);
      // activeSerial->printf("  NUM_SAMPLES: %u", NUM_RTD_SAMPLES);
      activeSerial->println(" ");
      // activeSerial->print("  RTD TIA Resistor: ");
      switch (RTD_RTIA)
      {
      case HSTIARTIA_200:
        // activeSerial->println("200 Ohm");
        break;
      case HSTIARTIA_1K:
        // activeSerial->println("1K Ohm");
        break;
      case HSTIARTIA_5K:
        // activeSerial->println("5K Ohm");
        break;
      case HSTIARTIA_10K:
        // activeSerial->println("10K Ohm");
        break;
      case HSTIARTIA_20K:
        // activeSerial->println("20K Ohm");
        break;
      case HSTIARTIA_40K:
        // activeSerial->println("40K Ohm");
        break;
      case HSTIARTIA_80K:
        // activeSerial->println("80K Ohm");
        break;
      case HSTIARTIA_160K:
        // activeSerial->println("160K Ohm");
        break;
      default:
        // activeSerial->println("Unknown");
        break;
      }
    }
    return rtd_resistance;
  }

  void updateEEPROMdata(String command)
  {
    updateEepromStatus = UPDATE_EEPROM_STATUS_SUCCESS;
    int firstComma = command.indexOf(',');
    if (firstComma < 0)
    {
      updateEepromStatus = UPDATE_EEPROM_STATUS_INVALID_CMD;
      // activeSerial->println("Error: Invalid save command format");
      return;
    }
    String dataStr = command.substring(firstComma + 1);

    // Calculate maximum values we can store in AT25010B (128 bytes)
    // DOUBLE_COUNT * 8 + float_count * 4 <= 128
    int maxFloats = (128 - (DOUBLE_COUNT * 8)) / 4;
    int maxTotal = DOUBLE_COUNT + maxFloats;

    double doubleValues[DOUBLE_COUNT];
    float floatValues[maxFloats];
    int valueIndex = 0;

    // Parse values
    while (valueIndex < maxTotal && dataStr.length() > 0)
    {
      int commaPos = dataStr.indexOf(',');
      String valueStr;
      if (commaPos >= 0)
      {
        valueStr = dataStr.substring(0, commaPos);
        dataStr = dataStr.substring(commaPos + 1);
      }
      else
      {
        // Last value
        valueStr = dataStr;
        dataStr = ""; // Clear dataStr to end the loop
      }

      // Convert string and store in appropriate array
      if (valueStr.length() > 0)
      {
        if (valueIndex < DOUBLE_COUNT)
        {
          // Store as double
          doubleValues[valueIndex] = strtod(valueStr.c_str(), NULL);
        }
        else
        {
          // Store as float
          floatValues[valueIndex - DOUBLE_COUNT] = strtof(valueStr.c_str(), NULL);
        }
        valueIndex++;
      }
    }

    // Check if we got at least 1 value
    if (valueIndex == 0)
    {
      // activeSerial->println("Error: No valid values found");
      updateEepromStatus = UPDATE_EEPROM_STATUS_NO_VALID_VALUES;
      return;
    }
    if (valueIndex > maxTotal)
    {
      updateEepromStatus = UPDATE_EEPROM_STATUS_TOO_MANY_VALUES;
      return;
    }

    int doubleCount = min(valueIndex, DOUBLE_COUNT);
    int floatCount = max(0, valueIndex - DOUBLE_COUNT);

    unsigned int baseAddress = 0;
    writeMixedValues(baseAddress, doubleValues, doubleCount, floatValues, floatCount);
    // activeSerial->println("save attempted");

    // Read back the values to confirm they were written correctly
    double readDoubles[DOUBLE_COUNT];
    float readFloats[maxFloats];
    readMixedValues(baseAddress, readDoubles, doubleCount, readFloats, floatCount);

    // activeSerial->println("Verification - Values read back from EEPROM:");
    bool DoubleVerificationSuccess = true;
    bool FloatVerificationSuccess = true;

    // Verify doubles
    for (int i = 0; i < doubleCount; i++)
    {
      // activeSerial->print(i);
      // activeSerial->print(" (double): ");
      // activeSerial->println(readDoubles[i], 10);
      if (abs(doubleValues[i] - readDoubles[i]) > 0.000000001)
      {
        DoubleVerificationSuccess = false;
        break;
        // activeSerial->print("Warning: Double value ");
        // activeSerial->print(i);
        // activeSerial->println(" does not match exactly what was written");
      }
    }

    // Verify floats
    for (int i = 0; i < floatCount; i++)
    {
      // activeSerial->print(i + DOUBLE_COUNT);
      // activeSerial->print(" (float): ");
      // activeSerial->println(readFloats[i], 6);
      if (abs(floatValues[i] - readFloats[i]) > 0.000001)
      {
        FloatVerificationSuccess = false;
        break;
        // activeSerial->print("Warning: Float value ");
        // activeSerial->print(i + DOUBLE_COUNT);
        // activeSerial->println(" does not match exactly what was written");
      }
    }

    if (DoubleVerificationSuccess && FloatVerificationSuccess)
    {
      updateEepromStatus = UPDATE_EEPROM_STATUS_SUCCESS;
      // activeSerial->println("Verification successful: All values match!");
    }
    else
    {
      updateEepromStatus = UPDATE_EEPROM_STATUS_VALUES_VERIFICATION_FAILED;
    }
  }

  void getEEPROMdata(void)
  {
    unsigned int baseAddress = 0; // Same address used in save

    // Calculate maximum values we can store
    int maxFloats = (128 - (DOUBLE_COUNT * 8)) / 4;

    double doubleValues[DOUBLE_COUNT];
    float floatValues[maxFloats];

    readMixedValues(baseAddress, doubleValues, DOUBLE_COUNT, floatValues, maxFloats);

    // Display doubles
    for (int i = 0; i < DOUBLE_COUNT; i++)
    {
      eepromDataPacket.doubleValue[i] = doubleValues[i];
    }

    // Display floats
    for (int i = 0; i < maxFloats; i++)
    {
      eepromDataPacket.floatValue[i] = floatValues[i];
    }

    activeSerial->write((uint8_t *)&eepromDataPacket, sizeof(eepromDataPacket));
  }

  void handleMstCommand(String cmd)
  {
    // Check if the first 4 characters are "mst,"
    if (cmd.startsWith("mst,"))
    {
      // Remove "mst," prefix
      String dataPart = cmd.substring(4);

      // Split by comma
      int idx = 0;
      int start = 0;
      int end = 0;

      while (idx < MAX_CONDUCTIVITY_MST_PARAM_IDX && start < dataPart.length())
      {
        end = dataPart.indexOf(',', start);
        if (end == -1)
          end = dataPart.length(); // Last value

        String update_param = "mst,";
        String token = dataPart.substring(start, end);

        // Assign to tempSettings
        switch (idx)
        {
        case CONDUCTIVITY_MST_PARAM_IDX_SINFREQ:
          tempSettings.SinFreq = token.toFloat();
          update_param += String(cfgSettingsParam[idx]) + "," + String(tempSettings.SinFreq, 4);
          break;

        case CONDUCTIVITY_MST_PARAM_IDX_DACPP:
          tempSettings.DacVoltPP = token.toFloat();
          update_param += String(cfgSettingsParam[idx]) + "," + String(tempSettings.DacVoltPP, 4);
          break;

        case CONDUCTIVITY_MST_PARAM_IDX_BIAS:
          tempSettings.BiasVolt = token.toFloat();
          update_param += String(cfgSettingsParam[idx]) + "," + String(tempSettings.BiasVolt, 4);
          break;

        case CONDUCTIVITY_MST_PARAM_IDX_RTIA:
          tempSettings.HstiaRtiaSel = token.toInt();
          update_param += String(cfgSettingsParam[idx]) + "," + String(tempSettings.HstiaRtiaSel);
          break;

        case CONDUCTIVITY_MST_PARAM_IDX_PGA:
          tempSettings.AdcPgaGain = token.toInt();
          update_param += String(cfgSettingsParam[idx]) + "," + String(tempSettings.AdcPgaGain);
          break;

        case CONDUCTIVITY_MST_PARAM_IDX_DFTNUM:
          tempSettings.DftNum = token.toInt();
          update_param += String(cfgSettingsParam[idx]) + "," + String(tempSettings.DftNum);
          break;

        case CONDUCTIVITY_MST_PARAM_IDX_AVGNUM:
          tempSettings.ADCAvgNum = token.toInt();
          update_param += String(cfgSettingsParam[idx]) + "," + String(tempSettings.ADCAvgNum);
          break;
        }

        // Call handleMstCommand for this parameter
        updateMeasurementSettings(update_param, idx);

        start = end + 1;
        idx++;
      }
    }
  }

  void updateMeasurementSettings(String cmd, uint8_t idx)
  {
    if ((idx >= CONDUCTIVITY_MST_PARAM_IDX_SINFREQ) && (idx < MAX_CONDUCTIVITY_MST_PARAM_IDX))
    {
      updateCfgStatus[idx] = UPDATE_CFG_STATUS_SUCCESS;
    }
    else
    {
      return;
    }

    // Command format: cfg,parameter,value
    // Examples:
    // - cfg,sinfreq,1000.0    (Set sine wave frequency to 1000 Hz)

    // Array to hold parsed command parts
    String params[3];
    int paramIndex = 0;
    int startIndex = 0;

    // Parse command string using comma delimiter
    for (uint i = 0; i < cmd.length(); i++)
    {
      if (cmd.charAt(i) == ',')
      {
        params[paramIndex] = cmd.substring(startIndex, i);
        startIndex = i + 1;
        paramIndex++;

        // Safety check to prevent array overflow
        if (paramIndex >= 3)
          break;
      }
    }

    // Get last parameter (after the last comma)
    if (paramIndex < 3)
    {
      params[paramIndex] = cmd.substring(startIndex);
    }

    // Get configuration structure pointer
    AppIMPCfg_Type *pImpedanceCfg;
    AppIMPGetCfg(&pImpedanceCfg);

    // Extract parameter name and value
    String param = params[1];
    String value = params[2];

    // Handle special commands for saving/recalling configurations
    if (param == "save")
    {
      saveCurrentConfig();
      // activeSerial->println("Configuration saved");
      return;
    }
    else if (param == "recall")
    {
      recallSavedConfig();
      // activeSerial->println("Configuration recalled");
      return;
    }

    // Process different configuration parameters
    if (param == "sinfreq")
    {
      // Set sine wave frequency (Hz)
      pImpedanceCfg->SinFreq = value.toFloat();
      // activeSerial->printf("Set SinFreq to: %.2f Hz\n", pImpedanceCfg->SinFreq);
    }
    else if (param == "dacpp")
    {
      // Set DAC peak-to-peak voltage (mV)
      pImpedanceCfg->DacVoltPP = value.toFloat();
      // activeSerial->printf("Set DacVoltPP to: %.2f mV\n", pImpedanceCfg->DacVoltPP);
    }
    else if (param == "bias")
    {
      // Set DC bias voltage (mV)
      pImpedanceCfg->BiasVolt = value.toFloat();
      // activeSerial->printf("Set BiasVolt to: %.2f mV\n", pImpedanceCfg->BiasVolt);
    }
    else if (param == "rtia")
    {
      // Set TIA (Transimpedance Amplifier) feedback resistor value
      uint32_t rtia = DEFAULT_RTIA; // default value
      int valueTest = value.toInt();
      // activeSerial->print("Value:");
      // activeSerial->println(value);
      // Map string values to corresponding constants
      if (valueTest == 0)
        rtia = HSTIARTIA_200;
      else if (valueTest == 1)
        rtia = HSTIARTIA_1K;
      else if (valueTest == 2)
        rtia = HSTIARTIA_5K;
      else if (valueTest == 3)
        rtia = HSTIARTIA_10K;
      else if (valueTest == 4)
        rtia = HSTIARTIA_20K;
      else if (valueTest == 5)
        rtia = HSTIARTIA_40K;
      else if (valueTest == 6)
        rtia = HSTIARTIA_80K;
      else if (valueTest == 7)
        rtia = HSTIARTIA_160K;
      else
        updateCfgStatus[idx] = UPDATE_CFG_STATUS_NO_CHANGE_TO_TIA;
      // activeSerial->print("no change to tia");

      pImpedanceCfg->HstiaRtiaSel = rtia;
      // activeSerial->printf("Set RTIA to: %s\n", value.c_str());
      currentTIA = rtia;
      // activeSerial->print("currentTIA: ");
      // activeSerial->println(currentTIA);
    }
    else if (param == "pga")
    {
      // Set ADC Programmable Gain Amplifier gain
      uint32_t pga = ADCPGA_1; // default value (1x gain)

      // Map string values to corresponding constants
      if (value == "1")
        pga = ADCPGA_1;
      else if (value == "1.5")
        pga = ADCPGA_1P5;
      else if (value == "2")
        pga = ADCPGA_2;
      else if (value == "4")
        pga = ADCPGA_4;
      else if (value == "9")
        pga = ADCPGA_9;
      else
        updateCfgStatus[idx] = UPDATE_CFG_STATUS_NO_CHANGE_TO_PGA;

      pImpedanceCfg->AdcPgaGain = pga;
      // activeSerial->printf("Set PGA gain to: %s\n", value.c_str());
    }
    else if (param == "dftnum")
    {
      // Set DFT (Discrete Fourier Transform) number of points
      uint32_t dft = DFTNUM_4096; // default value (4096 points)

      // Map string values to corresponding constants
      if (value == "4096")
        dft = DFTNUM_4096;
      else if (value == "2048")
        dft = DFTNUM_2048;
      else if (value == "1024")
        dft = DFTNUM_1024;
      else if (value == "512")
        dft = DFTNUM_512;
      else if (value == "256")
        dft = DFTNUM_256;
      else
        updateCfgStatus[idx] = UPDATE_CFG_STATUS_NO_CHANGE_TO_DFT_NUM;

      pImpedanceCfg->DftNum = dft;
      // activeSerial->printf("Set DFT number to: %s\n", value.c_str());
    }
    else if (param == "avgnum")
    {
      // Set ADC averaging number (how many samples to average)
      uint32_t avg = ADCAVGNUM_16; // default value (16 samples)

      // Map string values to corresponding constants
      if (value == "2")
        avg = ADCAVGNUM_2;
      else if (value == "4")
        avg = ADCAVGNUM_4;
      else if (value == "8")
        avg = ADCAVGNUM_8;
      else if (value == "16")
        avg = ADCAVGNUM_16;
      else
        updateCfgStatus[idx] = UPDATE_CFG_STATUS_NO_CHANGE_TO_AVG_NUM;

      pImpedanceCfg->ADCAvgNum = avg;
      // activeSerial->printf("Set ADC average number to: %s\n", value.c_str());
    }
    else if (param == "sweep")
    {
      // Enable/disable frequency sweep mode
      bool enableSweep = (value == "1" || value.equalsIgnoreCase("true"));
      pImpedanceCfg->SweepCfg.SweepEn = enableSweep ? bTRUE : bFALSE;
      // activeSerial->printf("Set sweep enable to: %d\n", pImpedanceCfg->SweepCfg.SweepEn);
    }
    else if (param == "sweepstart")
    {
      // Set sweep start frequency (Hz)
      pImpedanceCfg->SweepCfg.SweepStart = value.toFloat();
      // activeSerial->printf("Set sweep start frequency to: %.2f Hz\n", pImpedanceCfg->SweepCfg.SweepStart);
    }
    else if (param == "sweepstop")
    {
      // Set sweep stop frequency (Hz)
      pImpedanceCfg->SweepCfg.SweepStop = value.toFloat();
      // activeSerial->printf("Set sweep stop frequency to: %.2f Hz\n", pImpedanceCfg->SweepCfg.SweepStop);
    }
    else if (param == "sweeppoints")
    {
      // Set number of frequency points in the sweep
      pImpedanceCfg->SweepCfg.SweepPoints = value.toInt();
      // activeSerial->printf("Set sweep points to: %d\n", pImpedanceCfg->SweepCfg.SweepPoints);
    }
    else if (param == "sweeplog")
    {
      // Set logarithmic (true) or linear (false) frequency spacing
      bool logSpacing = (value == "1" || value.equalsIgnoreCase("true"));
      pImpedanceCfg->SweepCfg.SweepLog = logSpacing ? bTRUE : bFALSE;
      // activeSerial->printf("Set sweep log mode to: %d\n", pImpedanceCfg->SweepCfg.SweepLog);
    }
    else
    {
      // Handle unrecognized parameter
      // activeSerial->println("Malformed command: unrecognized parameter");
      updateCfgStatus[idx] = UPDATE_CFG_STATUS_ERR_UNRECOGNIZED_PARAM;
      return;
    }

    // Mark that parameters have been changed and need to be applied
    pImpedanceCfg->bParaChanged = bTRUE;

    // Automatically save the configuration after any successful change
    saveCurrentConfig();
  }

  void getMesaurementSettings(void)
  {
    // Pointer to store configuration structure
    AppIMPCfg_Type *cfg;

    // Get the current impedance measurement configuration
    AppIMPGetCfg(&cfg);

    measurementSettingsPacket.SinFreq = cfg->SinFreq;
    measurementSettingsPacket.DacVoltPP = cfg->DacVoltPP;
    measurementSettingsPacket.BiasVolt = cfg->BiasVolt;
    measurementSettingsPacket.HstiaRtiaSel = cfg->HstiaRtiaSel;
    measurementSettingsPacket.AdcPgaGain = cfg->AdcPgaGain;
    measurementSettingsPacket.DftNum = cfg->DftNum;
    measurementSettingsPacket.ADCAvgNum = cfg->ADCAvgNum;
    activeSerial->write((uint8_t *)&measurementSettingsPacket, sizeof(measurementSettingsPacket));
  }

  void getInitStatus(void)
  {
    // Count units by status
    int skippedCount = 0;
    int failedCount = 0;
    int successCount = 0;

    // If Initialization function has been completely executed
    if (true == isInitialized)
    {
      // Count number of skipped, failed and successful sensor initialization
      for (int i = 0; i < 6; i++)
      {
        if (unitStatus[i] == STATUS_SKIPPED)
        {
          skippedCount++;
        }
        else if (unitStatus[i] == STATUS_FAILED)
        {
          failedCount++;
        }
        else
        { // STATUS_SUCCESS
          successCount++;
        }
      }

      // If any of the sensor was skipped or failed, then
      if ((skippedCount > 0) || (failedCount > 0))
      {
        // Set init status to failed
        initStatus = INIT_STATUS_FAILED;
      }
      // If all the sensors were initialized successfully, then
      else if (successCount == MAX_NUM_OF_SENSORS)
      {
        // Set the init status to initialized.
        initStatus = INIT_STATUS_INITIALIZED;
      }
      else
      {
        // Still need to decide, what to do here.
      }
    }

    activeSerial->write((uint8_t *)&initStatus, sizeof(initStatus));
  }

  void sendAllSensorData(void)
  {
    while (sendSensorData)
    {
      String command = activeSerial->readStringUntil('\n');
      if (command.length() > 0)
      {
        activeSerial->write((uint8_t)1);
        break; // Exit if command received
      }
      else
      {
        if (true == getAllMesaurements())
        {
          // Send all the sensor data to DD
          activeSerial->write((uint8_t *)&singleSensorPacket, sizeof(singleSensorPacket));
        }
        delay(250);
      }
  }
}

void initSinglePacketToDefault(void)
{
  singleSensorPacket.sensorNum = 0;  // Conductivity Sensor number i.e unit 1 to 6.
  singleSensorPacket.impFreq = 0.0;     // Impedance Frequency
  singleSensorPacket.impDataPoints = 0; // Impedance Data Points. Hard coded to 1
  singleSensorPacket.impRzMag = 0.0;    // Value of medianMag
  singleSensorPacket.impRzPhase = 0.0;  // Value of medianPhase
  singleSensorPacket.rtdFreq = 0.0;     // RTD Frequency. Hard coded to 0.0
  singleSensorPacket.rtdDataPoints = 0; // Impedance Data Points. Hard coded to 1
  singleSensorPacket.rtdRzMag = 0.0;    // Value of rtd_resistance
  singleSensorPacket.rtdRzPhase = 0.0;  // RTD Rz Phase. Hard coded to 0.0
}

void initPacketToDefault(int unit)
{
  if (true == isSensorValid(unit))
  {
    // Pack the sensor data
    // The ALY code loops from 1 to 6. Our packet index from 0 to 5. So, index at i-1.
    int sensorIdx = unit - 1;
    sensorPacket[sensorIdx].sensorNum = 0;  // Conductivity Sensor number i.e unit 1 to 6.
    sensorPacket[sensorIdx].impFreq = 0.0;     // Impedance Frequency
    sensorPacket[sensorIdx].impDataPoints = 0; // Impedance Data Points. Hard coded to 1
    sensorPacket[sensorIdx].impRzMag = 0.0;    // Value of medianMag
    sensorPacket[sensorIdx].impRzPhase = 0.0;  // Value of medianPhase
    sensorPacket[sensorIdx].rtdFreq = 0.0;     // RTD Frequency. Hard coded to 0.0
    sensorPacket[sensorIdx].rtdDataPoints = 0; // Impedance Data Points. Hard coded to 1
    sensorPacket[sensorIdx].rtdRzMag = 0.0;    // Value of rtd_resistance
    sensorPacket[sensorIdx].rtdRzPhase = 0.0;  // RTD Rz Phase. Hard coded to 0.0
  }
}

bool isSensorValid(int unit)
{
  bool retVal = false;
  if ((unit >= 1) && (unit <= 6))
  {
    retVal = true;
  }
  return retVal;
}

bool isSensorInitialized(int unit)
{
  bool retVal = false;
  if (true == isSensorValid(unit))
  {
    int sensorIdx = unit - 1;
    if (unitStatus[sensorIdx] == STATUS_SUCCESS)
    {
      retVal = true;
    }
  }
  return retVal;
}
#endif /* ad5940_library_extension_C */