#include <Arduino.h>
#include <ctype.h>
#include <math.h>
#include <FastLED.h>

static const uint8_t GAUGE_COUNT = 3;

// One shared WS2812B strip, split into per-gauge segments.
static const uint8_t LED_DATA_PIN = 22;

// For now, command and debug traffic share the same serial port.
#define CMD_PORT Serial1
#define DEBUG_PORT Serial1
static const unsigned long SERIAL_BAUD = 38400;

namespace vfd {

constexpr uint8_t kDataPin = 46;
constexpr uint8_t kClockPin = 47;
constexpr uint8_t kLatchPin = 48;
constexpr int8_t kBlankPin = 49;   // Set to -1 if BL/OE is not connected
constexpr bool kBlankActiveHigh = true;

constexpr unsigned long kDigitHoldMicros = 2000;
constexpr uint8_t kDigitCount = 4;
constexpr uint8_t kSegmentCount = 7;
constexpr uint8_t kDriverBits = 20;

constexpr uint8_t kSegmentStartBit = 0;    // HVOut1 -> bit 0
constexpr uint8_t kPointSegmentBit = 7;    // HVOut8 -> bit 7
constexpr uint8_t kBellSegmentBit = 8;     // HVOut9 -> bit 8
constexpr uint8_t kGridStartBit = 9;       // HVOut10 -> bit 9
constexpr uint8_t kIndicatorGridBit = 13;  // HVOut14 -> bit 13

char displayBuffer[kDigitCount] = {' ', ' ', ' ', ' '};
bool pointEnabled = false;
bool bellEnabled = false;
uint8_t currentPhase = 0;

uint8_t encodeCharacter(char c) {
  switch (c) {
    case '0': return 0b0111111;
    case '1': return 0b0000110;
    case '2': return 0b1011011;
    case '3': return 0b1001111;
    case '4': return 0b1100110;
    case '5': return 0b1101101;
    case '6': return 0b1111101;
    case '7': return 0b0000111;
    case '8': return 0b1111111;
    case '9': return 0b1101111;
    case 'A':
    case 'a': return 0b1110111;
    case 'B':
    case 'b': return 0b1111100;
    case 'C':
    case 'c': return 0b0111001;
    case 'D':
    case 'd': return 0b1011110;
    case 'E':
    case 'e': return 0b1111001;
    case 'F':
    case 'f': return 0b1110001;
    case '-': return 0b1000000;
    default: return 0;
  }
}

void shiftDriverWord(uint32_t word) {
  digitalWrite(kLatchPin, HIGH);
  digitalWrite(kClockPin, HIGH);

  for (int8_t bit = kDriverBits - 1; bit >= 0; --bit) {
    digitalWrite(kDataPin, (word >> bit) & 0x1U ? HIGH : LOW);
    digitalWrite(kClockPin, LOW);
    digitalWrite(kClockPin, HIGH);
  }

  digitalWrite(kLatchPin, LOW);
  digitalWrite(kLatchPin, HIGH);
}

void setBlanked(bool blanked) {
  if (kBlankPin < 0) return;

  const bool level = kBlankActiveHigh ? blanked : !blanked;
  digitalWrite(kBlankPin, level ? HIGH : LOW);
}

void writeText(const char* text) {
  for (uint8_t i = 0; i < kDigitCount; ++i) {
    displayBuffer[i] = ' ';
  }

  size_t len = strlen(text);
  if (len > kDigitCount) {
    text += len - kDigitCount;
    len = kDigitCount;
  }

  const uint8_t start = kDigitCount - len;
  for (uint8_t i = 0; i < len; ++i) {
    displayBuffer[start + i] = text[i];
  }
}

void clear() {
  writeText("");
  pointEnabled = false;
  bellEnabled = false;
}

bool parseCommand(const String& command) {
  char displayText[16];
  size_t inputIndex = 0;
  size_t displayIndex = 0;

  if (command.length() == 0) {
    return false;
  }

  if (command[inputIndex] == '-') {
    if (displayIndex + 1 >= sizeof(displayText)) {
      return false;
    }
    displayText[displayIndex++] = command[inputIndex++];
  }

  const size_t digitStart = inputIndex;
  while (inputIndex < static_cast<size_t>(command.length()) &&
         isxdigit(static_cast<unsigned char>(command[inputIndex]))) {
    if (displayIndex + 1 >= sizeof(displayText)) {
      return false;
    }
    displayText[displayIndex++] = toupper(static_cast<unsigned char>(command[inputIndex]));
    ++inputIndex;
  }

  if (inputIndex == digitStart) {
    return false;
  }

  bool newPointEnabled = false;
  bool newBellEnabled = false;
  while (inputIndex < static_cast<size_t>(command.length())) {
    if (command[inputIndex] == '.') {
      newPointEnabled = true;
    } else if (command[inputIndex] == '!') {
      newBellEnabled = true;
    } else {
      return false;
    }
    ++inputIndex;
  }

  displayText[displayIndex] = '\0';
  writeText(displayText);
  pointEnabled = newPointEnabled;
  bellEnabled = newBellEnabled;
  return true;
}

void renderDigit(uint8_t digitIndex) {
  uint32_t word = 0;
  const uint8_t segments = encodeCharacter(displayBuffer[digitIndex]);

  for (uint8_t segment = 0; segment < kSegmentCount; ++segment) {
    if ((segments >> segment) & 0x1U) {
      word |= (1UL << (kSegmentStartBit + segment));
    }
  }

  word |= (1UL << (kGridStartBit + digitIndex));
  shiftDriverWord(word);
}

void renderIndicator() {
  uint32_t word = 1UL << kIndicatorGridBit;
  if (pointEnabled) {
    word |= 1UL << kPointSegmentBit;
  }
  if (bellEnabled) {
    word |= 1UL << kBellSegmentBit;
  }
  shiftDriverWord(word);
}

void begin() {
  pinMode(kDataPin, OUTPUT);
  pinMode(kClockPin, OUTPUT);
  pinMode(kLatchPin, OUTPUT);
  if (kBlankPin >= 0) {
    pinMode(kBlankPin, OUTPUT);
  }

  digitalWrite(kDataPin, LOW);
  digitalWrite(kClockPin, HIGH);
  digitalWrite(kLatchPin, HIGH);
  setBlanked(true);
  writeText("0");
  shiftDriverWord(0);
}

void refresh() {
  setBlanked(true);
  if (currentPhase < kDigitCount) {
    renderDigit(currentPhase);
  } else if (pointEnabled || bellEnabled) {
    renderIndicator();
  } else {
    shiftDriverWord(0);
  }
  setBlanked(false);
  delayMicroseconds(kDigitHoldMicros);
  setBlanked(true);

  currentPhase = (currentPhase + 1) % (kDigitCount + 1);
}

}  // namespace vfd

