/*
  Nixie Vintage Dashboard (Gear Indicator) - ESP32 + Wheel Pulses + OBD (ELM327)
  ----------------------------------------------------------------------------
  This sketch turns an ESP32 into a motorcycle gear indicator with a vintage twist:
  - The engaged gear (1..5) is shown on a real cold-cathode Nixie tube.
  - Engine coolant temperature is shown on a TM1637 4-digit 7-segment display.

  Core idea (why it works):
  - The ECU provides engine RPM via OBD (PID 010C) through a Bluetooth ELM327 dongle.
  - The wheel speed sensor provides a pulse train (not km/h).
  - For each gear, the ratio  ratio = RPM / wheelPulseFrequency  stays near a specific value.
    Lower gears produce higher ratios; higher gears produce lower ratios.

  Auto-learning:
  - On first boot (or after flash erase), the sketch enters LEARN mode.
  - You ride steadily in each gear; the sketch records a stable ratio value per gear and stores it
    in EEPROM (flash-backed). Once all gears are learned, it switches to RUN mode automatically.

  Resetting wrong learning:
  - If gears were learned incorrectly, re-upload the sketch with "Erase Flash" set to
    "All Flash Contents" so EEPROM/flash data is cleared and learning starts from scratch.

  Signal quality / stability:
  - Wheel pulses are measured using micros() to obtain the period between pulses.
    This is more stable at low speeds than counting pulses in a short time window.
  - Debounce + stale timeout + EMA smoothing reduce noise and jitter from real-world wiring/sensors.

   Project: Nixie Vintage Dashboard (Gear Indicator)
  Author: Gabriele De Cosimo
  Contact: gabridecotpnr@gmail.com
  Date: 12-01-2026
  License: MIT 
*/

#include <Arduino.h>
#include <BluetoothSerial.h>
#include <TM1637Display.h>
#include <EEPROM.h>

/* =======================
   PIN ASSIGNMENT
   ======================= */
// TM1637 4-digit display interface (CLK/DIO are generic GPIO lines)
#define CLK 27
#define DIO 25
TM1637Display display(CLK, DIO);

// NIXIE TUBE (5 cathodes used for gear 1..5)
// Each GPIO drives one transistor that enables a single Nixie cathode.
// Only one output should be ON at a time to avoid ghosting.
const int nixiePins[5] = {22, 1, 18, 16, 15}; // 1..5

// WHEEL ENCODER / WHEEL SPEED PULSE INPUT
// GPIO34 is input-only on ESP32 (no internal pullups/pulldowns), so an external pulldown is required.
#define WHEEL_PIN 34  // INPUT ONLY (external pulldown)

/* =======================
   BLUETOOTH / OBD
   ======================= */
// BluetoothSerial uses Classic BT SPP, which is what most ELM327 dongles implement.
BluetoothSerial SerialBT;

// Bluetooth pairing PIN for YOUR ELM327 dongle.
// Most ELM327 modules use "1234" or "0000" by default, but it depends on the exact device.
// Set this to the PIN required by your dongle (check its manual / label / phone pairing prompt).
static const char* BT_PIN = "1234";

// Bluetooth MAC address of YOUR specific ELM327 dongle.
// You must replace these 6 bytes with your dongle's real MAC address, otherwise the ESP32
// cannot connect in master mode. You can read the MAC from your phone Bluetooth settings
// (or with a BT scanner app) when the dongle is powered ON.
// Format note: a Bluetooth MAC is 6 bytes (48-bit). Here it is written in HEX, one byte per entry.
// Example: if your MAC is "66:1E:32:1E:F5:D7" you must write {0x66, 0x1E, 0x32, 0x1E, 0xF5, 0xD7}.
static uint8_t ELM_ADDR[6] = {0x66, 0x1E, 0x32, 0x1E, 0xF5, 0xD7};


/* =======================
   WHEEL: PERIOD MEASUREMENT USING micros()
   (better at low speed)
   ======================= */
// We measure time between two consecutive pulses (period in microseconds).
// This gives a stable speed estimate even when pulses are slow (low speed).
volatile uint32_t g_lastPulseUs  = 0;   // timestamp (us) of the last valid pulse edge
volatile uint32_t g_lastPeriodUs = 0;   // period (us) between last two valid pulses
volatile bool     g_periodValid  = false;

