// 2026-05-24 ChatGPT/Codex generated
// Exercise 26: BLE Discovery
// $Id$
// $HeadURL$
//
// Build:
//   cd /usr/local/src/microreticulum/microReticulumTbeam/exercises/26_Bluetooth_discover
//   pio run -e amy
//   pio run -e bob
//   pio run -e cy
//   pio run -e dan
//   pio run -e ed
//   pio run -e flo
//   pio run -e guy
//
// Upload/monitor example:
//   pio run -e bob -t upload
//   pio device monitor -b 115200 -p /dev/ttytBOB

#include <Arduino.h>
#include <BLEAdvertisedDevice.h>
#include <BLEDevice.h>
#include <BLEScan.h>
#include <BLEUtils.h>
#include <SD.h>
#include <TBeamDisplay.h>
#include <TBeamStorage.h>
#include <TBeamWeb.h>
#include <Wire.h>
#include <XPowersLib.h>
#include <algorithm>
#include <ctype.h>
#include <esp_system.h>
#include <math.h>
#include <stdarg.h>
#include <stdio.h>
#include <string.h>

#include "ClockDiscipline.h"
#include "GnssManager.h"
#include "GnssTypes.h"
#include "tbeam_supreme_adapter.h"

#ifndef BOARD_ID
#define BOARD_ID "CY"
#endif

#ifndef NODE_NAME
#define NODE_NAME BOARD_ID
#endif

#ifndef LOG_AP_IP_OCTET
#define LOG_AP_IP_OCTET 25
#endif

#ifndef BUTTON_PIN
#define BUTTON_PIN 0
#endif

namespace {

using field_qa::ClockDateTime;
using field_qa::ClockDiscipline;
using field_qa::GnssSample;

static constexpr const char* kAppTitle = "BLE Discovery";
static constexpr const char* kProjectPrefix = "TBMSND";
static constexpr uint8_t kProtocolVersion = 1;
static constexpr const char* kTimePayloadPrefix = "B2";
static constexpr uint32_t kStaleMs = 20000;
static constexpr uint32_t kDisplayPeriodMs = 1000;
static constexpr uint32_t kAdvertisePeriodMs = 1000;
static constexpr uint32_t kScanPeriodMs = 2500;
static constexpr uint32_t kScanWindowSeconds = 2;
static constexpr uint32_t kLogFlushPeriodMs = 1000;
static constexpr uint32_t kStatusLogPeriodMs = 10000;
static constexpr uint32_t kStartupStatusPeriodMs = 1000;
static constexpr uint32_t kGpsRefreshPeriodMs = 1000;
static constexpr uint32_t kMaxGpsFixAgeMs = 30000;
static constexpr uint32_t kMaxDisciplineAgeMs = 4UL * 60UL * 60UL * 1000UL;
static constexpr uint8_t kRssiWindow = 5;

struct NodeState {
  explicit NodeState(const char* nodeName) : name(nodeName) {}

