#include <SPI.h> // ARDUINO SPI PINS USEDD FOR MCP320X.H
#include <Mcp320x.h>
#include <ShiftRegister74HC595.h>
#include <EEPROM.h>
#include <avr/pgmspace.h>

// Pin definitions FOR 74HC595 USING SHIFTGRGISTER74HC595.H LIBRARY
#define PEAK_RESET_PIN 2
#define SR_DATA_PIN 5
#define SR_CLK_PIN 3
#define SR_LATCH_PIN 4

//CS PIN FOR MC320X
#define CS_PIN 10

// SPI settings
#define ADC_VREF 5000  // Reference voltage in mV
#define ADC_CLK 1600000  // SPI clock speed in Hz
#define SATURATION_THRESHOLD 3500  // Adjusted based on data

// Resistance steps in PROGMEM (64 steps)
const PROGMEM int resistanceSteps[] = {
  5175, 5000, 4800, 4600, 4400, 4200, 4000, 3800, 3600, 3400, 3200, 3000, 2800, 2600, 2400, 2200,
  2000, 1900, 1800, 1700, 1600, 1500, 1400, 1300, 1200, 1100, 1000, 900, 800, 700, 600, 500,
  475, 450, 425, 400, 375, 350, 325, 300, 275, 250, 225, 200, 175, 150, 125, 100,
  90, 80, 70, 60, 50, 40, 30, 20, 15, 12, 10, 8, 6, 4, 2, 1
};
const int numSteps = sizeof(resistanceSteps) / sizeof(resistanceSteps[0]);
int currentStep = 0;

// Attenuation settings
const int ATTENUATION_STEPS[] = {0, 10, 20, 30};
uint8_t currentAttenuationIndex = 0;
float attenuationFactor = 1.0;

// Measurement variables
float V_shunt_RMS = 0.0;
float emaRMS = 0.0;
float emaPosPeak = 0.0;
float emaNegPeak = 0.0;
uint16_t rawRMS0 = 0, rawRMS1 = 0, rawRMS2 = 0;
uint16_t rawPosPeak = 0, rawNegPeak = 0;
const float ALPHA = 0.3;
float rmsVoltage = 0.0;
float posPeak = 0.0;
float negPeak = 0.0;
float current = 0.0;
float power = 0.0;
float crestFactor = 0.0;
float peakToPeak = 0.0;
uint8_t activeChannel = 1;
float V_cal_RMS = 0.0;
float V_cal_posPeak = 0.0;
float V_cal_negPeak = 0.0;

// Calibration variables
const int CAL_POINTS = 10;
float rmsOffset0 = 0.0, rmsOffset1 = 0.0, rmsOffset2 = 0.0;
float posPeakOffset = 0.0, negPeakOffset = 0.0;
const PROGMEM float calRMS0Values[CAL_POINTS] = {0.0, 0.00354, 0.01061, 0.01768, 0.03536, 0.07071, 0.10607, 0.14142, 0.17678, 0.21213};  // Gain 100, 0 to 0.60 Vpp
const PROGMEM float calRMS1Values[CAL_POINTS] = {0.0, 0.28284, 0.56568, 0.84853, 1.13137, 1.41421, 1.69706, 1.97990, 2.26274, 2.82843};  // Gain 1, 0 to 8 Vpp
const PROGMEM float calRMS2Values[CAL_POINTS] = {0.0, 0.07071, 0.14142, 0.21213, 0.28284, 0.35355, 0.42426, 0.49497, 0.56568, 0.63640};  // Gain 10, 0 to 1.8 Vpp
const PROGMEM float calPPValues[CAL_POINTS] = {0.0, 0.5, 1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0};  // Peak-to-Peak, max 8 Vpp
bool calibrationChan0Complete = false;
bool calibrationChan1Complete = false;
bool calibrationChan2Complete = false;

// EEPROM addresses
const int EEPROM_RMS0ADC = 20;
const int EEPROM_RMS1ADC = 60;
const int EEPROM_RMS2ADC = 100;
const int EEPROM_POSPEAKADC = 140;
const int EEPROM_NEGPEAKADC = 180;