struct GaugePins {
  uint8_t dirPin;
  uint8_t stepPin;
  int8_t enablePin;          // -1 means there is no enable pin
  bool dirInverted;
  bool stepActiveHigh;
  bool enableActiveLow;
  uint8_t ledCount;          // LEDs assigned to this gauge
};

constexpr GaugePins gaugePins[GAUGE_COUNT] = {
  // dir, step, en, dirInv, stepHigh, enActiveLow, leds
  {50, 51, -1, false, true, true, 7},   // Gauge 0
  {8,  9,  -1, true,  true, true, 7},   // Gauge 1
  {52, 53, -1, false, true, true, 7},   // Gauge 2
};

constexpr uint8_t sumLedCounts(uint8_t i = 0) {
  return i >= GAUGE_COUNT ? 0 : gaugePins[i].ledCount + sumLedCounts(i + 1);
}
static const uint8_t TOTAL_LEDS = sumLedCounts();

enum HomingState : uint8_t {
  HS_IDLE,
  HS_START,
  HS_BACKING,
  HS_SETTLE,
  HS_DONE
};

struct Gauge {
  long currentPos = 0;
  long targetPos = 0;

  long minPos = 0;
  long maxPos = 3780;
  long homingBackoffSteps = 3800; // Deliberately a touch past full reverse travel.

  float velocity = 0.0f;
  float maxSpeed = 4000.0f;
  float accel = 6000.0f;
  float homingSpeed = 500.0f;

  float stepAccumulator = 0.0f;
  unsigned long lastUpdateMicros = 0;

  bool enabled = true;
  bool homed = false;

  HomingState homingState = HS_IDLE;
  long homingStepsRemaining = 0;
  unsigned long homingLastStepMicros = 0;
  unsigned long homingStateStartMs = 0;

  bool sweepEnabled = false;
  bool sweepTowardMax = true;
};

enum LedFx : uint8_t { FX_BLINK = 0, FX_BREATHE = 1, FX_DFLASH = 2 };

struct BlinkState {
  bool          active      = false;
  LedFx         fx          = FX_BLINK;
  CRGB          onColor;
  unsigned long lastMs      = 0;
  uint16_t      onMs        = 500;
  uint16_t      offMs       = 500;
  bool          currentlyOn = false;
  uint16_t      periodMs    = 2000;
  uint16_t      cyclePos    = 0;
  uint8_t       dphase      = 0;
};

Gauge gauges[GAUGE_COUNT];
String rxLine;

CRGB leds[TOTAL_LEDS];
uint8_t gaugeLedOffset[GAUGE_COUNT];
BlinkState blinkState[TOTAL_LEDS];
bool ledsDirty = false;

