//
// Auto_Seat
// Automatic seat controller with EEPROM settings and optional serial tools.
//
#include <EEPROM.h>
#include <SPI.h>
#include <mcp_can.h>

#define seatRelayD8 8   // HIGH = OFF, LOW = ON (up relay)
#define seatRelayD9 9   // HIGH = OFF, LOW = ON (down relay)

#define parkingPin A0   // PARKING raw analog value, 0~1023
#define ignitionPin 7   // HIGH = IG1 ON, LOW = IG1 OFF

// ============================================================
// !!! SAFETY LIMIT !!!
//
// Physical maximum travel time from bottom to the highest allowed point.
// User settings saved in EEPROM must stay inside 0..6000ms.
// ============================================================
const uint16_t SEAT_HARD_LIMIT_UP_MS = 6000;

// Parking raw threshold hysteresis.
// P turns ON at 950 or higher, and turns OFF at 940 or lower.
const int PARKING_ON_THRESHOLD = 950;
const int PARKING_OFF_THRESHOLD = 940;

// Down movement is estimated as 1.1x faster than up movement.
const int DOWN_SPEED_NUM = 11;
const int DOWN_SPEED_DEN = 10;

// Input and relay protection.
const unsigned long IGNITION_STABILIZE_MS = 1000UL;
const unsigned long DRIVE_CONFIRM_MS = 80UL;
const unsigned long PARK_CONFIRM_MS = 250UL;
const unsigned long RELAY_DEADTIME_MS = 120UL;
const unsigned long DOWN_EDGE_MARGIN_MS = 300UL;

// After 10 ignition OFF events, wait 5 minutes while ignition is OFF,
// then auto-calibrate: up 7 seconds -> down 6 seconds -> set position to bottom.
const byte USES_BEFORE_AUTO_CALIBRATION = 10;
const unsigned long AUTO_CALIBRATION_DELAY_MS = 300000UL;
const unsigned long AUTO_CALIBRATION_UP_MS = 7000UL;
const unsigned long AUTO_CALIBRATION_DOWN_MS = 6000UL;

// Serial debug output.
const unsigned long DEBUG_INTERVAL_MS = 1000UL;
const long MANUAL_STEP_MS = 500;

// Chaegeon-CAN heartbeat. MCP2515 module uses an 8MHz crystal.
const byte CAN_CS_PIN = 10;
const unsigned long CAN_ID_MAIN_HEARTBEAT = 0x100UL;
const unsigned long CAN_ID_CONSOLE_COMMAND = 0x101UL;
const unsigned long CAN_ID_CONSOLE_STATUS = 0x200UL;
const unsigned long CAN_HEARTBEAT_INTERVAL_MS = 250UL;

// Center-console node protocol. Nano stores memory and sends pulse commands.
const unsigned long CONSOLE_CAN_TIMEOUT_MS = 2000UL;
const unsigned long CONSOLE_COMMAND_HOLD_MS = 2500UL;
const unsigned long CONSOLE_MANUAL_LEARN_SETTLE_MS = 700UL;
const unsigned long CONSOLE_STARTUP_DELAY_MS = 3000UL;
const unsigned long CONSOLE_MEMORY_REFRESH_INTERVAL_MS = 1170000UL; // 19m 30s
const unsigned long CONSOLE_STEERING_RESTART_INTERVAL_MS = 1170000UL;
const unsigned int CONSOLE_SHORT_PRESS_MS = 300;
const unsigned int CONSOLE_SHORT_PRESS_GAP_MS = 450;
const unsigned int CONSOLE_CLIMATE_LONG_OFF_MS = 2100;
const unsigned int CONSOLE_CLIMATE_LONG_OFF_GAP_MS = 700;
const unsigned int CONSOLE_STEERING_RESTART_GAP_MS = 1200;

const byte CONSOLE_CHANNEL_COUNT = 7;
const byte CONSOLE_CLIMATE_COUNT = 4;
const byte CONSOLE_TOGGLE_COUNT = 3;
const byte CONSOLE_PULSE_QUEUE_SIZE = 24;

const byte CH_DRIVER_VENT = 0;
const byte CH_DRIVER_HEAT = 1;
const byte CH_PASSENGER_VENT = 2;
const byte CH_PASSENGER_HEAT = 3;
const byte CH_STEERING_HEAT = 4;
const byte CH_FRONT_SENSOR = 5;
const byte CH_VESS = 6;

const byte TG_STEERING_HEAT = 0;
const byte TG_FRONT_SENSOR = 1;
const byte TG_VESS = 2;

const byte CONSOLE_OPPOSITE_CLIMATE[CONSOLE_CLIMATE_COUNT] = {
  CH_DRIVER_HEAT,
  CH_DRIVER_VENT,
  CH_PASSENGER_HEAT,
  CH_PASSENGER_VENT
};

// EEPROM map.
const int EEPROM_MAGIC_ADDR = 0;
const byte EEPROM_MAGIC_VALUE = 0x64;
const int EEPROM_USE_COUNT_ADDR = 1;
const int EEPROM_SEAT_POSITION_ADDR = 2; // uint16_t, low byte first
const int EEPROM_CONSOLE_MAGIC_ADDR = 16;
const byte EEPROM_CONSOLE_MAGIC_VALUE = 0xC2;
const int EEPROM_CONSOLE_CLIMATE_TARGET_ADDR = 17; // 4 bytes, driver vent/heat/passenger vent/heat
const int EEPROM_CONSOLE_TOGGLE_TARGET_ADDR = 21;  // 3 bytes, steering/front sensor/VESS

const byte CALIBRATION_IDLE = 0;
const byte CALIBRATION_UP = 1;
const byte CALIBRATION_DOWN = 2;

boolean ignitionState = LOW;
boolean parkingState = LOW;
boolean prevIgnitionState = LOW;
boolean isStabilizing = false;
boolean confirmedDriveMode = LOW;
boolean pendingDriveMode = LOW;

byte useCount = 0;
byte autoCalibrationState = CALIBRATION_IDLE;
bool autoCalibrationPending = false;
bool forceCalibration = false;
bool manualMode = false;

int parkingRawValue = 0;
uint16_t storedSeatPositionMs = 0;
long currentPosMs = 0;
long targetPosMs = 0;
long downSpeedRemainder = 0;
unsigned long downEdgeMarginTime = DOWN_EDGE_MARGIN_MS;
bool downEdgeMarginActive = false;