// Safety limits
const float MAX_VOLTAGE = 10000;
const float MAX_CURRENT = 2.2;
const float R_shunt = 2.0;

// Test configuration
enum TestMode { MANUAL, CAL_CHAN0, CAL_CHAN1, CAL_CHAN2, CAL_PEAK };
TestMode currentMode = MANUAL;
unsigned long previousMillis = 0;
bool testRunning = false;
const uint8_t SAMPLE_COUNT = 30;

// Objects
MCP3208 adc(ADC_VREF, CS_PIN);
ShiftRegister74HC595 sr(2, SR_DATA_PIN, SR_CLK_PIN, SR_LATCH_PIN);

void setup() {
  pinMode(PEAK_RESET_PIN, OUTPUT);
  Serial.begin(115200);
  while (!Serial) ; // Wait for Serial to initialize
  SPI.begin();
  SPI.beginTransaction(SPISettings(ADC_CLK, MSBFIRST, SPI_MODE0));
  loadCalibrationFromEEPROM();
  updateVariableResistance();
  setAttenuation(0);
  Serial.println(F("QA-ES Initialized"));
}

void loop() {
  unsigned long currentMillis = millis();
  switch (currentMode) {
    case MANUAL: runManualMeasurement(currentMillis); break;
    case CAL_CHAN0: runCalibrationChan0(currentMillis); break;
    case CAL_CHAN1: runCalibrationChan1(currentMillis); break;
    case CAL_CHAN2: runCalibrationChan2(currentMillis); break;
    case CAL_PEAK: runCalibrationPeak(currentMillis); break;
  }
  if (Serial.available()) handleSerialCommand();
}

void takeMeasurement() {
  resetPeaks();
  delay(5);

  const uint8_t SAMPLE_AVG = 20;
  uint32_t rms0Sum = 0, rms2Sum = 0, rms1Sum = 0, posPeakSum = 0, negPeakSum = 0;
  for (uint8_t i = 0; i < SAMPLE_AVG; i++) {
    rms0Sum += adc.read(MCP3208::Channel::SINGLE_0);
    rms2Sum += adc.read(MCP3208::Channel::SINGLE_2);
    rms1Sum += adc.read(MCP3208::Channel::SINGLE_1);
    posPeakSum += adc.read(MCP3208::Channel::SINGLE_3);
    negPeakSum += adc.read(MCP3208::Channel::SINGLE_4);
    delayMicroseconds(100);
  }
  rawRMS0 = rms0Sum / SAMPLE_AVG;
  rawRMS2 = rms2Sum / SAMPLE_AVG;
  rawRMS1 = rms1Sum / SAMPLE_AVG;
  rawPosPeak = posPeakSum / SAMPLE_AVG;
  rawNegPeak = negPeakSum / SAMPLE_AVG;

  if (rawRMS0 < SATURATION_THRESHOLD) {
    activeChannel = 0;
    emaRMS = ALPHA * rawRMS0 + (1 - ALPHA) * emaRMS;
  } else if (rawRMS2 < SATURATION_THRESHOLD) {
    activeChannel = 2;
    emaRMS = ALPHA * rawRMS2 + (1 - ALPHA) * emaRMS;
  } else {
    activeChannel = 1;
    emaRMS = ALPHA * rawRMS1 + (1 - ALPHA) * emaRMS;
  }
  
  emaPosPeak = ALPHA * rawPosPeak + (1 - ALPHA) * emaPosPeak;
  emaNegPeak = ALPHA * rawNegPeak + (1 - ALPHA) * emaNegPeak;

  float calRMSADC[CAL_POINTS];
  switch (activeChannel) {
    case 0: 
      loadArrayFromEEPROM(calRMSADC, EEPROM_RMS0ADC);
      V_cal_RMS = calibrationChan0Complete ? getCorrectedRMS(emaRMS, calRMSADC, calRMS0Values, rmsOffset0) : 
                    (emaRMS / 4095.0 * (ADC_VREF / 1000.0) / 100.0);
      break;
    case 2: 
      loadArrayFromEEPROM(calRMSADC, EEPROM_RMS2ADC);
      V_cal_RMS = calibrationChan2Complete ? getCorrectedRMS(emaRMS, calRMSADC, calRMS2Values, rmsOffset2) : 
                    (emaRMS / 4095.0 * (ADC_VREF / 1000.0) / 10.0);
      break;
    case 1: 
      loadArrayFromEEPROM(calRMSADC, EEPROM_RMS1ADC);
      V_cal_RMS = calibrationChan1Complete ? getCorrectedRMS(emaRMS, calRMSADC, calRMS1Values, rmsOffset1) : 
                    (emaRMS / 4095.0 * (ADC_VREF / 1000.0));
      break;
  }

  loadArrayFromEEPROM(calRMSADC, EEPROM_POSPEAKADC);
  V_cal_posPeak = getCorrectedRMS(emaPosPeak, calRMSADC, calPPValues, posPeakOffset) / 2.0;
  loadArrayFromEEPROM(calRMSADC, EEPROM_NEGPEAKADC);
  V_cal_negPeak = -getCorrectedRMS(emaNegPeak, calRMSADC, calPPValues, negPeakOffset) / 2.0;

  V_shunt_RMS = V_cal_RMS * attenuationFactor;
  float V_shunt_posPeak = V_cal_posPeak * attenuationFactor;
  float V_shunt_negPeak = V_cal_negPeak * attenuationFactor;

  current = V_shunt_RMS / R_shunt;
  float resistance = pgm_read_word(&resistanceSteps[currentStep]);
  rmsVoltage = current * resistance;

  posPeak = V_shunt_posPeak;
  negPeak = V_shunt_negPeak;

  power = rmsVoltage * current;
  float maxPeak = max(fabs(posPeak), fabs(negPeak));
  crestFactor = (V_shunt_RMS > 0.001) ? (maxPeak / V_shunt_RMS) : 0.0;
  peakToPeak = posPeak - negPeak;
}

