#include <Arduino.h>
#include <SD.h>
#include <SPI.h>
#include <U8g2lib.h>
#include <Wire.h>
#include <WiFi.h>
#include <WebServer.h>
#include <time.h>
#include <sys/time.h>

#include "SensorQMC6310.hpp"
#include "StartupSdManager.h"
#include "tbeam_supreme_adapter.h"

namespace {

#ifndef BOARD_ID
#define BOARD_ID "CY"
#endif

#ifndef NODE_LABEL
#define NODE_LABEL "Cy"
#endif

#ifndef FW_BUILD_UTC
#define FW_BUILD_UTC unknown
#endif

#ifndef OLED_SDA
#define OLED_SDA 17
#endif

#ifndef OLED_SCL
#define OLED_SCL 18
#endif

#ifndef OLED_ADDR
#define OLED_ADDR 0x3C
#endif

#ifndef LOG_AP_IP_OCTET
#define LOG_AP_IP_OCTET 25
#endif

#ifndef MAG_DECLINATION_DEG
#define MAG_DECLINATION_DEG 0.0f
#endif

#define STR_INNER(x) #x
#define STR(x) STR_INNER(x)

static constexpr const char* kBoardId = BOARD_ID;
static constexpr const char* kNodeLabel = NODE_LABEL;
static constexpr const char* kBuild = STR(FW_BUILD_UTC);
static constexpr const char* kExerciseName = "Exercise 22";
static constexpr uint32_t kSampleIntervalMs = 200;
static constexpr uint32_t kDisplayIntervalMs = 200;
static constexpr uint32_t kUiSplashMs = 1400;
static constexpr uint8_t kRtcAddress = 0x51;
static constexpr uint8_t kMagCandidateCount = 3;
static constexpr uint8_t kMagCandidates[kMagCandidateCount] = {0x1C, 0x3C, 0x0D};
static constexpr float kDeclinationDeg = MAG_DECLINATION_DEG;
static constexpr float kDegPerRad = 57.29577951308232f;


struct ClockDateTime {
  uint16_t year = 0;
  uint8_t month = 0;
  uint8_t day = 0;
  uint8_t hour = 0;
  uint8_t minute = 0;
  uint8_t second = 0;
};

struct MagSample {
  bool valid = false;
  uint32_t seq = 0;
  uint32_t millisSinceBoot = 0;
  time_t epoch = 0;
  int16_t rawX = 0;
  int16_t rawY = 0;
  int16_t rawZ = 0;
  float x_uT = 0.0f;
  float y_uT = 0.0f;
  float z_uT = 0.0f;
  float field_uT = 0.0f;
  float headingMagDeg = 0.0f;
  float headingTrueDeg = 0.0f;
};

XPowersLibInterface* g_pmu = nullptr;
StartupSdManager g_sd(Serial);
U8G2_SH1106_128X64_NONAME_F_HW_I2C g_oled(U8G2_R0, U8X8_PIN_NONE);
SensorQMC6310 g_qmc;
WebServer g_server(80);

ClockDateTime g_rtcUtc{};
MagSample g_lastSample{};

bool g_displayReady = false;
bool g_sdMounted = false;
bool g_logOpen = false;
bool g_magReady = false;
bool g_timeValid = false;
bool g_webReady = false;

uint8_t g_magAddress = 0;
uint8_t g_magChipId = 0;
char g_magLabel[16] = "UNKNOWN";
char g_logPath[96] = {0};
char g_apSsid[32] = {0};
File g_logFile;

uint32_t g_lastSampleMs = 0;
uint32_t g_lastDisplayMs = 0;
uint32_t g_lastHeartbeatMs = 0;

uint8_t toBcd(uint8_t value) {
  return (uint8_t)(((value / 10U) << 4U) | (value % 10U));
}

uint8_t fromBcd(uint8_t value) {
  return (uint8_t)(((value >> 4U) * 10U) + (value & 0x0FU));
}

bool isLeapYear(uint16_t year) {
  return ((year % 4U) == 0U && (year % 100U) != 0U) || ((year % 400U) == 0U);
}

uint8_t daysInMonth(uint16_t year, uint8_t month) {
  static const uint8_t kDays[12] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
  if (month == 2U) {
    return (uint8_t)(isLeapYear(year) ? 29U : 28U);
  }
  if (month >= 1U && month <= 12U) {
    return kDays[month - 1U];
  }
  return 0;
}

bool isValidDateTime(const ClockDateTime& dt) {
  if (dt.year < 2000U || dt.year > 2099U) return false;
  if (dt.month < 1U || dt.month > 12U) return false;
  if (dt.day < 1U || dt.day > daysInMonth(dt.year, dt.month)) return false;
  if (dt.hour > 23U || dt.minute > 59U || dt.second > 59U) return false;
  return true;
}

int64_t daysFromCivil(int year, unsigned month, unsigned day) {
  year -= (month <= 2U);
  const int era = (year >= 0 ? year : year - 399) / 400;
  const unsigned yoe = (unsigned)(year - era * 400);
  const unsigned doy = (153U * (month + (month > 2U ? (unsigned)-3 : 9U)) + 2U) / 5U + day - 1U;
  const unsigned doe = yoe * 365U + yoe / 4U - yoe / 100U + doy;
  return era * 146097 + (int)doe - 719468;
}

time_t toEpochSeconds(const ClockDateTime& dt) {
  const int64_t days = daysFromCivil((int)dt.year, dt.month, dt.day);
  return (time_t)(days * 86400LL + (int64_t)dt.hour * 3600LL + (int64_t)dt.minute * 60LL + (int64_t)dt.second);
}

bool readRtc(ClockDateTime& out, bool& lowVoltageFlag) {
  Wire1.beginTransmission(kRtcAddress);
  Wire1.write(0x02);
  if (Wire1.endTransmission(false) != 0) {
    return false;
  }

  const uint8_t need = 7;
  const uint8_t got = Wire1.requestFrom((int)kRtcAddress, (int)need);
  if (got != need) {
    return false;
  }

  const uint8_t sec = Wire1.read();
  const uint8_t min = Wire1.read();
  const uint8_t hour = Wire1.read();
  const uint8_t day = Wire1.read();
  (void)Wire1.read();
  const uint8_t month = Wire1.read();
  const uint8_t year = Wire1.read();

  lowVoltageFlag = (sec & 0x80U) != 0;
  out.second = fromBcd(sec & 0x7FU);
  out.minute = fromBcd(min & 0x7FU);
  out.hour = fromBcd(hour & 0x3FU);
  out.day = fromBcd(day & 0x3FU);
  out.month = fromBcd(month & 0x1FU);
  out.year = (month & 0x80U) ? (1900U + fromBcd(year)) : (2000U + fromBcd(year));
  return true;
}

bool writeRtc(const ClockDateTime& dt) {
  if (!isValidDateTime(dt)) {
    return false;
  }

  Wire1.beginTransmission(kRtcAddress);
  Wire1.write(0x02);
  Wire1.write(toBcd(dt.second & 0x7FU));
  Wire1.write(toBcd(dt.minute));
  Wire1.write(toBcd(dt.hour));
  Wire1.write(toBcd(dt.day));
  Wire1.write(0x00);
  uint8_t monthReg = toBcd(dt.month);
  if (dt.year < 2000U) {
    monthReg |= 0x80U;
  }
  Wire1.write(monthReg);
  Wire1.write(toBcd((uint8_t)(dt.year % 100U)));
  return Wire1.endTransmission() == 0;
}

void setSystemTimeFromRtc(const ClockDateTime& dt) {
  const time_t epoch = toEpochSeconds(dt);
  const timeval tv = {.tv_sec = epoch, .tv_usec = 0};
  settimeofday(&tv, nullptr);
}

void formatCompactUtc(const ClockDateTime& dt, char* out, size_t outSize) {
  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 drawLines(const char* l1,
               const char* l2 = nullptr,
               const char* l3 = nullptr,
               const char* l4 = nullptr,
               const char* l5 = nullptr) {
  if (!g_displayReady) {
    return;
  }
  g_oled.clearBuffer();
  g_oled.setFont(u8g2_font_6x12_tf);
  if (l1) g_oled.drawUTF8(0, 12, l1);
  if (l2) g_oled.drawUTF8(0, 24, l2);
  if (l3) g_oled.drawUTF8(0, 36, l3);
  if (l4) g_oled.drawUTF8(0, 48, l4);
  if (l5) g_oled.drawUTF8(0, 60, l5);
  g_oled.sendBuffer();
}

void initDisplay() {
  Wire.begin(OLED_SDA, OLED_SCL);
  Wire.setClock(400000);
  g_oled.setI2CAddress(OLED_ADDR << 1);
  g_oled.setBusClock(400000);
  g_oled.begin();
  g_oled.setPowerSave(0);
  g_displayReady = true;
  drawLines("Exercise 22", "Magnetometer", kBoardId, "starting...");
}

String htmlEscape(const String& in) {
  String out;
  out.reserve(in.length() + 16);
  for (size_t i = 0; i < in.length(); ++i) {
    const char c = in[i];
    if (c == '&') out += "&amp;";
    else if (c == '<') out += "&lt;";
    else if (c == '>') out += "&gt;";
    else if (c == '"') out += "&quot;";
    else out += c;
  }
  return out;
}

String urlEncode(const String& in) {
  String out;
  char hex[4];
  for (size_t i = 0; i < in.length(); ++i) {
    const unsigned char c = (unsigned char)in[i];
    if (isalnum(c) || c == '-' || c == '_' || c == '.' || c == '/' || c == '~') {
      out += (char)c;
    } else {
      snprintf(hex, sizeof(hex), "%%%02X", c);
      out += hex;
    }
  }
  return out;
}

void handleWebIndex() {
  String html;
  html.reserve(4096);
  html += "<!doctype html><html><head><meta charset='utf-8'><title>Exercise 22</title></head><body>";
  html += "<h1>Exercise 22 Magnetometer</h1>";
  html += "<p>Board: ";
  html += htmlEscape(String(kBoardId));
  html += " Build: ";
  html += htmlEscape(String(kBuild));
  html += "</p>";
  html += "<p>Mag: ";
  html += htmlEscape(String(g_magLabel));
  html += " addr=0x";
  char hex[8];
  snprintf(hex, sizeof(hex), "%02X", g_magAddress);
  html += hex;
  html += " chip=0x";
  snprintf(hex, sizeof(hex), "%02X", g_magChipId);
  html += hex;
  html += "</p>";
  html += "<p>Declination: ";
  html += String(kDeclinationDeg, 2);
  html += " deg</p>";
  html += "<p>Log: ";
  html += htmlEscape(String(g_logOpen ? g_logPath : "(not open)"));
  html += "</p>";
  if (g_logOpen) {
    html += "<p><a href='/download?path=";
    html += urlEncode(String(g_logPath));
    html += "'>Download current log</a></p>";
  }
  html += "<p><a href='/files'>List SD root</a></p>";
  html += "</body></html>";
  g_server.send(200, "text/html", html);
}

void handleWebFiles() {
  if (!g_sdMounted) {
    g_server.send(503, "text/plain", "SD not mounted\n");
    return;
  }

  File dir = SD.open("/", FILE_READ);
  if (!dir || !dir.isDirectory()) {
    g_server.send(500, "text/plain", "Failed to open SD root\n");
    return;
  }

  String body;
  body.reserve(4096);
  body += "<!doctype html><html><body><h1>SD Files</h1><ul>";
  File entry = dir.openNextFile();
  while (entry) {
    body += "<li>";
    const String name = String(entry.name());
    body += htmlEscape(name);
    if (!entry.isDirectory()) {
      body += " <a href='/download?path=";
      body += urlEncode(name);
      body += "'>download</a>";
    }
    body += "</li>";
    entry.close();
    entry = dir.openNextFile();
  }
  body += "</ul><p><a href='/'>Back</a></p></body></html>";
  dir.close();
  g_server.send(200, "text/html", body);
}

void handleWebDownload() {
  if (!g_server.hasArg("path")) {
    g_server.send(400, "text/plain", "missing path\n");
    return;
  }
  if (!g_sdMounted) {
    g_server.send(503, "text/plain", "SD not mounted\n");
    return;
  }

  String path = g_server.arg("path");
  if (!path.startsWith("/")) {
    path = "/" + path;
  }

  File file = SD.open(path.c_str(), FILE_READ);
  if (!file || file.isDirectory()) {
    g_server.send(404, "text/plain", "file not found\n");
    return;
  }

  g_server.sendHeader("Content-Type", "text/plain");
  g_server.sendHeader("Content-Disposition", "attachment; filename=\"" + path.substring(path.lastIndexOf('/') + 1) + "\"");
  g_server.streamFile(file, "text/plain");
  file.close();
}

void startWebServerOLD() {
  // GPSQA-CY is a carry-over from previous exercises, but we can keep the SSID for continuity. 
  //The unique board ID is in the suffix, and the IP address is fixed based on LOG_AP_IP_OCTET.
  snprintf(g_apSsid, sizeof(g_apSsid), "GPSQA-%s", kBoardId);
  WiFi.mode(WIFI_AP);
  WiFi.setSleep(false);
  const IPAddress ip(192, 168, LOG_AP_IP_OCTET, 1);
  const IPAddress gw(192, 168, LOG_AP_IP_OCTET, 1);
  const IPAddress nm(255, 255, 255, 0);
  WiFi.softAPConfig(ip, gw, nm);
  // no password required.
  if (!WiFi.softAP(g_apSsid)) {
    Serial.println("wifi_ap=failed");
    return;
  }
  Serial.println("wifi_ap=started");
  g_server.on("/", HTTP_GET, handleWebIndex);
  g_server.on("/files", HTTP_GET, handleWebFiles);
  g_server.on("/download", HTTP_GET, handleWebDownload);
  g_server.begin();
  g_webReady = true;

  Serial.printf("wifi_ap_ssid=%s\n", g_apSsid);
  Serial.printf("wifi_ap_url=http://192.168.%u.1/\n", (unsigned)LOG_AP_IP_OCTET);
}

void startWebServer() {
  // GPSQA-CY is a carry-over from previous exercises, but we can keep the SSID for continuity. 
  //The unique board ID is in the suffix, and the IP address is fixed based on LOG_AP_IP_OCTET.
  snprintf(g_apSsid, sizeof(g_apSsid), "GPSQA-%s", kBoardId);
  WiFi.mode(WIFI_AP);
  WiFi.setSleep(false);

  const IPAddress ip(192, 168, LOG_AP_IP_OCTET, 1);
  const IPAddress gw(192, 168, LOG_AP_IP_OCTET, 1);
  const IPAddress nm(255, 255, 255, 0);

  const bool cfgOk = WiFi.softAPConfig(ip, gw, nm);
  Serial.printf("wifi_ap_config=%s\n", cfgOk ? "ok" : "failed");
  // no password required.
  if (!WiFi.softAP(g_apSsid)) {
    Serial.println("wifi_ap=failed");
    return;
  }

  Serial.println("wifi_ap=started");
  Serial.printf("wifi_ap_ssid=%s\n", g_apSsid);
  Serial.printf("wifi_ap_ip=%s\n", WiFi.softAPIP().toString().c_str());
  Serial.printf("wifi_station_count=%d\n", WiFi.softAPgetStationNum());

  g_server.on("/", HTTP_GET, []() {
    Serial.println("http_hit=/");
    handleWebIndex();
  });
  g_server.on("/files", HTTP_GET, []() {
    Serial.println("http_hit=/files");
    handleWebFiles();
  });
  g_server.on("/download", HTTP_GET, []() {
    Serial.println("http_hit=/download");
    handleWebDownload();
  });

  g_server.onNotFound([]() {
    Serial.printf("http_404 uri=%s\n", g_server.uri().c_str());
    g_server.send(404, "text/plain", "not found\n");
  });

  g_server.begin();
  g_webReady = true;
}

bool probeI2cAddr(TwoWire& wire, uint8_t addr) {
  wire.beginTransmission(addr);
  return wire.endTransmission() == 0;
}

bool detectMagnetometer() {
  Serial.printf("Detecting magnetometer, kMagCandidateCount: %u\n", kMagCandidateCount);
  for (uint8_t i = 0; i < kMagCandidateCount; ++i) {
    const uint8_t addr = kMagCandidates[i];
    Serial.printf("  candidate[%u] = 0x%02X\n", i, kMagCandidates[i]);
    //
    if (!probeI2cAddr(Wire, addr)) {
      continue;
    }
    Serial.printf("Found device at 0x%02X, probing for magnetometer...\n", addr);

    if (addr == 0x3C || addr == 0x3D) {
      //Wire1.beginTransmission(addr);
      //Wire1.write((uint8_t)0x00);
      //if (Wire1.endTransmission(false) == 0 && Wire1.requestFrom((int)addr, 1) == 1) {
      Wire.beginTransmission(addr);
      Wire.write((uint8_t)0x00);
      if (Wire.endTransmission(false) == 0 && Wire.requestFrom((int)addr, 1) == 1) {
        const uint8_t marker = Wire.read(); // was Wire1.read() in original code, but that seems wrong since Wire1 is a different bus
        if (marker != 0x80) {
          continue;
        }
      }
    }

    g_magAddress = addr;
    if (addr == 0x1C) {
      strlcpy(g_magLabel, "QMC6310U", sizeof(g_magLabel));
    } else if (addr == 0x3C) {
      strlcpy(g_magLabel, "QMC6310N", sizeof(g_magLabel));
    } else if (addr == 0x0D) {
      strlcpy(g_magLabel, "QMC6309?", sizeof(g_magLabel));
    } else {
      strlcpy(g_magLabel, "QMC?", sizeof(g_magLabel));
    }
    return true;
  }
  return false;
}

bool initMagnetometer() {
  if (!detectMagnetometer()) {
    return false;
  }

  if (!g_qmc.begin(Wire, g_magAddress, tbeam_supreme::i2cSda(), tbeam_supreme::i2cScl())) {
    return false;
  }

  g_magChipId = g_qmc.getChipID();
  const int rc = g_qmc.configMagnetometer(
      SensorQMC6310::MODE_CONTINUOUS,
      SensorQMC6310::RANGE_8G,
      SensorQMC6310::DATARATE_200HZ,
      SensorQMC6310::OSR_1,
      SensorQMC6310::DSR_1);
  return rc == 0;
}

bool mountSd() {
  SdWatcherConfig cfg;
  cfg.enablePinDumps = false;
  if (!g_sd.begin(cfg, nullptr)) {
    return false;
  }
  g_sdMounted = g_sd.isMounted();
  return g_sdMounted;
}

bool openLogFile() {
  if (!g_sdMounted) {
    return false;
  }

  time_t now = time(nullptr);
  if (now < 946684800) {
    return false;
  }

  struct tm tmUtc;
  gmtime_r(&now, &tmUtc);
  snprintf(g_logPath,
           sizeof(g_logPath),
           "/%04d%02d%02d_%02d%02d%02d_magnetometer_readings.log",
           tmUtc.tm_year + 1900,
           tmUtc.tm_mon + 1,
           tmUtc.tm_mday,
           tmUtc.tm_hour,
           tmUtc.tm_min,
           tmUtc.tm_sec);

  g_logFile = SD.open(g_logPath, FILE_WRITE);
  if (!g_logFile) {
    return false;
  }

  g_logFile.println("# Exercise 22 magnetometer calibration capture");
  g_logFile.printf("# board_id\t%s\n", kBoardId);
  g_logFile.printf("# build\t%s\n", kBuild);
  g_logFile.printf("# mag_label\t%s\n", g_magLabel);
  g_logFile.printf("# mag_address\t0x%02X\n", g_magAddress);
  g_logFile.printf("# mag_chip_id\t0x%02X\n", g_magChipId);
  g_logFile.printf("# declination_deg\t%.2f\n", kDeclinationDeg);
  g_logFile.println("# date_utc\ttime_utc\tsample_seq\tmillis_since_boot\traw_x\traw_y\traw_z\tx_uT\ty_uT\tz_uT\tfield_uT\theading_mag_deg\theading_true_deg");
  g_logFile.flush();
  g_logOpen = true;
  return true;
}

float normalizeHeadingDeg(float heading) {
  while (heading < 0.0f) heading += 360.0f;
  while (heading >= 360.0f) heading -= 360.0f;
  return heading;
}

bool captureSample(MagSample& sample) {
  if (!g_magReady) {
    return false;
  }
  if (!g_qmc.isDataReady()) {
    return false;
  }

  g_qmc.readData();

  sample.valid = true;
  sample.seq = g_lastSample.seq + 1;
  sample.millisSinceBoot = millis();
  sample.epoch = time(nullptr);
  sample.rawX = g_qmc.getRawX();
  sample.rawY = g_qmc.getRawY();
  sample.rawZ = g_qmc.getRawZ();
  sample.x_uT = g_qmc.getX();
  sample.y_uT = g_qmc.getY();
  sample.z_uT = g_qmc.getZ();
  sample.field_uT = sqrtf(sample.x_uT * sample.x_uT + sample.y_uT * sample.y_uT + sample.z_uT * sample.z_uT);
  sample.headingMagDeg = normalizeHeadingDeg(atan2f(sample.y_uT, sample.x_uT) * kDegPerRad);
  sample.headingTrueDeg = normalizeHeadingDeg(sample.headingMagDeg + kDeclinationDeg);
  return true;
}

void formatDateTimeUtc(time_t epoch, char* dateOut, size_t dateSize, char* timeOut, size_t timeSize) {
  struct tm tmUtc;
  gmtime_r(&epoch, &tmUtc);
  snprintf(dateOut, dateSize, "%04d%02d%02d", tmUtc.tm_year + 1900, tmUtc.tm_mon + 1, tmUtc.tm_mday);
  snprintf(timeOut, timeSize, "%02d%02d%02d", tmUtc.tm_hour, tmUtc.tm_min, tmUtc.tm_sec);
}

void printSampleToSerial(const MagSample& sample) {
  char dateBuf[16];
  char timeBuf[16];
  formatDateTimeUtc(sample.epoch, dateBuf, sizeof(dateBuf), timeBuf, sizeof(timeBuf));
  Serial.printf(
      "%s\t%s\t%lu\t%lu\t%d\t%d\t%d\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\n",
      dateBuf,
      timeBuf,
      (unsigned long)sample.seq,
      (unsigned long)sample.millisSinceBoot,
      (int)sample.rawX,
      (int)sample.rawY,
      (int)sample.rawZ,
      sample.x_uT,
      sample.y_uT,
      sample.z_uT,
      sample.field_uT,
      sample.headingMagDeg,
      sample.headingTrueDeg);
}

void appendSampleToLog(const MagSample& sample) {
  if (!g_logOpen) {
    return;
  }
  char dateBuf[16];
  char timeBuf[16];
  formatDateTimeUtc(sample.epoch, dateBuf, sizeof(dateBuf), timeBuf, sizeof(timeBuf));
  g_logFile.printf(
      "%s\t%s\t%lu\t%lu\t%d\t%d\t%d\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\t%.2f\n",
      dateBuf,
      timeBuf,
      (unsigned long)sample.seq,
      (unsigned long)sample.millisSinceBoot,
      (int)sample.rawX,
      (int)sample.rawY,
      (int)sample.rawZ,
      sample.x_uT,
      sample.y_uT,
      sample.z_uT,
      sample.field_uT,
      sample.headingMagDeg,
      sample.headingTrueDeg);
  g_logFile.flush();
}

void drawLiveUi() {
  if (!g_displayReady) {
    return;
  }

  time_t now = time(nullptr);
  struct tm tmUtc;
  char line1[24];
  char line2[28];
  char line3[28];
  char line4[28];
  char line5[28];

  if (now >= 946684800 && gmtime_r(&now, &tmUtc) != nullptr) {
    snprintf(line1, sizeof(line1), "%02d:%02d:%02d UTC", tmUtc.tm_hour, tmUtc.tm_min, tmUtc.tm_sec);
  } else {
    snprintf(line1, sizeof(line1), "time invalid");
  }

  snprintf(line2, sizeof(line2), "X:% 7.2f Y:% 7.2f", g_lastSample.x_uT, g_lastSample.y_uT);
  snprintf(line3, sizeof(line3), "Z:% 7.2f F:% 7.2f", g_lastSample.z_uT, g_lastSample.field_uT);
  snprintf(line4, sizeof(line4), "HdM:%6.1f T:%6.1f", g_lastSample.headingMagDeg, g_lastSample.headingTrueDeg);
  snprintf(line5, sizeof(line5), "%s 0x%02X N:%lu", g_magLabel, g_magAddress, (unsigned long)g_lastSample.seq);

  g_oled.clearBuffer();
  g_oled.setFont(u8g2_font_6x12_tf);
  g_oled.drawUTF8(0, 12, line1);
  g_oled.drawUTF8(0, 24, line2);
  g_oled.drawUTF8(0, 36, line3);
  g_oled.drawUTF8(0, 48, line4);
  g_oled.drawUTF8(0, 60, line5);
  g_oled.sendBuffer();
}

void printBootSummary() {
  Serial.printf("exercise=%s\n", kExerciseName);
  Serial.printf("board_id=%s\n", kBoardId);
  Serial.printf("node_label=%s\n", kNodeLabel);
  Serial.printf("build=%s\n", kBuild);
  Serial.printf("pmu_wire_pins=sda:%d scl:%d\n", tbeam_supreme::i2cSda(), tbeam_supreme::i2cScl());
  Serial.printf("oled_wire_pins=sda:%d scl:%d addr:0x%02X\n", OLED_SDA, OLED_SCL, OLED_ADDR);
  Serial.printf("declination_deg=%.2f\n", kDeclinationDeg);
  Serial.printf("sample_interval_ms=%lu\n", (unsigned long)kSampleIntervalMs);
}

void appSetup() {
  Serial.begin(115200);
  const uint32_t serialWaitStart = millis();
  while (!Serial && (millis() - serialWaitStart) < 4000) {
    delay(10);
  }
  delay(300);

  printBootSummary();
  initDisplay();
  drawLines("Exercise 22", "Magnetometer", "& calibration", kBoardId, "bring-up");

  if (!tbeam_supreme::initPmuForPeripherals(g_pmu, &Serial)) {
    Serial.println("pmu_init=failed");
    drawLines("Exercise 22", "PMU init failed", kBoardId, "see serial");
    return;
  }
  Serial.println("pmu_init=ok");

  bool lowVoltage = false;
  if (readRtc(g_rtcUtc, lowVoltage) && !lowVoltage && isValidDateTime(g_rtcUtc)) {
    setSystemTimeFromRtc(g_rtcUtc);
    g_timeValid = true;
    char rtcStamp[32];
    formatCompactUtc(g_rtcUtc, rtcStamp, sizeof(rtcStamp));
    Serial.printf("rtc_sync=ok utc=%s\n", rtcStamp);
  } else {
    Serial.println("rtc_sync=invalid");
  }

  if (!mountSd()) {
    Serial.println("sd_mount=failed");
    drawLines("Exercise 22", "SD mount failed", kBoardId, "see serial");
  } else {
    Serial.println("sd_mount=ok");
    g_sd.printCardInfo();
  }
  // Next is the failure point
  g_magReady = initMagnetometer();
  if (!g_magReady) {
    Serial.println("magnetometer_init=failed");
    drawLines("Exercise 22", "MAG init failed", kBoardId, "see serial");
    return;
  }

  Serial.printf("magnetometer_init=ok label=%s addr=0x%02X chip=0x%02X\n", g_magLabel, g_magAddress, g_magChipId);

  if (g_timeValid && g_sdMounted) {
    if (openLogFile()) {
      Serial.printf("log_open=ok path=%s\n", g_logPath);
    } else {
      Serial.println("log_open=failed");
    }
  } else {
    Serial.printf("log_open=skipped time_valid=%s sd_mounted=%s\n",
                  g_timeValid ? "yes" : "no",
                  g_sdMounted ? "yes" : "no");
  }

  startWebServer();

  drawLines("Exercise 22", "Magnetometer", g_magLabel, "rotate slowly", "logging @200ms");
  delay(kUiSplashMs);
  g_lastSampleMs = millis();
  g_lastDisplayMs = millis();
  g_lastHeartbeatMs = millis();
}

// We want to capture a reasonable amount of data for calibration and testing, 
// bu we also want to avoid creating huge logs or overwhelming the display. 
// 600 samples at 200ms intervals is 2 minutes of data, which should be enough 
// for calibration and testing.
// Adjust the sample_limit as needed based on your specific requirements and constraints.

uint32_t sample_limit = 600;
uint32_t sample_count = 0;
void appLoop() {
  if (g_webReady) {
    g_server.handleClient();
  }

  g_sd.update();
  g_sdMounted = g_sd.isMounted();

  const uint32_t now = millis();
  if (sample_count <= sample_limit) { 
    if ((uint32_t)(now - g_lastSampleMs) >= kSampleIntervalMs) {
      g_lastSampleMs = now;
      MagSample sample{};
      if (captureSample(sample)) {
        sample_count++; 
        g_lastSample = sample;
        printSampleToSerial(sample);
        appendSampleToLog(sample);
      }
    }

    if ((uint32_t)(now - g_lastDisplayMs) >= kDisplayIntervalMs) {
      g_lastDisplayMs = now;
      drawLiveUi();
    }
  }

  if ((uint32_t)(now - g_lastHeartbeatMs) >= 5000) {
    g_lastHeartbeatMs = now;
    Serial.printf("alive seq=%lu log=%s web=%s sd=%s sta=%d\n",
              (unsigned long)g_lastSample.seq,
              g_logOpen ? "open" : "closed",
              g_webReady ? "up" : "down",
              g_sdMounted ? "mounted" : "absent",
              WiFi.softAPgetStationNum());
  }
}

}  // namespace

void setup() {
  appSetup();
}

void loop() {
  appLoop();
}