ATVLED/QuadLightsV3.ino

#include <Button.h>
#include <Adafruit_NeoPixel.h>
#include <Wire.h>

// without USB CDC, Serial falls back to USART1 on PA9/PA10, hijacking the mode and right buttons
#ifndef USBD_USE_CDC
#error "Set Tools > USB support to CDC (generic 'Serial' supersede U(S)ART)"
#endif

constexpr bool KEEPALIVE_ENABLE = true;
constexpr int KEEPALIVE_TIMEOUT = 45000; // ms

constexpr int LEDS_TAIL_PIN = PA6;
constexpr int LEDS_TAIL_LENGTH = 10;
constexpr int LEDS_TAIL_BRIGHTNESS = 255;
constexpr int LEDS_TAIL_SIDE_LENGTH = 2;
constexpr int LEDS_TAIL_SIDE_MARGIN = 2;

constexpr int LEDS_LEFT_PIN = PA5;
constexpr int LEDS_RIGHT_PIN = PA7;

/* default configuration right lamp (left from the front)
RING						          EDGE
31 32 33 34 35 36 37 38		0  1  2  3  4  5  6  7  8  9  10 11
30 29 28 27 26 25 24 23		   22 21 20 19 18 17 16 15 14 13 12
*/
constexpr int LEDS_TOP_LENGTH = 12;
constexpr int LEDS_BOTTOM_LENGTH = 11;
constexpr int LEDS_RING_LENGTH = 16;
constexpr int LEDS_EDGE_LENGTH = LEDS_TOP_LENGTH + LEDS_BOTTOM_LENGTH;
constexpr int LEDS_HEAD_LENGTH = LEDS_TOP_LENGTH + LEDS_BOTTOM_LENGTH + LEDS_RING_LENGTH;
constexpr int LEDS_HEAD_BRIGHTNESS = 200;

constexpr int LEDS_DASH_PIN = PB0;
constexpr int LEDS_DASH_LENGTH = 2;
constexpr int LEDS_DASH_BRIGHTNESS = 255;

constexpr int PIN_ALIVE = PA2;
constexpr int PIN_BUCK = PA1;

constexpr int PIN_PEDAL = PA0;
constexpr int PIN_PEDAL_FORWARD = PA3;
constexpr int PIN_PEDAL_REVERSE = PA4;

constexpr int PIN_BUTTON_MODE = PA9;
constexpr int PIN_BUTTON_LEFT = PA8;
constexpr int PIN_BUTTON_RIGHT = PA10;

constexpr int PIN_IMU_SCL = PB6;
constexpr int PIN_IMU_SDA = PB7;
constexpr unsigned long IMU_RETRY_INTERVAL = 250; // ms, after a supply dip the IMU is usually back quickly
constexpr unsigned long IMU_RESET_TIME = 110; // ms, the IMU needs ~100ms after a reset before it takes configuration

// MPU9250/9255, configured like CrashRecorder but only buffering the accelerometer
constexpr uint8_t IMU_ADDRESSES[] = {0x68, 0x69}; // AD0 low, AD0 high
constexpr uint8_t IMU_REG_SMPLRT_DIV = 0x19;
constexpr uint8_t IMU_REG_CONFIG = 0x1A;
constexpr uint8_t IMU_REG_ACCEL_CONFIG = 0x1C;
constexpr uint8_t IMU_REG_ACCEL_CONFIG2 = 0x1D;
constexpr uint8_t IMU_REG_FIFO_EN = 0x23;
constexpr uint8_t IMU_REG_INT_ENABLE = 0x38;
constexpr uint8_t IMU_REG_INT_STATUS = 0x3A;
constexpr uint8_t IMU_REG_ACCEL_XOUT_H = 0x3B;
constexpr uint8_t IMU_REG_USER_CTRL = 0x6A;
constexpr uint8_t IMU_REG_PWR_MGMT_1 = 0x6B;
constexpr uint8_t IMU_REG_FIFO_COUNT_H = 0x72;
constexpr uint8_t IMU_REG_FIFO_R_W = 0x74;
constexpr uint8_t IMU_REG_WHO_AM_I = 0x75;

constexpr uint8_t IMU_FIFO_EN_ACCEL = 0x08;
constexpr uint8_t IMU_USER_CTRL_FIFO_EN = 0x40;
constexpr uint8_t IMU_USER_CTRL_FIFO_RST = 0x04;
constexpr uint8_t IMU_INT_FIFO_OFLOW = 0x10;
constexpr uint8_t IMU_DLPF_184HZ = 0x01;
constexpr uint8_t IMU_ACCEL_16G = 0x18;
constexpr float ACCEL_LSB_PER_G = 2048;

constexpr int IMU_SAMPLE_RATE = 500; // Hz
constexpr int FIFO_SIZE = 512;
constexpr int FIFO_SAMPLE_SIZE = 6; // accel xyz, big-endian
constexpr int FIFO_READ_BATCH = 5; // samples per I2C read, stays within the 32 byte Wire buffer
constexpr unsigned long FIFO_INTERVAL = 10; // ms, the fifo holds ~170ms at 500Hz
constexpr unsigned long IMU_HEALTH_INTERVAL = 1000; // ms
constexpr float IMU_MISALIGNED_DIFFERENCE = 0.5; // g between the fifo stream and a direct read, a shifted stream is ~1.5g off
constexpr int IMU_MISALIGNED_CHECKS = 2; // in a row, so a single check during a crash doesn't count

/* crash detection, from the CrashRecorder recordings of 2026-10-21 (recordings/, tools/crash_dump.py)

speed lost against the direction of travel within 100ms:
  normal stops <= 0.19 m/s, handling by hand <= 0.25 m/s (parked, so not armed)
  wall hits 0.58 - 1.07 m/s, driving off a threshold into the doorframe 0.51 m/s

the sensor is mounted on its side with +z pointing backward, its offset is removed by the gravity baseline
*/
constexpr float CRASH_SPEED_LOSS = 0.6; // m/s, only proper hits, 0.35-0.45 also triggered on grazes and lighter bumps
constexpr float CRASH_LOG_SPEED_LOSS = 0.2; // m/s, anything above is logged to help tune the threshold
constexpr int CRASH_WINDOW = 50; // samples, 100ms
constexpr unsigned long CRASH_ARMED_AFTER_RELEASE = 1500; // ms, impacts happen with the pedal still down or just released
constexpr unsigned long CRASH_HAZARD_DURATION = 5000; // ms
constexpr float GRAVITY_BASELINE_TIME = 2.0; // s, follows gravity leaking into the forward axis on slopes
constexpr float STANDARD_GRAVITY = 9.81; // m/s2

// per-sensor calibration from the tilt test of 2026-10-21, keep in sync with tools/crash_dump.py
constexpr float ACCEL_OFFSET_X = 0.490; // g, sensor axes
constexpr float ACCEL_OFFSET_Y = -0.277;
constexpr float ACCEL_OFFSET_Z = -0.004;
constexpr float FLAT_FORWARD = -0.060; // g, gravity in the vehicle frame when standing flat
constexpr float FLAT_LEFT = -0.019;
constexpr float FLAT_UP = 0.996;

constexpr unsigned long STARTUP_SWEEP = 500; // ms, the headlight edges sweep on before the normal lights
constexpr unsigned long STARTUP_RING_DELAY = 200; // ms from the start, STARTUP_SWEEP waits for the sweep to complete
constexpr unsigned long STARTUP_RING_FADE = 300; // ms
constexpr unsigned long STARTUP_DURATION = STARTUP_RING_DELAY + STARTUP_RING_FADE > STARTUP_SWEEP
  ? STARTUP_RING_DELAY + STARTUP_RING_FADE
  : STARTUP_SWEEP;
