#include <Arduino.h>
#include <avr/interrupt.h>
#include <math.h>
#include <stdlib.h>
#include <FastLED.h>

static const uint8_t GAUGE_COUNT = 4;

// Backlight/status LEDs use an addressable strip. Indicator LEDs are
// single-colour active-high outputs on per-gauge pins. The command protocol
// still exposes one logical LED segment per gauge.
static const uint8_t LED_DATA_PIN = 22;
static const uint8_t BREATHE_FRAME_MS = 16;
static const uint8_t LED_SHOW_MIN_INTERVAL_MS = 16;
static const uint8_t LED_SHOW_MOTION_INTERVAL_MS = 50;
static const uint8_t RX_LINE_MAX = 120;
static const uint16_t STEPPER_TIMER_HZ = 20000;
static const uint8_t STEPPER_TIMER_PRESCALER = 8;
static const uint32_t STEPPER_RATE_SCALE = 65536UL;
static const uint32_t LED_SHOW_FULL_RATE_LIMIT_Q16 = (500UL * STEPPER_RATE_SCALE) / STEPPER_TIMER_HZ;
static const uint32_t LED_SHOW_PAUSE_RATE_Q16 = (1500UL * STEPPER_RATE_SCALE) / STEPPER_TIMER_HZ;

// 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;

struct GaugePins {
  uint8_t dirPin;
  uint8_t stepPin;
  int8_t enablePin;          // -1 means there is no enable pin
  bool dirInverted;
  bool stepActiveHigh;
  bool enableActiveLow;
  const char* ledOrder;      // one char per LED: 'G' = GRB, 'R' = RGB; length defines ledCount
  int8_t indicatorRedPin;    // logical LED index 3; -1 means not fitted
  int8_t indicatorGreenPin;  // logical LED index 4; -1 means not fitted
};

constexpr GaugePins gaugePins[GAUGE_COUNT] = {
  // dir, step, en, dirInv, stepHigh, enActiveLow, ledOrder, indRed, indGreen
  {48, 49, -1, false, true, true, "RRRGGRR", 2, 3},   // Gauge 0
  {8,  9,  -1, true,  true, true, "GGGRRRR", 35, 36},   // Gauge 1
  {52, 53, -1, false, true, true, "GGGRRRR", 37, 38},   // Gauge 2
  {50, 51, -1, false, true, true, "GGGRRRR", 39, 40},   // Gauge 3
};

constexpr uint8_t cstrLen(const char* s) {
  return *s ? uint8_t(1 + cstrLen(s + 1)) : uint8_t(0);
}

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

constexpr bool isIndicatorLedIndex(uint8_t localIdx) {
  return localIdx == 3 || localIdx == 4;
}

constexpr uint8_t countIndicatorLedsForGauge(uint8_t gaugeIdx) {
  return (cstrLen(gaugePins[gaugeIdx].ledOrder) > 3 ? 1 : 0) +
         (cstrLen(gaugePins[gaugeIdx].ledOrder) > 4 ? 1 : 0);
}

constexpr uint8_t sumIndicatorLedCounts(uint8_t i = 0) {
  return i >= GAUGE_COUNT ? 0 : countIndicatorLedsForGauge(i) + sumIndicatorLedCounts(i + 1);
}
static const uint8_t TOTAL_INDICATOR_LEDS = sumIndicatorLedCounts();
static const uint8_t TOTAL_MAIN_LEDS = TOTAL_LEDS - TOTAL_INDICATOR_LEDS;

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;
  uint32_t maxSpeed = 4000;
  uint32_t accel = 6000;
  uint32_t homingSpeed = 500;

  unsigned long lastUpdateMicros = 0;

  bool enabled = true;
  bool homed = false;

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

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

struct StepperRuntime {
  volatile uint8_t* stepPort = nullptr;
  volatile uint8_t* dirPort = nullptr;
  uint8_t stepMask = 0;
  uint8_t dirMask = 0;
  bool stepActiveHigh = true;
  bool dirInverted = false;