unsigned long lastTime = 0;
unsigned long stabilizeStartTime = 0;
unsigned long ignitionOffStartTime = 0;
unsigned long calibrationPhaseStartTime = 0;
unsigned long pendingDriveModeStartTime = 0;
unsigned long relayDeadtimeStartTime = 0;
unsigned long lastDebugPrintTime = 0;

int currentSeatAction = 0;
int requestedSeatAction = 0;
int lastStoppedSeatAction = 0;

bool serialIntroPrinted = false;
bool debugEnabled = false;
bool manualStepStatusPending = false;

MCP_CAN chaegeonCan(CAN_CS_PIN);
bool chaegeonCanReady = false;
byte chaegeonCanSeq = 0;
unsigned long lastCanHeartbeatTime = 0;

bool consoleAlive = false;
bool consoleBusy = false;
bool consoleStartupApplyDone = false;
bool previousConsoleIgnitionOn = false;
bool consoleIgnitionApplyPending = false;
bool consoleCommandInFlight = false;
byte consoleCommandSeq = 0;
byte consoleCommandInFlightSeq = 0;
byte consoleLastAcceptedSeq = 0;
byte consoleActualClimatePack = 0;
byte consoleActualToggleBits = 0;
byte consoleButtonBits = 0;
byte consolePreviousButtonBits = 0;
byte consoleClimateTarget[CONSOLE_CLIMATE_COUNT] = {0, 0, 0, 0};
bool consoleToggleTarget[CONSOLE_TOGGLE_COUNT] = {false, false, false};
bool consoleManualLearnPending[CONSOLE_CHANNEL_COUNT] = {false};
unsigned long consoleManualLearnTime[CONSOLE_CHANNEL_COUNT] = {0};
unsigned long lastConsoleStatusTime = 0;
unsigned long lastConsoleCommandTime = 0;
unsigned long lastConsoleClimateRefreshTime = 0;
unsigned long lastConsoleSteeringRestartTime = 0;
byte consolePulseQueueChannel[CONSOLE_PULSE_QUEUE_SIZE] = {0};
unsigned int consolePulseQueueOnMs[CONSOLE_PULSE_QUEUE_SIZE] = {0};
unsigned int consolePulseQueueGapMs[CONSOLE_PULSE_QUEUE_SIZE] = {0};
byte consolePulseQueueHead = 0;
byte consolePulseQueueTail = 0;

void setup() {
  Serial.begin(115200);
  Serial.setTimeout(25);

  pinMode(parkingPin, INPUT);
  pinMode(ignitionPin, INPUT);
  pinMode(seatRelayD8, OUTPUT);
  pinMode(seatRelayD9, OUTPUT);

  applySeatAction(0);
  seat(0);
  setupChaegeonCan();
  loadSettings();

  measurement();
  confirmedDriveMode = getRawDriveMode();
  pendingDriveMode = confirmedDriveMode;
  targetPosMs = getTargetForConfirmedMode();
  if (confirmedDriveMode == HIGH) {
    // USB serial reconnect can reset the board while the car is already out of P.
    // In that case, assume the seat is already at the saved driving position.
    currentPosMs = storedSeatPositionMs;
    targetPosMs = storedSeatPositionMs;
  }
  if (ignitionState == LOW) {
    ignitionOffStartTime = millis();
  }

  lastTime = millis();
}

void loop() {
  measurement();
  handleSerial();

  unsigned long currentTime = millis();
  unsigned long deltaTime = currentTime - lastTime;
  lastTime = currentTime;

  if (prevIgnitionState == LOW && ignitionState == HIGH) {
    isStabilizing = true;
    stabilizeStartTime = currentTime;
  } else if (prevIgnitionState == HIGH && ignitionState == LOW) {
    registerIgnitionOffUse(currentTime);
  }
  prevIgnitionState = ignitionState;
  updateChaegeonCan(currentTime);

  if (autoCalibrationState != CALIBRATION_IDLE) {
    updateAutoCalibration(currentTime);
    updateRelayOutput(currentTime);
    updateDebugOutput(currentTime);
    return;
  }

  if (autoCalibrationPending && ignitionState == LOW &&
      currentTime - ignitionOffStartTime >= AUTO_CALIBRATION_DELAY_MS) {
    startAutoCalibration(currentTime);
    updateRelayOutput(currentTime);
    updateDebugOutput(currentTime);
    return;
  }

  if (isStabilizing) {
    if (currentTime - stabilizeStartTime < IGNITION_STABILIZE_MS) {
      seat(0);
      updateRelayOutput(currentTime);
      updateDebugOutput(currentTime);
      return;
    }
    isStabilizing = false;
  }

  updateConfirmedDriveMode(currentTime);
  if (!manualMode) {
    targetPosMs = getTargetForConfirmedMode();
  }
  normalizeMotionState();
  updateMovement(deltaTime);
  normalizeMotionState();
  updateRelayOutput(currentTime);
  updateManualStepStatus();
  updateDebugOutput(currentTime);
}

void measurement() {
  ignitionState = digitalRead(ignitionPin);

  parkingRawValue = analogRead(parkingPin);
  if (parkingState == LOW && parkingRawValue >= PARKING_ON_THRESHOLD) {
    parkingState = HIGH;
  } else if (parkingState == HIGH && parkingRawValue <= PARKING_OFF_THRESHOLD) {
    parkingState = LOW;
  }
}

boolean getRawDriveMode() {
  if (ignitionState == HIGH && parkingState == LOW) {
    return HIGH;
  }
  return LOW;
}

long getTargetForConfirmedMode() {
  if (confirmedDriveMode == HIGH) {
    return storedSeatPositionMs;
  }
  return 0;
}

void updateConfirmedDriveMode(unsigned long currentTime) {
  boolean rawDriveMode = getRawDriveMode();

  if (rawDriveMode != pendingDriveMode) {
    pendingDriveMode = rawDriveMode;
    pendingDriveModeStartTime = currentTime;
  }

  unsigned long confirmMs = pendingDriveMode == HIGH ? DRIVE_CONFIRM_MS : PARK_CONFIRM_MS;
  if (pendingDriveMode != confirmedDriveMode &&
      currentTime - pendingDriveModeStartTime >= confirmMs) {
    confirmedDriveMode = pendingDriveMode;
    manualMode = false;
    manualStepStatusPending = false;
  }
}