// Debounce filter: ignore pulses that arrive too fast to be real (noise/bounce).
// Tune depending on sensor + conditioning (typical range: 300..2000 us).
static const uint32_t PULSE_DEBOUNCE_US = 800;

// If no pulse arrives within this time, consider wheel speed = 0 (bike stopped or signal missing).
static const uint32_t PULSE_STALE_US = 900000; // 0.9s

// EMA smoothing on the measured period:
// alpha closer to 1.0 -> more responsive; closer to 0.0 -> more smoothing.
static const float PERIOD_ALPHA = 0.35f;

void IRAM_ATTR wheelISR() {
  // ISR: keep it short. We only capture timestamps and compute dt.
  uint32_t now = micros();
  uint32_t prev = g_lastPulseUs;
  g_lastPulseUs = now;

  if (prev != 0) {
    // Unsigned subtraction is wrap-safe with micros() overflow.
    uint32_t dt = now - prev;

    // Accept only realistic dt to reject noise and extreme outliers.
    if (dt > PULSE_DEBOUNCE_US && dt < 2000000UL) { // max 2s between pulses
      g_lastPeriodUs = dt;
      g_periodValid = true;
    }
  }
}

// Returns pulse frequency in Hz (PULSES per second, not wheel revolutions).
// This is fine because the algorithm learns and compares using the same signal.
float getWheelFreqHz() {
  // Copy volatile variables atomically to avoid tearing.
  uint32_t lp, per;
  bool valid;
  noInterrupts();
  lp = g_lastPulseUs;
  per = g_lastPeriodUs;
  valid = g_periodValid;
  interrupts();

  if (!valid || lp == 0) return 0.0f;

  // If the last pulse is too old, treat as stopped.
  uint32_t age = micros() - lp;
  if (age > PULSE_STALE_US) return 0.0f;

  // Filter the period with EMA to reduce jitter.
  static bool periodFiltInit = false;
  static float periodFiltUs = 0;

  if (!periodFiltInit) {
    periodFiltUs = (float)per;
    periodFiltInit = true;
  } else {
    periodFiltUs = periodFiltUs * (1.0f - PERIOD_ALPHA) + (float)per * PERIOD_ALPHA;
  }

  // Convert filtered period (us) to frequency (Hz).
  if (periodFiltUs <= 1.0f) return 0.0f;
  return 1000000.0f / periodFiltUs;
}

/* =======================
   PARAMETERS (tweak)
   ======================= */
// Main loop update rate (logic + displays)
static const uint32_t UPDATE_MS   = 200;

// In LEARN mode, wait this long between gears to let speed/throttle/clutch settle.
static const uint32_t SETTLE_MS   = 4000;

// Blink interval for the LEARN prompt on TM1637 + Nixie
static const uint32_t BLINK_MS    = 350;

// Ignore RPM below this threshold (idle can be unstable / not representative for ratio)
static const int      RPM_MIN     = 900;

// With period measurement we can allow a lower frequency threshold.
// Below this, the bike is almost stopped and ratios become unreliable.
static const float    FREQ_MIN_HZ = 0.8f;

// Gear matching tolerance in RUN mode (relative error, e.g. 0.10 = ±10%)
static const float    RATIO_TOL   = 0.10f;

// LEARN mode: number of stable consecutive samples required to accept a gear
static const int      LEARN_CONSECUTIVE_OK = 3;

// LEARN mode: max allowed change between consecutive ratios (relative), to ensure stability
static const float    LEARN_STABILITY_TOL  = 0.06f;

// Temperature does not need fast updates (reduces BT/ELM traffic)
static const uint32_t TEMP_READ_MS = 1200;

/* =======================
   PERSISTENCE
   ======================= */
// Simple "magic" header to detect if EEPROM contains valid data.
static const uint32_t MAGIC = 0x47454152; // 'GEAR'
static const int EEPROM_SIZE = 128;

// Stored calibration:
// - valid=1 means learning was completed successfully
// - ratio[1..5] holds the learned ratio per gear
struct Persist {
  uint32_t magic;
  uint8_t  valid;      // 1 = learning complete
  float    ratio[6];   // index 1..5 used
};

Persist P;

void loadPersist() {
  // ESP32 EEPROM library is flash-backed; EEPROM.begin() reserves a flash sector.
  EEPROM.begin(EEPROM_SIZE);
  EEPROM.get(0, P);

  // If magic or valid flag do not match, reset to defaults (forces LEARN mode).
  bool ok = (P.magic == MAGIC) && (P.valid == 1);
  if (!ok) {
    memset(&P, 0, sizeof(P));
    P.magic = MAGIC;
    P.valid = 0;
  }
}