  volatile long currentPos = 0;
  volatile long targetPos = 0;
  volatile long minPos = 0;
  volatile long maxPos = 3780;
  volatile uint32_t rateQ16 = 0;
  volatile uint32_t phaseQ16 = 0;
  volatile int8_t dir = 0;
  volatile bool enabled = true;
  volatile bool allowPastMin = false;
  volatile bool limitToTarget = true;
  volatile bool pulseActive = false;
  volatile bool dirKnown = false;
  volatile bool dirForward = 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];
StepperRuntime steppers[GAUGE_COUNT];
char rxLine[RX_LINE_MAX + 1];
uint8_t rxLen = 0;
bool rxOverflowed = false;

CRGB logicalLeds[TOTAL_LEDS];
CRGB mainLeds[TOTAL_MAIN_LEDS];
CLEDController* mainLedController = nullptr;
uint8_t gaugeLedOffset[GAUGE_COUNT];
uint8_t gaugeLedCount[GAUGE_COUNT];
uint8_t gaugeMainLedOffset[GAUGE_COUNT];
uint8_t ledPhysicalIdx[TOTAL_LEDS];
uint8_t ledGaugeIdx[TOTAL_LEDS];
uint8_t ledLocalIdx[TOTAL_LEDS];
bool ledIsIndicator[TOTAL_LEDS];
bool ledRgSwap[TOTAL_LEDS];
BlinkState blinkState[TOTAL_LEDS];
bool mainLedsDirty = false;
unsigned long lastLedShowMs = 0;

// FastLED drives the shared strip as RGB. Each gauge's ledOrder string marks per-LED
// type ('R' = RGB, 'G' = GRB); writes to GRB-ordered LEDs pre-swap R and G to compensate.
inline bool ledNeedsRgSwap(uint8_t globalIdx) {
  return ledRgSwap[globalIdx];
}

inline CRGB encodeForStrip(uint8_t globalIdx, CRGB color) {
  if (ledNeedsRgSwap(globalIdx)) {
    uint8_t tmp = color.r;
    color.r = color.g;
    color.g = tmp;
  }
  return color;
}

inline int8_t indicatorPinFor(uint8_t gaugeIdx, uint8_t localIdx) {
  if (localIdx == 3) return gaugePins[gaugeIdx].indicatorRedPin;
  if (localIdx == 4) return gaugePins[gaugeIdx].indicatorGreenPin;
  return -1;
}

inline bool indicatorIsOn(uint8_t localIdx, CRGB color) {
  if (localIdx == 3) return color.r >= 128;
  if (localIdx == 4) return color.g >= 128;
  return false;
}

inline void writeIndicatorLed(uint8_t globalIdx, CRGB color) {
  uint8_t gaugeIdx = ledGaugeIdx[globalIdx];
  uint8_t localIdx = ledLocalIdx[globalIdx];
  int8_t pin = indicatorPinFor(gaugeIdx, localIdx);
  if (pin >= 0) {
    digitalWrite(pin, indicatorIsOn(localIdx, color) ? HIGH : LOW);
  }
}

inline void writeLed(uint8_t globalIdx, CRGB color) {
  logicalLeds[globalIdx] = color;

  if (ledIsIndicator[globalIdx]) {
    writeIndicatorLed(globalIdx, color);
  } else {
    mainLeds[ledPhysicalIdx[globalIdx]] = encodeForStrip(globalIdx, color);
    mainLedsDirty = true;
  }
}

inline CRGB readLed(uint8_t globalIdx) {
  return logicalLeds[globalIdx];
}

uint32_t maxStepperRateQ16() {
  uint32_t maxRate = 0;
  uint8_t oldSreg = SREG;
  cli();
  for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
    uint32_t rate = steppers[i].rateQ16;
    if (steppers[i].enabled && steppers[i].dir != 0 && rate > maxRate) {
      maxRate = rate;
    }
  }
  SREG = oldSreg;
  return maxRate;
}

void showDirtyLeds() {
  if (!mainLedsDirty) return;

  uint32_t maxStepRate = maxStepperRateQ16();
  if (maxStepRate >= LED_SHOW_PAUSE_RATE_Q16) return;

  unsigned long nowMs = millis();
  uint8_t intervalMs = (maxStepRate > LED_SHOW_FULL_RATE_LIMIT_Q16)
      ? LED_SHOW_MOTION_INTERVAL_MS
      : LED_SHOW_MIN_INTERVAL_MS;
  if (nowMs - lastLedShowMs < intervalMs) return;

  if (mainLedsDirty && mainLedController != nullptr) {
    mainLedController->showLeds(255);
    mainLedsDirty = false;
  } else {
    return;
  }

  lastLedShowMs = nowMs;
}