void updateMovement(unsigned long deltaTime) {
  if (downEdgeMarginActive) {
    if (currentPosMs == 0 && targetPosMs == 0) {
      seat(-1);
      downEdgeMarginTime += deltaTime;
      if (downEdgeMarginTime >= DOWN_EDGE_MARGIN_MS) {
        downEdgeMarginTime = DOWN_EDGE_MARGIN_MS;
        downEdgeMarginActive = false;
        seat(0);
      }
      return;
    }

    downEdgeMarginActive = false;
    downEdgeMarginTime = DOWN_EDGE_MARGIN_MS;
  }

  if (currentPosMs < targetPosMs) {
    seat(1);
    currentPosMs += deltaTime;
    if (currentPosMs >= targetPosMs) {
      currentPosMs = targetPosMs;
      seat(0);
    }
  } else if (currentPosMs > targetPosMs) {
    seat(-1);
    long downMoveScaled = deltaTime * DOWN_SPEED_NUM + downSpeedRemainder;
    currentPosMs -= downMoveScaled / DOWN_SPEED_DEN;
    downSpeedRemainder = downMoveScaled % DOWN_SPEED_DEN;
    if (currentPosMs <= targetPosMs) {
      currentPosMs = targetPosMs;
      downSpeedRemainder = 0;
      if (targetPosMs == 0) {
        startDownEdgeMargin();
      } else {
        seat(0);
      }
    }
  } else {
    seat(0);
  }
}

void startDownEdgeMargin() {
  downEdgeMarginActive = true;
  downEdgeMarginTime = 0;
  seat(-1);
}

void registerIgnitionOffUse(unsigned long currentTime) {
  ignitionOffStartTime = currentTime;

  if (useCount < USES_BEFORE_AUTO_CALIBRATION) {
    useCount++;
    EEPROM.update(EEPROM_USE_COUNT_ADDR, useCount);
  }

  if (useCount >= USES_BEFORE_AUTO_CALIBRATION) {
    autoCalibrationPending = true;
  }
}

void startAutoCalibration(unsigned long currentTime) {
  manualMode = false;
  manualStepStatusPending = false;
  downSpeedRemainder = 0;
  downEdgeMarginActive = false;
  downEdgeMarginTime = DOWN_EDGE_MARGIN_MS;
  autoCalibrationState = CALIBRATION_UP;
  calibrationPhaseStartTime = currentTime;
  seat(1);
}

void updateAutoCalibration(unsigned long currentTime) {
  if (ignitionState == HIGH && !forceCalibration) {
    seat(0);
    autoCalibrationState = CALIBRATION_IDLE;
    manualStepStatusPending = false;
    downSpeedRemainder = 0;
    downEdgeMarginActive = false;
    downEdgeMarginTime = DOWN_EDGE_MARGIN_MS;
    ignitionOffStartTime = currentTime;
    return;
  }

  if (autoCalibrationState == CALIBRATION_UP) {
    if (currentTime - calibrationPhaseStartTime >= AUTO_CALIBRATION_UP_MS) {
      autoCalibrationState = CALIBRATION_DOWN;
      calibrationPhaseStartTime = currentTime;
      seat(-1);
    } else {
      seat(1);
    }
  } else if (autoCalibrationState == CALIBRATION_DOWN) {
    if (currentTime - calibrationPhaseStartTime >= AUTO_CALIBRATION_DOWN_MS) {
      seat(0);
      autoCalibrationState = CALIBRATION_IDLE;
      autoCalibrationPending = false;
      forceCalibration = false;
      useCount = 0;
      EEPROM.update(EEPROM_USE_COUNT_ADDR, useCount);
      currentPosMs = 0;
      targetPosMs = 0;
      downSpeedRemainder = 0;
      downEdgeMarginActive = false;
      downEdgeMarginTime = DOWN_EDGE_MARGIN_MS;
    } else {
      seat(-1);
    }
  }
}

void handleSerial() {
  if (!Serial.available()) {
    return;
  }

  String command = Serial.readStringUntil('\n');
  command.trim();

  if (debugEnabled) {
    command.toLowerCase();
    if (command == "x") {
      debugEnabled = false;
      Serial.println(F("DEBUG_STOPPED"));
      printStatus(F("STATUS"));
    }
    return;
  }

  if (!serialIntroPrinted) {
    if (command.length() == 0) {
      printIntro();
      serialIntroPrinted = true;
    }
    return;
  }

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

  processSerialCommand(command);
}

bool isSeatCommandBusy() {
  return manualStepStatusPending ||
         autoCalibrationState != CALIBRATION_IDLE ||
         downEdgeMarginActive ||
         currentPosMs != targetPosMs ||
         requestedSeatAction != 0 ||
         currentSeatAction != 0;
}

void processSerialCommand(String command) {
  command.trim();
  command.toLowerCase();
  if (command.length() == 0) {
    printStatus(F("EMPTY_INPUT_STATUS"));
    return;
  }

  if (command.length() != 1) {
    Serial.println(F("ERR: one-character command only. Send h for help."));
    printStatus(F("ACK"));
    return;
  }

  char name = command.charAt(0);

  if (requiresParkingForCommand(name) && parkingState != HIGH) {
    Serial.println(F("ERR: check gear state. Command requires P."));
    printStatus(F("GEAR_CHECK_FAILED"));
    return;
  }

  if (isSeatCommandBusy()) {
    Serial.println(F("ERR: busy. Wait until seat movement is done."));
    printStatus(F("BUSY"));
    return;
  }

  if (name == 'h') {
    printIntro();
  } else if (name == 's') {
    printStatus(F("STATUS"));
  } else if (name == 'p') {
    saveCurrentPositionCommand();
  } else if (name == 'u') {
    handleRelativeStep(1);
  } else if (name == 'd') {
    handleRelativeStep(-1);
  } else if (name == 'g') {
    debugEnabled = true;
    lastDebugPrintTime = 0;
    printStatus(F("DEBUG_STARTED_X_TO_STOP"));
  } else if (name == 'a') {
    manualStepStatusPending = false;
    manualMode = false;
    targetPosMs = getTargetForConfirmedMode();
    printStatus(F("AUTO_MODE_RESTORED"));
  } else if (name == 'c') {
    manualStepStatusPending = false;
    forceCalibration = true;
    autoCalibrationPending = false;
    startAutoCalibration(millis());
    printStatus(F("CALIBRATION_STARTED_UP7_DOWN6"));
  } else if (name == 'r') {
    useCount = 0;
    autoCalibrationPending = false;
    EEPROM.update(EEPROM_USE_COUNT_ADDR, useCount);
    printStatus(F("USE_COUNT_RESET"));
  } else {
    Serial.println(F("Unknown command. Send h for help."));
    printStatus(F("ACK"));
  }
}