void savePersist(bool markValid) {
  // Mark as valid only after all gears are learned.
  if (markValid) P.valid = 1;
  EEPROM.put(0, P);
  EEPROM.commit(); // actually writes to flash
}

/* =======================
   ELM327 UTILS
   ======================= */
// Drain any leftover characters from the BT buffer before sending a command.
void btFlush() { while (SerialBT.available()) SerialBT.read(); }

// Reads characters until the ELM327 prompt '>' appears, or timeout expires.
// ELM327 typically ends each command response with '>'.
bool readUntilPrompt(String &out, uint32_t timeoutMs) {
  out = "";
  uint32_t t0 = millis();
  while (millis() - t0 < timeoutMs) {
    while (SerialBT.available()) {
      char c = SerialBT.read();
      // Build a compact response string by ignoring CR/LF.
      if (c != '\r' && c != '\n') out += c;
      if (c == '>') return true;
    }
  }
  return false;
}

// Sends an AT/OBD command and captures the response up to the prompt.
bool elmCmd(const char* cmd, String &resp, uint32_t timeoutMs) {
  btFlush();
  SerialBT.print(cmd);
  SerialBT.print("\r");
  return readUntilPrompt(resp, timeoutMs);
}

// Basic ELM327 setup for cleaner parsing and automatic protocol selection.
void elmInit() {
  String r;
  elmCmd("ATZ",   r, 2000); // reset
  elmCmd("ATE0",  r, 1000); // echo off
  elmCmd("ATL0",  r, 1000); // linefeeds off
  elmCmd("ATS0",  r, 1000); // spaces off
  elmCmd("ATH0",  r, 1000); // headers off
  elmCmd("ATSP0", r, 2000); // automatic protocol
}

/* =======================
   RPM OBD (PID 010C)
   ======================= */
// Reads engine RPM from OBD.
// Response format contains "41 0C A B" (spaces may be disabled by ATS0).
// Standard OBD formula: RPM = ((A*256)+B)/4
bool obdReadRPM(int &rpmOut) {
  String r;
  if (!elmCmd("010C", r, 1500)) return false;

  r.toUpperCase();

  int p = r.indexOf("410C");
  if (p < 0) return false;
  if (r.length() < p + 8) return false;

  int A = strtol(r.substring(p + 4, p + 6).c_str(), nullptr, 16);
  int B = strtol(r.substring(p + 6, p + 8).c_str(), nullptr, 16);

  rpmOut = ((A * 256) + B) / 4;

  // Reject unrealistic values and very low RPM (idle zone is not good for gear ratio estimation).
  return (rpmOut > RPM_MIN && rpmOut < 15000);
}

/* =======================
   TEMP OBD (PID 0105: coolant)
   tempC = A - 40
   ======================= */
// Reads coolant temperature in °C.
// Standard OBD formula: tempC = A - 40
bool obdReadCoolantTempC(int &tempCOut) {
  String r;
  if (!elmCmd("0105", r, 1500)) return false;

  r.toUpperCase();

  int p = r.indexOf("4105");
  if (p < 0) return false;
  if (r.length() < p + 6) return false;

  int A = strtol(r.substring(p + 4, p + 6).c_str(), nullptr, 16);
  int t = A - 40;

  // Sanity check: reject obviously invalid values
  if (t < -50 || t > 220) return false;

  tempCOut = t;
  return true;
}

/* =======================
   NIXIE OUTPUT
   ======================= */
// Shows the gear on the Nixie by enabling exactly one cathode driver.
// If on=false or gear=0, all outputs are turned off.
void nixieShowGear(int gear, bool on = true) {
  // First, ensure all cathodes are off (prevents ghosting).
  for (int i = 0; i < 5; i++) digitalWrite(nixiePins[i], LOW);

  if (!on) return;

  // Enable one cathode corresponding to gear 1..5.
  if (gear >= 1 && gear <= 5) digitalWrite(nixiePins[gear - 1], HIGH);
}

/* =======================
   TM1637: "L   gear" (LEARN mode prompt)
   ======================= */
// Displays a blinking "L" and the target gear number to teach (1..5).
void showLearningPrompt(int gear, bool blinkOn) {
  uint8_t segs[4] = {0,0,0,0};
  segs[0] = blinkOn ? 0x38 : 0x00; // segment pattern for 'L'
  segs[3] = blinkOn ? display.encodeDigit(gear) : 0x00;
  display.setSegments(segs);
}