// 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>
//   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.
//   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 char* s) {
  CMD_PORT.println(s);
}

void sendReply(const __FlashStringHelper* s) {
  CMD_PORT.println(s);
}

char* trimLine(char* s) {
  while (*s == ' ' || *s == '\t') s++;
  char* end = s + strlen(s);
  while (end > s && (end[-1] == ' ' || end[-1] == '\t')) {
    *--end = '\0';
  }
  return s;
}

inline void skipSpaces(const char*& s) {
  while (*s == ' ' || *s == '\t') s++;
}

bool commandArgs(const char* line, const char* command, const char*& args) {
  size_t len = strlen(command);
  if (strncmp(line, command, len) != 0) return false;
  if (line[len] != ' ' && line[len] != '\t') return false;

  args = line + len;
  skipSpaces(args);
  return true;
}

bool parseIntegerToken(const char*& s, long& value) {
  bool negative = false;
  bool sawDigit = false;
  long parsed = 0;

  if (*s == '+' || *s == '-') {
    negative = (*s == '-');
    s++;
  }

  while (*s >= '0' && *s <= '9') {
    sawDigit = true;
    parsed = parsed * 10 + (*s - '0');
    s++;
  }

  if (*s == '.' || *s == ',') {
    s++;
    while (*s >= '0' && *s <= '9') {
      s++;
    }
  }

  if (!sawDigit) return false;

  value = negative ? -parsed : parsed;
  skipSpaces(s);
  return true;
}

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

static inline void writeFastPin(volatile uint8_t* port, uint8_t mask, bool high) {
  if (high) {
    *port |= mask;
  } else {
    *port &= (uint8_t)~mask;
  }
}

static inline void writeStepperOutput(uint8_t id, bool active) {
  StepperRuntime& s = steppers[id];
  writeFastPin(s.stepPort, s.stepMask, active == s.stepActiveHigh);
}

static inline void writeStepperDir(uint8_t id, bool forward) {
  StepperRuntime& s = steppers[id];
  writeFastPin(s.dirPort, s.dirMask, forward != s.dirInverted);
  s.dirKnown = true;
  s.dirForward = forward;
}

uint32_t velocityToRateQ16(float velocity) {
  float speed = absf(velocity);
  if (speed < 0.5f) return 0;
  return (uint32_t)(speed * ((float)STEPPER_RATE_SCALE / (float)STEPPER_TIMER_HZ));
}

void initStepperRuntime(uint8_t id) {
  StepperRuntime& s = steppers[id];
  s.stepPort = portOutputRegister(digitalPinToPort(gaugePins[id].stepPin));
  s.dirPort = portOutputRegister(digitalPinToPort(gaugePins[id].dirPin));
  s.stepMask = digitalPinToBitMask(gaugePins[id].stepPin);
  s.dirMask = digitalPinToBitMask(gaugePins[id].dirPin);
  s.stepActiveHigh = gaugePins[id].stepActiveHigh;
  s.dirInverted = gaugePins[id].dirInverted;
  s.currentPos = gauges[id].currentPos;
  s.targetPos = gauges[id].targetPos;
  s.minPos = gauges[id].minPos;
  s.maxPos = gauges[id].maxPos;
  s.enabled = gauges[id].enabled;
  writeStepperOutput(id, false);
}

long readStepperCurrent(uint8_t id) {
  uint8_t oldSreg = SREG;
  cli();
  long pos = steppers[id].currentPos;
  SREG = oldSreg;
  return pos;
}

void setStepperCurrent(uint8_t id, long pos) {
  uint8_t oldSreg = SREG;
  cli();
  steppers[id].currentPos = pos;
  steppers[id].phaseQ16 = 0;
  SREG = oldSreg;
}

void setStepperTarget(uint8_t id, long target) {
  uint8_t oldSreg = SREG;
  cli();
  steppers[id].targetPos = target;
  SREG = oldSreg;
}