bool requiresParkingForCommand(char name) {
  return name == 'a' ||
         name == 'c' ||
         name == 'r';
}

void saveCurrentPositionCommand() {
  currentPosMs = clampPosition(currentPosMs);
  targetPosMs = currentPosMs;

  storedSeatPositionMs = (uint16_t)currentPosMs;
  writeUint16(EEPROM_SEAT_POSITION_ADDR, storedSeatPositionMs);
  manualMode = true;
  targetPosMs = currentPosMs;
  printStatus(F("CURRENT_POSITION_SAVED"));
}

void handleRelativeStep(int direction) {
  long nextTarget = currentPosMs + (direction * MANUAL_STEP_MS);
  if (nextTarget < 0 || nextTarget > SEAT_HARD_LIMIT_UP_MS) {
    Serial.print(F("WARN: ignored. Requested target "));
    Serial.print(nextTarget);
    Serial.print(F("ms is outside 0.."));
    Serial.print(SEAT_HARD_LIMIT_UP_MS);
    Serial.println(F("ms."));
    printStatus(F("ACK"));
    return;
  }

  manualMode = true;
  targetPosMs = nextTarget;
  manualStepStatusPending = true;
}

void updateManualStepStatus() {
  if (!manualStepStatusPending) {
    return;
  }

  if (currentPosMs == targetPosMs && requestedSeatAction == 0 && currentSeatAction == 0) {
    manualStepStatusPending = false;
    printStatus(F("MANUAL_STEP_DONE"));
  }
}

long clampPosition(long value) {
  if (value < 0) {
    return 0;
  }
  if (value > SEAT_HARD_LIMIT_UP_MS) {
    return SEAT_HARD_LIMIT_UP_MS;
  }
  return value;
}

void normalizeMotionState() {
  currentPosMs = clampPosition(currentPosMs);
  targetPosMs = clampPosition(targetPosMs);

  if (storedSeatPositionMs > SEAT_HARD_LIMIT_UP_MS) {
    storedSeatPositionMs = SEAT_HARD_LIMIT_UP_MS;
  }

  if (targetPosMs != 0 && downEdgeMarginActive) {
    downEdgeMarginActive = false;
    downEdgeMarginTime = DOWN_EDGE_MARGIN_MS;
  }

  if (currentPosMs <= targetPosMs) {
    downSpeedRemainder = 0;
  }
}

void updateDebugOutput(unsigned long currentTime) {
  if (!debugEnabled) {
    return;
  }

  if (lastDebugPrintTime == 0 || currentTime - lastDebugPrintTime >= DEBUG_INTERVAL_MS) {
    lastDebugPrintTime = currentTime;
    printDebugLine();
  }
}

void loadSettings() {
  if (EEPROM.read(EEPROM_MAGIC_ADDR) != EEPROM_MAGIC_VALUE) {
    EEPROM.update(EEPROM_MAGIC_ADDR, EEPROM_MAGIC_VALUE);
    EEPROM.update(EEPROM_USE_COUNT_ADDR, 0);
    writeUint16(EEPROM_SEAT_POSITION_ADDR, SEAT_HARD_LIMIT_UP_MS);
  }

  useCount = EEPROM.read(EEPROM_USE_COUNT_ADDR);
  if (useCount > USES_BEFORE_AUTO_CALIBRATION) {
    useCount = 0;
    EEPROM.update(EEPROM_USE_COUNT_ADDR, useCount);
  }
  autoCalibrationPending = (useCount >= USES_BEFORE_AUTO_CALIBRATION);

  storedSeatPositionMs = readUint16(EEPROM_SEAT_POSITION_ADDR);
  if (storedSeatPositionMs > SEAT_HARD_LIMIT_UP_MS) {
    storedSeatPositionMs = SEAT_HARD_LIMIT_UP_MS;
    writeUint16(EEPROM_SEAT_POSITION_ADDR, storedSeatPositionMs);
  }

  loadConsoleSettings();
}

uint16_t readUint16(int addr) {
  byte lowByte = EEPROM.read(addr);
  byte highByte = EEPROM.read(addr + 1);
  return (uint16_t)lowByte | ((uint16_t)highByte << 8);
}

void writeUint16(int addr, uint16_t value) {
  EEPROM.update(addr, value & 0xFF);
  EEPROM.update(addr + 1, (value >> 8) & 0xFF);
}

void printIntro() {
  Serial.println();
  Serial.println(F("+--------------------------------------------------+"));
  Serial.println(F("| Auto Seat Serial Console                         |"));
  Serial.println(F("+--------------------------------------------------+"));
  Serial.println(F("| Units: milliseconds. 0 = bottom. 6000 = top cap. |"));
  Serial.println(F("| Normal auto move target is saved in EEPROM.      |"));
  Serial.println(F("| a/c/r commands require gear P for safety.        |"));
  Serial.println(F("| Serial output is quiet unless a command is used. |"));
  Serial.println(F("+--------------------------------------------------+"));
  Serial.println(F("Commands:"));
  Serial.println(F("  s                   Print current state once"));
  Serial.println(F("  p                   Save current position to EEPROM"));
  Serial.println(F("  u                   Manual up +500ms if inside 0..6000"));
  Serial.println(F("  d                   Manual down -500ms if inside 0..6000"));
  Serial.println(F("  g                   Debug every 1s; only x works in debug"));
  Serial.println(F("  x                   Stop debug mode only"));
  Serial.println(F("                      d=down, g=debug to avoid command overlap"));
  Serial.println(F("  a                   Return to gear-based automatic target"));
  Serial.println(F("  c                   Run calibration now: up 7s, down 6s, set 0"));
  Serial.println(F("  r                   Clear 10-use auto calibration counter"));
  Serial.println(F("  h                   Print this help"));
  Serial.println(F("+--------------------------------------------------+"));
  printStatus(F("CURRENT_SETTINGS"));
}