/* =======================
   TM1637: temperature "***C"
   ======================= */
// Segment helpers for displaying negative sign and 'C'
static const uint8_t SEG_DASH   = 0x40; // middle segment (G) used as '-'
static const uint8_t SEG_CHAR_C = 0x39; // approximate 'C'

void showTempC(int tempC, bool valid) {
  // If temperature is not valid, show dashes.
  if (!valid) {
    display.showNumberDecEx(0, 0b01000000, true); // ----
    return;
  }

  // Manual segment composition to show "XXXC" without using a full font table.
  uint8_t segs[4] = {0,0,0,0};
  segs[3] = SEG_CHAR_C;

  bool neg = (tempC < 0);
  int v = neg ? -tempC : tempC;
  if (v > 999) v = 999;

  int d0 = v / 100;
  int d1 = (v / 10) % 10;
  int d2 = v % 10;

  // Place digits right-aligned (supports 1..3 digits).
  if (v >= 100) {
    segs[0] = display.encodeDigit(d0);
    segs[1] = display.encodeDigit(d1);
    segs[2] = display.encodeDigit(d2);
  } else if (v >= 10) {
    segs[1] = display.encodeDigit(d1);
    segs[2] = display.encodeDigit(d2);
  } else {
    segs[2] = display.encodeDigit(d2);
  }

  // If negative, show '-' on the leftmost digit position.
  if (neg) segs[0] = SEG_DASH;

  display.setSegments(segs);
}

/* =======================
   GEAR ESTIMATE
   ======================= */
// Given the current ratio, choose the closest learned gear ratio.
// Uses relative error so the same tolerance works across different magnitudes.
int estimateGear(float ratio) {
  int best = 0;
  float bestErr = 999;

  for (int g = 1; g <= 5; g++) {
    if (P.ratio[g] <= 0.0f) continue;

    // Relative error: |measured - learned| / learned
    float err = fabsf(ratio - P.ratio[g]) / P.ratio[g];

    if (err < bestErr) { bestErr = err; best = g; }
  }

  // If nothing was learned or best match is too far, return 0 (unknown).
  if (best == 0) return 0;
  if (bestErr > RATIO_TOL) return 0;
  return best;
}

/* =======================
   MODE / LEARNING STATE
   ======================= */
// Two modes:
// - MODE_LEARN: acquire and store ratio per gear
// - MODE_RUN: show gear + temperature continuously
enum Mode { MODE_LEARN, MODE_RUN };
Mode mode;

// LEARN mode progress (which gear the user should teach now)
int learnGear = 1;
uint32_t learnStepStartMs = 0;

// Stability counters in LEARN mode
int consecOk = 0;
float lastRatio = 0;

/* =======================
   TEMP CACHE
   ======================= */
// Temperature is sampled slower than the main loop; cache last value.
int lastTempC = 0;
bool tempValid = false;
uint32_t lastTempReadMs = 0;

/* =======================
   SETUP
   ======================= */
void setup() {
  // Display init
  display.setBrightness(7);
  display.clear();

  // Configure Nixie cathode driver outputs
  for (int i = 0; i < 5; i++) pinMode(nixiePins[i], OUTPUT);

  // Wheel pulse input: external pulldown + interrupt on rising edge.
  // RISING is used because the signal is conditioned to produce clean positive pulses.
  pinMode(WHEEL_PIN, INPUT);
  attachInterrupt(digitalPinToInterrupt(WHEEL_PIN), wheelISR, RISING);

  // Load learned ratios (if present)
  loadPersist();

  // Bluetooth configuration for ELM327 connection
  SerialBT.setPin(BT_PIN);

  // Begin in master mode (true) so ESP32 actively connects to the known dongle MAC
  SerialBT.begin("GEAR_IND", true);

  // Hard fail if BT connection cannot be established (project requires ECU RPM).
  if (!SerialBT.connect(ELM_ADDR)) {
    display.showNumberDec(9999); // BT error indicator
    while (1) {}
  }

  // Initialize ELM327 to a predictable output format
  elmInit();

  // Decide mode: if ratios were learned and marked valid -> RUN, otherwise -> LEARN
  mode = (P.valid == 1) ? MODE_RUN : MODE_LEARN;

  if (mode == MODE_LEARN) {
    // Start learning from 1st gear
    learnGear = 1;
    learnStepStartMs = millis();
    consecOk = 0;
    lastRatio = 0;
  } else {
    // Force the first temperature read quickly
    lastTempReadMs = 0;
    tempValid = false;
  }
}