// the top and bottom edge rows start side by side in the same corner, so the first LEDs light up as a bright pair
// they start dimmer, the first at STARTUP_CORNER_LEVEL rising to full over STARTUP_CORNER_LEDS, all full when the sweep completes
constexpr int STARTUP_CORNER_LEDS = 4;
constexpr float STARTUP_CORNER_LEVEL = 0;

// scrambled lights while tilted, an easter egg
constexpr float SCRAMBLE_TILT = 20; // degrees from flat to start
constexpr float SCRAMBLE_TILT_END = 8; // degrees, lower so it doesn't flicker around the threshold
constexpr unsigned long SCRAMBLE_HOLD = 300; // ms above SCRAMBLE_TILT before it starts
constexpr float TILT_FILTER_TIME = 1.0; // s, the motor pulling the quad forward reads like tilt but doesn't last
constexpr int SCRAMBLE_FRAMES = 4; // between changes, at full frame rate it would blur into white
constexpr int SCRAMBLE_CHANGE_PERCENT = 35; // of the pixels changed each time

// the dash shows what crash detection sees, for testing without serial, replacing its normal use
// first pixel white: debug mode is on
// second pixel, by priority: red: IMU problem (read failed, overflow, reconfigured, not found), yellow flash: near miss,
// blue: armed and travelling backward (only expected in reverse), green: armed, off: not armed
constexpr bool DEBUG_DASH = false;

// temporary, for testing by pushing the quad into a wall without the motor:
// detects in both directions without the pedal, and disables the keepalive timeout (hold mode to power off)
constexpr bool DEBUG_CRASH_WITHOUT_PEDAL = false;

// temporary, logs a status line every IMU_HEALTH_INTERVAL to find crash detection dropouts from the log alone
constexpr bool DEBUG_IMU_LOG = false;
constexpr unsigned long DEBUG_NEAR_MISS_DURATION = 500; // ms
constexpr unsigned long DEBUG_IMU_PROBLEM_DURATION = 3000; // ms

constexpr int LED_ON = LOW;
constexpr int LED_OFF = HIGH;

constexpr int PEDAL_ON = HIGH;
constexpr int PEDAL_OFF = LOW;

constexpr int BUCK_ON = HIGH;
constexpr int BUCK_OFF = LOW;

constexpr int ANIMATION_FPS = 30;
constexpr int ANIMATION_INTERVAL = lround(1000 / ANIMATION_FPS);

Button pedal(PIN_PEDAL);
Button pedalForward(PIN_PEDAL_FORWARD);
Button pedalReverse(PIN_PEDAL_REVERSE);

Button buttonMode(PIN_BUTTON_MODE);
Button buttonLeft(PIN_BUTTON_LEFT);
Button buttonRight(PIN_BUTTON_RIGHT);

Adafruit_NeoPixel ledsTail(LEDS_TAIL_LENGTH, LEDS_TAIL_PIN, NEO_GRBW + NEO_KHZ800);
Adafruit_NeoPixel ledsHeadLeft(LEDS_HEAD_LENGTH, LEDS_LEFT_PIN, NEO_GRBW + NEO_KHZ800);
Adafruit_NeoPixel ledsHeadRight(LEDS_HEAD_LENGTH, LEDS_RIGHT_PIN, NEO_GRBW + NEO_KHZ800);
Adafruit_NeoPixel ledsDash(LEDS_DASH_LENGTH, LEDS_DASH_PIN, NEO_GRBW + NEO_KHZ800);

enum Direction { LEFT, RIGHT };

enum Mode {
  MODE_HEAD,
  MODE_POLICE,
  MODE_RAINBOW,
  MODE_KITT,
  MODE_HAZARD,
  MODE_COUNT,
};

Mode activeMode = MODE_HEAD;

// polling stops as soon as the IMU fails, every I2C call to a missing IMU blocks the loop for up to 100ms
enum ImuState { IMU_MISSING, IMU_STARTING, IMU_RUNNING };
// Mode activeMode = MODE_KITT;

uint32_t colorHSV(float hue, float sat = 1.0, float val = 1.0) {
  return Adafruit_NeoPixel::ColorHSV(
    (uint16_t)(hue / 360.0 * 65535),
    (uint8_t)(sat * 255),
    (uint8_t)(val * 255)
  );
}

const uint32_t COLOR_OFF = ledsTail.Color(0, 0, 0, 0);
const uint32_t COLOR_WHITE = ledsTail.Color(0, 0, 0, 255);
const uint32_t COLOR_WHITE_DIM = ledsTail.Color(0, 0, 0, 100);
const uint32_t COLOR_RED = ledsTail.Color(255, 0, 0, 0);
const uint32_t COLOR_AMBER = colorHSV(10); // easier value fade
const uint32_t COLOR_POLICE_BLUE = ledsTail.Color(0, 0, 255, 0);
const uint32_t COLOR_POLICE_RED = ledsTail.Color(255, 0, 0, 0);
const uint32_t COLOR_GREEN = ledsTail.Color(0, 255, 0, 0);
const uint32_t COLOR_YELLOW = ledsTail.Color(255, 150, 0, 0);

unsigned long animationFrame = 0;
unsigned long stableAnimationFrame = 0;
unsigned long lastFrame = 0;

unsigned long lastPedalPress = millis();
unsigned long lastPedalRelease = 0;

unsigned long lastButtonPress = 0;

bool isReversing = false;

unsigned long indicatorLeftStart = 0;
unsigned long indicatorRightStart = 0;
bool indicatorLeftActive = false;
bool indicatorRightActive = false;

ImuState imuState = IMU_MISSING;
unsigned long imuStateSince = 0;
bool imuMissingReported = false; // only the first failure of an outage is logged
uint8_t imuAddress = 0;
uint8_t imuWhoAmI = 0;
unsigned long lastFifoRead = 0;
unsigned long lastImuHealth = 0;
unsigned long imuSamples = 0; // since the last health check
unsigned long lastImuProblem = 0; // 0 when none yet
unsigned long lastNearMiss = 0;
int16_t lastFifoSample[3] = {}; // raw accel xyz
int imuMisalignedChecks = 0;
float debugSpeedChangeMin = 0; // since the last health check
float debugSpeedChangeMax = 0;

unsigned long startupStart = 0;
bool shuttingDown = false;
unsigned long shutdownStart = 0;

bool travellingBackward = false; // last direction while the pedal was down
bool crashHazardActive = false;
unsigned long crashHazardStart = 0;

float gravityBaseline = 0;
bool gravityBaselineReady = false;

float tiltForward = 0; // g, filtered gravity in the vehicle frame
float tiltLeft = 0;
float tiltUp = 0;
bool tiltReady = false;

bool scrambleActive = false;
bool scrambleTilted = false; // above SCRAMBLE_TILT, waiting for SCRAMBLE_HOLD
unsigned long scrambleTiltedSince = 0;
uint32_t scrambleHeadLeft[LEDS_HEAD_LENGTH];
uint32_t scrambleHeadRight[LEDS_HEAD_LENGTH];
uint32_t scrambleTail[LEDS_TAIL_LENGTH];
uint32_t scrambleDash[LEDS_DASH_LENGTH];
float crashWindow[CRASH_WINDOW] = {};
float crashWindowSum = 0;
int crashWindowIndex = 0;
float crashEventPeak = 0; // largest speed loss of the current event above CRASH_LOG_SPEED_LOSS
bool crashEventTriggered = false;