void printStatus(const __FlashStringHelper *tag) {
  Serial.println();
  Serial.print(F("["));
  Serial.print(tag);
  Serial.println(F("]"));
  printUptime();
  printNamedInt(F("parkingRawValue"), parkingRawValue);
  printNamedBool(F("ignition"), ignitionState == HIGH);
  printNamedBool(F("parking"), parkingState == HIGH);
  printNamedBool(F("rawDriveMode"), getRawDriveMode() == HIGH);
  printNamedBool(F("confirmedDriveMode"), confirmedDriveMode == HIGH);
  printNamedLong(F("currentPositionMs"), currentPosMs);
  printNamedLong(F("targetPositionMs"), targetPosMs);
  printNamedInt(F("savedPositionMs"), storedSeatPositionMs);
  printNamedInt(F("hardLimitPositionMs"), SEAT_HARD_LIMIT_UP_MS);
  printNamedInt(F("requestedRelayAction"), requestedSeatAction);
  printNamedInt(F("actualRelayAction"), currentSeatAction);
  printNamedBool(F("manualMode"), manualMode);
  printNamedBool(F("stabilizing"), isStabilizing);
  Serial.print(F("useCount="));
  Serial.print(useCount);
  Serial.print(F("/"));
  Serial.println(USES_BEFORE_AUTO_CALIBRATION);
  printNamedBool(F("autoCalibrationPending"), autoCalibrationPending);
  printNamedBool(F("forceCalibration"), forceCalibration);
  printNamedInt(F("autoCalibrationState"), autoCalibrationState);
  printAutoCalibrationDelayStatus();
  printConsoleStatusFields();
}

void printDebugLine() {
  Serial.println();
  Serial.println(F("[DEBUG]"));
  printUptime();
  printNamedInt(F("parkingRawValue"), parkingRawValue);
  printNamedBool(F("ignition"), ignitionState == HIGH);
  printNamedBool(F("parking"), parkingState == HIGH);
  printNamedBool(F("rawDriveMode"), getRawDriveMode() == HIGH);
  printNamedBool(F("confirmedDriveMode"), confirmedDriveMode == HIGH);
  printNamedLong(F("currentPositionMs"), currentPosMs);
  printNamedLong(F("targetPositionMs"), targetPosMs);
  printNamedInt(F("savedPositionMs"), storedSeatPositionMs);
  printNamedInt(F("hardLimitPositionMs"), SEAT_HARD_LIMIT_UP_MS);
  printNamedInt(F("requestedRelayAction"), requestedSeatAction);
  printNamedInt(F("actualRelayAction"), currentSeatAction);
  printNamedBool(F("manualMode"), manualMode);
  printNamedBool(F("stabilizing"), isStabilizing);
  Serial.print(F("useCount="));
  Serial.print(useCount);
  Serial.print(F("/"));
  Serial.println(USES_BEFORE_AUTO_CALIBRATION);
  printNamedBool(F("autoCalibrationPending"), autoCalibrationPending);
  printNamedBool(F("forceCalibration"), forceCalibration);
  printNamedInt(F("autoCalibrationState"), autoCalibrationState);
  printAutoCalibrationDelayStatus();
  printConsoleStatusFields();
}

void printUptime() {
  Serial.print(F("uptime="));
  printDuration(millis());
  Serial.println();
}

void printUptimeInline() {
  Serial.print(F("uptime="));
  printDuration(millis());
}

void printBool(bool value) {
  Serial.print(value ? F("true") : F("false"));
}

void printNamedBool(const __FlashStringHelper *name, bool value) {
  Serial.print(name);
  Serial.print(F("="));
  printBool(value);
  Serial.println();
}

void printNamedInt(const __FlashStringHelper *name, long value) {
  Serial.print(name);
  Serial.print(F("="));
  Serial.println(value);
}

void printNamedLong(const __FlashStringHelper *name, long value) {
  printNamedInt(name, value);
}

void printDuration(unsigned long totalMs) {
  unsigned long hours = totalMs / 3600000UL;
  totalMs %= 3600000UL;
  unsigned long minutes = totalMs / 60000UL;
  totalMs %= 60000UL;
  unsigned long seconds = totalMs / 1000UL;
  unsigned long milliseconds = totalMs % 1000UL;

  Serial.print(hours);
  Serial.print(F("h "));
  Serial.print(minutes);
  Serial.print(F("m "));
  Serial.print(seconds);
  Serial.print(F("s "));
  Serial.print(milliseconds);
  Serial.print(F("ms"));
}

void printAutoCalibrationDelayStatusInline() {
  if (useCount < USES_BEFORE_AUTO_CALIBRATION) {
    return;
  }

  Serial.print(F("autoCalibrationOffTimer="));
  if (ignitionState == HIGH) {
    Serial.print(F("waiting_for_ig_off"));
    return;
  }

  unsigned long elapsedMs = millis() - ignitionOffStartTime;
  unsigned long remainingMs = 0;
  if (elapsedMs < AUTO_CALIBRATION_DELAY_MS) {
    remainingMs = AUTO_CALIBRATION_DELAY_MS - elapsedMs;
  }

  Serial.print(F("elapsed "));
  printDuration(elapsedMs);
  Serial.print(F(" remaining "));
  printDuration(remainingMs);
}

void printAutoCalibrationDelayStatus() {
  if (useCount < USES_BEFORE_AUTO_CALIBRATION) {
    return;
  }

  printAutoCalibrationDelayStatusInline();
  Serial.println();
}