// Sends one-line command replies back over the control port.
//
// Serial protocol summary.
//
// Host -> controller commands (newline-terminated ASCII):
//   SET <id> <pos>
//   SPEED <id> <steps_per_s>
//   ACCEL <id> <steps_per_s2>
//   ENABLE <id> <0|1>
//   ZERO <id>
//   HOME <id>
//   HOMEALL
//   SWEEP <id> <accel> <speed>
//   POS?
//   LED?
//   LED <id> <idx|a-b> <r> <g> <b>
//   BLINK <id> <idx|a-b> <on_ms> <off_ms> [<r> <g> <b>]
//   BREATHE <id> <idx|a-b> <period_ms> <r> <g> <b>
//   DFLASH <id> <idx|a-b> <r> <ig> <b>
//   VFD <text[.!]>
//   PING
//
// Controller -> host replies / events:
//   READY
//     Sent once from setup() after boot completes.
//   OK
//     Sent after a valid mutating command, and after POS?/LED? once all data lines
//     for that query have been emitted.
//   PONG
//     Sent in response to PING.
//   ERR BAD_CMD
//     Sent when a complete line matches no parser.
//   ERR TOO_LONG
//     Sent when an input line exceeds the receive buffer limit.
//   ERR BAD_ID
//     Sent by commands that take a gauge id when the id is outside 0..GAUGE_COUNT-1.
//   ERR BAD_SPEED
//     Sent by SPEED when the requested speed is <= 0.
//   ERR BAD_ACCEL
//     Sent by ACCEL when the requested acceleration is <= 0.
//   ERR BAD_IDX
//     Sent by LED/BLINK/BREATHE/DFLASH when an LED index or range is invalid.
//   ERR BAD_TIME
//     Sent by BLINK/BREATHE when the timing parameter is invalid.
//   ERR BAD_VFD
//     Sent by VFD when the text payload is malformed.
//   POS <id> <currentPos> <targetPos> <homed> <homingState> <sweepEnabled>
//     Emitted once per gauge before the trailing OK reply to POS?.
//   LED <id> <idx> <r> <g> <b>
//     Emitted once per configured LED before the trailing OK reply to LED?.
//   HOMED <id>
//     Debug event printed on DEBUG_PORT when a homing sequence settles successfully.
void sendReply(const String& s) {
  CMD_PORT.println(s);
}

// Tiny float absolute-value helper to avoid dragging more machinery into the sketch.
float absf(float x) {
  return (x < 0.0f) ? -x : x;
}

// Updates the cached enable state and toggles the hardware pin if one exists.
void setEnable(uint8_t id, bool en) {
  if (id >= GAUGE_COUNT) return;
  gauges[id].enabled = en;

  int8_t pin = gaugePins[id].enablePin;
  if (pin < 0) return;

  bool level = gaugePins[id].enableActiveLow ? !en : en;
  digitalWrite(pin, level ? HIGH : LOW);
}

// Applies the logical direction after accounting for per-gauge inversion.
void setDir(uint8_t id, bool forward) {
  bool level = gaugePins[id].dirInverted ? !forward : forward;
  digitalWrite(gaugePins[id].dirPin, level ? HIGH : LOW);
}

// Emits one step pulse with the polarity expected by the driver.
void pulseStep(uint8_t id) {
  bool active = gaugePins[id].stepActiveHigh;
  digitalWrite(gaugePins[id].stepPin, active ? HIGH : LOW);
  delayMicroseconds(4);
  digitalWrite(gaugePins[id].stepPin, active ? LOW : HIGH);
}

// Moves the motor by one step if the requested direction is still within allowed travel.
void doStep(uint8_t id, int dir, bool allowPastMin = false) {
  Gauge& g = gauges[id];
  if (!g.enabled) return;

  if (dir > 0) {
    if (g.currentPos >= g.maxPos) return;
    setDir(id, true);
    pulseStep(id);
    g.currentPos++;
  } else if (dir < 0) {
    if (!allowPastMin && g.currentPos <= g.minPos) return;
    setDir(id, false);
    pulseStep(id);
    g.currentPos--;
  }
}

// Arms the homing state machine for one gauge and clears any in-flight motion.
void requestHome(uint8_t id) {
  if (id >= GAUGE_COUNT) return;
  Gauge& g = gauges[id];
  g.homingState = HS_START;
  g.homed = false;
  g.velocity = 0.0f;
  g.stepAccumulator = 0.0f;
  g.sweepEnabled = false;
}

// Starts the same homing sequence on every configured gauge.
void requestHomeAll() {
  for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
    requestHome(i);
  }
}

// Advances the simple homing state machine until the gauge is parked at logical zero.
void updateHoming(uint8_t id) {
  Gauge& g = gauges[id];
  unsigned long nowUs = micros();
  unsigned long nowMs = millis();

  switch (g.homingState) {
    case HS_IDLE:
      return;

    case HS_START:
      // No endstop here; homing just walks back far enough to hit the hard stop.
      g.velocity = 0.0f;
      g.stepAccumulator = 0.0f;
      g.homingStepsRemaining = g.homingBackoffSteps;
      g.homingLastStepMicros = nowUs;
      g.homingState = HS_BACKING;
      break;

    case HS_BACKING: {
      float intervalUs = 1000000.0f / g.homingSpeed;
      if ((nowUs - g.homingLastStepMicros) >= intervalUs) {
        g.homingLastStepMicros = nowUs;

        if (g.homingStepsRemaining > 0) {
          doStep(id, -1, true);
          g.homingStepsRemaining--;
        } else {
          g.homingState = HS_SETTLE;
          g.homingStateStartMs = nowMs;
        }
      }
      break;
    }

    case HS_SETTLE:
      if (nowMs - g.homingStateStartMs >= 100) {
        g.currentPos = 0;
        g.targetPos = 0;
        g.velocity = 0.0f;
        g.stepAccumulator = 0.0f;
        g.homed = true;
        g.homingState = HS_DONE;

        DEBUG_PORT.print("HOMED ");
        DEBUG_PORT.println(id);
      }
      break;

    case HS_DONE:
      g.homingState = HS_IDLE;
      break;
  }
}