void printResults() {
  float totalImpedance = pgm_read_word(&resistanceSteps[currentStep]);
  Serial.print(F("Chan: ")); Serial.print(activeChannel); Serial.print(F("\t"));
  Serial.print(F("V_shunt: ")); Serial.print(V_shunt_RMS, 3); Serial.print(F(" V\t"));
  Serial.print(F("V_load: ")); Serial.print(rmsVoltage, 3); Serial.print(F(" V\t"));
  Serial.print(F("PosP: ")); Serial.print(posPeak, 3); Serial.print(F(" V\t"));
  Serial.print(F("NegP: ")); Serial.print(negPeak, 3); Serial.print(F(" V\t"));
  Serial.print(F("Curr: ")); Serial.print(current * 1000, 3); Serial.print(F(" mA\t"));
  Serial.print(F("Pwr: ")); Serial.print(power, 3); Serial.print(F(" W\t"));
  Serial.print(F("Crest: ")); Serial.print(crestFactor, 2); Serial.print(F("\t"));
  Serial.print(F("P-P: ")); Serial.print(peakToPeak, 3); Serial.print(F(" V\t"));
  Serial.print(F("Imp: ")); Serial.print(totalImpedance); Serial.print(F(" ohm\t"));
  Serial.print(F("Att: ")); Serial.print(ATTENUATION_STEPS[currentAttenuationIndex]); Serial.print(F(" dB\t"));
  Serial.print(F("Raw0: ")); Serial.print(rawRMS0); Serial.print(F("\t"));
  Serial.print(F("Raw1: ")); Serial.print(rawRMS1); Serial.print(F("\t"));
  Serial.print(F("Raw2: ")); Serial.print(rawRMS2); Serial.print(F("\t"));
  Serial.print(F("RawPP: ")); Serial.print(rawPosPeak); Serial.print(F("\t"));
  Serial.print(F("RawNP: ")); Serial.print(rawNegPeak); Serial.println();
}

void runManualMeasurement(unsigned long currentMillis) {
  takeMeasurement();
  if (currentMillis - previousMillis >= 500) {
    printResults();
    previousMillis = currentMillis;
  }
}