  const char* name = "";
  bool self = false;
  bool heard = false;
  int lastRssi = 0;
  int samples[kRssiWindow] = {};
  uint8_t sampleCount = 0;
  uint8_t nextSample = 0;
  uint32_t seenCount = 0;
  uint32_t lastSeenMs = 0;
  uint32_t lastSeq = 0;
  int64_t lastTxPayloadEpochMs = 0;
};

XPowersLibInterface* g_pmu = nullptr;
tbeam::TBeamDisplay g_display;
tbeam::TBeamStorage g_storage(Serial);
tbeam::TBeamWeb g_web(Serial);
field_qa::GnssManager g_gnss;
field_qa::ClockDiscipline g_clock(Wire1);
BLEScan* g_scan = nullptr;

NodeState g_nodes[] = {
    NodeState("AMY"),
    NodeState("BOB"),
    NodeState("CY"),
    NodeState("DAN"),
    NodeState("ED"),
    NodeState("FLO"),
    NodeState("GUY"),
};

bool g_disciplined = false;
bool g_clockValid = false;
bool g_bleStarted = false;
bool g_sdReady = false;
bool g_logReady = false;
bool g_webReady = false;
bool g_buttonWasPressed = false;
bool g_prevGpsValid = false;
bool g_prevClockValid = false;
bool g_statusInitialized = false;
bool g_lastWriteOk = true;
uint8_t g_displayMode = 0;
uint32_t g_sequence = 0;
volatile uint32_t g_ppsEdgeCount = 0;
volatile uint32_t g_lastPpsMs = 0;
uint32_t g_lastAdvertiseMs = 0;
uint32_t g_lastScanMs = 0;
uint32_t g_lastDisplayMs = 0;
uint32_t g_lastFlushMs = 0;
uint32_t g_lastStatusLogMs = 0;
uint32_t g_lastGpsRefreshMs = 0;
uint32_t g_rowsWritten = 0;
int64_t g_epochBase = 0;
uint32_t g_epochBaseMs = 0;
int64_t g_lastDisciplineEpochMs = 0;
uint32_t g_lastDisciplineMillis = 0;
double g_latitude = 0.0;
double g_longitude = 0.0;
uint32_t g_gpsFixMs = 0;
bool g_hasLocation = false;
char g_logPath[128] = {};

void IRAM_ATTR onPpsEdge() {
  g_lastPpsMs = millis();
  ++g_ppsEdgeCount;
}

int nodeIndexFor(const char* name) {
  if (!name) {
    return -1;
  }
  for (size_t i = 0; i < sizeof(g_nodes) / sizeof(g_nodes[0]); ++i) {
    if (strcmp(g_nodes[i].name, name) == 0) {
      return (int)i;
    }
  }
  return -1;
}

bool isSelfName(const char* name) {
  return name && strcmp(name, NODE_NAME) == 0;
}

int averageRssi(const NodeState& node) {
  if (node.sampleCount == 0) {
    return 0;
  }
  int total = 0;
  for (uint8_t i = 0; i < node.sampleCount; ++i) {
    total += node.samples[i];
  }
  return (int)lroundf((float)total / (float)node.sampleCount);
}

uint32_t ageSeconds(const NodeState& node, uint32_t now) {
  if (!node.heard) {
    return 9999;
  }
  return (now - node.lastSeenMs) / 1000UL;
}

bool freshEnough(const NodeState& node, uint32_t now) {
  return node.heard && (uint32_t)(now - node.lastSeenMs) <= kStaleMs;
}

int64_t currentEpochMs();

int64_t currentEpoch() {
  return currentEpochMs() / 1000LL;
}

int64_t currentEpochMs() {
  if (!g_clockValid || g_epochBase <= 0) {
    return 0;
  }
  return (g_epochBase * 1000LL) + (int64_t)(millis() - g_epochBaseMs);
}

uint32_t gpsAgeMs(uint32_t now) {
  if (!g_hasLocation || g_gpsFixMs == 0) {
    return UINT32_MAX;
  }
  return now - g_gpsFixMs;
}

uint32_t disciplineAgeMs(uint32_t now) {
  if (!g_clockValid || g_lastDisciplineMillis == 0) {
    return UINT32_MAX;
  }
  return now - g_lastDisciplineMillis;
}

bool clockValidNow(uint32_t now) {
  g_clockValid = g_disciplined && g_lastDisciplineMillis > 0 && (uint32_t)(now - g_lastDisciplineMillis) <= kMaxDisciplineAgeMs;
  return g_clockValid;
}

bool gpsValidNow(uint32_t now) {
  return g_hasLocation && gpsAgeMs(now) <= kMaxGpsFixAgeMs;
}

int batteryMillivolts() {
  if (!g_pmu) {
    return -1;
  }
  return (int)g_pmu->getBattVoltage();
}

uint8_t heardTotal() {
  uint8_t count = 0;
  for (size_t i = 0; i < sizeof(g_nodes) / sizeof(g_nodes[0]); ++i) {
    if (!g_nodes[i].self && g_nodes[i].heard) {
      ++count;
    }
  }
  return count;
}

uint8_t heardFresh(uint32_t now) {
  uint8_t count = 0;
  for (size_t i = 0; i < sizeof(g_nodes) / sizeof(g_nodes[0]); ++i) {
    if (!g_nodes[i].self && freshEnough(g_nodes[i], now)) {
      ++count;
    }
  }
  return count;
}

void emitEvent(const char* event, const char* extra = nullptr) {
  char line[256];
  const int64_t epochMs = currentEpochMs();
  const int n = snprintf(line,
                         sizeof(line),
                         "# EVENT epoch_ms=%lld ms=%lu unit=%s event=%s vbat_mv=%d%s%s",
                         (long long)epochMs,
                         (unsigned long)millis(),
                         NODE_NAME,
                         event ? event : "unknown",
                         batteryMillivolts(),
                         (extra && extra[0]) ? " " : "",
                         (extra && extra[0]) ? extra : "");
  Serial.println(line);
  if (n > 0 && g_logReady && g_storage.isLogOpen()) {
    g_storage.println(line);
  }
}

void emitStatus(bool force = false) {
  const uint32_t now = millis();
  if (!force && (uint32_t)(now - g_lastStatusLogMs) < kStatusLogPeriodMs) {
    return;
  }
  g_lastStatusLogMs = now;

  const bool clockValid = clockValidNow(now);
  const bool gpsValid = gpsValidNow(now);
  char line[256];
  const int n = snprintf(line,
                         sizeof(line),
                         "# STATUS epoch_ms=%lld ms=%lu unit=%s clock_valid=%u gps_valid=%u gps_fix_age_ms=%lu discipline_age_ms=%lu last_discipline_epoch_ms=%lld sd_ready=%u log_ready=%u rows_written=%lu heard_total=%u heard_fresh=%u vbat_mv=%d",
                         (long long)currentEpochMs(),
                         (unsigned long)now,
                         NODE_NAME,
                         clockValid ? 1U : 0U,
                         gpsValid ? 1U : 0U,
                         (unsigned long)gpsAgeMs(now),
                         (unsigned long)disciplineAgeMs(now),
                         (long long)g_lastDisciplineEpochMs,
                         g_storage.ready() ? 1U : 0U,
                         (g_logReady && g_storage.isLogOpen()) ? 1U : 0U,
                         (unsigned long)g_rowsWritten,
                         heardTotal(),
                         heardFresh(now),
                         batteryMillivolts());
  Serial.println(line);
  if (n > 0 && g_logReady && g_storage.isLogOpen()) {
    const size_t expected = (size_t)n + 1U;
    const size_t wrote = g_storage.println(line);
    if (wrote != expected && g_lastWriteOk) {
      char extra[96];
      snprintf(extra, sizeof(extra), "bytes_expected=%lu bytes_written=%lu", (unsigned long)expected, (unsigned long)wrote);
      emitEvent("sd_write_short", extra);
      g_lastWriteOk = false;
    }
  }
}

void pollStatusEvents() {
  const uint32_t now = millis();
  const bool clockValid = clockValidNow(now);
  const bool gpsValid = gpsValidNow(now);
  if (!g_statusInitialized) {
    g_statusInitialized = true;
    g_prevClockValid = clockValid;
    g_prevGpsValid = gpsValid;
    return;
  }
  if (g_prevGpsValid && !gpsValid) {
    char extra[80];
    snprintf(extra, sizeof(extra), "gps_fix_age_ms=%lu", (unsigned long)gpsAgeMs(now));
    emitEvent("gps_stale", extra);
  } else if (!g_prevGpsValid && gpsValid) {
    char extra[80];
    snprintf(extra, sizeof(extra), "gps_fix_age_ms=%lu", (unsigned long)gpsAgeMs(now));
    emitEvent("gps_fresh", extra);
  }
  if (g_prevClockValid && !clockValid) {
    char extra[96];
    snprintf(extra, sizeof(extra), "discipline_age_ms=%lu", (unsigned long)disciplineAgeMs(now));
    emitEvent("clock_stale", extra);
  }
  g_prevGpsValid = gpsValid;
  g_prevClockValid = clockValid;
}

void refreshGpsPosition(bool force = false) {
  const uint32_t now = millis();
  if (!force && (uint32_t)(now - g_lastGpsRefreshMs) < kGpsRefreshPeriodMs) {
    return;
  }
  g_lastGpsRefreshMs = now;
  const GnssSample sample = g_gnss.makeSample();
  if (sample.validLocation && sample.validFix) {
    g_latitude = sample.latitude;
    g_longitude = sample.longitude;
    g_gpsFixMs = sample.sampleMillis;
    g_hasLocation = true;
  }
}

void formatDateTime(int64_t epoch, char* out, size_t outSize) {
  ClockDateTime dt{};
  if (epoch <= 0 || !ClockDiscipline::fromEpochSeconds(epoch, dt)) {
    snprintf(out, outSize, "UNKNOWN");
    return;
  }
  snprintf(out,
           outSize,
           "%04u-%02u-%02u %02u:%02u:%02u",
           (unsigned)dt.year,
           (unsigned)dt.month,
           (unsigned)dt.day,
           (unsigned)dt.hour,
           (unsigned)dt.minute,
           (unsigned)dt.second);
}

void formatCompactDateTime(int64_t epoch, char* out, size_t outSize) {
  ClockDateTime dt{};
  if (epoch <= 0 || !ClockDiscipline::fromEpochSeconds(epoch, dt)) {
    snprintf(out, outSize, "00000000_000000");
    return;
  }
  snprintf(out,
           outSize,
           "%04u%02u%02u_%02u%02u%02u",
           (unsigned)dt.year,
           (unsigned)dt.month,
           (unsigned)dt.day,
           (unsigned)dt.hour,
           (unsigned)dt.minute,
           (unsigned)dt.second);
}

void showLines(const char* l1,
               const char* l2 = nullptr,
               const char* l3 = nullptr,
               const char* l4 = nullptr,
               const char* l5 = nullptr,
               const char* l6 = nullptr) {
  g_display.setFont(tbeam::DisplayFont::SMALL);
  g_display.showLines(l1, l2, l3, l4, l5, l6);
}

void makeTitle(char* out, size_t outSize) {
  snprintf(out, outSize, "EX 26 %s %s", NODE_NAME, kAppTitle);
}

void showBootScreen(const char* phase) {
  char title[32];
  makeTitle(title, sizeof(title));
  showLines(title, "Take me outside", "Obtain GPS signal", phase, "BLE disabled");
}

void showGpsGateScreen(const GnssSample& sample, uint32_t attemptCount) {
  char title[32];
  char gpsLine[32];
  char fixLine[32];
  makeTitle(title, sizeof(title));
  snprintf(gpsLine,
           sizeof(gpsLine),
           "time:%c loc:%c fix:%c",
           sample.validTime ? 'Y' : 'N',
           sample.validLocation ? 'Y' : 'N',
           sample.validFix ? 'Y' : 'N');
  snprintf(fixLine, sizeof(fixLine), "sats:%d hdop:%.1f", sample.satsUsed, sample.hdop);
  showLines(title, "Take me outside", "Obtain GPS signal", gpsLine, fixLine, "BLE disabled");
  (void)attemptCount;
}

bool waitForPps(void*, uint32_t timeoutMs) {
  const uint32_t startEdges = g_ppsEdgeCount;
  const uint32_t startMs = millis();
  while ((uint32_t)(millis() - startMs) < timeoutMs) {
    g_gnss.poll();
    if (g_ppsEdgeCount != startEdges) {
      return true;
    }
    delay(2);
  }
  return false;
}

void printBootBanner() {
  Serial.println();
  Serial.println("==================================================");
  Serial.println("Exercise 26: BLE Discovery");
  Serial.println("==================================================");
  Serial.printf("node=%s\n", NODE_NAME);
  Serial.printf("prefix=%s version=%u\n", kProjectPrefix, (unsigned)kProtocolVersion);
  Serial.printf("known_nodes=AMY,BOB,CY,DAN,ED,FLO,GUY\n");
  Serial.printf("stale_timeout_ms=%lu\n", (unsigned long)kStaleMs);
  Serial.printf("scan_window_s=%lu scan_period_ms=%lu\n",
                (unsigned long)kScanWindowSeconds,
                (unsigned long)kScanPeriodMs);
  Serial.printf("advertise_period_ms=%lu\n", (unsigned long)kAdvertisePeriodMs);
  Serial.printf("clock_disciplined=%u time_source=GPS_PPS_RTC_REQUIRED\n", g_clockValid ? 1U : 0U);
  Serial.printf("max_gps_fix_age_ms=%lu max_discipline_age_ms=%lu\n",
                (unsigned long)kMaxGpsFixAgeMs,
                (unsigned long)kMaxDisciplineAgeMs);
  Serial.printf("web_url=http://192.168.%u.1/\n", (unsigned)LOG_AP_IP_OCTET);
}

void markSelfNode() {
  for (size_t i = 0; i < sizeof(g_nodes) / sizeof(g_nodes[0]); ++i) {
    g_nodes[i].self = isSelfName(g_nodes[i].name);
  }
}

void pollStorageWeb();

bool disciplineStartupClock() {
  uint32_t lastStatusMs = millis() - kStartupStatusPeriodMs;
  uint32_t attemptCount = 0;
  while (true) {
    g_gnss.poll();
    const GnssSample sample = g_gnss.makeSample();
    const bool ready = sample.validTime && sample.validLocation && sample.validFix;

    const uint32_t now = millis();
    if ((uint32_t)(now - lastStatusMs) >= kStartupStatusPeriodMs) {
      lastStatusMs = now;
      showGpsGateScreen(sample, attemptCount);
      Serial.printf("startup_gate node=%s gps_time=%u gps_loc=%u gps_fix=%u sats=%d hdop=%.1f attempts=%lu\n",
                    NODE_NAME,
                    sample.validTime ? 1U : 0U,
                    sample.validLocation ? 1U : 0U,
                    sample.validFix ? 1U : 0U,
                    sample.satsUsed,
                    sample.hdop,
                    (unsigned long)attemptCount);
    }

    if (ready) {
      ClockDateTime disciplined{};
      bool hadPriorRtc = false;
      int64_t driftSeconds = 0;
      ++attemptCount;
      if (g_clock.disciplineFromGnss(sample, waitForPps, nullptr, disciplined, hadPriorRtc, driftSeconds)) {
        g_epochBase = ClockDiscipline::toEpochSeconds(disciplined);
        g_epochBaseMs = g_lastPpsMs > 0 ? g_lastPpsMs : millis();
        g_clockValid = true;
        g_lastDisciplineMillis = g_epochBaseMs;
        g_lastDisciplineEpochMs = currentEpochMs();
        g_latitude = sample.latitude;
        g_longitude = sample.longitude;
        g_gpsFixMs = sample.sampleMillis;
        g_hasLocation = true;
        g_lastGpsRefreshMs = millis();
        char iso[32];
        ClockDiscipline::formatIsoUtc(disciplined, iso, sizeof(iso));
        Serial.printf("clock_disciplined node=%s utc=%s prior_rtc=%u drift_s=%lld last_discipline_epoch_ms=%lld lat=%.7f lon=%.7f\n",
                      NODE_NAME,
                      iso,
                      hadPriorRtc ? 1U : 0U,
                      (long long)driftSeconds,
                      (long long)g_lastDisciplineEpochMs,
                      g_latitude,
                      g_longitude);
        return true;
      }
      Serial.println("clock discipline attempt failed; waiting for next valid GPS sample");
    }
    pollStorageWeb();
    delay(20);
  }
}

bool openDatedLog() {
  if (!clockValidNow(millis()) || !g_hasLocation || !g_storage.ready()) {
    return false;
  }
  char stamp[24];
  formatCompactDateTime(currentEpoch(), stamp, sizeof(stamp));
  snprintf(g_logPath, sizeof(g_logPath), "/logs/%s_%s_ble_search.log", stamp, NODE_NAME);
  if (!g_storage.ensureDirRecursive("/logs")) {
    Serial.printf("sd_log_dir_failed err=%s\n", g_storage.lastError());
    return false;
  }
  if (!g_storage.openLog(g_logPath)) {
    Serial.printf("sd_log_open_failed path=%s err=%s\n", g_logPath, g_storage.lastError());
    return false;
  }
  g_storage.println("# schema_version=2");
  g_storage.println("# fields=human_time,rx_epoch_ms,receiver,rx_lat,rx_lon,gps_fix_age_ms,clock_valid,gps_valid,discipline_age_ms,last_discipline_epoch_ms,heard,rssi,avg_rssi,age_s,count,seq,tx_payload_epoch_ms,payload,vbat_mv");
  g_storage.println("human_time,rx_epoch_ms,receiver,rx_lat,rx_lon,gps_fix_age_ms,clock_valid,gps_valid,discipline_age_ms,last_discipline_epoch_ms,heard,rssi,avg_rssi,age_s,count,seq,tx_payload_epoch_ms,payload,vbat_mv");
  g_storage.flush();
  Serial.printf("sd_log_open path=%s\n", g_logPath);
  g_logReady = true;
  char extra[160];
  snprintf(extra, sizeof(extra), "reset_reason=%d logfile=%s", (int)esp_reset_reason(), g_logPath);
  emitEvent("log_opened", extra);
  emitStatus(true);
  return true;
}

void startStorageAndWeb() {
  tbeam::StorageConfig storageConfig;
  storageConfig.logDir = "/logs";
  storageConfig.startupWarmupMs = 2500;
  storageConfig.recoveryRailOffMs = 400;
  storageConfig.recoveryRailOnSettleMs = 1200;

  g_sdReady = g_storage.begin(storageConfig);
  if (!g_sdReady) {
    Serial.printf("sd_unavailable node=%s err=%s\n", NODE_NAME, g_storage.lastError());
    if (!isSelfName("AMY")) {
      Serial.println("WARNING: SD unavailable; BLE will run, but non-AMY units should be fixed before field logging");
    }
  }

  tbeam::WebConfig webConfig;
  webConfig.ssidPrefix = "TBEAM";
  webConfig.boardId = NODE_NAME;
  webConfig.password = nullptr;
  webConfig.ipOctet = LOG_AP_IP_OCTET;
  webConfig.enableDelete = true;
  g_webReady = g_web.begin(g_storage, webConfig);
  Serial.printf("web_ready=%u ssid=%s url=http://%s/\n",
                g_webReady ? 1U : 0U,
                g_web.ssid(),
                g_web.ip().toString().c_str());

  if (g_disciplined && g_sdReady) {
    g_logReady = openDatedLog();
  }
}

void updateAdvertisement() {
  char payload[32];
  snprintf(payload,
           sizeof(payload),
           "%s|%s|%04lu|%lld",
           kTimePayloadPrefix,
           NODE_NAME,
           (unsigned long)(g_sequence % 10000UL),
           (long long)currentEpochMs());

  BLEAdvertising* advertising = BLEDevice::getAdvertising();
  BLEAdvertisementData data;
  data.setFlags(0x06);
  data.setManufacturerData(std::string(payload));
  advertising->stop();
  advertising->setAdvertisementData(data);
  advertising->setScanResponse(false);
  advertising->setMinPreferred(0x00);
  BLEDevice::startAdvertising();
  ++g_sequence;
  g_lastAdvertiseMs = millis();
}

bool parsePayload(const std::string& data, char* outName, size_t outNameSize, uint32_t& outSeq, int64_t& outTxPayloadEpochMs, char* outPayload, size_t outPayloadSize) {
  if (outNameSize == 0 || outPayloadSize == 0) {
    return false;
  }
  outName[0] = '\0';
  outPayload[0] = '\0';
  outSeq = 0;
  outTxPayloadEpochMs = 0;
  if (data.empty() || data.size() >= outPayloadSize) {
    return false;
  }
  memcpy(outPayload, data.data(), data.size());
  outPayload[data.size()] = '\0';

  char work[48];
  strlcpy(work, outPayload, sizeof(work));
  char* save = nullptr;
  const char* prefix = strtok_r(work, "|", &save);
  if (!prefix) {
    return false;
  }

  const char* name = nullptr;
  const char* seq = nullptr;
  const char* txEpochMs = nullptr;
  if (strcmp(prefix, kTimePayloadPrefix) == 0) {
    name = strtok_r(nullptr, "|", &save);
    seq = strtok_r(nullptr, "|", &save);
    txEpochMs = strtok_r(nullptr, "|", &save);
  } else {
    const char* version = strtok_r(nullptr, "|", &save);
    name = strtok_r(nullptr, "|", &save);
    seq = strtok_r(nullptr, "|", &save);
    if (strcmp(prefix, kProjectPrefix) != 0 || !version || atoi(version) != kProtocolVersion) {
      return false;
    }
  }
  if (!name || !seq) {
    return false;
  }
  if (nodeIndexFor(name) < 0 || isSelfName(name)) {
    return false;
  }
  strlcpy(outName, name, outNameSize);
  outSeq = (uint32_t)strtoul(seq, nullptr, 10);
  if (txEpochMs && txEpochMs[0]) {
    outTxPayloadEpochMs = (int64_t)strtoll(txEpochMs, nullptr, 10);
  }
  return true;
}

void logAcceptedAdvertisement(const NodeState& node, const char* payload) {
  const uint32_t now = millis();
  const bool clockValid = clockValidNow(now);
  const uint32_t fixAge = gpsAgeMs(now);
  const bool gpsValid = gpsValidNow(now);
  const uint32_t discAge = disciplineAgeMs(now);
  if (!clockValid) {
    static uint32_t lastSuppressedMs = 0;
    if ((uint32_t)(now - lastSuppressedMs) >= 1000U) {
      lastSuppressedMs = now;
      Serial.printf("observation_suppressed node=%s heard=%s clock_valid=%u gps_valid=%u gps_fix_age_ms=%lu discipline_age_ms=%lu\n",
                    NODE_NAME,
                    node.name,
                    clockValid ? 1U : 0U,
                    gpsValid ? 1U : 0U,
                    (unsigned long)fixAge,
                    (unsigned long)discAge);
    }
    return;
  }

  const int avg = averageRssi(node);
  const uint32_t age = ageSeconds(node, now);
  const int64_t epochMs = currentEpochMs();
  const int64_t epoch = epochMs / 1000LL;
  char human[32];
  formatDateTime(epoch, human, sizeof(human));

  Serial.printf("%lu,%s,%s,%d,%d,%lu,%lu,%lu,%lld,%s\n",
                (unsigned long)now,
                NODE_NAME,
                node.name,
                node.lastRssi,
                avg,
                (unsigned long)age,
                (unsigned long)node.seenCount,
                (unsigned long)node.lastSeq,
                (long long)node.lastTxPayloadEpochMs,
                payload);

  if (g_logReady && g_storage.isLogOpen()) {
    char line[320];
    const int n = snprintf(line,
                           sizeof(line),
                           "%s,%lld,%s,%.7f,%.7f,%lu,%u,%u,%lu,%lld,%s,%d,%d,%lu,%lu,%lu,%lld,%s,%d",
                           human,
                           (long long)epochMs,
                           NODE_NAME,
                           g_latitude,
                           g_longitude,
                           (unsigned long)fixAge,
                           clockValid ? 1U : 0U,
                           gpsValid ? 1U : 0U,
                           (unsigned long)discAge,
                           (long long)g_lastDisciplineEpochMs,
                           node.name,
                           node.lastRssi,
                           avg,
                           (unsigned long)age,
                           (unsigned long)node.seenCount,
                           (unsigned long)node.lastSeq,
                           (long long)node.lastTxPayloadEpochMs,
                           payload,
                           batteryMillivolts());
    const size_t expected = n > 0 ? (size_t)n + 1U : 0U;
    const size_t wrote = n > 0 ? g_storage.println(line) : 0U;
    const bool writeOk = expected > 0 && wrote == expected;
    if (!writeOk && g_lastWriteOk) {
      char extra[96];
      snprintf(extra, sizeof(extra), "bytes_expected=%lu bytes_written=%lu", (unsigned long)expected, (unsigned long)wrote);
      emitEvent("sd_write_short", extra);
    } else if (writeOk && !g_lastWriteOk) {
      emitEvent("sd_write_ok");
    }
    g_lastWriteOk = writeOk;
    if (writeOk) {
      ++g_rowsWritten;
    }
  }
}

void acceptAdvertisement(const char* name, int rssi, uint32_t seq, int64_t txPayloadEpochMs, const char* payload) {
  const int idx = nodeIndexFor(name);
  if (idx < 0) {
    return;
  }
  NodeState& node = g_nodes[idx];
  node.heard = true;
  node.lastRssi = rssi;
  node.samples[node.nextSample] = rssi;
  node.nextSample = (node.nextSample + 1U) % kRssiWindow;
  if (node.sampleCount < kRssiWindow) {
    ++node.sampleCount;
  }
  ++node.seenCount;
  node.lastSeenMs = millis();
  node.lastSeq = seq;
  node.lastTxPayloadEpochMs = txPayloadEpochMs;
  logAcceptedAdvertisement(node, payload);
}

class DiscoveryAdvertisedCallbacks : public BLEAdvertisedDeviceCallbacks {
 public:
  void onResult(BLEAdvertisedDevice advertisedDevice) override {
    if (!advertisedDevice.haveManufacturerData()) {
      return;
    }
    char name[8];
    char payload[48];
    uint32_t seq = 0;
    int64_t txPayloadEpochMs = 0;
    if (!parsePayload(advertisedDevice.getManufacturerData(), name, sizeof(name), seq, txPayloadEpochMs, payload, sizeof(payload))) {
      return;
    }
    acceptAdvertisement(name, advertisedDevice.getRSSI(), seq, txPayloadEpochMs, payload);
  }
};

void startBle() {
  BLEDevice::init(std::string("TBMSND-") + NODE_NAME);
  updateAdvertisement();
  g_scan = BLEDevice::getScan();
  g_scan->setAdvertisedDeviceCallbacks(new DiscoveryAdvertisedCallbacks(), true);
  g_scan->setInterval(1349);
  g_scan->setWindow(449);
  g_scan->setActiveScan(false);
  g_bleStarted = true;
  Serial.println("BLE started: advertising and scanning");
}

void pollBle() {
  const uint32_t now = millis();
  if (!clockValidNow(now)) {
    BLEDevice::getAdvertising()->stop();
    return;
  }
  if ((uint32_t)(now - g_lastAdvertiseMs) >= kAdvertisePeriodMs) {
    updateAdvertisement();
  }
  if (g_scan && (uint32_t)(now - g_lastScanMs) >= kScanPeriodMs) {
    g_lastScanMs = now;
    g_scan->start(kScanWindowSeconds, false);
    g_scan->clearResults();
    updateAdvertisement();
  }
}

void pollButton() {
  const bool pressed = digitalRead(BUTTON_PIN) == LOW;
  if (pressed && !g_buttonWasPressed) {
    g_displayMode = (g_displayMode + 1U) % 3U;
    g_lastDisplayMs = 0;
  }
  g_buttonWasPressed = pressed;
}

void renderDisplay() {
  const uint32_t now = millis();
  if ((uint32_t)(now - g_lastDisplayMs) < kDisplayPeriodMs) {
    return;
  }
  g_lastDisplayMs = now;

  char title[32];
  makeTitle(title, sizeof(title));
  const bool clockValid = clockValidNow(now);
  const bool gpsValid = gpsValidNow(now);

  if (!clockValid) {
    showLines(title, "CLOCK STALE", "BLE survey paused", "logging paused");
    return;
  }

  if (g_displayMode == 1) {
    char rows[5][32] = {};
    uint8_t row = 0;
    for (size_t i = 0; i < sizeof(g_nodes) / sizeof(g_nodes[0]) && row < 5; ++i) {
      if (g_nodes[i].self) {
        continue;
      }
      if (freshEnough(g_nodes[i], now)) {
        snprintf(rows[row], sizeof(rows[row]), "%s %d avg%d %lus", g_nodes[i].name, g_nodes[i].lastRssi, averageRssi(g_nodes[i]), (unsigned long)ageSeconds(g_nodes[i], now));
      } else {
        snprintf(rows[row], sizeof(rows[row]), "%s not-heard", g_nodes[i].name);
      }
      ++row;
    }
    showLines(title, rows[0], rows[1], rows[2], rows[3], rows[4]);
    return;
  }

  if (g_displayMode == 2) {
    uint32_t heardTotal = 0;
    uint32_t freshTotal = 0;
    for (const NodeState& node : g_nodes) {
      if (node.self) {
        continue;
      }
      heardTotal += node.seenCount;
      if (freshEnough(node, now)) {
        ++freshTotal;
      }
    }
    char l2[32];
    char l3[32];
    char l4[32];
    snprintf(l2, sizeof(l2), "fresh:%lu total:%lu", (unsigned long)freshTotal, (unsigned long)heardTotal);
    snprintf(l3, sizeof(l3), "clk:%u gps:%u log:%u", clockValid ? 1U : 0U, gpsValid ? 1U : 0U, g_logReady ? 1U : 0U);
    snprintf(l4, sizeof(l4), "log:%s", g_logPath[0] ? g_logPath + 6 : "none");
    showLines(title, "DIAG", l2, l3, l4);
    return;
  }

  int order[7];
  uint8_t count = 0;
  for (size_t i = 0; i < sizeof(g_nodes) / sizeof(g_nodes[0]); ++i) {
    if (!g_nodes[i].self && freshEnough(g_nodes[i], now)) {
      order[count++] = (int)i;
    }
  }
  std::sort(order, order + count, [](int a, int b) {
    return averageRssi(g_nodes[a]) > averageRssi(g_nodes[b]);
  });

  char rows[5][32] = {};
  for (uint8_t row = 0; row < count && row < 5; ++row) {
    const NodeState& node = g_nodes[order[row]];
    snprintf(rows[row],
             sizeof(rows[row]),
             "%s %d avg%d %lus",
             node.name,
             node.lastRssi,
             averageRssi(node),
             (unsigned long)ageSeconds(node, now));
  }
  if (count == 0) {
    snprintf(rows[0], sizeof(rows[0]), "none heard");
  }
  showLines(title, rows[0], rows[1], rows[2], rows[3], rows[4]);
}

void pollStorageWeb() {
  g_storage.update();
  if (g_storage.consumeRemovedEvent()) {
    emitEvent("log_closed");
    emitEvent("sd_removed");
    g_sdReady = false;
    g_logReady = false;
    g_logPath[0] = '\0';
    Serial.println("sd_removed logging_disabled");
  }
  if (g_storage.consumeMountedEvent()) {
    g_sdReady = true;
    emitEvent("sd_remounted");
    Serial.println("sd_mounted");
  }
  if (clockValidNow(millis()) && g_hasLocation && g_storage.ready() && !g_logReady) {
    g_sdReady = true;
    g_logReady = openDatedLog();
  }
  if (g_webReady) {
    g_web.update();
  }
  if (g_logReady && (uint32_t)(millis() - g_lastFlushMs) >= kLogFlushPeriodMs) {
    g_lastFlushMs = millis();
    g_storage.flush();
  }
  if (g_logReady) {
    emitStatus();
  }
}

}  // namespace