// Flips the sweep destination when the gauge has settled at either end of travel.
void updateSweepTarget(uint8_t id) {
  Gauge& g = gauges[id];
  if (!g.sweepEnabled || !g.homed || g.homingState != HS_IDLE) return;

  if (g.sweepTowardMax) {
    g.targetPos = g.maxPos;
    if (g.currentPos >= g.maxPos && absf(g.velocity) < 1.0f) {
      g.sweepTowardMax = false;
      g.targetPos = g.minPos;
    }
  } else {
    g.targetPos = g.minPos;
    if (g.currentPos <= g.minPos && absf(g.velocity) < 1.0f) {
      g.sweepTowardMax = true;
      g.targetPos = g.maxPos;
    }
  }
}

// Runs one gauge worth of motion control, including homing and optional sweeping.
void updateGauge(uint8_t id) {
  Gauge& g = gauges[id];

  if (g.homingState != HS_IDLE) {
    updateHoming(id);
    return;
  }

  if (!g.homed) return;

  if (g.sweepEnabled) {
    updateSweepTarget(id);
  }

  unsigned long now = micros();
  if (g.lastUpdateMicros == 0) {
    g.lastUpdateMicros = now;
    return;
  }

  float dt = (now - g.lastUpdateMicros) / 1000000.0f;
  g.lastUpdateMicros = now;

  if (dt <= 0.0f || dt > 0.1f) return;

  long error = g.targetPos - g.currentPos;

  if (error == 0 && absf(g.velocity) < 0.01f) {
    g.velocity = 0.0f;
    g.stepAccumulator = 0.0f;
    return;
  }

  float dir = (error > 0) ? 1.0f : (error < 0 ? -1.0f : 0.0f);
  // Basic trapezoidal profile: brake if the remaining travel is shorter than the stop distance.
  float brakingDistance = (g.velocity * g.velocity) / (2.0f * g.accel + 0.0001f);

  if ((float)labs(error) <= brakingDistance) {
    if (g.velocity > 0.0f) {
      g.velocity -= g.accel * dt;
      if (g.velocity < 0.0f) g.velocity = 0.0f;
    } else if (g.velocity < 0.0f) {
      g.velocity += g.accel * dt;
      if (g.velocity > 0.0f) g.velocity = 0.0f;
    }
  } else {
    g.velocity += dir * g.accel * dt;
    if (g.velocity > g.maxSpeed) g.velocity = g.maxSpeed;
    if (g.velocity < -g.maxSpeed) g.velocity = -g.maxSpeed;
  }

  if (fabs(g.velocity) < 0.01f && error != 0) {
    g.velocity = dir * 5.0f;
  }

  // Integrate fractional steps until there is enough to emit a real pulse.
  g.stepAccumulator += g.velocity * dt;

  while (g.stepAccumulator >= 1.0f) {
    if (g.currentPos == g.targetPos) {
      g.stepAccumulator = 0.0f;
      g.velocity = 0.0f;
      break;
    }

    doStep(id, +1, false);
    g.stepAccumulator -= 1.0f;

    if (g.currentPos >= g.maxPos) {
      g.currentPos = g.maxPos;
      g.targetPos = g.maxPos;
      g.velocity = 0.0f;
      g.stepAccumulator = 0.0f;
      break;
    }
  }

  while (g.stepAccumulator <= -1.0f) {
    if (g.currentPos == g.targetPos) {
      g.stepAccumulator = 0.0f;
      g.velocity = 0.0f;
      break;
    }

    doStep(id, -1, false);
    g.stepAccumulator += 1.0f;

    if (g.currentPos <= g.minPos) {
      g.currentPos = g.minPos;
      g.targetPos = g.minPos;
      g.velocity = 0.0f;
      g.stepAccumulator = 0.0f;
      break;
    }
  }
}

// Parses `SET <id> <pos>` and updates the target position.
// Replies: `OK`, `ERR BAD_ID`.
bool parseSet(const String& line) {
  int id;
  long pos;
  if (sscanf(line.c_str(), "SET %d %ld", &id, &pos) == 2) {
    if (id < 0 || id >= GAUGE_COUNT) {
      sendReply("ERR BAD_ID");
      return true;
    }

    Gauge& g = gauges[id];
    pos = constrain(pos, g.minPos, g.maxPos);
    g.targetPos = pos;
    g.sweepEnabled = false;
    sendReply("OK");
    return true;
  }
  return false;
}