void runCalibrationChan0(unsigned long currentMillis) {
  static uint8_t calStep = 0;
  static bool promptPrinted = false;

  if (!testRunning) {
    Serial.println(F("Starting Calibration for Channel 0 (Gain 100) RMS..."));
    testRunning = true;
    calStep = 0;
    promptPrinted = false;
    calibrationChan0Complete = false;
    while (Serial.available()) Serial.read(); // Clear buffer
  }

  if (calStep < CAL_POINTS) {
    if (!promptPrinted) {
      float vrms = pgm_read_float(&calRMS0Values[calStep]);
      float vpp = 2 * sqrt(2) * vrms;
      Serial.print(F("Step ")); Serial.print(calStep + 1);
      Serial.print(F(": Set sine wave Vpp = ")); Serial.print(vpp, 4);
      Serial.print(F(" V (RMS = ")); Serial.print(vrms, 4);
      Serial.println(F(" V), then press 'Y' to proceed."));
      promptPrinted = true;
    }

    if (Serial.available()) {
      char c = Serial.read();
      while (Serial.available()) Serial.read(); // Clear remaining buffer
      if (c == 'Y' || c == 'y') {
        Serial.print(F("Calibrating step ")); Serial.print(calStep + 1); Serial.println(F("..."));
        calibratePoint(calStep, 0, EEPROM_RMS0ADC);
        calStep++;
        promptPrinted = false; // Reset for next step
        if (calStep >= CAL_POINTS) {
          calibrationChan0Complete = true;
          Serial.println(F("Channel 0 RMS Calibration Complete"));
          testRunning = false;
          currentMode = MANUAL;
        }
      } else {
        Serial.println(F("Invalid input. Press 'Y' to proceed."));
      }
    }
  }
}

void runCalibrationChan1(unsigned long currentMillis) {
  static uint8_t calStep = 0;
  static bool promptPrinted = false;

  if (!testRunning) {
    Serial.println(F("Starting Calibration for Channel 1 (Gain 1) RMS..."));
    testRunning = true;
    calStep = 0;
    promptPrinted = false;
    calibrationChan1Complete = false;
    while (Serial.available()) Serial.read(); // Clear buffer
  }

  if (calStep < CAL_POINTS) {
    if (!promptPrinted) {
      float vrms = pgm_read_float(&calRMS1Values[calStep]);
      float vpp = 2 * sqrt(2) * vrms;
      Serial.print(F("Step ")); Serial.print(calStep + 1);
      Serial.print(F(": Set sine wave Vpp = ")); Serial.print(vpp, 4);
      Serial.print(F(" V (RMS = ")); Serial.print(vrms, 4);
      Serial.println(F(" V), then press 'Y' to proceed."));
      promptPrinted = true;
    }

    if (Serial.available()) {
      char c = Serial.read();
      while (Serial.available()) Serial.read(); // Clear remaining buffer
      if (c == 'Y' || c == 'y') {
        Serial.print(F("Calibrating step ")); Serial.print(calStep + 1); Serial.println(F("..."));
        calibratePoint(calStep, 1, EEPROM_RMS1ADC);
        calStep++;
        promptPrinted = false; // Reset for next step
        if (calStep >= CAL_POINTS) {
          calibrationChan1Complete = true;
          Serial.println(F("Channel 1 RMS Calibration Complete"));
          testRunning = false;
          currentMode = MANUAL;
        }
      } else {
        Serial.println(F("Invalid input. Press 'Y' to proceed."));
      }
    }
  }
}

void runCalibrationChan2(unsigned long currentMillis) {
  static uint8_t calStep = 0;
  static bool promptPrinted = false;

  if (!testRunning) {
    Serial.println(F("Starting Calibration for Channel 2 (Gain 10) RMS..."));
    testRunning = true;
    calStep = 0;
    promptPrinted = false;
    calibrationChan2Complete = false;
    while (Serial.available()) Serial.read(); // Clear buffer
  }

  if (calStep < CAL_POINTS) {
    if (!promptPrinted) {
      float vrms = pgm_read_float(&calRMS2Values[calStep]);
      float vpp = 2 * sqrt(2) * vrms;
      Serial.print(F("Step ")); Serial.print(calStep + 1);
      Serial.print(F(": Set sine wave Vpp = ")); Serial.print(vpp, 4);
      Serial.print(F(" V (RMS = ")); Serial.print(vrms, 4);
      Serial.println(F(" V), then press 'Y' to proceed."));
      promptPrinted = true;
    }

    if (Serial.available()) {
      char c = Serial.read();
      while (Serial.available()) Serial.read(); // Clear remaining buffer
      if (c == 'Y' || c == 'y') {
        Serial.print(F("Calibrating step ")); Serial.print(calStep + 1); Serial.println(F("..."));
        calibratePoint(calStep, 2, EEPROM_RMS2ADC);
        calStep++;
        promptPrinted = false; // Reset for next step
        if (calStep >= CAL_POINTS) {
          calibrationChan2Complete = true;
          Serial.println(F("Channel 2 RMS Calibration Complete"));
          testRunning = false;
          currentMode = MANUAL;
        }
      } else {
        Serial.println(F("Invalid input. Press 'Y' to proceed."));
      }
    }
  }
}