void setup() {
  Serial.begin(115200);
  if (!tbeam_supreme::initPmuForPeripherals(g_pmu, &Serial)) {
    Serial.println("WARNING: PMU init failed");
  }
  g_display.begin();
  showBootScreen("PMU/OLED ready");
  printBootBanner();
  char bootExtra[48];
  snprintf(bootExtra, sizeof(bootExtra), "reset_reason=%d", (int)esp_reset_reason());
  emitEvent("boot", bootExtra);
  markSelfNode();
  pinMode(BUTTON_PIN, INPUT_PULLUP);

  showBootScreen("Starting Web");
  startStorageAndWeb();
  showBootScreen("Starting GPS");
  g_gnss.begin();
  showBootScreen("Probing GPS");
  (void)g_gnss.probeAtStartup(Serial);
#ifdef GPS_1PPS_PIN
  attachInterrupt(digitalPinToInterrupt(GPS_1PPS_PIN), onPpsEdge, RISING);
#endif

  showBootScreen("BLE disabled");
  g_disciplined = disciplineStartupClock();
  showBootScreen("GPS disciplined");
  pollStorageWeb();
  showBootScreen("Starting BLE");
  startBle();
}

void loop() {
  g_gnss.poll();
  refreshGpsPosition();
  pollStatusEvents();
  pollButton();
  pollStorageWeb();
  if (g_bleStarted) {
    pollBle();
  }
  renderDisplay();
}