// Parses `SPEED <id> <speed>` and updates the max step rate.
// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_SPEED`.
bool parseSpeed(const String& line) {
  int firstSpace = line.indexOf(' ');
  int secondSpace = line.indexOf(' ', firstSpace + 1);
  if (firstSpace < 0 || secondSpace < 0) return false;
  if (line.substring(0, firstSpace) != "SPEED") return false;

  int id = line.substring(firstSpace + 1, secondSpace).toInt();
  float speed = line.substring(secondSpace + 1).toFloat();

  if (id < 0 || id >= GAUGE_COUNT) {
    sendReply("ERR BAD_ID");
    return true;
  }
  if (speed <= 0.0f) {
    sendReply("ERR BAD_SPEED");
    return true;
  }

  gauges[id].maxSpeed = speed;
  sendReply("OK");
  return true;
}

// Parses `ACCEL <id> <accel>` and updates the acceleration limit.
// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_ACCEL`.
bool parseAccel(const String& line) {
  int firstSpace = line.indexOf(' ');
  int secondSpace = line.indexOf(' ', firstSpace + 1);
  if (firstSpace < 0 || secondSpace < 0) return false;
  if (line.substring(0, firstSpace) != "ACCEL") return false;

  int id = line.substring(firstSpace + 1, secondSpace).toInt();
  float accel = line.substring(secondSpace + 1).toFloat();

  if (id < 0 || id >= GAUGE_COUNT) {
    sendReply("ERR BAD_ID");
    return true;
  }
  if (accel <= 0.0f) {
    sendReply("ERR BAD_ACCEL");
    return true;
  }

  gauges[id].accel = accel;
  sendReply("OK");
  return true;
}

// Parses `ENABLE <id> <0|1>` and toggles the selected driver.
// Replies: `OK`, `ERR BAD_ID`.
bool parseEnable(const String& line) {
  int id, en;
  if (sscanf(line.c_str(), "ENABLE %d %d", &id, &en) == 2) {
    if (id < 0 || id >= GAUGE_COUNT) {
      sendReply("ERR BAD_ID");
      return true;
    }

    setEnable(id, en != 0);
    sendReply("OK");
    return true;
  }
  return false;
}

// Parses `ZERO <id>` and declares the current position to be home.
// Replies: `OK`, `ERR BAD_ID`.
bool parseZero(const String& line) {
  int id;
  if (sscanf(line.c_str(), "ZERO %d", &id) == 1) {
    if (id < 0 || id >= GAUGE_COUNT) {
      sendReply("ERR BAD_ID");
      return true;
    }

    Gauge& g = gauges[id];
    g.currentPos = 0;
    g.targetPos = 0;
    g.velocity = 0.0f;
    g.stepAccumulator = 0.0f;
    g.homed = true;
    g.sweepEnabled = false;
    sendReply("OK");
    return true;
  }
  return false;
}

// Parses `HOME <id>` or `HOMEALL` and kicks off the homing sequence.
// Replies: `OK`, `ERR BAD_ID`. Successful completion later emits debug line `HOMED <id>`.
bool parseHome(const String& line) {
  int id;
  if (sscanf(line.c_str(), "HOME %d", &id) == 1) {
    if (id < 0 || id >= GAUGE_COUNT) {
      sendReply("ERR BAD_ID");
      return true;
    }

    requestHome(id);
    sendReply("OK");
    return true;
  }

  if (line == "HOMEALL") {
    requestHomeAll();
    sendReply("OK");
    return true;
  }

  return false;
}

// Parses `SWEEP <id> <accel> <speed>` and enables or disables end-to-end motion.
// Replies: `OK`, `ERR BAD_ID`.
bool parseSweep(const String& line) {
  int firstSpace = line.indexOf(' ');
  int secondSpace = line.indexOf(' ', firstSpace + 1);
  int thirdSpace = line.indexOf(' ', secondSpace + 1);

  if (firstSpace < 0 || secondSpace < 0 || thirdSpace < 0) return false;
  if (line.substring(0, firstSpace) != "SWEEP") return false;

  int id = line.substring(firstSpace + 1, secondSpace).toInt();
  float accel = line.substring(secondSpace + 1, thirdSpace).toFloat();
  float speed = line.substring(thirdSpace + 1).toFloat();

  if (id < 0 || id >= GAUGE_COUNT) {
    sendReply("ERR BAD_ID");
    return true;
  }

  Gauge& g = gauges[id];

  if (accel <= 0.0f || speed <= 0.0f) {
    g.sweepEnabled = false;
    g.velocity = 0.0f;
    g.stepAccumulator = 0.0f;
    sendReply("OK");
    return true;
  }

  g.accel = accel;
  g.maxSpeed = speed;
  g.sweepEnabled = true;
  g.sweepTowardMax = true;
  g.targetPos = g.maxPos;
  sendReply("OK");
  return true;
}