void runCalibrationPeak(unsigned long currentMillis) {
  static uint8_t calStep = 0;
  static bool promptPrinted = false;

  if (!testRunning) {
    Serial.println(F("Starting Calibration for Peak-to-Peak (Channels 3 and 4)..."));
    testRunning = true;
    calStep = 0;
    promptPrinted = false;
    while (Serial.available()) Serial.read(); // Clear buffer
  }

  if (calStep < CAL_POINTS) {
    if (!promptPrinted) {
      float vpp = pgm_read_float(&calPPValues[calStep]);
      Serial.print(F("Step ")); Serial.print(calStep + 1);
      Serial.print(F(": Set sine wave Vpp = ")); Serial.print(vpp, 4);
      Serial.println(F(" V, then press 'Y' to proceed."));
      promptPrinted = true;
    }

    if (Serial.available()) {
      char c = Serial.read();
      while (Serial.available()) Serial.read(); // Clear remaining buffer
      if (c == 'Y' || c == 'y') {
        Serial.print(F("Calibrating step ")); Serial.print(calStep + 1); Serial.println(F("..."));
        calibratePeakPoint(calStep);
        calStep++;
        promptPrinted = false; // Reset for next step
        if (calStep >= CAL_POINTS) {
          Serial.println(F("Peak-to-Peak Calibration Complete"));
          testRunning = false;
          currentMode = MANUAL;
        }
      } else {
        Serial.println(F("Invalid input. Press 'Y' to proceed."));
      }
    }
  }
}

void calibratePoint(uint8_t point, uint8_t channel, int eepromBase) {
  uint32_t rms0Sum = 0, rms2Sum = 0, rms1Sum = 0;
  resetPeaks();
  delay(10);

  for (uint8_t i = 0; i < SAMPLE_COUNT; i++) {
    rms0Sum += adc.read(MCP3208::Channel::SINGLE_0);
    rms2Sum += adc.read(MCP3208::Channel::SINGLE_2);
    rms1Sum += adc.read(MCP3208::Channel::SINGLE_1);
    delayMicroseconds(1000);
  }

  float rms0Raw = rms0Sum / SAMPLE_COUNT;
  float rms2Raw = rms2Sum / SAMPLE_COUNT;
  float rms1Raw = rms1Sum / SAMPLE_COUNT;

  float value;
  if (channel == 0) {
    value = rms0Raw;
    if (point == 0) rmsOffset0 = value / 4095.0 * (ADC_VREF / 1000.0) / 100.0;
    EEPROM.put(0, rmsOffset0);
  } else if (channel == 1) {
    value = rms1Raw;
    if (point == 0) rmsOffset1 = value / 4095.0 * (ADC_VREF / 1000.0);
    EEPROM.put(4, rmsOffset1);
  } else if (channel == 2) {
    value = rms2Raw;
    if (point == 0) rmsOffset2 = value / 4095.0 * (ADC_VREF / 1000.0) / 10.0;
    EEPROM.put(8, rmsOffset2);
  }
  EEPROM.put(eepromBase + point * sizeof(float), value);
}