void printConsoleStatusFields() {
  printNamedBool(F("consoleCanReady"), chaegeonCanReady);
  printNamedBool(F("consoleAlive"), consoleAlive);
  printNamedBool(F("consoleBusy"), consoleBusy);
  printNamedInt(F("consolePulseQueueLength"), consolePulseQueueLength());
  printNamedInt(F("consoleDriverVentTarget"), consoleClimateTarget[CH_DRIVER_VENT]);
  printNamedInt(F("consoleDriverHeatTarget"), consoleClimateTarget[CH_DRIVER_HEAT]);
  printNamedInt(F("consolePassengerVentTarget"), consoleClimateTarget[CH_PASSENGER_VENT]);
  printNamedInt(F("consolePassengerHeatTarget"), consoleClimateTarget[CH_PASSENGER_HEAT]);
  printNamedBool(F("consoleSteeringHeatTarget"), consoleToggleTarget[TG_STEERING_HEAT]);
  printNamedBool(F("consoleFrontSensorTarget"), consoleToggleTarget[TG_FRONT_SENSOR]);
  printNamedBool(F("consoleVessTarget"), consoleToggleTarget[TG_VESS]);
  printNamedInt(F("consoleDriverVentActual"), getConsoleClimateActual(CH_DRIVER_VENT));
  printNamedInt(F("consoleDriverHeatActual"), getConsoleClimateActual(CH_DRIVER_HEAT));
  printNamedInt(F("consolePassengerVentActual"), getConsoleClimateActual(CH_PASSENGER_VENT));
  printNamedInt(F("consolePassengerHeatActual"), getConsoleClimateActual(CH_PASSENGER_HEAT));
  printNamedBool(F("consoleSteeringHeatActual"), getConsoleToggleActual(TG_STEERING_HEAT));
  printNamedBool(F("consoleFrontSensorActual"), getConsoleToggleActual(TG_FRONT_SENSOR));
  printNamedBool(F("consoleVessActual"), getConsoleToggleActual(TG_VESS));
}

void updateChaegeonCan(unsigned long currentTime) {
  updateChaegeonCanReceive(currentTime);
  updateConsoleAlive(currentTime);
  updateChaegeonCanHeartbeat(currentTime);
  updateConsoleManualLearn(currentTime);
  updateConsoleAutomation(currentTime);
  updateConsoleCommandSender(currentTime);
}

void updateChaegeonCanReceive(unsigned long currentTime) {
  if (!chaegeonCanReady) {
    return;
  }

  while (chaegeonCan.checkReceive() == CAN_MSGAVAIL) {
    unsigned long rxId = 0;
    byte len = 0;
    byte payload[8] = {0};
    if (chaegeonCan.readMsgBuf(&rxId, &len, payload) != CAN_OK) {
      return;
    }

    if (rxId == CAN_ID_CONSOLE_STATUS && len >= 6 && payload[0] == 1) {
      consoleBusy = (payload[1] & 0x02) != 0;
      consoleActualClimatePack = payload[2];
      consoleActualToggleBits = payload[3];
      consoleLastAcceptedSeq = payload[5];
      lastConsoleStatusTime = currentTime;
      consoleAlive = true;

      if (consoleCommandInFlight &&
          consoleLastAcceptedSeq == consoleCommandInFlightSeq) {
        consoleCommandInFlight = false;
      }

      handleConsoleButtonBits(payload[4], currentTime);
    }
  }
}

void updateConsoleAlive(unsigned long currentTime) {
  bool aliveNow = chaegeonCanReady &&
                  lastConsoleStatusTime != 0 &&
                  currentTime - lastConsoleStatusTime <= CONSOLE_CAN_TIMEOUT_MS;

  if (!aliveNow && consoleAlive) {
    consoleStartupApplyDone = false;
    consoleCommandInFlight = false;
    clearConsolePulseQueue();
  }

  consoleAlive = aliveNow;
}

void handleConsoleButtonBits(byte newButtonBits, unsigned long currentTime) {
  consolePreviousButtonBits = consoleButtonBits;
  consoleButtonBits = newButtonBits;

  for (byte channel = 0; channel < CONSOLE_CHANNEL_COUNT; channel++) {
    bool wasPressed = (consolePreviousButtonBits & (1 << channel)) != 0;
    bool isPressed = (consoleButtonBits & (1 << channel)) != 0;
    if (wasPressed && !isPressed) {
      consoleManualLearnPending[channel] = true;
      consoleManualLearnTime[channel] = currentTime + CONSOLE_MANUAL_LEARN_SETTLE_MS;
    }
  }
}

void updateConsoleManualLearn(unsigned long currentTime) {
  for (byte channel = 0; channel < CONSOLE_CHANNEL_COUNT; channel++) {
    if (!consoleManualLearnPending[channel]) {
      continue;
    }

    if ((long)(currentTime - consoleManualLearnTime[channel]) < 0) {
      continue;
    }

    consoleManualLearnPending[channel] = false;
    learnConsoleManualChannel(channel);
  }
}

void learnConsoleManualChannel(byte channel) {
  if (channel <= CH_PASSENGER_HEAT) {
    setConsoleClimateTarget(channel, getConsoleClimateActual(channel));
    return;
  }

  byte toggleIndex = channel - CH_STEERING_HEAT;
  if (toggleIndex < CONSOLE_TOGGLE_COUNT) {
    setConsoleToggleTarget(toggleIndex, getConsoleToggleActual(toggleIndex));
  }
}

void updateConsoleAutomation(unsigned long currentTime) {
  bool ignitionOn = ignitionState == HIGH;
  if (ignitionOn && !previousConsoleIgnitionOn) {
    consoleIgnitionApplyPending = true;
  }
  previousConsoleIgnitionOn = ignitionOn;

  if (!consoleAlive || !consoleReadyForNewSequence()) {
    return;
  }

  if (!consoleStartupApplyDone && currentTime >= CONSOLE_STARTUP_DELAY_MS) {
    applyConsoleClimateTargets(true);
    if (ignitionOn) {
      applyConsoleIgnitionTargets();
      consoleIgnitionApplyPending = false;
    }
    consoleStartupApplyDone = true;
    lastConsoleClimateRefreshTime = currentTime;
    lastConsoleSteeringRestartTime = currentTime;
    return;
  }

  if (ignitionOn && consoleIgnitionApplyPending) {
    applyConsoleIgnitionTargets();
    consoleIgnitionApplyPending = false;
    lastConsoleSteeringRestartTime = currentTime;
    previousConsoleIgnitionOn = ignitionOn;
    return;
  }

  if (currentTime - lastConsoleClimateRefreshTime >= CONSOLE_MEMORY_REFRESH_INTERVAL_MS) {
    applyConsoleClimateTargets(true);
    if (ignitionOn) {
      applyConsoleIgnitionTargets();
      consoleIgnitionApplyPending = false;
    }
    lastConsoleClimateRefreshTime = currentTime;
    return;
  }

  if (ignitionOn &&
      consoleToggleTarget[TG_STEERING_HEAT] &&
      currentTime - lastConsoleSteeringRestartTime >= CONSOLE_STEERING_RESTART_INTERVAL_MS) {
    restartConsoleSteeringHeat();
    lastConsoleSteeringRestartTime = currentTime;
  }
}