// Answers `POS?` with current motion state for every gauge.
// Emits one `POS <id> <cur> <tgt> <homed> <homingState> <sweep>` line per gauge,
// then replies `OK`.
bool parsePosQuery(const String& line) {
  if (line == "POS?") {
    for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
      CMD_PORT.print("POS ");
      CMD_PORT.print(i);
      CMD_PORT.print(' ');
      CMD_PORT.print(gauges[i].currentPos);
      CMD_PORT.print(' ');
      CMD_PORT.print(gauges[i].targetPos);
      CMD_PORT.print(' ');
      CMD_PORT.print(gauges[i].homed ? 1 : 0);
      CMD_PORT.print(' ');
      CMD_PORT.print((int)gauges[i].homingState);
      CMD_PORT.print(' ');
      CMD_PORT.println(gauges[i].sweepEnabled ? 1 : 0);
    }
    sendReply("OK");
    return true;
  }
  return false;
}

// Answers `CFG?` with speed and acceleration for every gauge.
// Emits one `CFG <id> <maxSpeed> <accel>` line per gauge, then replies `OK`.
bool parseCfgQuery(const String& line) {
  if (line == "CFG?") {
    for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
      CMD_PORT.print("CFG ");
      CMD_PORT.print(i);
      CMD_PORT.print(' ');
      CMD_PORT.print((int)gauges[i].maxSpeed);
      CMD_PORT.print(' ');
      CMD_PORT.println((int)gauges[i].accel);
    }
    sendReply("OK");
    return true;
  }
  return false;
}

// Answers the mandatory life question: are you there?
// Reply: `PONG`.
bool parsePing(const String& line) {
  if (line == "PING") {
    sendReply("PONG");
    return true;
  }
  return false;
}

// Parses `VFD <text>` where <text> is up to four hex characters with optional `.` and `!` suffixes.
// Replies: `OK`, `ERR BAD_VFD`.
bool parseVfd(const String& line) {
  if (line == "VFD") {
    vfd::clear();
    sendReply("OK");
    return true;
  }

  if (!line.startsWith("VFD ")) return false;

  const String payload = line.substring(4);
  if (payload.length() == 0) {
    vfd::clear();
    sendReply("OK");
    return true;
  }

  if (vfd::parseCommand(payload)) {
    sendReply("OK");
  } else {
    sendReply("ERR BAD_VFD");
  }
  return true;
}

// Answers `LED?` with the current RGB values for every configured LED.
// Emits one `LED <id> <idx> <r> <g> <b>` line per configured LED, then replies `OK`.
bool parseLedQuery(const String& line) {
  if (line == "LED?") {
    for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
      for (uint8_t j = 0; j < gaugePins[i].ledCount; j++) {
        const CRGB& c = leds[gaugeLedOffset[i] + j];
        CMD_PORT.print("LED ");
        CMD_PORT.print(i);
        CMD_PORT.print(' ');
        CMD_PORT.print(j);
        CMD_PORT.print(' ');
        CMD_PORT.print(c.r);
        CMD_PORT.print(' ');
        CMD_PORT.print(c.g);
        CMD_PORT.print(' ');
        CMD_PORT.println(c.b);
      }
    }
    sendReply("OK");
    return true;
  }
  return false;
}

// Parses `LED <id> <idx|a-b> <r> <g> <b>` and writes static colours.
// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_IDX`.
bool parseLed(const String& line) {
  int id, r, g, b;
  char idxToken[16];
  if (sscanf(line.c_str(), "LED %d %15s %d %d %d", &id, idxToken, &r, &g, &b) == 5) {
    if (id < 0 || id >= GAUGE_COUNT) { sendReply("ERR BAD_ID"); return true; }
    char* dash = strchr(idxToken, '-');
    int idxFirst = atoi(idxToken);
    int idxLast  = dash ? atoi(dash + 1) : idxFirst;
    if (idxFirst < 0 || idxLast >= gaugePins[id].ledCount || idxFirst > idxLast) {
      sendReply("ERR BAD_IDX"); return true;
    }
    CRGB color(constrain(r, 0, 255), constrain(g, 0, 255), constrain(b, 0, 255));
    for (int i = idxFirst; i <= idxLast; i++) {
      blinkState[gaugeLedOffset[id] + i].active = false;
      leds[gaugeLedOffset[id] + i] = color;
    }
    ledsDirty = true;
    sendReply("OK");
    return true;
  }
  return false;
}