void calibratePeakPoint(uint8_t point) {
  uint32_t posPeakSum = 0, negPeakSum = 0;
  resetPeaks();
  delay(10);

  for (uint8_t i = 0; i < SAMPLE_COUNT; i++) {
    posPeakSum += adc.read(MCP3208::Channel::SINGLE_3);
    negPeakSum += adc.read(MCP3208::Channel::SINGLE_4);
    delayMicroseconds(1000);
  }
  float posPeakRaw = posPeakSum / SAMPLE_COUNT;
  float negPeakRaw = negPeakSum / SAMPLE_COUNT;

  EEPROM.put(EEPROM_POSPEAKADC + point * sizeof(float), posPeakRaw);
  EEPROM.put(EEPROM_NEGPEAKADC + point * sizeof(float), negPeakRaw);

  if (point == 0) {
    posPeakOffset = posPeakRaw / 4095.0 * (ADC_VREF / 1000.0);
    negPeakOffset = negPeakRaw / 4095.0 * (ADC_VREF / 1000.0);
    EEPROM.put(12, posPeakOffset);
    EEPROM.put(16, negPeakOffset);
  }
}

void loadCalibrationFromEEPROM() {
  EEPROM.get(0, rmsOffset0);
  EEPROM.get(4, rmsOffset1);
  EEPROM.get(8, rmsOffset2);
  EEPROM.get(12, posPeakOffset);
  EEPROM.get(16, negPeakOffset);
  calibrationChan0Complete = (rmsOffset0 != 0);
  calibrationChan1Complete = (rmsOffset1 != 0);
  calibrationChan2Complete = (rmsOffset2 != 0);
}

void loadArrayFromEEPROM(float* array, int eepromBase) {
  for (uint8_t i = 0; i < CAL_POINTS; i++) {
    EEPROM.get(eepromBase + i * sizeof(float), array[i]);
  }
}

void printCalibrationData() {
  float tempArray[CAL_POINTS];
  
  Serial.println(F("RMS0 (Gain 100):"));
  loadArrayFromEEPROM(tempArray, EEPROM_RMS0ADC);
  for (uint8_t i = 0; i < CAL_POINTS; i++) {
    Serial.print(F("Point ")); Serial.print(i); Serial.print(F(": Vrms=")); Serial.print(pgm_read_float(&calRMS0Values[i]), 4);
    Serial.print(F(" V, ADC=")); Serial.println(tempArray[i]);
  }
  
  Serial.println(F("RMS1 (Gain 1):"));
  loadArrayFromEEPROM(tempArray, EEPROM_RMS1ADC);
  for (uint8_t i = 0; i < CAL_POINTS; i++) {
    Serial.print(F("Point ")); Serial.print(i); Serial.print(F(": Vrms=")); Serial.print(pgm_read_float(&calRMS1Values[i]), 4);
    Serial.print(F(" V, ADC=")); Serial.println(tempArray[i]);
  }
  
  Serial.println(F("RMS2 (Gain 10):"));
  loadArrayFromEEPROM(tempArray, EEPROM_RMS2ADC);
  for (uint8_t i = 0; i < CAL_POINTS; i++) {
    Serial.print(F("Point ")); Serial.print(i); Serial.print(F(": Vrms=")); Serial.print(pgm_read_float(&calRMS2Values[i]), 4);
    Serial.print(F(" V, ADC=")); Serial.println(tempArray[i]);
  }
  
  Serial.println(F("Positive Peak (Channel 3):"));
  loadArrayFromEEPROM(tempArray, EEPROM_POSPEAKADC);
  for (uint8_t i = 0; i < CAL_POINTS; i++) {
    Serial.print(F("Point ")); Serial.print(i); Serial.print(F(": Vpp=")); Serial.print(pgm_read_float(&calPPValues[i]), 4);
    Serial.print(F(" V, ADC=")); Serial.println(tempArray[i]);
  }
  
  Serial.println(F("Negative Peak (Channel 4):"));
  loadArrayFromEEPROM(tempArray, EEPROM_NEGPEAKADC);
  for (uint8_t i = 0; i < CAL_POINTS; i++) {
    Serial.print(F("Point ")); Serial.print(i); Serial.print(F(": Vpp=")); Serial.print(pgm_read_float(&calPPValues[i]), 4);
    Serial.print(F(" V, ADC=")); Serial.println(tempArray[i]);
  }
}