void writeRegister(uint8_t reg, uint8_t value) {
  Wire.beginTransmission(imuAddress);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

bool readRegisters(uint8_t address, uint8_t reg, uint8_t* buffer, uint8_t length) {
  Wire.beginTransmission(address);
  Wire.write(reg);

  if (Wire.endTransmission(false) != 0 || Wire.requestFrom(address, length) != length) {
    return false;
  }

  for (int i = 0; i < length; i++) {
    buffer[i] = Wire.read();
  }

  return true;
}

uint8_t readRegister(uint8_t reg) {
  uint8_t value = 0;
  readRegisters(imuAddress, reg, &value, 1);

  return value;
}

void reportImuProblem(const char* message) {
  Serial.println(message);
  lastImuProblem = max(millis(), 1ul);
}

void startFifo() {
  writeRegister(IMU_REG_FIFO_EN, 0);
  writeRegister(IMU_REG_USER_CTRL, IMU_USER_CTRL_FIFO_RST);
  readRegister(IMU_REG_INT_STATUS); // clears a stale overflow flag
  writeRegister(IMU_REG_USER_CTRL, IMU_USER_CTRL_FIFO_EN);
  writeRegister(IMU_REG_FIFO_EN, IMU_FIFO_EN_ACCEL);
}

void beginWire() {
  // setSCL/SDA have to be called before begin()
  Wire.setSCL(PIN_IMU_SCL);
  Wire.setSDA(PIN_IMU_SDA);
  Wire.begin();
  Wire.setClock(400000);
}

// the IMU can reset halfway through a transfer, likely browning out when the motor stalls against a wall, and keep
// holding SDA low, which blocks the bus until power is cut. clocking SCL until it lets go and sending a STOP frees it,
// restarting Wire also clears the I2C peripheral if it got stuck busy
void recoverWire() {
  constexpr int PULSE = 5; // us, ~100kHz
  constexpr int MAX_CLOCKS = 16;

  Wire.end();

  pinMode(PIN_IMU_SDA, INPUT_PULLUP);
  pinMode(PIN_IMU_SCL, OUTPUT_OPEN_DRAIN);
  digitalWrite(PIN_IMU_SCL, HIGH);
  delayMicroseconds(PULSE);

  for (int clock = 0; clock < MAX_CLOCKS && digitalRead(PIN_IMU_SDA) == LOW; clock++) {
    digitalWrite(PIN_IMU_SCL, LOW);
    delayMicroseconds(PULSE);
    digitalWrite(PIN_IMU_SCL, HIGH);
    delayMicroseconds(PULSE);
  }

  // STOP: SDA rising while SCL is high
  pinMode(PIN_IMU_SDA, OUTPUT_OPEN_DRAIN);
  digitalWrite(PIN_IMU_SDA, LOW);
  delayMicroseconds(PULSE);
  digitalWrite(PIN_IMU_SDA, HIGH);
  delayMicroseconds(PULSE);

  beginWire();
}

void setImuState(ImuState state, unsigned long now) {
  imuState = state;
  imuStateSince = now;
}

// stops polling until the next reconnect, retrying a failing IMU would only block the loop on I2C timeouts
void imuFailed(const char* message, unsigned long now) {
  if (!imuMissingReported) {
    reportImuProblem(message);
    imuMissingReported = true;
  }

  setImuState(IMU_MISSING, now);
}

// first half of connecting, configureImu() finishes after IMU_RESET_TIME without blocking the loop
void connectImu(unsigned long now, bool recover = true) {
  if (recover) {
    recoverWire();
  }

  imuAddress = 0;

  for (const uint8_t address : IMU_ADDRESSES) {
    if (readRegisters(address, IMU_REG_WHO_AM_I, &imuWhoAmI, 1)) {
      imuAddress = address;
      break;
    }
  }

  if (imuAddress == 0) {
    imuFailed("IMU NOT FOUND ON PB6/PB7, CRASH DETECTION DISABLED UNTIL IT IS BACK", now);
    return;
  }

  writeRegister(IMU_REG_PWR_MGMT_1, 0x80); // reset
  setImuState(IMU_STARTING, now);
}

void configureImu(unsigned long now) {
  writeRegister(IMU_REG_PWR_MGMT_1, 0x01); // gyro PLL clock
  writeRegister(IMU_REG_CONFIG, IMU_DLPF_184HZ);
  writeRegister(IMU_REG_SMPLRT_DIV, 1000 / IMU_SAMPLE_RATE - 1);
  writeRegister(IMU_REG_ACCEL_CONFIG, IMU_ACCEL_16G);
  writeRegister(IMU_REG_ACCEL_CONFIG2, IMU_DLPF_184HZ);
  writeRegister(IMU_REG_INT_ENABLE, IMU_INT_FIFO_OFLOW);

  startFifo();
  resetCrashMeasure();

  imuSamples = 0;
  lastImuHealth = now;
  imuMissingReported = false;
  setImuState(IMU_RUNNING, now);

  Serial.print("IMU FOUND AT 0x");
  Serial.print(imuAddress, HEX);
  Serial.print(", WHO_AM_I 0x");
  Serial.println(imuWhoAmI, HEX);
}

// the baseline and window follow whatever the stream delivered, start over after the stream was wrong
void resetCrashMeasure() {
  gravityBaselineReady = false;
  tiltReady = false;
  crashWindowSum = 0;
  crashWindowIndex = 0;

  for (float& value : crashWindow) {
    value = 0;
  }
}

// recovers from the IMU resetting itself, e.g. after a supply dip, which would otherwise disable crash detection silently
void checkImuHealth(unsigned long now) {
  uint8_t accelConfig = 0;
  uint8_t fifoEnabled = 0;

  if (!readRegisters(imuAddress, IMU_REG_ACCEL_CONFIG, &accelConfig, 1) || !readRegisters(imuAddress, IMU_REG_FIFO_EN, &fifoEnabled, 1)) {
    imuFailed("IMU NOT RESPONDING, RECONNECTING", now);
    return;
  }

  const unsigned long samples = imuSamples;

  if (accelConfig != IMU_ACCEL_16G || fifoEnabled != IMU_FIFO_EN_ACCEL) {
    imuFailed("IMU LOST ITS CONFIGURATION, RECONNECTING", now);
    return;
  }

  if (imuSamples == 0) {
    imuFailed("IMU STOPPED SENDING SAMPLES, RECONNECTING", now);
    return;
  }

  imuSamples = 0;
  float streamDifference = -1; // g, largest axis difference, -1 when the direct read failed

  // a byte lost without a read error shifts the stream onto another axis, which still looks sane but never shows a crash
  // a shift by either one or two axes moves the vertical axis (~1.5g with its offset) where it doesn't belong
  uint8_t direct[6];

  if (readRegisters(imuAddress, IMU_REG_ACCEL_XOUT_H, direct, 6)) {
    bool misaligned = false;

    for (int axis = 0; axis < 3; axis++) {
      const int16_t directValue = (int16_t)(direct[axis * 2] << 8 | direct[axis * 2 + 1]);
      const float difference = abs(directValue - lastFifoSample[axis]) / ACCEL_LSB_PER_G;

      streamDifference = max(streamDifference, difference);
      misaligned |= difference > IMU_MISALIGNED_DIFFERENCE;
    }

    imuMisalignedChecks = misaligned ? imuMisalignedChecks + 1 : 0;

    if (imuMisalignedChecks >= IMU_MISALIGNED_CHECKS) {
      reportImuProblem("IMU FIFO MISALIGNED, RESET");
      startFifo();
      resetCrashMeasure();
      imuMisalignedChecks = 0;
    }
  }

  if (DEBUG_IMU_LOG) {
    Serial.print("IMU ");
    Serial.print(samples);
    Serial.print(" SAMPLES, BASELINE ");
    Serial.print(gravityBaseline, 2);
    Serial.print(" G, SPEED CHANGE ");
    Serial.print(debugSpeedChangeMin, 2);
    Serial.print(" / ");
    Serial.print(debugSpeedChangeMax, 2);
    Serial.print(" M/S, STREAM VS DIRECT ");
    Serial.print(streamDifference, 2);
    Serial.println(" G");
  }

  debugSpeedChangeMin = 0;
  debugSpeedChangeMax = 0;
}

void triggerCrash(unsigned long now) {
  crashHazardActive = true;
  crashHazardStart = now;
  animationFrame = 0; // start the hazard sweep cleanly

  Serial.println("CRASH DETECTED, HAZARD ON");
}

bool isCrashArmed(unsigned long now) {
  return DEBUG_CRASH_WITHOUT_PEDAL
    || pedal.read() == Button::PRESSED
    || (lastPedalRelease > 0 && now - lastPedalRelease < CRASH_ARMED_AFTER_RELEASE);
}

void processImuSample(const uint8_t* sample, unsigned long now) {
  imuSamples++;

  for (int axis = 0; axis < 3; axis++) {
    lastFifoSample[axis] = (int16_t)(sample[axis * 2] << 8 | sample[axis * 2 + 1]);
  }

  // mounted on its side: +x up, +y left, +z backward
  const float sampleForward = -(lastFifoSample[2] / ACCEL_LSB_PER_G - ACCEL_OFFSET_Z);
  const float sampleLeft = lastFifoSample[1] / ACCEL_LSB_PER_G - ACCEL_OFFSET_Y;
  const float sampleUp = lastFifoSample[0] / ACCEL_LSB_PER_G - ACCEL_OFFSET_X;

  if (!tiltReady) {
    tiltForward = sampleForward;
    tiltLeft = sampleLeft;
    tiltUp = sampleUp;
    tiltReady = true;
  }

  tiltForward += (sampleForward - tiltForward) / (TILT_FILTER_TIME * IMU_SAMPLE_RATE);
  tiltLeft += (sampleLeft - tiltLeft) / (TILT_FILTER_TIME * IMU_SAMPLE_RATE);
  tiltUp += (sampleUp - tiltUp) / (TILT_FILTER_TIME * IMU_SAMPLE_RATE);

  const int16_t rawZ = lastFifoSample[2];
  const float forward = -rawZ / ACCEL_LSB_PER_G;

  if (!gravityBaselineReady) {
    gravityBaseline = forward;
    gravityBaselineReady = true;
  }

  gravityBaseline += (forward - gravityBaseline) / (GRAVITY_BASELINE_TIME * IMU_SAMPLE_RATE);

  crashWindowSum += forward - gravityBaseline - crashWindow[crashWindowIndex];
  crashWindow[crashWindowIndex] = forward - gravityBaseline;
  crashWindowIndex = (crashWindowIndex + 1) % CRASH_WINDOW;

  // recompute every window so float rounding can't accumulate
  if (crashWindowIndex == 0) {
    crashWindowSum = 0;

    for (const float value : crashWindow) {
      crashWindowSum += value;
    }
  }

  const float speedChange = crashWindowSum / IMU_SAMPLE_RATE * STANDARD_GRAVITY; // m/s gained forward within the window

  debugSpeedChangeMin = min(debugSpeedChangeMin, speedChange);
  debugSpeedChangeMax = max(debugSpeedChangeMax, speedChange);
  const float speedLoss = DEBUG_CRASH_WITHOUT_PEDAL
    ? fabsf(speedChange)
    : travellingBackward ? speedChange : -speedChange;

  if (isCrashArmed(now) && speedLoss > CRASH_LOG_SPEED_LOSS) {
    crashEventPeak = max(crashEventPeak, speedLoss);

    if (speedLoss >= CRASH_SPEED_LOSS && !crashEventTriggered) {
      crashEventTriggered = true;
      triggerCrash(now);
    }
  } else if (crashEventPeak > 0) {
    Serial.print(crashEventTriggered ? "CRASH " : "NEAR MISS ");
    Serial.print(crashEventPeak, 2);
    Serial.println(travellingBackward ? " M/S LOST REVERSING" : " M/S LOST");

    if (!crashEventTriggered) {
      lastNearMiss = now;
    }

    crashEventPeak = 0;
    crashEventTriggered = false;
  }
}

void readFifo(unsigned long now) {
  uint8_t status = 0;
  uint8_t countBytes[2] = {0, 0};

  if (!readRegisters(imuAddress, IMU_REG_INT_STATUS, &status, 1) || !readRegisters(imuAddress, IMU_REG_FIFO_COUNT_H, countBytes, 2)) {
    imuFailed("IMU NOT RESPONDING, RECONNECTING", now);
    return;
  }

  const int count = (countBytes[0] & 0x1F) << 8 | countBytes[1];

  // once full, new samples overwrite the oldest and the stream loses alignment
  if (status & IMU_INT_FIFO_OFLOW || count > FIFO_SIZE - FIFO_SAMPLE_SIZE) {
    reportImuProblem("IMU FIFO OVERFLOW, RESET");
    startFifo();
    return;
  }

  uint8_t buffer[FIFO_SAMPLE_SIZE * FIFO_READ_BATCH];

  for (int samples = count / FIFO_SAMPLE_SIZE; samples > 0; samples -= FIFO_READ_BATCH) {
    const int batch = min(samples, FIFO_READ_BATCH);

    // a partial read leaves the fifo misaligned, which silently shifts every following sample to another axis
    if (!readRegisters(imuAddress, IMU_REG_FIFO_R_W, buffer, batch * FIFO_SAMPLE_SIZE)) {
      imuFailed("IMU READ FAILED, RECONNECTING", now);
      return;
    }

    for (int i = 0; i < batch; i++) {
      processImuSample(buffer + i * FIFO_SAMPLE_SIZE, now);
    }
  }
}

void setup() {
  // initialize digital pin LED_BUILTIN as an output.
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(PIN_ALIVE, OUTPUT);  
  pinMode(PIN_BUCK, OUTPUT); 
  pinMode(PIN_PEDAL, INPUT_PULLUP);

  pinMode(PIN_PEDAL_FORWARD, INPUT_PULLUP);
  pinMode(PIN_PEDAL_REVERSE, INPUT_PULLUP);

  pinMode(PIN_BUTTON_MODE, INPUT_PULLUP);
  pinMode(PIN_BUTTON_LEFT, INPUT_PULLUP);
  pinMode(PIN_BUTTON_RIGHT, INPUT_PULLUP);

  // digitalWrite(LED_BUILTIN, LED_ON);
  digitalWrite(PIN_ALIVE, HIGH);
  digitalWrite(PIN_BUCK, BUCK_ON);
  digitalWrite(LED_BUILTIN, LED_OFF);

  ledsTail.setBrightness(LEDS_TAIL_BRIGHTNESS);
  ledsHeadLeft.setBrightness(LEDS_HEAD_BRIGHTNESS);
  ledsHeadRight.setBrightness(LEDS_HEAD_BRIGHTNESS);
  ledsDash.setBrightness(LEDS_DASH_BRIGHTNESS);

  ledsTail.begin();
  ledsHeadLeft.begin();
  ledsHeadRight.begin();
  ledsDash.begin();

  pedal.begin();
  pedalForward.begin();
  pedalReverse.begin();

  buttonMode.begin();
  buttonLeft.begin();
  buttonRight.begin();

  Serial.begin(9600);
  randomSeed(micros() ^ HAL_GetUIDw0() ^ HAL_GetUIDw1());

  beginWire();
  connectImu(millis(), false);

  ledsTail.clear();
  ledsHeadLeft.clear();
  ledsHeadRight.clear();
  ledsDash.clear();

  startupStart = millis();
}

void animateHeadlights() {
  ledsHeadLeft.fill(COLOR_WHITE);
  ledsHeadRight.fill(COLOR_WHITE);
}

void animateTailLight() {
  ledsTail.fill(COLOR_OFF, 0, LEDS_TAIL_LENGTH);
  ledsTail.fill(COLOR_RED, LEDS_TAIL_SIDE_LENGTH, LEDS_TAIL_LENGTH - LEDS_TAIL_SIDE_LENGTH * 2);
}

void animateRightTailSide(uint32_t color, bool withMargin = true) {
  ledsTail.fill(color, 0, LEDS_TAIL_SIDE_LENGTH);

  if (withMargin) {
    ledsTail.fill(COLOR_OFF, LEDS_TAIL_SIDE_LENGTH, LEDS_TAIL_SIDE_MARGIN);
  }
}

void animateLeftTailSide(uint32_t color, bool withMargin = true) {
  ledsTail.fill(color, LEDS_TAIL_LENGTH - LEDS_TAIL_SIDE_LENGTH, LEDS_TAIL_SIDE_LENGTH);

  if (withMargin) {
    ledsTail.fill(COLOR_OFF, LEDS_TAIL_LENGTH - LEDS_TAIL_SIDE_LENGTH -  LEDS_TAIL_SIDE_MARGIN, LEDS_TAIL_SIDE_MARGIN);
  }
}

void animateDash() {
  ledsDash.fill(COLOR_GREEN);
}

void animateReverse() {
  ledsTail.fill(COLOR_WHITE);
}

void animateBrake() {
    animateLeftTailSide(COLOR_RED, false);
    animateRightTailSide(COLOR_RED, false);
}

constexpr int INDICATOR_DURATION = 3200; // total ms
constexpr int INDICATOR_HOLD_FRAMES = 15;
constexpr int INDICATOR_OFF_FRAMES = 20;
constexpr float INDICATOR_SPEED = 2;

// shutdown blinks like locking a car with the key fob, on, off and on again, timed like a normal indicator blink
// the second blink stays on and fades out with everything else
constexpr unsigned long SHUTDOWN_BLINK_ON = (LEDS_TOP_LENGTH + INDICATOR_HOLD_FRAMES) / INDICATOR_SPEED * ANIMATION_INTERVAL; // ms, sweep and hold
constexpr unsigned long SHUTDOWN_BLINK_CYCLE = (LEDS_EDGE_LENGTH + INDICATOR_HOLD_FRAMES + INDICATOR_OFF_FRAMES) / INDICATOR_SPEED * ANIMATION_INTERVAL; // ms
constexpr unsigned long SHUTDOWN_FADE = 500; // ms
constexpr unsigned long SHUTDOWN_FADE_START = SHUTDOWN_BLINK_CYCLE + SHUTDOWN_BLINK_ON;
constexpr unsigned long SHUTDOWN_DURATION = SHUTDOWN_FADE_START + SHUTDOWN_FADE;

void animateIndicator(Direction direction) {
  Adafruit_NeoPixel& ledsTarget = direction == LEFT
    ? ledsHeadLeft
    : ledsHeadRight;

  const long indicatorFrame = lround(animationFrame * INDICATOR_SPEED) % (LEDS_EDGE_LENGTH + INDICATOR_HOLD_FRAMES + INDICATOR_OFF_FRAMES);

  const long sweepEnd = LEDS_TOP_LENGTH;
  const long holdEnd = LEDS_TOP_LENGTH + INDICATOR_HOLD_FRAMES;

  for (int ledIndex = LEDS_TOP_LENGTH - 1; ledIndex >= 0; ledIndex--) {
    if (indicatorFrame < sweepEnd) {
      // SWEEP
      bool isLit = ledIndex >= (LEDS_TOP_LENGTH - 1 - indicatorFrame);

      ledsTarget.setPixelColor(ledIndex, isLit ? COLOR_AMBER : COLOR_OFF);

      if (ledIndex > 0) {
        ledsTarget.setPixelColor(LEDS_EDGE_LENGTH - ledIndex, isLit ? COLOR_AMBER : COLOR_OFF);
      }
    } else if (indicatorFrame < holdEnd) {
      // HOLD
      ledsTarget.setPixelColor(ledIndex, COLOR_AMBER);

      if (ledIndex > 0) {
        ledsTarget.setPixelColor(LEDS_EDGE_LENGTH - ledIndex, COLOR_AMBER);
      }
    } else {
      // OFF
      ledsTarget.fill(COLOR_OFF, 0, LEDS_EDGE_LENGTH);
    }
  }

  void (*animateTailSide)(uint32_t, bool) = direction == LEFT
    ? animateLeftTailSide
    : animateRightTailSide;

  const int dashIndex = direction == LEFT
    ? 1
    : 0;

  if (indicatorFrame < sweepEnd) {
    const int colorAmberFaded = colorHSV(10, 1, (float)indicatorFrame / sweepEnd);

    animateTailSide(colorAmberFaded, true);
    ledsDash.setPixelColor(dashIndex, colorAmberFaded);
  } else if (indicatorFrame < holdEnd) {
    animateTailSide(COLOR_AMBER, true);
    ledsDash.setPixelColor(dashIndex, COLOR_AMBER);
  } else {
    animateTailSide(COLOR_OFF, true);
    ledsDash.setPixelColor(dashIndex, COLOR_OFF);
  }
}

constexpr float POLICE_SPEED = 0.75;

enum PoliceStates { POLICE_LEFT, POLICE_RIGHT, POLICE_OFF };

const PoliceStates policeBeaconFrames[12] = {
  POLICE_LEFT,
  POLICE_OFF,
  POLICE_LEFT,
  POLICE_OFF,
  POLICE_OFF,
  POLICE_OFF,
  POLICE_RIGHT,
  POLICE_OFF,
  POLICE_RIGHT,
  POLICE_OFF,
  POLICE_OFF,
  POLICE_OFF,
};

void animatePolice() {
  const long policeFrame = lround(animationFrame * POLICE_SPEED) % 12;

  if (policeBeaconFrames[policeFrame] == POLICE_LEFT) {
    ledsHeadLeft.fill(COLOR_POLICE_BLUE, 0, LEDS_EDGE_LENGTH);
    ledsHeadRight.fill(COLOR_OFF, 0, LEDS_EDGE_LENGTH);

    // deliberately inverted from edge lights for a more alternating effect
    animateLeftTailSide(COLOR_OFF);
    animateRightTailSide(COLOR_POLICE_BLUE);

    ledsDash.setPixelColor(1, COLOR_POLICE_BLUE);
    ledsDash.setPixelColor(0, COLOR_OFF);
  } else if (policeBeaconFrames[policeFrame] == POLICE_RIGHT) {
    ledsHeadLeft.fill(COLOR_OFF, 0, LEDS_EDGE_LENGTH);
    ledsHeadRight.fill(COLOR_POLICE_BLUE, 0, LEDS_EDGE_LENGTH);

    animateLeftTailSide(COLOR_POLICE_BLUE);
    animateRightTailSide(COLOR_OFF);

    ledsDash.setPixelColor(1, COLOR_OFF);
    ledsDash.setPixelColor(0, COLOR_POLICE_BLUE);
  } else {
    ledsHeadLeft.fill(COLOR_OFF, 0, LEDS_EDGE_LENGTH);
    ledsHeadRight.fill(COLOR_OFF, 0, LEDS_EDGE_LENGTH);

    animateLeftTailSide(COLOR_OFF);
    animateRightTailSide(COLOR_OFF);

    ledsDash.setPixelColor(1, COLOR_OFF);
    ledsDash.setPixelColor(0, COLOR_OFF);
  }

  if (policeFrame < 6) {
    ledsHeadLeft.fill(COLOR_WHITE_DIM, LEDS_EDGE_LENGTH, LEDS_RING_LENGTH);
    ledsHeadRight.fill(COLOR_WHITE, LEDS_EDGE_LENGTH, LEDS_RING_LENGTH);
  } else {
    ledsHeadLeft.fill(COLOR_WHITE, LEDS_EDGE_LENGTH, LEDS_RING_LENGTH);
    ledsHeadRight.fill(COLOR_WHITE_DIM, LEDS_EDGE_LENGTH, LEDS_RING_LENGTH);
  }
}

constexpr float RAINBOW_SPEED = 2.0;
constexpr float RAINBOW_DENSITY = 10.0;

void animateRainbow() {
  // EDGES, flowing color
  for (int ledIndex = LEDS_TOP_LENGTH - 1; ledIndex >= 0; ledIndex--) {
    const uint32_t pixelColor = colorHSV((stableAnimationFrame * RAINBOW_SPEED) + (ledIndex * RAINBOW_DENSITY));

    ledsHeadLeft.setPixelColor(ledIndex, pixelColor);
    ledsHeadLeft.setPixelColor(LEDS_EDGE_LENGTH - ledIndex, pixelColor);

    ledsHeadRight.setPixelColor(ledIndex, pixelColor);
    ledsHeadRight.setPixelColor(LEDS_EDGE_LENGTH - ledIndex, pixelColor);
  }

  // RINGS, single color
  ledsHeadLeft.fill(colorHSV(stableAnimationFrame * RAINBOW_SPEED), LEDS_EDGE_LENGTH, LEDS_RING_LENGTH);
  ledsHeadRight.fill(colorHSV(stableAnimationFrame * RAINBOW_SPEED), LEDS_EDGE_LENGTH, LEDS_RING_LENGTH);

  for (int ledIndex = 0; ledIndex < LEDS_TAIL_LENGTH; ledIndex++) {
    const int distFromCenter = abs(ledIndex * 2 - (LEDS_TAIL_LENGTH - 1));
    const uint32_t pixelColor = colorHSV((stableAnimationFrame * RAINBOW_SPEED) - (distFromCenter * RAINBOW_DENSITY * 2));

    ledsTail.setPixelColor(ledIndex, pixelColor);
  }

  ledsDash.setPixelColor(1, colorHSV(stableAnimationFrame * RAINBOW_SPEED));
  ledsDash.setPixelColor(0, colorHSV(stableAnimationFrame * RAINBOW_SPEED + 90));
}

constexpr float KITT_SPEED = 0.5;
constexpr int KITT_TRAIL = 4;
constexpr float KITT_TAIL_SPEED = 3.0f; 

int kittScanPos(int vPos) {
  if (vPos < LEDS_RING_LENGTH) {
    // first ring — reversed so it flows naturally into the edge
    return LEDS_EDGE_LENGTH + (LEDS_RING_LENGTH - 1 - vPos);
  } else if (vPos < LEDS_RING_LENGTH + LEDS_EDGE_LENGTH) {
    // edge forward
    return vPos - LEDS_RING_LENGTH;
  } else if (vPos < 2 * LEDS_RING_LENGTH + LEDS_EDGE_LENGTH) {
    // second ring
    return LEDS_EDGE_LENGTH + (vPos - LEDS_RING_LENGTH - LEDS_EDGE_LENGTH);
  } else {
    // edge backward
    return LEDS_EDGE_LENGTH - 1 - (vPos - 2 * LEDS_RING_LENGTH - LEDS_EDGE_LENGTH);
  }
}

void animateKitt() {
  const long kittFrame = lround(stableAnimationFrame * KITT_SPEED);
  const int cycleLength = 2 * LEDS_EDGE_LENGTH + 2 * LEDS_RING_LENGTH;
  const int vPos = kittFrame % cycleLength;

  ledsHeadLeft.fill(COLOR_OFF);
  ledsHeadRight.fill(COLOR_OFF);

  // trail behind
  for (int t = 0; t < KITT_TRAIL; t++) {
    const int pastVPos = ((vPos - t) % cycleLength + cycleLength) % cycleLength;
    const int scanPos = kittScanPos(pastVPos);

    const uint32_t color = colorHSV(0, 1, 1.0f - (float)t / KITT_TRAIL);
    ledsHeadLeft.setPixelColor(scanPos, color);
    ledsHeadRight.setPixelColor(scanPos, color);
  }

  // lead ahead — shorter and dimmer than trail
  constexpr int KITT_LEAD = 2;
  for (int t = 1; t <= KITT_LEAD; t++) {
    const int futureVPos = ((vPos + t) % cycleLength + cycleLength) % cycleLength;
    const int scanPos = kittScanPos(futureVPos);

    const uint32_t color = colorHSV(0, 1, 0.3f * (1.0f - (float)t / KITT_LEAD));
    ledsHeadLeft.setPixelColor(scanPos, color);
    ledsHeadRight.setPixelColor(scanPos, color);
  }

  const int scanPos = kittScanPos(vPos);
  

  // normalized cycle (0–1), sped up
  float tailCycle = (float)vPos * KITT_TAIL_SPEED / cycleLength;
  tailCycle -= (int)tailCycle;  // wrap to 0–1

  // triangle wave: 0 → 1 → 0 (KITT bounce)
  float tailT = tailCycle * 2.0f;
  if (tailT > 1.0f) tailT = 2.0f - tailT;

  // optional smoothing (gives that analog feel)
  tailT = tailT * tailT * (3.0f - 2.0f * tailT); // smoothstep

  // map to LED index
  const int tailPos = lround(tailT * (LEDS_TAIL_LENGTH - 1));

  // draw tail with falloff
  for (int i = 0; i < LEDS_TAIL_LENGTH; i++) {
    const int dist = abs(i - tailPos);
    float brightness = 0.0f;

    if (dist < KITT_TRAIL) {
      brightness = 1.0f - (float)dist / KITT_TRAIL;
    }

    ledsTail.setPixelColor(i, colorHSV(0, 1, brightness));
  }

  ledsDash.fill(COLOR_OFF);

  float speed = 3.0;

  float dashCycle = (float)vPos * speed / cycleLength;
  dashCycle -= (int)dashCycle;  // keep it in 0–1

  // triangle wave: 0→1→0 over the cycle
  float t = dashCycle * 2.0;
  if (t > 1.0) t = 2.0 - t;

  ledsDash.setPixelColor(0, colorHSV(0, 1, t));
  ledsDash.setPixelColor(1, colorHSV(0, 1, 1.0 - t));
}

void animateDebugDash(unsigned long now) {
  uint32_t status = COLOR_OFF;

  if (lastImuProblem > 0 && now - lastImuProblem < DEBUG_IMU_PROBLEM_DURATION) {
    status = COLOR_RED;
  } else if (lastNearMiss > 0 && now - lastNearMiss < DEBUG_NEAR_MISS_DURATION) {
    status = COLOR_YELLOW;
  } else if (isCrashArmed(now)) {
    status = travellingBackward ? COLOR_POLICE_BLUE : COLOR_GREEN;
  }

  ledsDash.setPixelColor(0, COLOR_WHITE);
  ledsDash.setPixelColor(1, status);
}

// degrees between gravity now and gravity when standing flat, 0 without the IMU
float tiltAngle() {
  if (!tiltReady || imuState != IMU_RUNNING) {
    return 0;
  }

  const float dot = tiltForward * FLAT_FORWARD + tiltLeft * FLAT_LEFT + tiltUp * FLAT_UP;
  const float lengths = sqrtf(tiltForward * tiltForward + tiltLeft * tiltLeft + tiltUp * tiltUp)
    * sqrtf(FLAT_FORWARD * FLAT_FORWARD + FLAT_LEFT * FLAT_LEFT + FLAT_UP * FLAT_UP);

  if (lengths == 0) {
    return 0;
  }

  return degrees(acosf(constrain(dot / lengths, -1.0f, 1.0f)));
}

void updateScramble(unsigned long now) {
  const float tilt = tiltAngle();

  if (scrambleActive) {
    if (tilt < SCRAMBLE_TILT_END) {
      scrambleActive = false;
      Serial.println("LEVEL AGAIN, UNSCRAMBLED");
    }

    return;
  }

  if (tilt <= SCRAMBLE_TILT) {
    scrambleTilted = false;
  } else if (!scrambleTilted) {
    scrambleTilted = true;
    scrambleTiltedSince = now;
  } else if (now - scrambleTiltedSince >= SCRAMBLE_HOLD) {
    scrambleActive = true;
    scrambleTilted = false;

    Serial.print("TILTED ");
    Serial.print(tilt, 0);
    Serial.println(" DEGREES, SCRAMBLED");
  }
}

// saturated and far apart on the colour wheel, random hues and brightness blended into pastel
const float SCRAMBLE_HUES[] = {0, 60, 120, 180, 240, 300};

void scramblePixels(uint32_t* pixels, int length, bool all) {
  if (all) {
    for (int i = 0; i < length; i++) {
      pixels[i] = COLOR_OFF;
    }
  }

  for (int i = 0; i < length; i++) {
    if (!all && random(100) >= SCRAMBLE_CHANGE_PERCENT) {
      continue;
    }

    // lit pixels always get a dark neighbour, so the colours don't blend
    const bool neighbourLit = (i > 0 && pixels[i - 1] != COLOR_OFF)
      || (i < length - 1 && pixels[i + 1] != COLOR_OFF);

    pixels[i] = neighbourLit || random(2) == 0
      ? COLOR_OFF
      : colorHSV(SCRAMBLE_HUES[random(sizeof(SCRAMBLE_HUES) / sizeof(SCRAMBLE_HUES[0]))]);
  }
}

void drawPixels(Adafruit_NeoPixel& leds, const uint32_t* pixels, int length) {
  for (int i = 0; i < length; i++) {
    leds.setPixelColor(i, pixels[i]);
  }
}

void animateScramble(bool start) {
  if (start || stableAnimationFrame % SCRAMBLE_FRAMES == 0) {
    scramblePixels(scrambleHeadLeft, LEDS_HEAD_LENGTH, start);
    scramblePixels(scrambleHeadRight, LEDS_HEAD_LENGTH, start);
    scramblePixels(scrambleTail, LEDS_TAIL_LENGTH, start);
    scramblePixels(scrambleDash, LEDS_DASH_LENGTH, start);
  }

  drawPixels(ledsHeadLeft, scrambleHeadLeft, LEDS_HEAD_LENGTH);
  drawPixels(ledsHeadRight, scrambleHeadRight, LEDS_HEAD_LENGTH);
  drawPixels(ledsTail, scrambleTail, LEDS_TAIL_LENGTH);
  drawPixels(ledsDash, scrambleDash, LEDS_DASH_LENGTH);
}

// LED output looks much brighter than its level at the dark end, a linear fade seems done halfway through
float perceived(float level) {
  return Adafruit_NeoPixel::gamma8(level * 255) / 255.0f;
}

// edges sweep on in the same direction as the indicator sweep, the leading LED fading in so it moves smoothly, and the
// tail light sweeps from the centre outward and the dash fades in over the whole startup, the rings fade in from STARTUP_RING_DELAY
void animateStartup(unsigned long now) {
  const unsigned long elapsed = now - startupStart;
  const float progress = min(1.0f, (float)elapsed / STARTUP_SWEEP);
  const float ringLevel = elapsed < STARTUP_RING_DELAY ? 0 : min(1.0f, (float)(elapsed - STARTUP_RING_DELAY) / STARTUP_RING_FADE);
  const uint32_t ringColor = ledsHeadLeft.Color(0, 0, 0, 255 * perceived(ringLevel));

  // the tail and dash span the whole startup, so they finish when the normal lights take over
  const float durationProgress = min(1.0f, (float)elapsed / STARTUP_DURATION);

  constexpr int TAIL_PAIRS = (LEDS_TAIL_LENGTH - LEDS_TAIL_SIDE_LENGTH * 2) / 2;

  ledsTail.clear();

  for (int ledIndex = LEDS_TAIL_SIDE_LENGTH; ledIndex < LEDS_TAIL_LENGTH - LEDS_TAIL_SIDE_LENGTH; ledIndex++) {
    const int pair = abs(ledIndex * 2 - (LEDS_TAIL_LENGTH - 1)) / 2; // 0 in the centre
    const float level = constrain(durationProgress * TAIL_PAIRS - pair, 0.0f, 1.0f);

    ledsTail.setPixelColor(ledIndex, ledsTail.Color(255 * perceived(level), 0, 0, 0));
  }

  ledsDash.fill(ledsDash.Color(0, 255 * perceived(durationProgress), 0, 0));

  for (Adafruit_NeoPixel* leds : {&ledsHeadLeft, &ledsHeadRight}) {
    leds->clear();
    leds->fill(ringColor, LEDS_EDGE_LENGTH, LEDS_RING_LENGTH);

    for (int ledIndex = LEDS_TOP_LENGTH - 1; ledIndex >= 0; ledIndex--) {
      const int position = LEDS_TOP_LENGTH - 1 - ledIndex; // 0 is reached first
      const float sweepLevel = constrain(progress * LEDS_TOP_LENGTH - position, 0.0f, 1.0f);

      const float cornerStart = position < STARTUP_CORNER_LEDS
        ? STARTUP_CORNER_LEVEL + (1 - STARTUP_CORNER_LEVEL) * position / STARTUP_CORNER_LEDS
        : 1;
      const float cornerLevel = cornerStart + (1 - cornerStart) * progress;

      const uint32_t color = leds->Color(0, 0, 0, 255 * perceived(sweepLevel * cornerLevel));

      leds->setPixelColor(ledIndex, color);

      if (ledIndex > 0) {
        leds->setPixelColor(LEDS_EDGE_LENGTH - ledIndex, color);
      }
    }
  }
}

void fadeLeds(Adafruit_NeoPixel& leds, float level) {
  for (uint16_t i = 0; i < leds.numPixels(); i++) {
    const uint32_t color = leds.getPixelColor(i);

    leds.setPixelColor(i, leds.Color(
      (uint8_t)(color >> 16 & 0xFF) * level,
      (uint8_t)(color >> 8 & 0xFF) * level,
      (uint8_t)(color & 0xFF) * level,
      (uint8_t)(color >> 24 & 0xFF) * level
    ));
  }
}

// one amber blink like locking a car, the second stays on and fades out with the other lights
// only the indicator areas blink, like the hazard lights, the rings and tail light keep showing as normal
void animateShutdown(unsigned long now) {
  const unsigned long elapsed = now - shutdownStart;
  const bool indicatorsOn = elapsed < SHUTDOWN_BLINK_ON || elapsed >= SHUTDOWN_BLINK_CYCLE;
  const uint32_t indicatorColor = indicatorsOn ? COLOR_AMBER : COLOR_OFF;

  ledsHeadLeft.fill(indicatorColor, 0, LEDS_EDGE_LENGTH);
  ledsHeadRight.fill(indicatorColor, 0, LEDS_EDGE_LENGTH);
  animateLeftTailSide(indicatorColor, true);
  animateRightTailSide(indicatorColor, true);
  ledsDash.fill(indicatorColor);

  if (elapsed < SHUTDOWN_FADE_START) {
    return;
  }

  const float level = perceived(1 - min(1.0f, (float)(elapsed - SHUTDOWN_FADE_START) / SHUTDOWN_FADE));

  fadeLeds(ledsHeadLeft, level);
  fadeLeds(ledsHeadRight, level);
  fadeLeds(ledsTail, level);
  fadeLeds(ledsDash, level);
}

void animate(long now) {
  if ((unsigned long)now - startupStart < STARTUP_DURATION) {
    animateStartup(now);
    return;
  }

  // always animate headlights, let other modes override
  animateHeadlights();
  animateTailLight();
  animateDash();

  if (isReversing) {
    animateReverse();
  }

  if (activeMode == MODE_POLICE) {
    animatePolice();
  }

  if (activeMode == MODE_HAZARD) {
    animateIndicator(LEFT);
    animateIndicator(RIGHT);
  }

  if (activeMode == MODE_KITT) {
    animateKitt();
  }

  // lastPedalRelease is 0 until the first release, prevents brake lights right after boot
  if (lastPedalRelease > 0 && now - lastPedalRelease < 1500) {
    animateBrake();
  }

  if (activeMode == MODE_RAINBOW) {
    animateRainbow();
  }

  // we need the boolean to prevent the indicators from turning on right after boot
  if (indicatorLeftActive && now - indicatorLeftStart < INDICATOR_DURATION) {
    animateIndicator(LEFT);
  }

  if (indicatorRightActive && now - indicatorRightStart < INDICATOR_DURATION) {
    animateIndicator(RIGHT);
  }

  const bool wasScrambled = scrambleActive;
  updateScramble(now);

  // overrides every mode, only the crash hazard goes on top so a rollover still warns
  if (scrambleActive) {
    animateScramble(!wasScrambled);
  }

  // drawn last so it overrides every mode
  if (crashHazardActive && now - crashHazardStart < CRASH_HAZARD_DURATION) {
    animateIndicator(LEFT);
    animateIndicator(RIGHT);
  } else {
    crashHazardActive = false;
  }

  if (DEBUG_DASH) {
    animateDebugDash(now);
  }

  if (shuttingDown) {
    animateShutdown(now);
  }
}

// any button dismisses the crash hazard, that press does nothing else
bool dismissCrashHazard() {
  if (!crashHazardActive) {
    return false;
  }

  crashHazardActive = false;
  Serial.println("CRASH HAZARD DISMISSED");

  return true;
}

void render() {
    ledsTail.show();
    ledsHeadLeft.show();
    ledsHeadRight.show();
    ledsDash.show();
}

bool isOff = false;

unsigned long modePressStart = 0;
bool modePressed = false;

// the lights play the shutdown animation first, powerOff() follows after SHUTDOWN_DURATION
void beginShutdown(unsigned long now) {
  if (isOff || shuttingDown) {
    return;
  }

  shuttingDown = true;
  shutdownStart = now;
  modePressed = false; // releasing the long press shouldn't switch mode

  Serial.println("SHUTTING DOWN");
}

// any press during the shutdown animation keeps it on, that press does nothing else
bool cancelShutdown() {
  if (!shuttingDown) {
    return false;
  }

  shuttingDown = false;
  Serial.println("SHUTDOWN CANCELLED");

  return true;
}

void powerOff() {
  // when STLink or USB prevents full power cut, loop keeps running, only power off once
  if (isOff) {
    return;
  }

  shuttingDown = false;

  Serial.println("BUCK OFF");
  digitalWrite(PIN_BUCK, BUCK_OFF);

  delay(100);

  Serial.println("SELF OFF, GOODBYE");
  digitalWrite(PIN_ALIVE, LOW);

  isOff = true;
}

// only used when STLink or USB prevents full power cut
void restart() {
  Serial.println("RESTARTING");

  lastFrame = millis();
  startupStart = lastFrame;
  animationFrame = 0;
  stableAnimationFrame = 0;
  isOff = false;

  digitalWrite(PIN_BUCK, BUCK_ON);
}

bool indicatorCombo = false; // both indicator buttons pressed, until both are released
Mode modeBeforeHazard = MODE_HEAD;

void toggleHazardMode() {
  if (activeMode == MODE_HAZARD) {
    activeMode = modeBeforeHazard;
  } else {
    modeBeforeHazard = activeMode;
    activeMode = MODE_HAZARD;
  }

  animationFrame = 0; // ensures animations start cleanly instead of halfway

  Serial.print("BUTTONS LEFT AND RIGHT, MODE ");
  Serial.println(activeMode);
}

void handleIndicatorPress(Direction direction, unsigned long now) {
  if (isOff) {
    restart();
  }

  lastButtonPress = now;

  if (cancelShutdown() || dismissCrashHazard() || indicatorCombo) {
    return;
  }

  // both buttons are never pressed at exactly the same time, so the first one already started its indicator
  Button& otherButton = direction == LEFT ? buttonRight : buttonLeft;

  if (otherButton.read() == Button::PRESSED) {
    indicatorCombo = true;
    indicatorLeftActive = false;
    indicatorRightActive = false;

    toggleHazardMode();
    return;
  }

  animationFrame = 0;

  indicatorLeftActive = direction == LEFT;
  indicatorLeftStart = direction == LEFT ? now : 0;

  indicatorRightActive = direction == RIGHT;
  indicatorRightStart = direction == RIGHT ? now : 0;

  Serial.println(direction == LEFT ? "BUTTON LEFT" : "BUTTON RIGHT");
}

// the loop function runs over and over again forever
void loop() {
  const unsigned long now = millis();

  if (buttonMode.pressed()) {
    if (isOff) {
      // wake only, this press shouldn't switch mode on release or count towards a shutdown hold
      restart();
    } else if (cancelShutdown() || dismissCrashHazard()) {
      // same for cancelling and dismissing
    } else {
      modePressed = true;
      modePressStart = now;
    }

    lastButtonPress = now;
  }

  // switch mode on release, so a long press for shutdown doesn't switch mode first
  if (buttonMode.released()) {
    if (modePressed && !isOff) {
      activeMode = (Mode)((activeMode + 1) % MODE_COUNT);
      animationFrame = 0; // ensures animations start cleanly instead of halfway

      Serial.print("BUTTON MODE ");
      Serial.println(activeMode);
    }

    modePressed = false;
  }

  if (modePressed && now - modePressStart > 4000) {
    beginShutdown(now);
  }

  if (buttonLeft.pressed()) {
    handleIndicatorPress(LEFT, now);
  }

  if (buttonRight.pressed()) {
    handleIndicatorPress(RIGHT, now);
  }

  if (indicatorCombo && buttonLeft.read() != Button::PRESSED && buttonRight.read() != Button::PRESSED) {
    indicatorCombo = false;
  }

  // only the pedal counts as activity, forward and reverse inputs can read active while parked
  if (pedal.read() == Button::PRESSED) {
    lastPedalPress = now;
    cancelShutdown();

    if (isOff) {
      restart();
    }
  }

  if (pedal.pressed()) {
    Serial.println("PEDAL PRESSED");
  }

  if (pedal.released()) {
    lastPedalRelease = now;

    Serial.println("PEDAL RELEASED");
  }

  const bool wasReversing = isReversing;
  isReversing = pedalReverse.read() == Button::PRESSED && pedalForward.read() != Button::PRESSED;

  if (isReversing != wasReversing) {
    Serial.println(isReversing ? "REVERSE ON" : "REVERSE OFF");
  }

  // parked in reverse both inputs read active, so the direction only counts while driving
  if (pedal.read() == Button::PRESSED) {
    travellingBackward = isReversing;
  }

  if (!isOff && imuState == IMU_RUNNING && now - lastFifoRead >= FIFO_INTERVAL) {
    lastFifoRead = now;
    readFifo(now);
  }

  if (!isOff && imuState == IMU_RUNNING && now - lastImuHealth >= IMU_HEALTH_INTERVAL) {
    lastImuHealth = now;
    checkImuHealth(now);
  }

  if (imuState == IMU_STARTING && now - imuStateSince >= IMU_RESET_TIME) {
    configureImu(now);
  }

  // retry while missing, so a supply dip or reseating a wire doesn't need a reboot
  if (imuState == IMU_MISSING && now - imuStateSince >= IMU_RETRY_INTERVAL) {
    connectImu(now);
  }

  if (!isOff && now - lastFrame >= ANIMATION_INTERVAL) {
    lastFrame += ANIMATION_INTERVAL;
    animationFrame += 1;
    stableAnimationFrame += 1;

    animate(now);
    render();
  }

  if (shuttingDown && now - shutdownStart >= SHUTDOWN_DURATION) {
    powerOff();
  }

  if (KEEPALIVE_ENABLE && !DEBUG_CRASH_WITHOUT_PEDAL && now - lastPedalPress > KEEPALIVE_TIMEOUT && now - lastButtonPress > KEEPALIVE_TIMEOUT) {
    beginShutdown(now);
  }
}