// Parses `BLINK ...` and assigns a simple on/off effect to one LED or a range.
// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_IDX`, `ERR BAD_TIME`.
bool parseBlink(const String& line) {
  int id, onMs, offMs, r, g, b;
  char idxToken[16];
  // Optional RGB values let BLINK either reuse or replace the current colour.
  int count = sscanf(line.c_str(), "BLINK %d %15s %d %d %d %d %d",
                     &id, idxToken, &onMs, &offMs, &r, &g, &b);
  if (count != 4 && count != 7) return false;

  if (id < 0 || id >= GAUGE_COUNT) { sendReply("ERR BAD_ID"); return true; }
  char* dash = strchr(idxToken, '-');
  int idxFirst = atoi(idxToken);
  int idxLast  = dash ? atoi(dash + 1) : idxFirst;
  if (idxFirst < 0 || idxLast >= gaugePins[id].ledCount || idxFirst > idxLast) {
    sendReply("ERR BAD_IDX"); return true;
  }

  if (onMs == 0 && offMs == 0) {
    for (int i = idxFirst; i <= idxLast; i++)
      blinkState[gaugeLedOffset[id] + i].active = false;
    sendReply("OK");
    return true;
  }
  if (onMs <= 0 || offMs <= 0) { sendReply("ERR BAD_TIME"); return true; }

  CRGB color = (count == 7)
    ? CRGB(constrain(r, 0, 255), constrain(g, 0, 255), constrain(b, 0, 255))
    : CRGB(0, 0, 0);  // Placeholder; replaced with the live LED colour below.

  unsigned long nowMs = millis();
  for (int i = idxFirst; i <= idxLast; i++) {
    uint8_t globalIdx = gaugeLedOffset[id] + i;
    BlinkState& bs = blinkState[globalIdx];
    bs.fx       = FX_BLINK;
    bs.onColor  = (count == 7) ? color : leds[globalIdx];
    bs.onMs     = (uint16_t)onMs;
    bs.offMs    = (uint16_t)offMs;
    bs.currentlyOn = true;
    bs.lastMs   = nowMs;
    bs.active   = true;
    leds[globalIdx] = bs.onColor;
  }
  ledsDirty = true;
  sendReply("OK");
  return true;
}

// Parses `BREATHE ...` and assigns a triangle-wave fade effect.
// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_IDX`, `ERR BAD_TIME`.
bool parseBreathe(const String& line) {
  int id, periodMs, r, g, b;
  char idxToken[16];
  if (sscanf(line.c_str(), "BREATHE %d %15s %d %d %d %d",
             &id, idxToken, &periodMs, &r, &g, &b) != 6) return false;
  if (id < 0 || id >= GAUGE_COUNT) { sendReply("ERR BAD_ID"); return true; }
  char* dash = strchr(idxToken, '-');
  int idxFirst = atoi(idxToken);
  int idxLast  = dash ? atoi(dash + 1) : idxFirst;
  if (idxFirst < 0 || idxLast >= gaugePins[id].ledCount || idxFirst > idxLast) {
    sendReply("ERR BAD_IDX"); return true;
  }
  if (periodMs <= 0) { sendReply("ERR BAD_TIME"); return true; }
  CRGB color(constrain(r, 0, 255), constrain(g, 0, 255), constrain(b, 0, 255));
  unsigned long nowMs = millis();
  for (int i = idxFirst; i <= idxLast; i++) {
    uint8_t gi = gaugeLedOffset[id] + i;
    BlinkState& bs = blinkState[gi];
    bs.fx       = FX_BREATHE;
    bs.onColor  = color;
    bs.periodMs = (uint16_t)constrain(periodMs, 100, 30000);
    bs.cyclePos = 0;
    bs.lastMs   = nowMs;
    bs.active   = true;
    leds[gi]    = CRGB::Black;
  }
  ledsDirty = true;
  sendReply("OK");
  return true;
}

// Parses `DFLASH ...` and assigns the double-flash pattern.
// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_IDX`.
bool parseDflash(const String& line) {
  int id, r, g, b;
  char idxToken[16];
  if (sscanf(line.c_str(), "DFLASH %d %15s %d %d %d",
             &id, idxToken, &r, &g, &b) != 5) return false;
  if (id < 0 || id >= GAUGE_COUNT) { sendReply("ERR BAD_ID"); return true; }
  char* dash = strchr(idxToken, '-');
  int idxFirst = atoi(idxToken);
  int idxLast  = dash ? atoi(dash + 1) : idxFirst;
  if (idxFirst < 0 || idxLast >= gaugePins[id].ledCount || idxFirst > idxLast) {
    sendReply("ERR BAD_IDX"); return true;
  }
  CRGB color(constrain(r, 0, 255), constrain(g, 0, 255), constrain(b, 0, 255));
  unsigned long nowMs = millis();
  for (int i = idxFirst; i <= idxLast; i++) {
    uint8_t gi = gaugeLedOffset[id] + i;
    BlinkState& bs = blinkState[gi];
    bs.fx      = FX_DFLASH;
    bs.onColor = color;
    bs.dphase  = 0;
    bs.lastMs  = nowMs;
    bs.active  = true;
    leds[gi]   = color;  // phase 0 = on
  }
  ledsDirty = true;
  sendReply("OK");
  return true;
}

