#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

#ifndef COMPASS_MODE
#define COMPASS_MODE "display"
#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 const char* kCompassMode = COMPASS_MODE;
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;

//static constexpr float kMagOffsetRawX = -7993.0f;
//static constexpr float kMagOffsetRawY = 4608.0f;
//static constexpr float kMagOffsetRawZ = 3456.0f;
// Above are CODEX, below are ChatGPT, v. 2
static constexpr float kMagOffsetRawX = -8089.0f;
static constexpr float kMagOffsetRawY = 4606.0f;
static constexpr float kMagOffsetRawZ = 3428.0f;

static constexpr float kMagSoft00 = 0.002173745f;
static constexpr float kMagSoft01 = 0.000184341f;
static constexpr float kMagSoft10 = 0.000184341f;
static constexpr float kMagSoft11 = 0.002412928f;

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 corrX = 0.0f;
  float corrY = 0.0f;
  float corrZ = 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;

bool isCalibrateMode() {
  return strcmp(kCompassMode, "calibrate") == 0;
}

bool isDisplayMode() {
  return strcmp(kCompassMode, "display") == 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 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);
  if (rc != 0) {
    return false;
  }

  g_qmc.setOffset(kMagOffsetRawX, kMagOffsetRawY, kMagOffsetRawZ);
  return true;
}

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;
}

const char* headingToCompassPoint(float headingDeg) {
  static constexpr const char* kPoints[8] = {"N", "NE", "E", "SE", "S", "SW", "W", "NW"};
  const float normalized = normalizeHeadingDeg(headingDeg);
  const int index = ((int)((normalized + 22.5f) / 45.0f)) % 8;
  return kPoints[index];
}

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);
  // Below is ChatGPT's suggestion for applying declination, but for calibration purposes we may want to keep the raw magnetic heading instead of applying declination, since the point of the exercise is to determine the declination experimentally. We can calculate the true heading in post-processing after we have determined the declination.
  //const float rawHeadingDeg =
  //  normalizeHeadingDeg(atan2f(sample.y_uT, sample.x_uT) * kDegPerRad);

  //sample.headingMagDeg = normalizeHeadingDeg(360.0f - rawHeadingDeg);
  //sample.headingTrueDeg = normalizeHeadingDeg(sample.headingMagDeg + kDeclinationDeg);
    // Preserve the library-scaled values for logging/reference.
  sample.x_uT = g_qmc.getX();
  sample.y_uT = g_qmc.getY();
  sample.z_uT = g_qmc.getZ();

  // Apply raw-count hard-iron offsets.
  const float mx0 = (float)sample.rawX - kMagOffsetRawX;
  const float my0 = (float)sample.rawY - kMagOffsetRawY;
  const float mz0 = (float)sample.rawZ - kMagOffsetRawZ;

  // Apply 2x2 XY soft-iron correction matrix.
  const float mx = kMagSoft00 * mx0 + kMagSoft01 * my0;
  const float my = kMagSoft10 * mx0 + kMagSoft11 * my0;
  const float mz = mz0;

  // Store corrected values into the existing x_uT/y_uT/z_uT fields so the
  // rest of the program uses the calibrated vector.
  sample.x_uT = mx;
  sample.y_uT = my;
  sample.z_uT = mz;

  sample.field_uT = sqrtf(sample.x_uT * sample.x_uT +
                          sample.y_uT * sample.y_uT +
                          sample.z_uT * sample.z_uT);

  //const float rawHeadingDeg =
  //  normalizeHeadingDeg(atan2f(sample.y_uT, sample.x_uT) * kDegPerRad);
  const float rawHeadingDeg =
    normalizeHeadingDeg(atan2f(my, mx) * kDegPerRad);
  
  sample.headingMagDeg = normalizeHeadingDeg(360.0f - rawHeadingDeg);
  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 line3[24];
  char line4[28];
  char line5[28];
  char bearingLine[24];
  const int bearing = (int)lroundf(normalizeHeadingDeg(g_lastSample.headingTrueDeg));
  const char* compassPoint = headingToCompassPoint(g_lastSample.headingTrueDeg);

  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(bearingLine, sizeof(bearingLine), "Bearing:%3d", bearing);
  snprintf(line3, sizeof(line3), "Dir:%s", compassPoint);
  snprintf(line4, sizeof(line4), "Mag:%6.1f Tru:%6.1f", g_lastSample.headingMagDeg, g_lastSample.headingTrueDeg);
  snprintf(line5, sizeof(line5), "%s N:%lu", g_magLabel, (unsigned long)g_lastSample.seq);

  g_oled.clearBuffer();
  g_oled.setFont(u8g2_font_6x12_tf);
  g_oled.drawUTF8(0, 12, line1);
  g_oled.setFont(u8g2_font_7x14B_tf);
  g_oled.drawUTF8(0, 30, bearingLine);
  g_oled.setFont(u8g2_font_6x12_tf);
  g_oled.drawUTF8(0, 42, line3);
  g_oled.drawUTF8(0, 54, 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("compass_mode=%s\n", kCompassMode);
  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);
  Serial.printf("mag_offsets_raw=%.1f,%.1f,%.1f\n", kMagOffsetRawX, kMagOffsetRawY, kMagOffsetRawZ);
}

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", kCompassMode, 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);
  Serial.printf("magnetometer_offsets_applied=%.1f,%.1f,%.1f\n", kMagOffsetRawX, kMagOffsetRawY, kMagOffsetRawZ);

  if (isCalibrateMode() && 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 mode=%s time_valid=%s sd_mounted=%s\n",
                  kCompassMode,
                  g_timeValid ? "yes" : "no",
                  g_sdMounted ? "yes" : "no");
  }

  if (isCalibrateMode()) {
    startWebServer();
  } else {
    Serial.printf("wifi_ap=skipped mode=%s\n", kCompassMode);
  }

  if (isCalibrateMode()) {
    drawLines("Exercise 22", "Magnetometer", "calibrate", "logging @200ms", "limit 600");
  } else {
    drawLines("Exercise 22", "Magnetometer", "display", "bearing live", "no logging");
  }
  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();
  const bool allowSampling = isDisplayMode() || (sample_count <= sample_limit);
  if (allowSampling) {
    if ((uint32_t)(now - g_lastSampleMs) >= kSampleIntervalMs) {
      g_lastSampleMs = now;
      MagSample sample{};
      if (captureSample(sample)) {
        g_lastSample = sample;
        if (isCalibrateMode()) {
          sample_count++;
          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();
}
