FaderBuddy

/*
 * Copyright 2026 Scott Bezek
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <Arduino.h>
#include <Wire.h>
#include <ESP32Servo.h>
#include <TFT_eSPI.h>
#include <FS.h>
#include <LittleFS.h>

#include "Adafruit_INA3221.h"
#include "fader_buddy_i2c.h"
#include "fader_buddy_bootloader.h"
#include "fader_app_image.h"

#define FlashFS LittleFS

#define PIN_LED_RED 21
#define PIN_LED_GREEN 22

#define PIN_PRESENCE_SWITCH 36

// LilyGO T-Display left button (active low). Press-and-hold toggles dummy mode.
#define PIN_BUTTON_LEFT 0
#define BUTTON_HOLD_MS 1000

#define PIN_PHOTODIODE_PWR 37
#define PIN_PHOTODIODE_DBG 38


#define PIN_INA_SCL 13
#define PIN_INA_SDA 17

#define PIN_FADER_BUDDY_SCL 27
#define PIN_FADER_BUDDY_SDA 26

#define PIN_SERVO 32
#define SERVO_TOUCH_POS 88
#define SERVO_CLEAR_POS 50

// Fixed "old firmware" baseline UPDI-flashed at the start of TEST_FW_BOOTSTRAP
// (see factory_test_images/old_app_fw0.hex and its README). Must match the
// FW_VERSION baked into that checked-in image.
#define OLD_FW_VERSION_FOR_TEST (0)

// Bounded linear interpolation macro (float only)
#define LERP(x, in_min, in_max, out_min, out_max) \
  ({ \
    float _x = (float)(x); \
    float _in_min = (float)(in_min); \
    float _in_max = (float)(in_max); \
    float _out_min = (float)(out_min); \
    float _out_max = (float)(out_max); \
    _x = (_x < _in_min) ? _in_min : ((_x > _in_max) ? _in_max : _x); \
    (_out_min + (_out_max - _out_min) * (_x - _in_min) / (_in_max - _in_min)); \
  })

Adafruit_INA3221 ina3221;
TFT_eSPI tft = TFT_eSPI();
TFT_eSprite sprite = TFT_eSprite(&tft);

TwoWire WireFaderBuddy = TwoWire(1);  // Use second I2C peripheral
FaderBuddyI2C faderBuddy;
FaderBuddyBootloader faderBootloader(BL_I2C_BASE_ADDRESS);

uint32_t bootTime = 0;

Servo servo;

// ============================================================================
// Test State Machine
// ============================================================================
enum TestState {
  TEST_IDLE,              // Waiting for presence switch press
  TEST_LOGIC_POWER,       // Testing 3.3V logic rail
  TEST_MOTOR_POWER,       // Testing 5V motor rail
  TEST_POWER_LED,         // Testing power LED
  TEST_FW_BOOTSTRAP,      // UPDI-flash the fixed old-version image + validate it came up
  TEST_FW_I2C_UPDATE,     // I2C bootloader update to the current app + validate
  TEST_DEBUG_LED,         // Testing debug LED blink pattern
  TEST_SELF_CALIBRATION,  // Self-calibration test
  TEST_DIAGNOSTICS,       // I2C diagnostics (10 seconds)
  TEST_TOUCH_SENSOR,      // Touch sensor test with servo
  TEST_PASSED,            // All tests passed
  TEST_FAILED             // One or more tests failed
};

TestState currentTestState = TEST_IDLE;
TestState lastReportedTestState = TEST_IDLE;  // Last state reported to host script over serial
bool lastPresenceState = false;

// Dummy mode: skips the firmware bootstrap/I2C-update steps (which need real
// UPDI hardware and a real fader) so the rest of the jig flow can be
// exercised without them. Toggled by holding the left button on the T-Display.
bool dummyMode = false;
bool lastButtonPressed = false;
uint32_t buttonPressStartTime = 0;
bool buttonHoldHandled = false;

// Motor fader version info (read once per test run)
struct FaderBuddyVersion {
  uint8_t protocolVersion;
  uint16_t fwVersion;
  bool valid;
} versionInfo = {0, 0, false};

// Motor fader state (read continuously during diagnostics)
struct FaderBuddyState {
  bool touch;
  uint8_t mode;
  uint8_t position;
  uint16_t rawADC;
  uint32_t uptime;
  int16_t touchDelta;
  uint16_t touchReference;
  uint16_t touchRecalCount;
  bool valid;
} faderState = {false, 0, 0, 0, 0, 0, 0, 0, false};

// Test tracking
struct TestTracking {
  uint32_t testStartTime;
  uint8_t diagnosticsMovementStep;

  // Power test accumulators
  float v0Sum;
  float i0Sum;
  float v1Sum;
  float i1Sum;
  uint16_t sampleCount;

  // Firmware upload tracking
  enum FirmwareUploadPhase {
    FIRMWARE_PHASE_PING,
    FIRMWARE_PHASE_UPLOAD,
    FIRMWARE_PHASE_VALIDATE_OLD,     // confirm the UPDI-flashed old app reports OLD_FW_VERSION_FOR_TEST
    FIRMWARE_PHASE_BOOTLOAD_UPDATE,  // drive the I2C bootloader to the current app image
    FIRMWARE_PHASE_READ_SERIAL,
    FIRMWARE_PHASE_COMPLETE
  };
  FirmwareUploadPhase firmwarePhase;
  uint32_t firmwarePhaseStartTime;
  bool firmwareCommandSent;
  char serialBuffer[256];  // Buffer for incoming serial data
  uint16_t serialBufferPos;  // Current write position in buffer

  // Debug LED test tracking
  uint32_t debugLedTestStartTime;
  uint8_t debugLedTransitionCount;
  bool debugLedState;  // Current state for hysteresis tracking

  // Self-calibration test tracking
  uint32_t selfCalStartTime;
  bool selfCalRequestSent;
  bool selfCalEnteredMode;
  uint16_t selfCalMinADC;
  uint16_t selfCalMaxADC;

  // Touch sensor test tracking
  enum TouchTestPhase {
    TOUCH_PHASE_CHECK_NO_TOUCH,
    TOUCH_PHASE_MOVE_TO_POSITION,
    TOUCH_PHASE_WAIT_FOR_IDLE,
    TOUCH_PHASE_SERVO_TOUCH,
    TOUCH_PHASE_WAIT_FOR_TOUCH_DETECT,
    TOUCH_PHASE_CONFIRM_TOUCH,
    TOUCH_PHASE_SERVO_CLEAR,
    TOUCH_PHASE_WAIT_FOR_TOUCH_CLEAR,
    TOUCH_PHASE_FINAL_WAIT,
    TOUCH_PHASE_CLEANUP_ON_FAILURE
  };
  TouchTestPhase touchTestPhase;
  uint32_t touchTestPhaseStartTime;

  // Test results
  String failedTestName;

  // I2C bootloader update sub-state (FIRMWARE_PHASE_BOOTLOAD_UPDATE): streams
  // one page per call instead of blocking for the whole transfer, so loop()
  // keeps running (display/servo/presence-abort) and progress is visible.
  enum BootloadUpdateStep {
    BL_STEP_ENTER_CMD,     // send REG_ENTER_BOOTLOADER if not already resident
    BL_STEP_ENTER_WAIT,    // poll for the bootloader marker
    BL_STEP_STATUS,        // sanity GET_STATUS check
    BL_STEP_ERASE,         // erase the application section
    BL_STEP_STREAM,        // stream one page (4 frames) per call
    BL_STEP_VERIFY,        // whole-image CRC verify
    BL_STEP_RUN,           // RUN_APP command
    BL_STEP_WAIT_APP,      // poll for the new app to boot
    BL_STEP_CHECK_VERSION, // read + compare REG_FW_VERSION
  };
  BootloadUpdateStep bootloadStep;
  uint32_t bootloadStepStartTime;
  uint32_t bootloadPageIndex;
  uint32_t bootloadTotalPages;
  // (bootload fields intentionally left off the initializer below -- an
  // aggregate value-initializes any trailing members not listed, so they
  // start at 0 / BL_STEP_ENTER_CMD, same as if explicitly zeroed.)
} testTracking = {0, 0, 0, 0, 0, 0, 0, TestTracking::FIRMWARE_PHASE_PING, 0, false, {0}, 0, 0, 0, false, 0, false, false, 0xFFFF, 0, TestTracking::TOUCH_PHASE_CHECK_NO_TOUCH, 0, ""};

void setup() {
  pinMode(PIN_LED_RED, OUTPUT);
  pinMode(PIN_LED_GREEN, OUTPUT);
  pinMode(PIN_PRESENCE_SWITCH, INPUT);
  pinMode(PIN_BUTTON_LEFT, INPUT_PULLUP);

  pinMode(PIN_PHOTODIODE_PWR, INPUT);
  pinMode(PIN_PHOTODIODE_DBG, INPUT);

  // Configure LED PWM for high resolution (10-bit = 1024 levels)
  analogWriteResolution(10);

  Serial.begin(115200);
  Wire.begin(PIN_INA_SDA, PIN_INA_SCL);

  // Initialize motor fader I2C on separate bus
  WireFaderBuddy.begin(PIN_FADER_BUDDY_SDA, PIN_FADER_BUDDY_SCL);
  WireFaderBuddy.setClock(100000);
  WireFaderBuddy.setTimeOut(250);  // tolerate bootloader CRC-compute clock stretching
  faderBuddy.begin(&WireFaderBuddy);
  faderBootloader.begin(&WireFaderBuddy);

  // Initialize servo - move to clear position and disable after 2 seconds
  servo.attach(PIN_SERVO);
  servo.write(SERVO_CLEAR_POS);
  // delay(2000);  // Allow servo to reach clear position
  // servo.detach();

  // Initialize display
  tft.init();
  tft.setRotation(0);
  tft.fillScreen(TFT_BLACK);
  sprite.createSprite(135, 240);
  sprite.setTextColor(TFT_WHITE, TFT_BLACK);
  sprite.setTextSize(2);


  delay(100);

  // Initialize the INA3221
  if (!ina3221.begin(0x40, &Wire)) { // can use other I2C addresses or buses
    Serial.println("Failed to find INA3221 chip");
    while (1) {
      digitalWrite(PIN_LED_RED, HIGH);
      delay(200);
      digitalWrite(PIN_LED_RED, LOW);
      delay(200);
    }
  }
  Serial.println("INA3221 Found!");

  ina3221.setAveragingMode(INA3221_AVG_4_SAMPLES);

  // Set shunt resistances for all channels to 0.1 ohms
  for (uint8_t i = 0; i < 3; i++) {
    ina3221.setShuntResistance(i, 0.1);
  }

  // Initialize LittleFS for smooth font loading
  if (!LittleFS.begin()) {
    Serial.println("Flash FS initialisation failed!");
    while (1) {
      digitalWrite(PIN_LED_RED, HIGH);
      delay(100);
      digitalWrite(PIN_LED_RED, LOW);
      delay(100);
    }
  }
  Serial.println("Flash FS available!");

  // Check if roboto font exists
  if (LittleFS.exists("/roboto_14.vlw") == false) {
    Serial.println("Roboto font missing in Flash FS, did you upload it?");
    while (1) {
      digitalWrite(PIN_LED_RED, HIGH);
      delay(500);
      digitalWrite(PIN_LED_RED, LOW);
      delay(500);
    }
  } else {
    Serial.println("Roboto font found OK.");
  }

  // Load the Roboto smooth font once during setup
  sprite.loadFont("roboto_14", LittleFS); 

  // Record boot time for delayed programming trigger
  bootTime = millis();
}


const char* getModeString(uint8_t mode) {
  switch (mode) {
    case MODE_REMOTE_MOVEMENT_IN_PROGRESS: return "REMOTE";
    case MODE_INPUT_ACTIVE: return "INPUT_ACT";
    case MODE_INPUT_IDLE: return "INPUT_IDL";
    case MODE_ERROR: return "ERROR";
    case MODE_SELF_CALIBRATION: return "SELF_CAL";
    default: return "UNKNOWN";
  }
}

const String getTestStateName(TestState state) {
  switch (state) {
    case TEST_IDLE: return "IDLE";
    case TEST_LOGIC_POWER: return "1: LOGIC PWR";
    case TEST_MOTOR_POWER: return "2: MOTOR PWR";
    case TEST_POWER_LED: return "3: PWR LED";
    case TEST_FW_BOOTSTRAP: return "4: FW BOOTSTRAP";
    case TEST_FW_I2C_UPDATE: return "5: FW I2C UP";
    case TEST_DEBUG_LED: return "6: DBG LED";
    case TEST_SELF_CALIBRATION: return "7: SELF CAL";
    case TEST_DIAGNOSTICS: return "8: I2C DIAG";
    case TEST_TOUCH_SENSOR: return "9: TOUCH";
    case TEST_PASSED: return "PASSED";
    case TEST_FAILED:
      if (!testTracking.failedTestName.isEmpty()) {
        return String("FAIL: ") + testTracking.failedTestName;
      }
      return "FAILED";
    default: return "UNKNOWN";
  }
}

uint16_t getTestStateColor(TestState state) {
  switch (state) {
    case TEST_IDLE: return TFT_DARKGREY;
    case TEST_PASSED: return TFT_GREEN;
    case TEST_FAILED: return TFT_RED;
    default: return TFT_BLUE;  // Testing states
  }
}

// Tiny 3x5 pixel bitmap font, only the glyphs needed to spell "DUMMY" -- used
// for the dummy-mode indicator, which needs to be much smaller than the
// smallest size the loaded smooth font (roboto_14) can render.
const uint8_t TINY_FONT_D[5] = {0b110, 0b101, 0b101, 0b101, 0b110};
const uint8_t TINY_FONT_U[5] = {0b101, 0b101, 0b101, 0b101, 0b010};
const uint8_t TINY_FONT_M[5] = {0b101, 0b111, 0b101, 0b101, 0b101};
const uint8_t TINY_FONT_Y[5] = {0b101, 0b101, 0b010, 0b010, 0b010};

const uint8_t* tinyFontGlyph(char c) {
  switch (c) {
    case 'D': return TINY_FONT_D;
    case 'U': return TINY_FONT_U;
    case 'M': return TINY_FONT_M;
    case 'Y': return TINY_FONT_Y;
    default: return nullptr;
  }
}

// Draws text using the tiny 3x5 font directly onto the sprite (bypasses the
// loaded smooth font entirely, so it doesn't need swapping in/out per frame).
void drawTinyText(const char* text, int x, int y, uint16_t color) {
  int cx = x;
  for (const char* p = text; *p; p++) {
    const uint8_t* glyph = tinyFontGlyph(*p);
    if (glyph) {
      for (int row = 0; row < 5; row++) {
        for (int col = 0; col < 3; col++) {
          if (glyph[row] & (0b100 >> col)) {
            sprite.drawPixel(cx + col, y + row, color);
          }
        }
      }
    }
    cx += 4;  // 3px glyph + 1px space
  }
}

uint8_t render_count = 0;
void updateDisplay(float v0, float c0, float v1, float c1) {
  sprite.fillSprite(TFT_BLACK);
  sprite.setTextColor(TFT_WHITE, TFT_BLACK);

  // Status bar at top (40px tall)
  uint16_t statusColor = getTestStateColor(currentTestState);
  sprite.fillRect(0, 0, 135, 40, statusColor);

  // Center text in status bar
  uint16_t fg_color = currentTestState == TEST_PASSED ? TFT_BLACK : TFT_WHITE;
  sprite.setTextColor(fg_color, statusColor);
  String stateName = getTestStateName(currentTestState);
  int textWidth = sprite.textWidth(stateName);
  int textX = (currentTestState == TEST_IDLE || currentTestState == TEST_PASSED || currentTestState == TEST_FAILED) ? (135 - textWidth) / 2 : 10;
  sprite.setCursor(textX, 12);  // Vertically centered in 40px bar
  sprite.print(stateName);

  // I2C bootloader update progress bar, along the bottom edge of the status
  // bar (only meaningful during TEST_FW_I2C_UPDATE). Erase/verify/run/etc.
  // show a full bar; page streaming fills it incrementally as it runs.
  if (currentTestState == TEST_FW_I2C_UPDATE && testTracking.bootloadTotalPages > 0) {
    uint32_t done = testTracking.bootloadPageIndex;
    if (testTracking.bootloadStep > TestTracking::BL_STEP_STREAM) {
      done = testTracking.bootloadTotalPages;
    }
    int barX = 4, barY = 34, barW = 127, barH = 5;
    sprite.drawRect(barX, barY, barW, barH, TFT_WHITE);
    int fillW = (int)((uint32_t)(barW - 2) * done / testTracking.bootloadTotalPages);
    if (fillW > 0) {
      sprite.fillRect(barX + 1, barY + 1, fillW, barH - 2, TFT_WHITE);
    }
  }

  sprite.setTextColor(TFT_WHITE, TFT_BLACK);

  // Top-right readout, just below the status bar. During TEST_FW_I2C_UPDATE
  // the protocol/firmware version aren't queryable (the device is mid-update),
  // so that slot shows page-write progress instead; otherwise it shows the
  // protocol/firmware version on two lines, hidden entirely until known.
  if (currentTestState == TEST_FW_I2C_UPDATE && testTracking.bootloadTotalPages > 0) {
    char progBuf[16];
    snprintf(progBuf, sizeof(progBuf), "%lu/%lu", (unsigned long)testTracking.bootloadPageIndex,
             (unsigned long)testTracking.bootloadTotalPages);
    int w = sprite.textWidth(progBuf);
    sprite.setCursor(135 - w - 2, 44);
    sprite.print(progBuf);
  } else if (versionInfo.valid) {
    char protoBuf[16], fwBuf[16];
    snprintf(protoBuf, sizeof(protoBuf), "P:v%u", versionInfo.protocolVersion);
    snprintf(fwBuf, sizeof(fwBuf), "FW:v%u", versionInfo.fwVersion);
    int protoWidth = sprite.textWidth(protoBuf);
    int fwWidth = sprite.textWidth(fwBuf);
    sprite.setCursor(135 - protoWidth - 2, 44);
    sprite.print(protoBuf);
    sprite.setCursor(135 - fwWidth - 2, 58);
    sprite.print(fwBuf);
  }

  // TODO: move IO out of this method
  uint16_t pwr_led = analogRead(PIN_PHOTODIODE_PWR);
  if (pwr_led < 2800) {
    sprite.fillSmoothCircle(10, 58, 3, TFT_RED, TFT_BLACK);
  }
  sprite.setCursor(20, 50);
  sprite.print(pwr_led);

  uint16_t dbg_led = analogRead(PIN_PHOTODIODE_DBG);
  if (dbg_led < 2800) {
    sprite.fillSmoothCircle(10, 78, 3, TFT_RED, TFT_BLACK);
  }
  sprite.setCursor(20, 70);
  sprite.print(dbg_led);

  // Prepare voltage and current strings
  String v0_str = String(v0, 1) + "V";
  String c0_str = String((int)c0) + "mA";
  String v1_str = String(v1, 1) + "V";
  String c1_str = String((int)c1) + "mA";

  // Calculate right-aligned positions for channel 0 (left half, right align at x=67)
  int v0_width = sprite.textWidth(v0_str);
  int c0_width = sprite.textWidth(c0_str);
  sprite.setCursor(67 - v0_width, 200);
  sprite.print(v0_str);
  sprite.setCursor(67 - c0_width, 215);
  sprite.print(c0_str);
  int c0h = LERP(c0, 0, 15, 0, 240);
  sprite.fillRect(0, 240 - c0h, 2, c0h, TFT_GREEN);

  // Calculate right-aligned positions for channel 1 (right half, right align at x=135)
  int v1_width = sprite.textWidth(v1_str);
  int c1_width = sprite.textWidth(c1_str);
  sprite.setCursor(130 - v1_width, 200);
  sprite.print(v1_str);
  sprite.setCursor(130 - c1_width, 215);
  sprite.print(c1_str);
  int c1h = LERP(c1, 0, 300, 0, 240);
  sprite.fillRect(133, 240 - c1h, 2, c1h, TFT_RED);


  // Display motor fader diagnostics if available
  if (faderState.valid) {
    sprite.setCursor(5, 110);
    sprite.setTextColor(TFT_CYAN, TFT_BLACK);
    sprite.print("Up:");
    sprite.print(faderState.uptime / 1000.0, 1);
    sprite.print("s");

    sprite.setCursor(5, 130);
    sprite.setTextColor(TFT_YELLOW, TFT_BLACK);
    sprite.print(getModeString(faderState.mode));

    sprite.setCursor(5, 150);
    sprite.setTextColor(TFT_WHITE, TFT_BLACK);
    sprite.print("Pos:");
    sprite.print(faderState.position);
    sprite.print(" ADC:");
    sprite.print(faderState.rawADC);

    sprite.setCursor(5, 170);
    sprite.setTextColor(TFT_WHITE, TFT_BLACK);
    sprite.print("Tch:");
    sprite.setTextColor(faderState.touch ? TFT_GREEN : TFT_RED, TFT_BLACK);
    sprite.print(faderState.touch ? "Y" : "N");

    // Touch diagnostics (delta, reference, recal count)
    sprite.setTextColor(TFT_LIGHTGREY, TFT_BLACK);
    sprite.print(" d:");
    sprite.print(faderState.touchDelta);
    sprite.print(" r:");
    sprite.print(faderState.touchReference);
    sprite.print(" rc:");
    sprite.print(faderState.touchRecalCount);
  }

  render_count++;
  if (render_count & 0x01) {
    sprite.fillSmoothCircle(125, 235, 2, TFT_WHITE, TFT_BLACK);
  }

  // Dummy mode indicator: tiny label along the very bottom edge of the screen.
  if (dummyMode) {
    drawTinyText("DUMMY", 2, 240 - 5, TFT_YELLOW);
  }

  sprite.pushSprite(0, 0);
}


// ============================================================================
// Test Functions
// ============================================================================

bool testLogicPower(float v0, float i0) {
  // Accumulate samples over 1 second
  if (testTracking.sampleCount == 0) {
    testTracking.testStartTime = millis();
    testTracking.v0Sum = 0;
    testTracking.i0Sum = 0;
  }

  testTracking.v0Sum += v0;
  testTracking.i0Sum += i0;
  testTracking.sampleCount++;

  // Check if 1 second has elapsed
  if (millis() - testTracking.testStartTime < 1000) {
    return false;  // Still collecting samples
  }

  // Calculate averages
  float avgV0 = testTracking.v0Sum / testTracking.sampleCount;
  float avgI0 = testTracking.i0Sum / testTracking.sampleCount;

  Serial.print("Logic Power - Avg V0: ");
  Serial.print(avgV0, 3);
  Serial.print("V, Avg I0: ");
  Serial.print(avgI0, 2);
  Serial.println("mA");

  // Check ranges: 3.1-3.5V, 5-15mA
  if (avgV0 < 3.1 || avgV0 > 3.5) {
    testTracking.failedTestName = "LOG VOLT";
    Serial.println("FAILED: Logic voltage out of range");
  } else if (avgI0 > 15.0) {
    testTracking.failedTestName = "LOG CUR";
    Serial.println("FAILED: Logic current out of range");
  } else {
    Serial.println("PASSED: Logic power OK");
  }

  return true;  // Test complete
}

bool testMotorPower(float v1, float i1) {
  // Accumulate samples over 1 second
  if (testTracking.sampleCount == 0) {
    testTracking.testStartTime = millis();
    testTracking.v1Sum = 0;
    testTracking.i1Sum = 0;
  }

  testTracking.v1Sum += v1;
  testTracking.i1Sum += i1;
  testTracking.sampleCount++;

  // Check if 1 second has elapsed
  if (millis() - testTracking.testStartTime < 1000) {
    return false;  // Still collecting samples
  }

  // Calculate averages
  float avgV1 = testTracking.v1Sum / testTracking.sampleCount;
  float avgI1 = testTracking.i1Sum / testTracking.sampleCount;

  Serial.print("Motor Power - Avg V1: ");
  Serial.print(avgV1, 3);
  Serial.print("V, Avg I1: ");
  Serial.print(avgI1, 2);
  Serial.println("mA");

  // Check ranges: 4.3-5.5V, 0-10mA
  if (avgV1 < 4.3 || avgV1 > 5.5) {
    testTracking.failedTestName = "MOT VOLT";
    Serial.println("FAILED: Motor voltage out of range");
  } else if (avgI1 < 0.0 || avgI1 > 10.0) {
    testTracking.failedTestName = "MOT CUR";
    Serial.println("FAILED: Motor current out of range");
  } else {
    Serial.println("PASSED: Motor power OK");
  }

  return true;  // Test complete
}

bool testPowerLED() {
  uint16_t pwr_led = analogRead(PIN_PHOTODIODE_PWR);

  Serial.print("Power LED - Photodiode ADC: ");
  Serial.println(pwr_led);

  if (pwr_led >= 2800) {
    testTracking.failedTestName = "PWR LED";
    Serial.println("FAILED: Power LED not detected");
  } else {
    Serial.println("PASSED: Power LED OK");
  }

  return true;  // Test complete
}

// Non-blocking serial buffer update
// Call this regularly to read available serial data into the buffer
void updateSerialBuffer() {
  const size_t bufferSize = sizeof(testTracking.serialBuffer);

  while (Serial.available() > 0) {
    char c = Serial.read();

    // Add character to buffer if there's room
    if (testTracking.serialBufferPos < bufferSize - 1) {
      testTracking.serialBuffer[testTracking.serialBufferPos++] = c;
      testTracking.serialBuffer[testTracking.serialBufferPos] = '\0';  // Null terminate
    } else {
      // Buffer full - shift left by half to make room
      const size_t halfSize = bufferSize / 2;
      memmove(testTracking.serialBuffer,
              testTracking.serialBuffer + halfSize,
              halfSize);
      testTracking.serialBufferPos = halfSize;
      // Ensure we don't write past buffer
      if (testTracking.serialBufferPos < bufferSize - 1) {
        testTracking.serialBuffer[testTracking.serialBufferPos++] = c;
        testTracking.serialBuffer[testTracking.serialBufferPos] = '\0';
      }
    }
  }

  // Safety: ensure buffer is always null-terminated
  testTracking.serialBuffer[bufferSize - 1] = '\0';
}

// Check if a specific command is present in the serial buffer
// If found, removes it from the buffer and returns true
bool checkForSerialCommand(const char* command) {
  const size_t bufferSize = sizeof(testTracking.serialBuffer);

  // Ensure buffer is null-terminated before searching
  testTracking.serialBuffer[bufferSize - 1] = '\0';

  // Search for command in buffer
  char* found = strstr(testTracking.serialBuffer, command);
  if (found != nullptr) {
    // Command found! Calculate position after the command
    size_t commandLen = strlen(command);
    char* afterCommand = found + commandLen;

    // Use strnlen to safely get remaining length
    size_t maxRemaining = bufferSize - (afterCommand - testTracking.serialBuffer);
    size_t remainingLen = strnlen(afterCommand, maxRemaining);

    // Shift remaining data to start of buffer (safe because remainingLen is bounded)
    if (remainingLen > 0) {
      memmove(testTracking.serialBuffer, afterCommand, remainingLen);
    }
    testTracking.serialBuffer[remainingLen] = '\0';
    testTracking.serialBufferPos = remainingLen;

    Serial.print("Received command: ");
    Serial.println(command);
    return true;
  }
  return false;
}

// Clear the serial buffer
void clearSerialBuffer() {
  memset(testTracking.serialBuffer, 0, sizeof(testTracking.serialBuffer));
  testTracking.serialBufferPos = 0;
}

// TEST_FW_BOOTSTRAP: UPDI-flash the fixed old-version image (via test_host.py)
// and confirm the board comes up running it. Precondition for
// testFwI2cUpdate() below, which drives the actual I2C bootloader entry/update.
bool testFwBootstrap() {
  // Initialize on first call
  if (testTracking.firmwarePhase == TestTracking::FIRMWARE_PHASE_PING &&
      testTracking.firmwarePhaseStartTime == 0) {
    testTracking.firmwarePhase = TestTracking::FIRMWARE_PHASE_PING;
    testTracking.firmwarePhaseStartTime = millis();
    testTracking.firmwareCommandSent = false;
    clearSerialBuffer();  // Clear buffer
    Serial.println("=== Starting firmware bootstrap (old-version UPDI flash) ===");
  }

  // Non-blocking: read any available serial data into buffer
  updateSerialBuffer();

  switch (testTracking.firmwarePhase) {
    case TestTracking::FIRMWARE_PHASE_PING:
      if (!testTracking.firmwareCommandSent) {
        // Send ping command
        Serial.println(">>PING<<");
        testTracking.firmwareCommandSent = true;
        testTracking.firmwarePhaseStartTime = millis();
      }

      // Check for ACK (non-blocking)
      if (checkForSerialCommand(">>ACK<<")) {
        // ACK received, move to upload phase
        Serial.println("Host script acknowledged, requesting firmware upload");
        testTracking.firmwarePhase = TestTracking::FIRMWARE_PHASE_UPLOAD;
        testTracking.firmwareCommandSent = false;
        testTracking.firmwarePhaseStartTime = millis();
      } else if (millis() - testTracking.firmwarePhaseStartTime > 2000) {
        // Timeout
        testTracking.failedTestName = "FW NO HOST";
        Serial.println("FAILED: No response from host script (is test_host.py running?)");
        return true;  // Test complete (failed)
      }
      return false;  // Still waiting

    case TestTracking::FIRMWARE_PHASE_UPLOAD:
      if (!testTracking.firmwareCommandSent) {
        // Send upload command
        Serial.println(">>START_FIRMWARE_UPLOAD<<");
        testTracking.firmwareCommandSent = true;
        testTracking.firmwarePhaseStartTime = millis();
      }

      // Check for SUCCESS or FAILURE (non-blocking)
      if (checkForSerialCommand(">>SUCCESS<<")) {
        // UPDI flash of the fixed old-version image succeeded - validate it
        // came up correctly before asking it to self-update over I2C.
        Serial.println("PASSED: Old-firmware UPDI flash succeeded");
        testTracking.firmwarePhase = TestTracking::FIRMWARE_PHASE_VALIDATE_OLD;
        testTracking.firmwareCommandSent = false;
        testTracking.firmwarePhaseStartTime = millis();
      } else if (checkForSerialCommand(">>FAILURE<<")) {
        // Upload failed
        testTracking.failedTestName = "FW UPLOAD";
        Serial.println("FAILED: Firmware upload failed");
        testTracking.firmwarePhase = TestTracking::FIRMWARE_PHASE_COMPLETE;
        return true;  // Test complete (failed)
      } else if (millis() - testTracking.firmwarePhaseStartTime > 40000) {
        // Timeout (UPDI chip-erase + two hex writes + two fuse writes)
        testTracking.failedTestName = "FW TIMEOUT";
        Serial.println("FAILED: Firmware upload timed out");
        testTracking.firmwarePhase = TestTracking::FIRMWARE_PHASE_COMPLETE;
        return true;  // Test complete (failed)
      }
      return false;  // Still waiting

    case TestTracking::FIRMWARE_PHASE_VALIDATE_OLD:
      {
        // Confirm the board that was just UPDI-flashed is running the fixed old
        // application (not resident in the bootloader, not still booting) before
        // handing it off to the I2C bootloader-entry/update path.
        uint8_t protoVer;
        bool responded = faderBootloader.readVersionByte(protoVer);

        if (responded && protoVer != BL_VERSION_MARKER && protoVer != 0xFF) {
          uint16_t fwVer;
          if (faderBootloader.readFwVersion(fwVer)) {
            if (fwVer != OLD_FW_VERSION_FOR_TEST) {
              testTracking.failedTestName = "FW OLD VER";
              Serial.print("FAILED: old firmware reports FW_VERSION=");
              Serial.print(fwVer);
              Serial.print(", expected ");
              Serial.println(OLD_FW_VERSION_FOR_TEST);
              testTracking.firmwarePhase = TestTracking::FIRMWARE_PHASE_COMPLETE;
              return true;  // Test complete (failed)
            }
            Serial.print("Old firmware confirmed running: protocol=");
            Serial.print(protoVer);
            Serial.print(" fw_version=");
            Serial.println(fwVer);
            // Prime the phase for testFwI2cUpdate() and signal this TestState
            // (TEST_FW_BOOTSTRAP) is complete -- the outer state machine moves
            // on to TEST_FW_I2C_UPDATE.
            testTracking.firmwarePhase = TestTracking::FIRMWARE_PHASE_BOOTLOAD_UPDATE;
            testTracking.firmwareCommandSent = false;
            testTracking.firmwarePhaseStartTime = millis();
            return true;  // Test complete (passed)
          }
        }

        if (millis() - testTracking.firmwarePhaseStartTime > 2000) {
          testTracking.failedTestName =
              (responded && protoVer == BL_VERSION_MARKER) ? "FW OLD BL" : "FW OLD I2C";
          Serial.println("FAILED: could not confirm old firmware running over I2C after UPDI flash");
          testTracking.firmwarePhase = TestTracking::FIRMWARE_PHASE_COMPLETE;
          return true;  // Test complete (failed)
        }
        return false;  // Still waiting for the app to come up
      }

    case TestTracking::FIRMWARE_PHASE_COMPLETE:
      return true;  // Already complete

    default:
      return true;
  }
}

// TEST_FW_I2C_UPDATE: drive the I2C bootloader (entered from the running old
// app validated by testFwBootstrap()) to stream and run the current
// application, then read the serial number back for the host.
bool testFwI2cUpdate() {
  // Non-blocking: read any available serial data into buffer (test_host.py
  // isn't involved in this phase, but keep the buffer drained regardless).
  updateSerialBuffer();

  switch (testTracking.firmwarePhase) {
    case TestTracking::FIRMWARE_PHASE_BOOTLOAD_UPDATE:
      {
        // Non-blocking sub-state machine: enters the bootloader from the
        // running old app, erases, streams the pre-baked current app image one
        // PAGE PER CALL (instead of FaderBuddyBootloader::updateFirmware()'s
        // single blocking call for the whole transfer), verifies its CRC, runs
        // it, and confirms it reports FADER_APP_FW_VERSION. Keeping this
        // incremental means loop() keeps servicing the display/servo/presence-
        // abort check throughout, and lets updateDisplay() render live
        // page-write progress as a bar (see currentTestState ==
        // TEST_FW_I2C_UPDATE in updateDisplay()).
#define BOOTLOAD_FAIL(name)                                    \
  do {                                                         \
    testTracking.failedTestName = (name);                     \
    testTracking.firmwarePhase = TestTracking::FIRMWARE_PHASE_COMPLETE; \
    return true;                                               \
  } while (0)

        if (!testTracking.firmwareCommandSent) {
          testTracking.firmwareCommandSent = true;
          testTracking.bootloadStep = TestTracking::BL_STEP_ENTER_CMD;
          testTracking.bootloadStepStartTime = millis();
          testTracking.bootloadPageIndex = 0;
          testTracking.bootloadTotalPages = FADER_APP_IMAGE_SIZE / BL_PAGE_SIZE;
          Serial.println("Entering I2C bootloader and updating to the current application...");
        }

        // Overall watchdog for the whole sequence (generous margin over the
        // hardware-validated ~few-second full update).
        if (millis() - testTracking.firmwarePhaseStartTime > 15000) {
          Serial.println("FAILED: I2C bootloader update timed out");
          BOOTLOAD_FAIL("FW BL TMO");
        }

        switch (testTracking.bootloadStep) {
          case TestTracking::BL_STEP_ENTER_CMD:
            {
              uint8_t v;
              if (faderBootloader.readVersionByte(v) && v == BL_VERSION_MARKER) {
                // Already resident (defensive -- shouldn't normally happen
                // right after FIRMWARE_PHASE_VALIDATE_OLD confirmed an app).
                testTracking.bootloadStep = TestTracking::BL_STEP_STATUS;
              } else if (faderBootloader.enterBootloader()) {
                testTracking.bootloadStep = TestTracking::BL_STEP_ENTER_WAIT;
                testTracking.bootloadStepStartTime = millis();
              } else if (millis() - testTracking.bootloadStepStartTime > 2000) {
                Serial.println("FAILED: could not send REG_ENTER_BOOTLOADER");
                BOOTLOAD_FAIL("FW BL ENTER");
              }
            }
            return false;

          case TestTracking::BL_STEP_ENTER_WAIT:
            {
              uint8_t v;
              if (faderBootloader.readVersionByte(v) && v == BL_VERSION_MARKER) {
                Serial.println("Bootloader entered (marker seen)");
                testTracking.bootloadStep = TestTracking::BL_STEP_STATUS;
                testTracking.bootloadStepStartTime = millis();
              } else if (millis() - testTracking.bootloadStepStartTime > 2000) {
                Serial.println("FAILED: no bootloader marker after entry");
                BOOTLOAD_FAIL("FW BL ENTER");
              }
            }
            return false;

          case TestTracking::BL_STEP_STATUS:
            {
              uint8_t bv, st, le;
              if (faderBootloader.getStatus(bv, st, le)) {
                Serial.printf("Bootloader resident: ver=%u status=%u last_error=%u\n", bv, st, le);
                testTracking.bootloadStep = TestTracking::BL_STEP_ERASE;
                testTracking.bootloadStepStartTime = millis();
              } else if (millis() - testTracking.bootloadStepStartTime > 2000) {
                Serial.println("FAILED: no status response from bootloader");
                BOOTLOAD_FAIL("FW BL STAT");
              }
            }
            return false;

          case TestTracking::BL_STEP_ERASE:
            Serial.println("Erasing application section...");
            if (!faderBootloader.eraseApp()) {
              Serial.println("FAILED: erase failed");
              BOOTLOAD_FAIL("FW BL ERASE");
            }
            testTracking.bootloadPageIndex = 0;
            testTracking.bootloadStep = TestTracking::BL_STEP_STREAM;
            return false;

          case TestTracking::BL_STEP_STREAM:
            {
              // One page (4 frames) per call -- the ~180-page image is spread
              // across ~180 loop() iterations instead of one blocking call.
              uint32_t p = testTracking.bootloadPageIndex;
              uint16_t pageAddr = FADER_APP_FLASH_START + (uint16_t)(p * BL_PAGE_SIZE);
              bool ok = faderBootloader.setPageAddr(pageAddr);
              for (uint8_t f = 0; ok && f < BL_FRAMES_PER_PAGE; f++) {
                const uint8_t* chunk =
                    FADER_APP_IMAGE + (p * BL_PAGE_SIZE) + (f * BL_FRAME_DATA_LEN);
                ok = faderBootloader.sendFrame(chunk);
              }
              if (!ok) {
                Serial.printf("FAILED: write failed at page %u/%u\n", p, testTracking.bootloadTotalPages);
                BOOTLOAD_FAIL("FW BL WRITE");
              }
              testTracking.bootloadPageIndex++;
              if ((testTracking.bootloadPageIndex % 20) == 0 ||
                  testTracking.bootloadPageIndex == testTracking.bootloadTotalPages) {
                Serial.printf("  writing %u/%u\n", testTracking.bootloadPageIndex,
                              testTracking.bootloadTotalPages);
              }
              if (testTracking.bootloadPageIndex >= testTracking.bootloadTotalPages) {
                testTracking.bootloadStep = TestTracking::BL_STEP_VERIFY;
                testTracking.bootloadStepStartTime = millis();
              }
            }
            return false;

          case TestTracking::BL_STEP_VERIFY:
            {
              Serial.println("Verifying image CRC...");
              uint16_t crc;
              if (!faderBootloader.getImageCrc16(FADER_APP_FLASH_START,
                                                 (uint16_t)FADER_APP_IMAGE_SIZE, crc)) {
                Serial.println("FAILED: CRC read failed");
                BOOTLOAD_FAIL("FW BL CRCRD");
              }
              if (crc != FADER_APP_IMAGE_CRC16) {
                Serial.printf("FAILED: CRC got=0x%04X exp=0x%04X\n", crc, FADER_APP_IMAGE_CRC16);
                BOOTLOAD_FAIL("FW BL CRC");
              }
              testTracking.bootloadStep = TestTracking::BL_STEP_RUN;
            }
            return false;

          case TestTracking::BL_STEP_RUN:
            Serial.println("Running new application...");
            if (!faderBootloader.runApp()) {
              Serial.println("FAILED: run app command failed");
              BOOTLOAD_FAIL("FW BL RUN");
            }
            testTracking.bootloadStep = TestTracking::BL_STEP_WAIT_APP;
            testTracking.bootloadStepStartTime = millis();
            return false;

          case TestTracking::BL_STEP_WAIT_APP:
            {
              uint8_t v;
              if (faderBootloader.readVersionByte(v) && v != BL_VERSION_MARKER && v != 0xFF) {
                testTracking.bootloadStep = TestTracking::BL_STEP_CHECK_VERSION;
              } else if (millis() - testTracking.bootloadStepStartTime > 2000) {
                Serial.println("FAILED: app did not start after RUN_APP");
                BOOTLOAD_FAIL("FW BL BOOT");
              }
            }
            return false;

          case TestTracking::BL_STEP_CHECK_VERSION:
            {
              uint16_t fw;
              if (!faderBootloader.readFwVersion(fw)) {
                Serial.println("FAILED: could not read new app's FW_VERSION");
                BOOTLOAD_FAIL("FW BL FWVER");
              }
              if (fw != FADER_APP_FW_VERSION) {
                Serial.printf("FAILED: new app FW_VERSION=%u, expected %u\n", fw, FADER_APP_FW_VERSION);
                BOOTLOAD_FAIL("FW BL NEWVER");
              }
              Serial.println("PASSED: I2C bootloader update to current application succeeded");
              testTracking.firmwarePhase = TestTracking::FIRMWARE_PHASE_READ_SERIAL;
              testTracking.firmwarePhaseStartTime = millis();
            }
            return false;

          default:
            return false;
        }
#undef BOOTLOAD_FAIL
      }

    case TestTracking::FIRMWARE_PHASE_READ_SERIAL:
      {
        // Read serial number from motor fader and report to host
        uint8_t serial[10];
        if (faderBuddy.readSerialNumber(serial)) {
          // Format as hex string and send to host
          Serial.print(">>SERIAL:");
          for (int i = 0; i < 10; i++) {
            if (serial[i] < 0x10) Serial.print("0");
            Serial.print(serial[i], HEX);
          }
          Serial.println("<<");

          testTracking.firmwarePhase = TestTracking::FIRMWARE_PHASE_COMPLETE;
          return true;  // Test complete (passed)
        } else {
          // Failed to read serial number
          if (millis() - testTracking.firmwarePhaseStartTime > 2000) {
            // Timeout after 2 seconds
            testTracking.failedTestName = "FW SERIAL";
            Serial.println("FAILED: Could not read serial number from motor fader");
            testTracking.firmwarePhase = TestTracking::FIRMWARE_PHASE_COMPLETE;
            return true;  // Test complete (failed)
          }
        }
        return false;  // Still trying
      }

    case TestTracking::FIRMWARE_PHASE_COMPLETE:
      return true;  // Already complete

    default:
      return true;
  }
}

bool testDebugLED() {
  const uint16_t THRESHOLD = 2800;
  const uint16_t HYSTERESIS = 100;
  const uint16_t THRESHOLD_HIGH = THRESHOLD + HYSTERESIS;  // 2900
  const uint16_t THRESHOLD_LOW = THRESHOLD - HYSTERESIS;   // 2700
  const uint32_t TEST_DURATION_MS = 512*5/2;
  const uint8_t MIN_TRANSITIONS = 4;
  const uint8_t MAX_TRANSITIONS = 6;

  // Initialize on first call
  if (testTracking.debugLedTestStartTime == 0) {
    testTracking.debugLedTestStartTime = millis();
    testTracking.debugLedTransitionCount = 0;

    // Read initial LED state
    uint16_t dbg_led = analogRead(PIN_PHOTODIODE_DBG);
    testTracking.debugLedState = (dbg_led < THRESHOLD);  // true if LED is ON (low reading)

    Serial.println("Testing debug LED blink pattern (2 seconds)...");
    Serial.print("Initial LED reading: ");
    Serial.print(dbg_led);
    Serial.print(", state: ");
    Serial.println(testTracking.debugLedState ? "ON" : "OFF");
  }

  // Read current LED value
  uint16_t dbg_led = analogRead(PIN_PHOTODIODE_DBG);

  // Check for transitions with hysteresis
  if (testTracking.debugLedState) {
    // LED is currently ON (low value), check for transition to OFF (high value)
    if (dbg_led >= THRESHOLD_HIGH) {
      testTracking.debugLedState = false;  // LED is now OFF
      testTracking.debugLedTransitionCount++;
      Serial.print("LED OFF transition #");
      Serial.print(testTracking.debugLedTransitionCount);
      Serial.print(" (value: ");
      Serial.print(dbg_led);
      Serial.println(")");
    }
  } else {
    // LED is currently OFF (high value), check for transition to ON (low value)
    if (dbg_led <= THRESHOLD_LOW) {
      testTracking.debugLedState = true;  // LED is now ON
      testTracking.debugLedTransitionCount++;
      Serial.print("LED ON transition #");
      Serial.print(testTracking.debugLedTransitionCount);
      Serial.print(" (value: ");
      Serial.print(dbg_led);
      Serial.println(")");
    }
  }

  // Check if test duration has elapsed
  if (millis() - testTracking.debugLedTestStartTime < TEST_DURATION_MS) {
    return false;  // Still running
  }

  // Test complete - check if transition count is in valid range
  Serial.print("Debug LED test complete. Transitions: ");
  Serial.println(testTracking.debugLedTransitionCount);

  if (testTracking.debugLedTransitionCount < MIN_TRANSITIONS) {
    testTracking.failedTestName = "DBG LED FEW";
    Serial.print("FAILED: Too few transitions (");
    Serial.print(testTracking.debugLedTransitionCount);
    Serial.print(" < ");
    Serial.print(MIN_TRANSITIONS);
    Serial.println(")");
  } else if (testTracking.debugLedTransitionCount > MAX_TRANSITIONS) {
    testTracking.failedTestName = "DBG LED MANY";
    Serial.print("FAILED: Too many transitions (");
    Serial.print(testTracking.debugLedTransitionCount);
    Serial.print(" > ");
    Serial.print(MAX_TRANSITIONS);
    Serial.println(")");
  } else {
    Serial.println("PASSED: Debug LED blink pattern OK");
  }

  return true;  // Test complete
}

void readFaderBuddyVersionInfo() {
  versionInfo.valid = false;

  if (faderBuddy.readProtocolVersion(versionInfo.protocolVersion)) {
    Serial.print("Motor Fader Protocol Version: ");
    Serial.println(versionInfo.protocolVersion);

    // Require a valid app protocol version (see i2c_data.h REG_VERSION: >= 5 is
    // an app; BL_VERSION_MARKER/0xFF mean bootloader-resident/no response).
    if (versionInfo.protocolVersion < 5) {
      Serial.print("ERROR: Expected protocol version >= 5, got ");
      Serial.println(versionInfo.protocolVersion);
      versionInfo.valid = false;
      return;
    }

    if (!faderBootloader.readFwVersion(versionInfo.fwVersion)) {
      Serial.println("Failed to read firmware version");
      return;
    }

    versionInfo.valid = true;
  } else {
    Serial.println("Failed to read protocol version");
  }
}

void readFaderBuddyState() {
  faderState.valid = false;

  uint32_t state;
  if (!faderBuddy.readState(state)) {
    Serial.println("Failed to read motor fader state");
    return;
  }

  // Extract fields from state bitfield
  faderState.touch = (state & STATE_TOUCH_bm) >> STATE_TOUCH_bp;
  faderState.mode = (state & STATE_MODE_bm) >> STATE_MODE_bp;
  faderState.position = (state & STATE_POSITION_bm) >> STATE_POSITION_bp;
  faderState.rawADC = (state & STATE_RAW_ADC_bm) >> STATE_RAW_ADC_bp;

  // Read uptime separately
  if (!faderBuddy.readUptime(faderState.uptime)) {
    Serial.println("Failed to read uptime");
    return;
  }

  // Read touch diagnostics
  if (!faderBuddy.readTouchDelta(faderState.touchDelta)) {
    Serial.println("Failed to read touch delta");
    return;
  }

  if (!faderBuddy.readTouchReference(faderState.touchReference)) {
    Serial.println("Failed to read touch reference");
    return;
  }

  if (!faderBuddy.readTouchRecalCount(faderState.touchRecalCount)) {
    Serial.println("Failed to read touch recal count");
    return;
  }

  faderState.valid = true;
}

bool testSelfCalibration() {
  const uint32_t TOTAL_TIMEOUT_MS = 15000;
  const uint32_t MODE_ENTRY_TIMEOUT_MS = 1000;
  const uint16_t MIN_ADC_THRESHOLD = 100;
  const uint16_t MAX_ADC_THRESHOLD = 1900;

  // Initialize on first call
  if (testTracking.selfCalStartTime == 0) {
    testTracking.selfCalStartTime = millis();
    testTracking.selfCalRequestSent = false;
    testTracking.selfCalEnteredMode = false;
    testTracking.selfCalMinADC = 0xFFFF;  // Start with max value
    testTracking.selfCalMaxADC = 0;       // Start with min value
    Serial.println("Starting self-calibration test (8 seconds max)...");
  }

  // Send self-calibration command on first call
  if (!testTracking.selfCalRequestSent) {
    if (faderBuddy.selfCalibrate()) {
      Serial.println("Self-calibration command sent");
      testTracking.selfCalRequestSent = true;
    } else {
      testTracking.failedTestName = "CAL CMD";
      Serial.println("FAILED: Could not send self-calibration command");
      return true;  // Test complete (failed)
    }
  }

  // Read motor fader state continuously
  readFaderBuddyState();

  if (!faderState.valid) {
    // Can't read state, check for overall timeout
    if (millis() - testTracking.selfCalStartTime > TOTAL_TIMEOUT_MS) {
      testTracking.failedTestName = "CAL NO I2C";
      Serial.println("FAILED: Cannot read motor fader state");
      return true;  // Test complete (failed)
    }
    return false;  // Keep trying
  }

  // Update min/max ADC values
  if (faderState.rawADC < testTracking.selfCalMinADC) {
    testTracking.selfCalMinADC = faderState.rawADC;
    Serial.print("Self-cal: New min ADC = ");
    Serial.println(faderState.rawADC);
  }
  if (faderState.rawADC > testTracking.selfCalMaxADC) {
    testTracking.selfCalMaxADC = faderState.rawADC;
    Serial.print("Self-cal: New max ADC = ");
    Serial.println(faderState.rawADC);
  }

  // Check if mode entered self-calibration
  if (!testTracking.selfCalEnteredMode) {
    if (faderState.mode == MODE_SELF_CALIBRATION) {
      testTracking.selfCalEnteredMode = true;
      Serial.println("Self-calibration mode entered");
    } else if (millis() - testTracking.selfCalStartTime > MODE_ENTRY_TIMEOUT_MS) {
      testTracking.failedTestName = "CAL NO ENTRY";
      Serial.print("FAILED: Did not enter calibration mode (current mode: ");
      Serial.print(getModeString(faderState.mode));
      Serial.println(")");
      return true;  // Test complete (failed)
    }
  }

  // Check for ERROR mode
  if (faderState.mode == MODE_ERROR) {
    testTracking.failedTestName = "CAL ERROR";
    Serial.println("FAILED: Motor fader entered ERROR mode during calibration");
    return true;  // Test complete (failed)
  }

  // Check for calibration completion (transition away from MODE_SELF_CALIBRATION)
  if (testTracking.selfCalEnteredMode && faderState.mode == MODE_INPUT_IDLE) {
    Serial.print("Self-calibration completed. Mode: ");
    Serial.println(getModeString(faderState.mode));
    Serial.print("ADC range observed: ");
    Serial.print(testTracking.selfCalMinADC);
    Serial.print(" - ");
    Serial.println(testTracking.selfCalMaxADC);

    // Validate ADC extremes were reached
    if (testTracking.selfCalMinADC > MIN_ADC_THRESHOLD) {
      testTracking.failedTestName = "CAL ADC MIN";
      Serial.print("FAILED: ADC min (");
      Serial.print(testTracking.selfCalMinADC);
      Serial.print(") did not reach below ");
      Serial.println(MIN_ADC_THRESHOLD);
      return true;  // Test complete (failed)
    }

    if (testTracking.selfCalMaxADC < MAX_ADC_THRESHOLD) {
      testTracking.failedTestName = "CAL ADC MAX";
      Serial.print("FAILED: ADC max (");
      Serial.print(testTracking.selfCalMaxADC);
      Serial.print(") did not reach above ");
      Serial.println(MAX_ADC_THRESHOLD);
      return true;  // Test complete (failed)
    }

    Serial.println("PASSED: Self-calibration completed successfully");
    return true;  // Test complete (passed)
  }

  // Check for overall timeout
  if (millis() - testTracking.selfCalStartTime > TOTAL_TIMEOUT_MS) {
    testTracking.failedTestName = "CAL TIMEOUT";
    Serial.println("FAILED: Self-calibration timed out");
    return true;  // Test complete (failed)
  }

  return false;  // Still running
}

bool testDiagnostics() {
  const uint32_t MOVEMENT_TIMEOUT_MS = 3000;

  // Initialize on first call
  if (testTracking.diagnosticsMovementStep == 0) {
    Serial.println("Starting I2C diagnostics...");
    readFaderBuddyVersionInfo();
  }

  // Continuously read motor state
  readFaderBuddyState();

  // State machine for movements:
  // Step 0: Command position 10
  // Step 1: Wait for idle
  // Step 2: Command position 200
  // Step 3: Wait for idle
  // Step 4: Command position 80
  // Step 5: Wait for idle
  // Step 6: Complete

  switch (testTracking.diagnosticsMovementStep) {
    case 0:  // Command position 10
      faderBuddy.writeTargetPosition(10);
      Serial.println("Commanding position 10");
      testTracking.testStartTime = millis();  // Start timeout for idle wait
      testTracking.diagnosticsMovementStep = 1;
      return false;

    case 1:  // Wait for idle after position 10
      if (faderState.valid && faderState.mode == MODE_INPUT_IDLE) {
        Serial.println("Reached idle state");
        testTracking.diagnosticsMovementStep = 2;
        return false;
      }
      if (millis() - testTracking.testStartTime > MOVEMENT_TIMEOUT_MS) {
        testTracking.failedTestName = "DIAG MVT1";
        Serial.println("FAILED: Movement 1 timed out (did not reach idle within 3s)");
        return true;
      }
      return false;

    case 2:  // Command position 200
      faderBuddy.writeTargetPosition(200);
      Serial.println("Commanding position 200");
      testTracking.testStartTime = millis();  // Start timeout for idle wait
      testTracking.diagnosticsMovementStep = 3;
      return false;

    case 3:  // Wait for idle after position 200
      if (faderState.valid && faderState.mode == MODE_INPUT_IDLE) {
        Serial.println("Reached idle state");
        testTracking.diagnosticsMovementStep = 4;
        return false;
      }
      if (millis() - testTracking.testStartTime > MOVEMENT_TIMEOUT_MS) {
        testTracking.failedTestName = "DIAG MVT2";
        Serial.println("FAILED: Movement 2 timed out (did not reach idle within 3s)");
        return true;
      }
      return false;

    case 4:  // Command position 80
      faderBuddy.writeTargetPosition(80);
      Serial.println("Commanding position 80");
      testTracking.testStartTime = millis();  // Start timeout for idle wait
      testTracking.diagnosticsMovementStep = 5;
      return false;

    case 5:  // Wait for idle after position 80
      if (faderState.valid && faderState.mode == MODE_INPUT_IDLE) {
        Serial.println("Reached idle state");
        testTracking.diagnosticsMovementStep = 6;
        return false;
      }
      if (millis() - testTracking.testStartTime > MOVEMENT_TIMEOUT_MS) {
        testTracking.failedTestName = "DIAG MVT3";
        Serial.println("FAILED: Movement 3 timed out (did not reach idle within 3s)");
        return true;
      }
      return false;

    case 6:  // Complete
      Serial.println("PASSED: Diagnostics complete");
      return true;

    default:
      return true;
  }
}

bool testTouchSensor() {
  // Initialize on first call
  if (testTracking.touchTestPhase == TestTracking::TOUCH_PHASE_CHECK_NO_TOUCH &&
      testTracking.touchTestPhaseStartTime == 0) {
    testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_CHECK_NO_TOUCH;
    testTracking.touchTestPhaseStartTime = millis();
    Serial.println("Starting touch sensor test...");

    // Re-attach servo for test (it was detached in setup)
    // servo.attach(PIN_SERVO);
    // servo.write(SERVO_CLEAR_POS);
  }

  // Continuously read motor fader state throughout the test
  readFaderBuddyState();

  if (!faderState.valid) {
    Serial.println("Failed to read motor state during touch test");
    return false;  // Keep trying
  }

  switch (testTracking.touchTestPhase) {
    case TestTracking::TOUCH_PHASE_CHECK_NO_TOUCH:
      // Confirm touch is not detected before starting
      if (faderState.touch) {
        testTracking.failedTestName = "TCH INITIAL";
        Serial.println("FAILED: Touch detected at start of test");
        testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_CLEANUP_ON_FAILURE;
        testTracking.touchTestPhaseStartTime = millis();
        return false;
      }
      Serial.println("Touch not detected, proceeding to movement");
      testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_MOVE_TO_POSITION;
      testTracking.touchTestPhaseStartTime = millis();
      return false;

    case TestTracking::TOUCH_PHASE_MOVE_TO_POSITION:
      // Command fader to position 0
      if (millis() - testTracking.touchTestPhaseStartTime < 100) {
        // Give a small delay before commanding to ensure we're ready
        return false;
      }
      if (!faderBuddy.writeTargetPosition(42)) {
        testTracking.failedTestName = "TCH CMD POS";
        Serial.println("FAILED: Could not command position 0");
        testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_CLEANUP_ON_FAILURE;
        testTracking.touchTestPhaseStartTime = millis();
        return false;
      }
      Serial.println("Commanded position 0, waiting for idle");
      testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_WAIT_FOR_IDLE;
      testTracking.touchTestPhaseStartTime = millis();
      return false;

    case TestTracking::TOUCH_PHASE_WAIT_FOR_IDLE:
      // Wait for fader to report idle state (timeout 9 seconds)
      if (faderState.mode == MODE_ERROR) {
        testTracking.failedTestName = "TCH ERROR";
        Serial.println("FAILED: Fader entered error state during movement");
        testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_CLEANUP_ON_FAILURE;
        testTracking.touchTestPhaseStartTime = millis();
        return false;
      }

      if (faderState.mode == MODE_INPUT_IDLE) {
        Serial.println("Fader reached idle state");
        testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_SERVO_TOUCH;
        testTracking.touchTestPhaseStartTime = millis();
        return false;
      }

      if (millis() - testTracking.touchTestPhaseStartTime > 9000) {
        testTracking.failedTestName = "TCH IDLE TMO";
        Serial.print("FAILED: Fader did not reach idle within 9 seconds (mode: ");
        Serial.print(getModeString(faderState.mode));
        Serial.println(")");
        testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_CLEANUP_ON_FAILURE;
        testTracking.touchTestPhaseStartTime = millis();
        return false;
      }
      return false;  // Still waiting

    case TestTracking::TOUCH_PHASE_SERVO_TOUCH:
      // Activate servo to touch position
      servo.write(SERVO_TOUCH_POS);
      Serial.println("Servo activated to touch position");
      testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_WAIT_FOR_TOUCH_DETECT;
      testTracking.touchTestPhaseStartTime = millis();
      return false;

    case TestTracking::TOUCH_PHASE_WAIT_FOR_TOUCH_DETECT:
      // Wait for touch to be detected (timeout 3 seconds)
      if (faderState.touch) {
        Serial.println("Touch detected!");
        testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_CONFIRM_TOUCH;
        testTracking.touchTestPhaseStartTime = millis();
        return false;
      }

      if (millis() - testTracking.touchTestPhaseStartTime > 3000) {
        testTracking.failedTestName = "TCH NO DET";
        Serial.println("FAILED: Touch not detected within 3 seconds");
        testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_CLEANUP_ON_FAILURE;
        testTracking.touchTestPhaseStartTime = millis();
        return false;
      }
      return false;  // Still waiting

    case TestTracking::TOUCH_PHASE_CONFIRM_TOUCH:
      // Wait 1 second and confirm touch is still detected
      if (millis() - testTracking.touchTestPhaseStartTime < 200) {
        // Debounce for a bit after initial touch
        return false;
      }
      if (millis() - testTracking.touchTestPhaseStartTime < 1000) {
        if (!faderState.touch) {
          testTracking.failedTestName = "TCH LOST";
          Serial.println("FAILED: Touch lost during confirmation period");
          testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_CLEANUP_ON_FAILURE;
          testTracking.touchTestPhaseStartTime = millis();
          return false;
        }
        return false;  // Still confirming
      }

      Serial.println("Touch confirmed for 1 second");
      testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_SERVO_CLEAR;
      testTracking.touchTestPhaseStartTime = millis();
      return false;

    case TestTracking::TOUCH_PHASE_SERVO_CLEAR:
      // Move servo to clear position
      servo.write(SERVO_CLEAR_POS);
      Serial.println("Servo moved to clear position");
      testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_WAIT_FOR_TOUCH_CLEAR;
      testTracking.touchTestPhaseStartTime = millis();
      return false;

    case TestTracking::TOUCH_PHASE_WAIT_FOR_TOUCH_CLEAR:
      // Wait for touch to clear (timeout 3 seconds)
      if (!faderState.touch) {
        Serial.println("Touch cleared");
        testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_FINAL_WAIT;
        testTracking.touchTestPhaseStartTime = millis();
        return false;
      }

      if (millis() - testTracking.touchTestPhaseStartTime > 3000) {
        testTracking.failedTestName = "TCH NO CLR";
        Serial.println("FAILED: Touch did not clear within 3 seconds");
        testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_CLEANUP_ON_FAILURE;
        testTracking.touchTestPhaseStartTime = millis();
        return false;
      }
      return false;  // Still waiting

    case TestTracking::TOUCH_PHASE_FINAL_WAIT:
      // // Wait 2 seconds after touch cleared
      // if (millis() - testTracking.touchTestPhaseStartTime < 2000) {
      //   return false;  // Still waiting
      // }

      // // Disable servo
      // servo.detach();
      // Serial.println("Servo disabled");
      Serial.println("PASSED: Touch sensor test complete");
      return true;  // Test complete (passed)

    case TestTracking::TOUCH_PHASE_CLEANUP_ON_FAILURE:
      servo.write(SERVO_CLEAR_POS);
      Serial.println("Moving servo to clear position for cleanup");
      return true;  // Test complete (failed)
      // // On first entry to cleanup phase, move servo to clear and start timer
      // if (millis() - testTracking.touchTestPhaseStartTime < 100) {
      //   servo.write(SERVO_CLEAR_POS);
      //   Serial.println("Moving servo to clear position for cleanup");
      //   return false;
      // }

      // // Wait 2 seconds for servo to reach clear position
      // if (millis() - testTracking.touchTestPhaseStartTime < 2000) {
      //   return false;  // Still waiting
      // }

      // // Disable servo and complete test (failed)
      // servo.detach();
      // Serial.println("Servo disabled after failure");
      // return true;  // Test complete (failed)

    default:
      return true;
  }
}


void handleTestStateMachine(bool presencePressed, float v0, float i0, float v1, float i1) {
  bool presenceJustPressed = presencePressed && !lastPresenceState;
  bool presenceJustReleased = !presencePressed && lastPresenceState;

  // Abort tests if presence released during any active test
  if (presenceJustReleased &&
      currentTestState != TEST_IDLE &&
      currentTestState != TEST_PASSED &&
      currentTestState != TEST_FAILED) {
    Serial.println("\n=== Test Aborted (presence released) ===\n");

    // Ensure the servo isn't left resting on the fader (e.g. mid touch-sensor test)
    servo.write(SERVO_CLEAR_POS);

    // Report cancellation to host script for CSV logging
    Serial.print(">>TEST_RESULT:CANCELLED:");
    Serial.print(getTestStateName(currentTestState));
    Serial.println("<<");

    currentTestState = TEST_IDLE;
    lastReportedTestState = TEST_IDLE;
    clearSerialBuffer();  // Clear serial buffer
    versionInfo.valid = false;
    faderState.valid = false;
    lastPresenceState = presencePressed;
    return;
  }

  switch (currentTestState) {
    case TEST_IDLE:
      // Continue refreshing motor state while idle (if available)
      readFaderBuddyState();
      if (!versionInfo.valid) {
        readFaderBuddyVersionInfo();
      }

      if (presenceJustPressed) {
        // Debounce delay before starting tests
        delay(50);
        Serial.println("\n=== Starting Test Sequence ===");
        // Reset test tracking
        testTracking.sampleCount = 0;
        testTracking.diagnosticsMovementStep = 0;
        testTracking.failedTestName = "";
        testTracking.firmwarePhase = TestTracking::FIRMWARE_PHASE_PING;
        testTracking.firmwarePhaseStartTime = 0;
        testTracking.firmwareCommandSent = false;
        testTracking.bootloadStep = TestTracking::BL_STEP_ENTER_CMD;
        testTracking.bootloadStepStartTime = 0;
        testTracking.bootloadPageIndex = 0;
        testTracking.bootloadTotalPages = 0;
        testTracking.debugLedTestStartTime = 0;
        testTracking.debugLedTransitionCount = 0;
        testTracking.debugLedState = false;
        testTracking.selfCalStartTime = 0;
        testTracking.selfCalRequestSent = false;
        testTracking.selfCalEnteredMode = false;
        testTracking.selfCalMinADC = 0xFFFF;
        testTracking.selfCalMaxADC = 0;
        testTracking.touchTestPhase = TestTracking::TOUCH_PHASE_CHECK_NO_TOUCH;
        testTracking.touchTestPhaseStartTime = 0;
        clearSerialBuffer();  // Clear serial buffer
        versionInfo.valid = false;
        faderState.valid = false;
        currentTestState = TEST_LOGIC_POWER;
      }
      break;

    case TEST_LOGIC_POWER:
      if (testLogicPower(v0, i0)) {
        if (!testTracking.failedTestName.isEmpty()) {
          currentTestState = TEST_FAILED;
        } else {
          testTracking.sampleCount = 0;  // Reset for next test
          currentTestState = TEST_MOTOR_POWER;
        }
      }
      break;

    case TEST_MOTOR_POWER:
      if (testMotorPower(v1, i1)) {
        if (!testTracking.failedTestName.isEmpty()) {
          currentTestState = TEST_FAILED;
        } else {
          currentTestState = TEST_POWER_LED;
        }
      }
      break;

    case TEST_POWER_LED:
      if (testPowerLED()) {
        if (!testTracking.failedTestName.isEmpty()) {
          currentTestState = TEST_FAILED;
        } else {
          currentTestState = TEST_FW_BOOTSTRAP;
        }
      }
      break;

    case TEST_FW_BOOTSTRAP:
      // Dummy mode: skip both the bootstrap (needs test_host.py + UPDI) and
      // the I2C bootloader update (needs a real fader) after a simple delay,
      // so the rest of the jig flow can be exercised on the bench alone.
      if (dummyMode) {
        if (testTracking.firmwarePhaseStartTime == 0) {
          testTracking.firmwarePhaseStartTime = millis();
          Serial.println("DUMMY MODE: Skipping firmware bootstrap + I2C update (2s delay)");
        } else if (millis() - testTracking.firmwarePhaseStartTime >= 2000) {
          Serial.println("DUMMY MODE: Firmware bootstrap + I2C update skipped");
          currentTestState = TEST_DEBUG_LED;
        }
        break;
      }
      if (testFwBootstrap()) {
        if (!testTracking.failedTestName.isEmpty()) {
          currentTestState = TEST_FAILED;
        } else {
          currentTestState = TEST_FW_I2C_UPDATE;
        }
      }
      break;

    case TEST_FW_I2C_UPDATE:
      if (testFwI2cUpdate()) {
        if (!testTracking.failedTestName.isEmpty()) {
          currentTestState = TEST_FAILED;
        } else {
          currentTestState = TEST_DEBUG_LED;
        }
      }
      break;

    case TEST_DEBUG_LED:
      if (testDebugLED()) {
        if (!testTracking.failedTestName.isEmpty()) {
          currentTestState = TEST_FAILED;
        } else {
          currentTestState = TEST_SELF_CALIBRATION;
        }
      }
      break;

    case TEST_SELF_CALIBRATION:
      if (testSelfCalibration()) {
        if (!testTracking.failedTestName.isEmpty()) {
          currentTestState = TEST_FAILED;
        } else {
          currentTestState = TEST_DIAGNOSTICS;
        }
      }
      break;

    case TEST_DIAGNOSTICS:
      if (testDiagnostics()) {
        if (!testTracking.failedTestName.isEmpty()) {
          currentTestState = TEST_FAILED;
        } else {
          currentTestState = TEST_TOUCH_SENSOR;
        }
      }
      break;

    case TEST_TOUCH_SENSOR:
      if (testTouchSensor()) {
        if (!testTracking.failedTestName.isEmpty()) {
          currentTestState = TEST_FAILED;
        } else {
          Serial.println("\n=== ALL TESTS PASSED ===\n");
          currentTestState = TEST_PASSED;
        }
      }
      break;

    case TEST_PASSED:
      // Continue refreshing motor state while in passed state
      readFaderBuddyState();

      if (presenceJustReleased) {
        Serial.println("Returning to idle\n");
        currentTestState = TEST_IDLE;
        versionInfo.valid = false;
        faderState.valid = false;
      }
      break;

    case TEST_FAILED:
      // Continue refreshing motor state while in failed state
      readFaderBuddyState();

      if (presenceJustReleased) {
        Serial.println("Test failed, returning to idle\n");
        currentTestState = TEST_IDLE;
        versionInfo.valid = false;
        faderState.valid = false;
      }
      break;
  }

  // Report test lifecycle transitions to the host script so it can log results to CSV.
  // Only the interesting transitions (test start / pass / fail) are sent; everything
  // else (e.g. moving between individual sub-tests) updates lastReportedTestState
  // without emitting anything.
  if (currentTestState != lastReportedTestState) {
    if (currentTestState == TEST_LOGIC_POWER) {
      Serial.println(">>TEST_START<<");
    } else if (currentTestState == TEST_PASSED) {
      Serial.println(">>TEST_RESULT:PASS<<");
    } else if (currentTestState == TEST_FAILED) {
      Serial.print(">>TEST_RESULT:FAIL:");
      Serial.print(testTracking.failedTestName);
      Serial.println("<<");
    }
    lastReportedTestState = currentTestState;
  }

  lastPresenceState = presencePressed;
}


// ============================================================================
// Main Loop
// ============================================================================

void loop() {
  // Left button (active low): press-and-hold toggles dummy mode.
  bool buttonPressed = (digitalRead(PIN_BUTTON_LEFT) == LOW);
  if (buttonPressed && !lastButtonPressed) {
    buttonPressStartTime = millis();
    buttonHoldHandled = false;
  } else if (buttonPressed && !buttonHoldHandled &&
             millis() - buttonPressStartTime >= BUTTON_HOLD_MS) {
    dummyMode = !dummyMode;
    buttonHoldHandled = true;
    Serial.printf("Dummy mode %s (button hold)\n", dummyMode ? "ENABLED" : "DISABLED");
  }
  lastButtonPressed = buttonPressed;

  bool pressed = !digitalRead(PIN_PRESENCE_SWITCH);

  // Read power measurements
  float voltage0 = ina3221.getBusVoltage(0);
  float current0 = ina3221.getCurrentAmps(0) * 1000;
  float voltage1 = ina3221.getBusVoltage(1);
  float current1 = ina3221.getCurrentAmps(1) * 1000;

  // Handle test state machine
  handleTestStateMachine(pressed, voltage0, current0, voltage1, current1);

  // LED control based on state
  if (currentTestState == TEST_IDLE) {
    analogWrite(PIN_LED_RED, 20);
    analogWrite(PIN_LED_GREEN, 5);
  } else if (currentTestState == TEST_PASSED) {
    // Solid green for passed
    analogWrite(PIN_LED_RED, 0);
    analogWrite(PIN_LED_GREEN, 700);
  } else if (currentTestState == TEST_FAILED) {
    // Blinking red for failed
    analogWrite(PIN_LED_RED, (millis() % 350 < 175) * 500 + 500);
    analogWrite(PIN_LED_GREEN, 0);
  } else {
    // Alternating red/green breathing during tests (1s cycle, 30-60% brightness)
    float breathe = sin(millis() / 500.0 * 2.0 * PI);
    uint16_t brightnessRed = LERP(breathe, -1.0, 1.0, 0.1 * 1023, 0.5 * 1023);
    uint16_t brightnessGreen = LERP(-breathe, -1.0, 1.0, 0.03 * 1023, 0.15 * 1023);

    analogWrite(PIN_LED_RED, brightnessRed);
    analogWrite(PIN_LED_GREEN, brightnessGreen);
  }

  updateDisplay(voltage0, current0, voltage1, current1);

  delay(10);
}