void setStepperEnabled(uint8_t id, bool enabled) {
  uint8_t oldSreg = SREG;
  cli();
  StepperRuntime& s = steppers[id];
  s.enabled = enabled;
  if (!enabled) {
    s.rateQ16 = 0;
    s.phaseQ16 = 0;
  }
  SREG = oldSreg;
}

void setStepperLimits(uint8_t id, long minPos, long maxPos) {
  uint8_t oldSreg = SREG;
  cli();
  steppers[id].minPos = minPos;
  steppers[id].maxPos = maxPos;
  SREG = oldSreg;
}

void setStepperCommand(uint8_t id, float velocity, bool allowPastMin, bool limitToTarget) {
  int8_t dir = (velocity > 0.5f) ? 1 : (velocity < -0.5f ? -1 : 0);
  uint32_t rateQ16 = dir == 0 ? 0 : velocityToRateQ16(velocity);

  uint8_t oldSreg = SREG;
  cli();
  StepperRuntime& s = steppers[id];
  if (s.dir != dir) s.phaseQ16 = 0;
  s.dir = dir;
  s.rateQ16 = rateQ16;
  s.allowPastMin = allowPastMin;
  s.limitToTarget = limitToTarget;
  if (rateQ16 == 0) s.phaseQ16 = 0;
  SREG = oldSreg;
}

void setupStepperTimer() {
  uint8_t oldSreg = SREG;
  cli();
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1 = 0;
  OCR1A = (uint16_t)((F_CPU / STEPPER_TIMER_PRESCALER / STEPPER_TIMER_HZ) - 1);
  TCCR1B |= _BV(WGM12);
  TCCR1B |= _BV(CS11); // prescaler 8 on ATmega2560
  TIMSK1 |= _BV(OCIE1A);
  SREG = oldSreg;
}

ISR(TIMER1_COMPA_vect) {
  for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
    StepperRuntime& s = steppers[i];
    bool pulseJustEnded = false;

    if (s.pulseActive) {
      writeStepperOutput(i, false);
      s.pulseActive = false;
      pulseJustEnded = true;
    }

    if (!s.enabled || s.rateQ16 == 0 || s.dir == 0) continue;

    bool forward = s.dir > 0;
    if (!s.dirKnown || s.dirForward != forward) {
      writeStepperDir(i, forward);
      continue;
    }

    s.phaseQ16 += s.rateQ16;
    if (s.phaseQ16 < STEPPER_RATE_SCALE) continue;
    if (pulseJustEnded) continue;
    s.phaseQ16 -= STEPPER_RATE_SCALE;

    if (forward) {
      if (s.currentPos >= s.maxPos || (s.limitToTarget && s.currentPos >= s.targetPos)) {
        s.rateQ16 = 0;
        s.phaseQ16 = 0;
        continue;
      }
      writeStepperOutput(i, true);
      s.currentPos++;
    } else {
      if ((!s.allowPastMin && s.currentPos <= s.minPos) ||
          (s.limitToTarget && s.currentPos <= s.targetPos)) {
        s.rateQ16 = 0;
        s.phaseQ16 = 0;
        continue;
      }
      writeStepperOutput(i, true);
      s.currentPos--;
    }

    s.pulseActive = true;
  }
}

// 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;
  setStepperEnabled(id, en);

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

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