/* =======================
   LOOP
   ======================= */
void loop() {
  // Blink state for LEARN prompt
  static uint32_t lastBlink = 0;
  static bool blinkOn = true;
  if (millis() - lastBlink >= BLINK_MS) {
    lastBlink = millis();
    blinkOn = !blinkOn;
  }

  // Main scheduler: run heavy logic every UPDATE_MS
  static uint32_t lastUpdate = 0;
  uint32_t now = millis();

  // If it's not time to update yet, keep showing the LEARN prompt (fast UI refresh)
  if (now - lastUpdate < UPDATE_MS) {
    if (mode == MODE_LEARN) {
      showLearningPrompt(learnGear, blinkOn);
      nixieShowGear(learnGear, blinkOn);
    }
    return;
  }
  lastUpdate = now;

  // Read wheel pulse frequency (Hz) from period measurement
  float wheelFreq = getWheelFreqHz();

  // Read RPM from ECU via OBD
  int rpm = 0;
  bool okRpm = obdReadRPM(rpm);

  if (mode == MODE_LEARN) {
    // In LEARN mode we always show which gear we are teaching.
    showLearningPrompt(learnGear, blinkOn);
    nixieShowGear(learnGear, blinkOn);

    // Wait for the rider to stabilize speed/throttle after selecting the gear.
    if (millis() - learnStepStartMs < SETTLE_MS) return;

    // Learn only when the bike is actually moving and RPM is valid.
    if (okRpm && wheelFreq >= FREQ_MIN_HZ) {
      // Core measurement:
      // ratio = engine RPM / wheel pulse frequency
      float ratio = rpm / wheelFreq;

      // Sanity range: reject absurd ratios from glitches.
      if (ratio > 50.0f && ratio < 50000.0f) {
        // Require consecutive stable samples to accept a gear.
        if (consecOk == 0) {
          consecOk = 1;
          lastRatio = ratio;
        } else {
          // Stability check: ratio should not change too much between samples.
          float diff = fabsf(ratio - lastRatio) / lastRatio;

          if (diff <= LEARN_STABILITY_TOL) {
            // Stable: count it and slightly average to smooth noise.
            consecOk++;
            lastRatio = (lastRatio + ratio) * 0.5f;
          } else {
            // Not stable: restart the stability counter.
            consecOk = 1;
            lastRatio = ratio;
          }
        }

        // When enough stable samples are collected, store ratio for this gear.
        if (consecOk >= LEARN_CONSECUTIVE_OK) {
          P.ratio[learnGear] = lastRatio;

          // Move to the next gear
          learnGear++;
          learnStepStartMs = millis();
          consecOk = 0;
          lastRatio = 0;

          // If all gears are learned, mark data valid and switch to RUN mode.
          if (learnGear > 5) {
            savePersist(true);
            mode = MODE_RUN;

            // Force a temperature refresh after switching modes
            lastTempReadMs = 0;
            tempValid = false;

            // Clear outputs briefly
            nixieShowGear(0);
            display.showNumberDecEx(0, 0b01000000, true); // ----
          }
        }
      }
    }
    return;
  }

  /* ========= MODE_RUN =========
     In RUN mode:
     - Temperature is always displayed (even when stopped).
     - Gear is shown only when RPM and wheel speed are valid.
  */

  // Read temperature at a slower rate to reduce OBD traffic.
  if (millis() - lastTempReadMs >= TEMP_READ_MS) {
    lastTempReadMs = millis();
    int t;
    if (obdReadCoolantTempC(t)) {
      lastTempC = t;
      tempValid = true;
    } else {
      tempValid = false;
    }
  }

  // Always show the last known temperature state.
  showTempC(lastTempC, tempValid);

  // If RPM is not valid or wheel speed is too low, do not guess a gear.
  // Turn off the Nixie gear display but keep temperature visible.
  if (!okRpm || wheelFreq < FREQ_MIN_HZ) {
    nixieShowGear(0);
    return;
  }

  // Compute current ratio and estimate gear by comparing against learned values.
  float ratio = rpm / wheelFreq;
  int gear = estimateGear(ratio);
  nixieShowGear(gear);
}