// Advances all active LED effects and marks the strip dirty when something changed.
void updateBlink() {
  unsigned long nowMs = millis();
  bool changed = false;

  for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
    for (uint8_t j = 0; j < gaugePins[i].ledCount; j++) {
      uint8_t gi = gaugeLedOffset[i] + j;
      BlinkState& bs = blinkState[gi];
      if (!bs.active) continue;

      switch (bs.fx) {
        case FX_BLINK: {
          uint32_t period = bs.currentlyOn ? bs.onMs : bs.offMs;
          if ((nowMs - bs.lastMs) >= period) {
            bs.currentlyOn = !bs.currentlyOn;
            bs.lastMs = nowMs;
            leds[gi] = bs.currentlyOn ? bs.onColor : CRGB::Black;
            changed = true;
          }
          break;
        }
        case FX_BREATHE: {
          unsigned long dt = nowMs - bs.lastMs;
          if (dt < 64) break;
          uint32_t newPos = (uint32_t)bs.cyclePos + dt;
          bs.cyclePos = (uint16_t)(newPos % bs.periodMs);
          bs.lastMs = nowMs;
          // Cheap triangle wave. It does the job and nobody has complained yet.
          uint16_t half = bs.periodMs >> 1;
          uint8_t bri = (bs.cyclePos < half)
              ? (uint8_t)((uint32_t)bs.cyclePos * 255 / half)
              : (uint8_t)((uint32_t)(bs.periodMs - bs.cyclePos) * 255 / half);
          leds[gi] = bs.onColor;
          leds[gi].nscale8(bri ? bri : 1);
          changed = true;
          break;
        }
        case FX_DFLASH: {
          static const uint16_t dur[4] = {100, 100, 100, 700}; // on, off, on, longer off
          if ((nowMs - bs.lastMs) >= dur[bs.dphase]) {
            bs.lastMs = nowMs;
            bs.dphase = (bs.dphase + 1) & 3;
            leds[gi] = (bs.dphase == 0 || bs.dphase == 2) ? bs.onColor : CRGB::Black;
            changed = true;
          }
          break;
        }
      }
    }
  }

  if (changed) ledsDirty = true;
}

// Runs the command parsers in order until one claims the line.
// Reply: `ERR BAD_CMD` when no parser accepts the line.
void processLine(const String& line) {
  if (parseSet(line)) return;
  if (parseSpeed(line)) return;
  if (parseAccel(line)) return;
  if (parseEnable(line)) return;
  if (parseZero(line)) return;
  if (parseHome(line)) return;
  if (parseSweep(line)) return;
  if (parsePosQuery(line)) return;
  if (parseCfgQuery(line)) return;
  if (parseLedQuery(line)) return;
  if (parseLed(line)) return;
  if (parseBlink(line)) return;
  if (parseBreathe(line)) return;
  if (parseDflash(line)) return;
  if (parseVfd(line)) return;
  if (parsePing(line)) return;

  sendReply("ERR BAD_CMD");
}

// Reads newline-delimited commands from serial and hands complete lines to the parser.
// Reply: `ERR TOO_LONG` when the buffered line exceeds the receive limit before newline.
void readCommands() {
  while (CMD_PORT.available()) {
    char c = (char)CMD_PORT.read();

    if (c == '\n') {
      rxLine.trim();
      if (rxLine.length() > 0) {
        processLine(rxLine);
      }
      rxLine = "";
    } else if (c != '\r') {
      rxLine += c;
      if (rxLine.length() > 120) {
        rxLine = "";
        sendReply("ERR TOO_LONG");
      }
    }
  }
}

// Initialises pins, LED bookkeeping and the initial homing cycle.
// Reply/event: emits `READY` on CMD_PORT once boot is complete.
void setup() {
  DEBUG_PORT.begin(SERIAL_BAUD);
  DEBUG_PORT.println("Gauge controller booting");

  for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
    pinMode(gaugePins[i].dirPin, OUTPUT);
    pinMode(gaugePins[i].stepPin, OUTPUT);

    digitalWrite(gaugePins[i].dirPin, LOW);
    digitalWrite(gaugePins[i].stepPin, gaugePins[i].stepActiveHigh ? LOW : HIGH);

    if (gaugePins[i].enablePin >= 0) {
      pinMode(gaugePins[i].enablePin, OUTPUT);
      setEnable(i, true);
    }

    gauges[i].lastUpdateMicros = micros();
  }

  // Flatten the per-gauge LED counts into offsets on the shared strip.
  uint8_t ledOff = 0;
  for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
    gaugeLedOffset[i] = ledOff;
    ledOff += gaugePins[i].ledCount;
  }
  FastLED.addLeds<WS2812B, LED_DATA_PIN, GRB>(leds, TOTAL_LEDS);
  FastLED.setBrightness(255);
  FastLED.show();

  vfd::begin();

  requestHomeAll();

  DEBUG_PORT.println("READY");
  // Boot-complete handshake for the command channel.
  sendReply("READY");
}

// Main service loop: ingest commands, advance effects, move gauges, flush LEDs.
void loop() {
  readCommands();
  vfd::refresh();
  updateBlink();

  for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
    updateGauge(i);
  }
  
  if (ledsDirty) {
    FastLED.show();
    ledsDirty = false;
  }


}