// 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];
  setStepperCommand(id, 0.0f, false, true);
  g.homingState = HS_START;
  g.homed = false;
  g.velocity = 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, unsigned long nowUs, unsigned long nowMs) {
  Gauge& g = gauges[id];

  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.homingStepsRemaining = g.homingBackoffSteps;
      g.homingStartPos = readStepperCurrent(id);
      setStepperCommand(id, -(float)g.homingSpeed, true, false);
      g.homingState = HS_BACKING;
      break;

    case HS_BACKING: {
      g.currentPos = readStepperCurrent(id);
      long stepsDone = g.homingStartPos - g.currentPos;
      g.homingStepsRemaining = g.homingBackoffSteps - stepsDone;
      if (stepsDone >= g.homingBackoffSteps) {
        setStepperCommand(id, 0.0f, false, true);
        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;
        setStepperCurrent(id, 0);
        setStepperTarget(id, 0);
        g.homed = true;
        g.homingState = HS_DONE;

        DEBUG_PORT.print(F("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, unsigned long nowUs, unsigned long nowMs) {
  Gauge& g = gauges[id];

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

  g.currentPos = readStepperCurrent(id);

  if (!g.homed) {
    setStepperCommand(id, 0.0f, false, true);
    return;
  }

  if (g.sweepEnabled) {
    updateSweepTarget(id);
  }
  setStepperTarget(id, g.targetPos);

  if (g.lastUpdateMicros == 0) {
    g.lastUpdateMicros = nowUs;
    return;
  }

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

  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;
    setStepperCommand(id, 0.0f, false, true);
    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 accel = (float)g.accel;
  float maxSpeed = (float)g.maxSpeed;
  float brakingDistance = (g.velocity * g.velocity) / (2.0f * accel + 0.0001f);

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

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

  setStepperCommand(id, g.velocity, false, true);
}

// Parses `SET <id> <pos>` and updates the target position.
// Replies: `OK`, `ERR BAD_ID`.
bool parseSet(const char* line) {
  const char* args = nullptr;
  long id;
  long pos;
  if (!commandArgs(line, "SET", args)) return false;
  if (!parseIntegerToken(args, id) || !parseIntegerToken(args, pos)) return false;

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

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

// Parses `SPEED <id> <speed>` and updates the max step rate.
// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_SPEED`.
bool parseSpeed(const char* line) {
  const char* args = nullptr;
  long id;
  long speed;
  if (!commandArgs(line, "SPEED", args)) return false;
  if (!parseIntegerToken(args, id) || !parseIntegerToken(args, speed)) return false;

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

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

// Parses `ACCEL <id> <accel>` and updates the acceleration limit.
// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_ACCEL`.
bool parseAccel(const char* line) {
  const char* args = nullptr;
  long id;
  long accel;
  if (!commandArgs(line, "ACCEL", args)) return false;
  if (!parseIntegerToken(args, id) || !parseIntegerToken(args, accel)) return false;

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

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

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

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

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

    Gauge& g = gauges[id];
    g.currentPos = 0;
    g.targetPos = 0;
    g.velocity = 0.0f;
    setStepperCommand((uint8_t)id, 0.0f, false, true);
    setStepperCurrent((uint8_t)id, 0);
    setStepperTarget((uint8_t)id, 0);
    g.homed = true;
    g.sweepEnabled = false;
    sendReply(F("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 char* line) {
  int id;
  if (sscanf(line, "HOME %d", &id) == 1) {
    if (id < 0 || id >= GAUGE_COUNT) {
      sendReply(F("ERR BAD_ID"));
      return true;
    }

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

  if (strcmp(line, "HOMEALL") == 0) {
    requestHomeAll();
    sendReply(F("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 char* line) {
  const char* args = nullptr;
  long id;
  long accel;
  long speed;
  if (!commandArgs(line, "SWEEP", args)) return false;
  if (!parseIntegerToken(args, id) ||
      !parseIntegerToken(args, accel) ||
      !parseIntegerToken(args, speed)) {
    return false;
  }

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

  Gauge& g = gauges[(uint8_t)id];

  if (accel <= 0 || speed <= 0) {
    g.sweepEnabled = false;
    g.velocity = 0.0f;
    setStepperCommand((uint8_t)id, 0.0f, false, true);
    sendReply(F("OK"));
    return true;
  }

  g.accel = (uint32_t)accel;
  g.maxSpeed = (uint32_t)speed;
  g.sweepEnabled = true;
  g.sweepTowardMax = true;
  g.targetPos = g.maxPos;
  setStepperTarget((uint8_t)id, g.targetPos);
  sendReply(F("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 char* line) {
  if (strcmp(line, "POS?") == 0) {
    for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
      gauges[i].currentPos = readStepperCurrent(i);
      CMD_PORT.print(F("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(F("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 char* line) {
  if (strcmp(line, "CFG?") == 0) {
    for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
      CMD_PORT.print(F("CFG "));
      CMD_PORT.print(i);
      CMD_PORT.print(' ');
      CMD_PORT.print(gauges[i].maxSpeed);
      CMD_PORT.print(' ');
      CMD_PORT.println(gauges[i].accel);
    }
    sendReply(F("OK"));
    return true;
  }
  return false;
}

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

// 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 char* line) {
  if (strcmp(line, "LED?") == 0) {
    for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
      for (uint8_t j = 0; j < gaugeLedCount[i]; j++) {
        CRGB c = readLed(gaugeLedOffset[i] + j);
        CMD_PORT.print(F("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(F("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 char* line) {
  int id, r, g, b;
  char idxToken[16];
  if (sscanf(line, "LED %d %15s %d %d %d", &id, idxToken, &r, &g, &b) == 5) {
    if (id < 0 || id >= GAUGE_COUNT) { sendReply(F("ERR BAD_ID")); return true; }
    char* dash = strchr(idxToken, '-');
    int idxFirst = atoi(idxToken);
    int idxLast  = dash ? atoi(dash + 1) : idxFirst;
    if (idxFirst < 0 || idxLast >= gaugeLedCount[id] || idxFirst > idxLast) {
      sendReply(F("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;
      writeLed(gaugeLedOffset[id] + i, color);
    }
    sendReply(F("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 char* 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, "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(F("ERR BAD_ID")); return true; }
  char* dash = strchr(idxToken, '-');
  int idxFirst = atoi(idxToken);
  int idxLast  = dash ? atoi(dash + 1) : idxFirst;
  if (idxFirst < 0 || idxLast >= gaugeLedCount[id] || idxFirst > idxLast) {
    sendReply(F("ERR BAD_IDX")); return true;
  }

  if (onMs == 0 && offMs == 0) {
    for (int i = idxFirst; i <= idxLast; i++)
      blinkState[gaugeLedOffset[id] + i].active = false;
    sendReply(F("OK"));
    return true;
  }
  if (onMs <= 0 || offMs <= 0) { sendReply(F("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 : readLed(globalIdx);
    bs.onMs     = (uint16_t)onMs;
    bs.offMs    = (uint16_t)offMs;
    bs.currentlyOn = true;
    bs.lastMs   = nowMs;
    bs.active   = true;
    writeLed(globalIdx, bs.onColor);
  }
  sendReply(F("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 char* line) {
  int id, periodMs, r, g, b;
  char idxToken[16];
  if (sscanf(line, "BREATHE %d %15s %d %d %d %d",
             &id, idxToken, &periodMs, &r, &g, &b) != 6) return false;
  if (id < 0 || id >= GAUGE_COUNT) { sendReply(F("ERR BAD_ID")); return true; }
  char* dash = strchr(idxToken, '-');
  int idxFirst = atoi(idxToken);
  int idxLast  = dash ? atoi(dash + 1) : idxFirst;
  if (idxFirst < 0 || idxLast >= gaugeLedCount[id] || idxFirst > idxLast) {
    sendReply(F("ERR BAD_IDX")); return true;
  }
  if (periodMs <= 0) { sendReply(F("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;
    writeLed(gi, CRGB::Black);
  }
  sendReply(F("OK"));
  return true;
}

// Parses `DFLASH ...` and assigns the double-flash pattern.
// Replies: `OK`, `ERR BAD_ID`, `ERR BAD_IDX`.
bool parseDflash(const char* line) {
  int id, r, g, b;
  char idxToken[16];
  if (sscanf(line, "DFLASH %d %15s %d %d %d",
             &id, idxToken, &r, &g, &b) != 5) return false;
  if (id < 0 || id >= GAUGE_COUNT) { sendReply(F("ERR BAD_ID")); return true; }
  char* dash = strchr(idxToken, '-');
  int idxFirst = atoi(idxToken);
  int idxLast  = dash ? atoi(dash + 1) : idxFirst;
  if (idxFirst < 0 || idxLast >= gaugeLedCount[id] || idxFirst > idxLast) {
    sendReply(F("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;
    writeLed(gi, color);  // phase 0 = on
  }
  sendReply(F("OK"));
  return true;
}

// Advances all active LED effects. writeLed() marks the affected physical strip dirty.
void updateBlink() {
  unsigned long nowMs = millis();

  for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
    for (uint8_t j = 0; j < gaugeLedCount[i]; 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;
            writeLed(gi, bs.currentlyOn ? bs.onColor : CRGB::Black);
          }
          break;
        }
        case FX_BREATHE: {
          unsigned long dt = nowMs - bs.lastMs;
          if (dt < BREATHE_FRAME_MS) break;
          uint32_t newPos = (uint32_t)bs.cyclePos + dt;
          bs.cyclePos = (uint16_t)(newPos % bs.periodMs);
          bs.lastMs = nowMs;
          // Triangle wave brightness; frame-limited so breathe remains smooth
          // without refreshing the LED strips on every service-loop pass.
          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);
          CRGB scaled = bs.onColor;
          scaled.nscale8(bri ? bri : 1);
          writeLed(gi, scaled);
          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;
            writeLed(gi, (bs.dphase == 0 || bs.dphase == 2) ? bs.onColor : CRGB::Black);
          }
          break;
        }
      }
    }
  }
}

// Runs the command parsers in order until one claims the line.
// Reply: `ERR BAD_CMD` when no parser accepts the line.
void processLine(const char* 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 (parsePing(line)) return;

  sendReply(F("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') {
      if (rxOverflowed) {
        rxOverflowed = false;
        rxLen = 0;
        continue;
      }

      rxLine[rxLen] = '\0';
      char* line = trimLine(rxLine);
      if (*line != '\0') {
        processLine(line);
      }
      rxLen = 0;
    } else if (c != '\r' && !rxOverflowed) {
      if (rxLen < RX_LINE_MAX) {
        rxLine[rxLen++] = c;
      } else {
        rxLen = 0;
        rxOverflowed = true;
        sendReply(F("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(F("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);
    }
    if (gaugePins[i].indicatorRedPin >= 0) {
      pinMode(gaugePins[i].indicatorRedPin, OUTPUT);
      digitalWrite(gaugePins[i].indicatorRedPin, LOW);
    }
    if (gaugePins[i].indicatorGreenPin >= 0) {
      pinMode(gaugePins[i].indicatorGreenPin, OUTPUT);
      digitalWrite(gaugePins[i].indicatorGreenPin, LOW);
    }

    initStepperRuntime(i);
    setStepperLimits(i, gauges[i].minPos, gauges[i].maxPos);
    gauges[i].lastUpdateMicros = micros();
  }

  // Flatten the per-gauge LED counts into logical offsets and physical
  // offsets for the addressable main strip.
  uint8_t ledOff = 0;
  uint8_t mainLedOff = 0;
  for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
    gaugeLedCount[i] = cstrLen(gaugePins[i].ledOrder);
    gaugeLedOffset[i] = ledOff;
    gaugeMainLedOffset[i] = mainLedOff;

    for (uint8_t localIdx = 0; localIdx < gaugeLedCount[i]; localIdx++) {
      uint8_t globalIdx = ledOff + localIdx;
      bool indicator = isIndicatorLedIndex(localIdx);
      ledGaugeIdx[globalIdx] = i;
      ledLocalIdx[globalIdx] = localIdx;
      ledIsIndicator[globalIdx] = indicator;
      ledRgSwap[globalIdx] = gaugePins[i].ledOrder[localIdx] == 'G' ||
                             gaugePins[i].ledOrder[localIdx] == 'g';
      ledPhysicalIdx[globalIdx] = indicator
          ? 0
          : mainLedOff + localIdx - (localIdx > 4 ? 2 : 0);
    }

    ledOff += gaugeLedCount[i];
    mainLedOff += gaugeLedCount[i] - countIndicatorLedsForGauge(i);
  }
  mainLedController = &FastLED.addLeds<WS2812, LED_DATA_PIN, RGB>(mainLeds, TOTAL_MAIN_LEDS);
  FastLED.setBrightness(255);
  mainLedController->showLeds(255);

  setupStepperTimer();
  requestHomeAll();

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

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

  unsigned long nowUs = micros();
  unsigned long nowMs = millis();
  for (uint8_t i = 0; i < GAUGE_COUNT; i++) {
    updateGauge(i, nowUs, nowMs);
  }
  
  showDirtyLeds();


}