bool consoleReadyForNewSequence() {
  return !consoleBusy &&
         !consoleCommandInFlight &&
         consolePulseQueueHead == consolePulseQueueTail;
}

void applyConsoleClimateTargets(bool force) {
  for (byte climate = 0; climate < CONSOLE_CLIMATE_COUNT; climate++) {
    ensureConsoleClimateTarget(climate, force);
  }
}

void applyConsoleIgnitionTargets() {
  for (byte toggle = 0; toggle < CONSOLE_TOGGLE_COUNT; toggle++) {
    ensureConsoleToggleTarget(toggle);
  }
}

void ensureConsoleClimateTarget(byte climate, bool force) {
  byte target = consoleClimateTarget[climate];
  byte actual = getConsoleClimateActual(climate);

  if (target == 0) {
    if (actual != 0) {
      enqueueConsolePulse(climate, CONSOLE_CLIMATE_LONG_OFF_MS, CONSOLE_CLIMATE_LONG_OFF_GAP_MS);
    }
    return;
  }

  if (!force && actual == target) {
    return;
  }

  enqueueConsoleClimateSet(climate, target);
}

void ensureConsoleToggleTarget(byte toggle) {
  if (getConsoleToggleActual(toggle) == consoleToggleTarget[toggle]) {
    return;
  }

  enqueueConsolePulse(toggle + CH_STEERING_HEAT,
                      CONSOLE_SHORT_PRESS_MS,
                      CONSOLE_SHORT_PRESS_GAP_MS);
}

void restartConsoleSteeringHeat() {
  if (getConsoleToggleActual(TG_STEERING_HEAT)) {
    enqueueConsolePulse(CH_STEERING_HEAT,
                        CONSOLE_SHORT_PRESS_MS,
                        CONSOLE_STEERING_RESTART_GAP_MS);
    enqueueConsolePulse(CH_STEERING_HEAT,
                        CONSOLE_SHORT_PRESS_MS,
                        CONSOLE_SHORT_PRESS_GAP_MS);
  } else {
    enqueueConsolePulse(CH_STEERING_HEAT,
                        CONSOLE_SHORT_PRESS_MS,
                        CONSOLE_SHORT_PRESS_GAP_MS);
  }
}

void enqueueConsoleClimateSet(byte climate, byte targetLevel) {
  enqueueConsolePulse(climate, CONSOLE_CLIMATE_LONG_OFF_MS, CONSOLE_CLIMATE_LONG_OFF_GAP_MS);

  byte shortPresses = getConsoleShortPressCount(targetLevel);
  for (byte i = 0; i < shortPresses; i++) {
    enqueueConsolePulse(climate, CONSOLE_SHORT_PRESS_MS, CONSOLE_SHORT_PRESS_GAP_MS);
  }
}

byte getConsoleShortPressCount(byte level) {
  if (level == 3) {
    return 1;
  }
  if (level == 2) {
    return 2;
  }
  if (level == 1) {
    return 3;
  }
  return 0;
}

byte getConsoleClimateActual(byte climate) {
  return (consoleActualClimatePack >> (climate * 2)) & 0x03;
}

bool getConsoleToggleActual(byte toggle) {
  return (consoleActualToggleBits & (1 << toggle)) != 0;
}

void setConsoleClimateTarget(byte climate, byte level) {
  if (level > 3) {
    level = 3;
  }

  bool changed = false;
  if (consoleClimateTarget[climate] != level) {
    consoleClimateTarget[climate] = level;
    changed = true;
  }

  if (level > 0) {
    byte opposite = CONSOLE_OPPOSITE_CLIMATE[climate];
    if (consoleClimateTarget[opposite] != 0) {
      consoleClimateTarget[opposite] = 0;
      changed = true;
    }
  }

  if (changed) {
    saveConsoleSettings();
  }
}

void setConsoleToggleTarget(byte toggle, bool enabled) {
  if (consoleToggleTarget[toggle] == enabled) {
    return;
  }

  consoleToggleTarget[toggle] = enabled;
  saveConsoleSettings();
}

void updateConsoleCommandSender(unsigned long currentTime) {
  if (!chaegeonCanReady || !consoleAlive || consoleBusy) {
    return;
  }

  if (consoleCommandInFlight) {
    if (currentTime - lastConsoleCommandTime >= CONSOLE_COMMAND_HOLD_MS) {
      consoleCommandInFlight = false;
    }
    return;
  }

  if (consolePulseQueueHead == consolePulseQueueTail) {
    return;
  }

  byte channel = consolePulseQueueChannel[consolePulseQueueHead];
  unsigned int onMs = consolePulseQueueOnMs[consolePulseQueueHead];
  unsigned int gapMs = consolePulseQueueGapMs[consolePulseQueueHead];

  byte seq = nextConsoleCommandSeq();
  byte payload[8] = {
    1,
    1,
    channel,
    (byte)(onMs & 0xFF),
    (byte)(onMs >> 8),
    (byte)(gapMs & 0xFF),
    (byte)(gapMs >> 8),
    seq
  };

  if (chaegeonCan.sendMsgBuf(CAN_ID_CONSOLE_COMMAND, 0, 8, payload) == CAN_OK) {
    consolePulseQueueHead = nextConsoleQueueIndex(consolePulseQueueHead);
    consoleCommandInFlight = true;
    consoleCommandInFlightSeq = seq;
    lastConsoleCommandTime = currentTime;
  }
}

byte nextConsoleCommandSeq() {
  consoleCommandSeq++;
  if (consoleCommandSeq == 0) {
    consoleCommandSeq = 1;
  }
  return consoleCommandSeq;
}

void enqueueConsolePulse(byte channel, unsigned int onMs, unsigned int gapMs) {
  byte nextTail = nextConsoleQueueIndex(consolePulseQueueTail);
  if (nextTail == consolePulseQueueHead || channel >= CONSOLE_CHANNEL_COUNT) {
    return;
  }

  consolePulseQueueChannel[consolePulseQueueTail] = channel;
  consolePulseQueueOnMs[consolePulseQueueTail] = onMs;
  consolePulseQueueGapMs[consolePulseQueueTail] = gapMs;
  consolePulseQueueTail = nextTail;
}