float getCorrectedRMS(float adcReading, float* calADC, const float* calValues, float offset) { 
  float value = interpolate(adcReading, calADC, calValues);
  return max(0, value - offset);
}

float interpolate(float adcReading, float* calADC, const float* calValues) {
  if (adcReading <= calADC[0]) return pgm_read_float(&calValues[0]);
  if (adcReading >= calADC[CAL_POINTS - 1]) return pgm_read_float(&calValues[CAL_POINTS - 1]);
  for (uint8_t i = 0; i < CAL_POINTS - 1; i++) {
    if (adcReading >= calADC[i] && adcReading <= calADC[i + 1]) {
      float slope = (pgm_read_float(&calValues[i + 1]) - pgm_read_float(&calValues[i])) / (calADC[i + 1] - calADC[i]);
      return pgm_read_float(&calValues[i]) + slope * (adcReading - calADC[i]);
    }
  }
  return pgm_read_float(&calValues[CAL_POINTS - 1]);
}

void handleSerialCommand() {
  if (Serial.available()) {
    String command = Serial.readStringUntil('\n');
    command.trim();
    while (Serial.available()) Serial.read(); // Clear remaining buffer
    if (command == "C0") {
      currentMode = CAL_CHAN0;
      Serial.println(F("Entering Channel 0 Calibration"));
    }
    else if (command == "C1") {
      currentMode = CAL_CHAN1;
      Serial.println(F("Entering Channel 1 Calibration"));
    }
    else if (command == "C2") {
      currentMode = CAL_CHAN2;
      Serial.println(F("Entering Channel 2 Calibration"));
    }
    else if (command == "CP") {
      currentMode = CAL_PEAK;
      Serial.println(F("Entering Peak-to-Peak Calibration"));
    }
    else if (command == "CAL") {
      Serial.println(F("Displaying EEPROM Calibration Data:"));
      printCalibrationData();
    }
    else if (command == "0" || command == "1" || command == "2" || command == "3") {
      setAttenuation(command.toInt() * 10);
      Serial.print(F("Set attenuation to: ")); Serial.println(ATTENUATION_STEPS[currentAttenuationIndex]);
    }
    else if (command.toInt() > 0) {
      currentStep = findStep(command.toInt());
      updateVariableResistance();
      Serial.print(F("Set load to: ")); Serial.println(pgm_read_word(&resistanceSteps[currentStep]));
    }
    else {
      Serial.println(F("Unknown command"));
    }
  }
}

void resetPeaks() {
  digitalWrite(PEAK_RESET_PIN, HIGH);
  delayMicroseconds(20);
  digitalWrite(PEAK_RESET_PIN, LOW);
}

void setAttenuation(uint8_t db) {
  sr.setNoUpdate(10, db == 0 ? HIGH : db == 10 ? LOW : db == 30 ? LOW : HIGH);
  sr.setNoUpdate(11, db == 0 ? HIGH : db == 20 ? LOW : db == 30 ? LOW : HIGH);
  sr.updateRegisters();
  currentAttenuationIndex = (db == 0 ? 0 : db == 10 ? 1 : db == 20 ? 2 : 3);
  attenuationFactor = pow(10, db / 20.0);
}

void updateVariableResistance() {
  int targetResistance = pgm_read_word(&resistanceSteps[currentStep]);
  for (uint8_t i = 0; i < 10; i++) sr.setNoUpdate(i, HIGH);
  struct ResistorPin { int value; uint8_t pin; };
  const ResistorPin resistors[] = {{2000, 5}, {1600, 4}, {800, 3}, {400, 2}, {200, 1}, {100, 0}, {50, 8}, {15, 7}, {10, 9}};
  int remainingResistance = targetResistance;
  for (const ResistorPin& rp : resistors) {
    if (remainingResistance >= rp.value) {
      sr.setNoUpdate(rp.pin, LOW);
      remainingResistance -= rp.value;
    }
  }
  sr.setNoUpdate(6, HIGH);
  sr.updateRegisters();
}

int findStep(int targetResistance) {
  for (int i = 0; i < numSteps; i++) {
    if (pgm_read_word(&resistanceSteps[i]) <= targetResistance) return i;
  }
  return numSteps - 1;
}