void clearConsolePulseQueue() {
  consolePulseQueueHead = consolePulseQueueTail;
}

byte consolePulseQueueLength() {
  if (consolePulseQueueTail >= consolePulseQueueHead) {
    return consolePulseQueueTail - consolePulseQueueHead;
  }
  return CONSOLE_PULSE_QUEUE_SIZE - consolePulseQueueHead + consolePulseQueueTail;
}

byte nextConsoleQueueIndex(byte index) {
  index++;
  if (index >= CONSOLE_PULSE_QUEUE_SIZE) {
    index = 0;
  }
  return index;
}

void loadConsoleSettings() {
  if (EEPROM.read(EEPROM_CONSOLE_MAGIC_ADDR) != EEPROM_CONSOLE_MAGIC_VALUE) {
    EEPROM.update(EEPROM_CONSOLE_MAGIC_ADDR, EEPROM_CONSOLE_MAGIC_VALUE);
    for (byte i = 0; i < CONSOLE_CLIMATE_COUNT; i++) {
      EEPROM.update(EEPROM_CONSOLE_CLIMATE_TARGET_ADDR + i, 0);
    }
    for (byte i = 0; i < CONSOLE_TOGGLE_COUNT; i++) {
      EEPROM.update(EEPROM_CONSOLE_TOGGLE_TARGET_ADDR + i, 0);
    }
  }

  for (byte i = 0; i < CONSOLE_CLIMATE_COUNT; i++) {
    byte value = EEPROM.read(EEPROM_CONSOLE_CLIMATE_TARGET_ADDR + i);
    consoleClimateTarget[i] = value <= 3 ? value : 0;
  }
  for (byte i = 0; i < CONSOLE_TOGGLE_COUNT; i++) {
    consoleToggleTarget[i] = EEPROM.read(EEPROM_CONSOLE_TOGGLE_TARGET_ADDR + i) != 0;
  }

  if (consoleClimateTarget[CH_DRIVER_VENT] > 0 &&
      consoleClimateTarget[CH_DRIVER_HEAT] > 0) {
    consoleClimateTarget[CH_DRIVER_HEAT] = 0;
  }
  if (consoleClimateTarget[CH_PASSENGER_VENT] > 0 &&
      consoleClimateTarget[CH_PASSENGER_HEAT] > 0) {
    consoleClimateTarget[CH_PASSENGER_HEAT] = 0;
  }
  saveConsoleSettings();
}

void saveConsoleSettings() {
  for (byte i = 0; i < CONSOLE_CLIMATE_COUNT; i++) {
    EEPROM.update(EEPROM_CONSOLE_CLIMATE_TARGET_ADDR + i, consoleClimateTarget[i]);
  }
  for (byte i = 0; i < CONSOLE_TOGGLE_COUNT; i++) {
    EEPROM.update(EEPROM_CONSOLE_TOGGLE_TARGET_ADDR + i, consoleToggleTarget[i] ? 1 : 0);
  }
}

void setupChaegeonCan() {
  if (chaegeonCan.begin(MCP_ANY, CAN_125KBPS, MCP_8MHZ) == CAN_OK) {
    chaegeonCan.setMode(MCP_NORMAL);
    chaegeonCanReady = true;
  }
}

void updateChaegeonCanHeartbeat(unsigned long currentTime) {
  if (!chaegeonCanReady) {
    return;
  }

  if (currentTime - lastCanHeartbeatTime < CAN_HEARTBEAT_INTERVAL_MS) {
    return;
  }
  lastCanHeartbeatTime = currentTime;

  byte flags = 0;
  if (ignitionState == HIGH) {
    flags |= 0x01;
  }
  if (parkingState == HIGH) {
    flags |= 0x02;
  }
  if (getRawDriveMode() == HIGH) {
    flags |= 0x04;
  }
  if (confirmedDriveMode == HIGH) {
    flags |= 0x08;
  }
  if (isStabilizing) {
    flags |= 0x10;
  }
  if (currentSeatAction != 0 || requestedSeatAction != 0) {
    flags |= 0x20;
  }
  if (autoCalibrationPending || autoCalibrationState != CALIBRATION_IDLE) {
    flags |= 0x40;
  }

  uint16_t raw = (uint16_t)parkingRawValue;
  uint16_t uptimeSeconds = (uint16_t)(currentTime / 1000UL);
  byte payload[8] = {
    1,
    flags,
    chaegeonCanSeq++,
    useCount,
    (byte)(raw & 0xFF),
    (byte)(raw >> 8),
    (byte)(uptimeSeconds & 0xFF),
    (byte)(uptimeSeconds >> 8)
  };

  chaegeonCan.sendMsgBuf(CAN_ID_MAIN_HEARTBEAT, 0, 8, payload);
}

void setRelay(int relayPin, boolean state) {
  digitalWrite(relayPin, state ? LOW : HIGH);
}

void seat(int action) {
  requestedSeatAction = action;
}

void updateRelayOutput(unsigned long currentTime) {
  if (requestedSeatAction == currentSeatAction) {
    applySeatAction(currentSeatAction);
    return;
  }

  if (currentSeatAction != 0) {
    lastStoppedSeatAction = currentSeatAction;
    currentSeatAction = 0;
    relayDeadtimeStartTime = currentTime;
    applySeatAction(0);
    return;
  }

  bool sameDirectionResume = requestedSeatAction != 0 && requestedSeatAction == lastStoppedSeatAction;
  bool deadtimeDone = currentTime - relayDeadtimeStartTime >= RELAY_DEADTIME_MS;
  if (sameDirectionResume || deadtimeDone) {
    currentSeatAction = requestedSeatAction;
  }

  applySeatAction(currentSeatAction);
}

void applySeatAction(int action) {
  if (action == 1) {
    setRelay(seatRelayD8, 1);
    setRelay(seatRelayD9, 0);
  } else if (action == -1) {
    setRelay(seatRelayD8, 0);
    setRelay(seatRelayD9, 1);
  } else {
    setRelay(seatRelayD8, 0);
    setRelay(seatRelayD9, 0);
  }
}