// 20260219 ChatGPT
// Exercise 13: SD Card Diagnostics

#include <Arduino.h>
#include <Wire.h>
#include <SPI.h>
#include <SD.h>
#include <U8g2lib.h>
#include <stdarg.h>
#include <driver/gpio.h>

#include "StartupSdManager.h"
#include "tbeam_supreme_adapter.h"

#ifndef NODE_LABEL
#define NODE_LABEL "DIAG"
#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 FILE_APPEND
#define FILE_APPEND FILE_WRITE
#endif
#ifndef FW_BUILD_UTC
#define FW_BUILD_UTC "unknown"
#endif
#ifndef DIAG_TEST_NOTE
#define DIAG_TEST_NOTE "enclosure screws removed; board lightly constrained"
#endif

static const uint32_t kSerialDelayMs = 1500;
static const uint32_t kLoopDelayMs = 10;
static const uint32_t kHeartbeatMs = 2000;
static const uint32_t kDiagCycleMs = 20000;
static const uint32_t kRailRetestEvery = 3;

static XPowersLibInterface* g_pmu = nullptr;
static StartupSdManager g_sd(Serial);
static U8G2_SH1106_128X64_NONAME_F_HW_I2C g_oled(U8G2_R0, U8X8_PIN_NONE);
static SPIClass g_spiH(HSPI);
static SPIClass g_spiF(FSPI);

static uint32_t g_logSeq = 0;
static uint32_t g_lastHeartbeatMs = 0;
static uint32_t g_lastDiagMs = 0;
static uint32_t g_diagCycleCount = 0;

static bool g_lastMounted = false;
static char g_lastDiagLine1[28] = "Diag: waiting";
static char g_lastDiagLine2[28] = "No cycle yet";

struct PinSnapshot {
  int cs = -1;
  int sck = -1;
  int miso = -1;
  int mosi = -1;
};

struct ProbeSummary {
  uint8_t ffCount = 0;
  uint8_t zeroCount = 0;
  uint8_t otherCount = 0;
  uint8_t firstBytes[8] = {0};
};

struct MountMatrixResult {
  bool anySuccess = false;
  uint8_t attempts = 0;
  const char* successBus = "none";
  uint32_t successHz = 0;
};

static ProbeSummary g_lastProbeH{};
static ProbeSummary g_lastProbeF{};

static void logf(const char* fmt, ...) {
  char msg[240];
  va_list args;
  va_start(args, fmt);
  vsnprintf(msg, sizeof(msg), fmt, args);
  va_end(args);
  Serial.printf("[%10lu][%06lu] %s\r\n", (unsigned long)millis(), (unsigned long)g_logSeq++, msg);
}

static void oledShowLines(const char* l1,
                          const char* l2 = nullptr,
                          const char* l3 = nullptr,
                          const char* l4 = nullptr,
                          const char* l5 = nullptr) {
  g_oled.clearBuffer();
  g_oled.setFont(u8g2_font_5x8_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();
}

static void forceSpiDeselected() {
  pinMode(tbeam_supreme::sdCs(), OUTPUT);
  digitalWrite(tbeam_supreme::sdCs(), HIGH);
  pinMode(tbeam_supreme::imuCs(), OUTPUT);
  digitalWrite(tbeam_supreme::imuCs(), HIGH);
}

static PinSnapshot readPins() {
  PinSnapshot s;
  s.cs = gpio_get_level((gpio_num_t)tbeam_supreme::sdCs());
  s.sck = gpio_get_level((gpio_num_t)tbeam_supreme::sdSck());
  s.miso = gpio_get_level((gpio_num_t)tbeam_supreme::sdMiso());
  s.mosi = gpio_get_level((gpio_num_t)tbeam_supreme::sdMosi());
  return s;
}

static void logPins(const char* tag) {
  PinSnapshot p = readPins();
  logf("PINS(%s): CS=%d SCK=%d MISO=%d MOSI=%d", tag, p.cs, p.sck, p.miso, p.mosi);
}

static void readPmu(float& vbusV, float& battV, bool& bldo1On, bool& battPresent) {
  vbusV = -1.0f;
  battV = -1.0f;
  bldo1On = false;
  battPresent = false;
  if (!g_pmu) return;

  bldo1On = g_pmu->isPowerChannelEnable(XPOWERS_BLDO1);
  battPresent = g_pmu->isBatteryConnect();
  vbusV = g_pmu->getVbusVoltage() / 1000.0f;
  battV = g_pmu->getBattVoltage() / 1000.0f;
}

static bool cycleSdRail(uint32_t offMs = 300, uint32_t onSettleMs = 900) {
  if (!g_pmu) {
    logf("Rail cycle skipped: PMU unavailable");
    return false;
  }

  forceSpiDeselected();
  g_pmu->disablePowerOutput(XPOWERS_BLDO1);
  delay(offMs);
  g_pmu->setPowerChannelVoltage(XPOWERS_BLDO1, 3300);
  g_pmu->enablePowerOutput(XPOWERS_BLDO1);
  delay(onSettleMs);
  logf("Rail cycle complete (off=%lums on_settle=%lums)", (unsigned long)offMs, (unsigned long)onSettleMs);
  return true;
}

static ProbeSummary runIdleProbeOnBus(SPIClass& bus, const char* busName) {
  ProbeSummary out;

  SD.end();
  bus.end();
  delay(5);
  forceSpiDeselected();

  bus.begin(tbeam_supreme::sdSck(), tbeam_supreme::sdMiso(), tbeam_supreme::sdMosi(), tbeam_supreme::sdCs());
  digitalWrite(tbeam_supreme::sdCs(), HIGH);
  delay(1);

  for (int i = 0; i < 8; ++i) {
    uint8_t b = bus.transfer(0xFF);
    out.firstBytes[i] = b;
    if (b == 0xFF) out.ffCount++;
    else if (b == 0x00) out.zeroCount++;
    else out.otherCount++;
  }

  logf("SPI probe %s: ff=%u zero=%u other=%u bytes=%02X %02X %02X %02X %02X %02X %02X %02X",
       busName,
       (unsigned)out.ffCount,
       (unsigned)out.zeroCount,
       (unsigned)out.otherCount,
       out.firstBytes[0],
       out.firstBytes[1],
       out.firstBytes[2],
       out.firstBytes[3],
       out.firstBytes[4],
       out.firstBytes[5],
       out.firstBytes[6],
       out.firstBytes[7]);

  return out;
}

static bool tryMount(SPIClass& bus, const char* busName, uint32_t hz) {
  SD.end();
  bus.end();
  delay(5);
  forceSpiDeselected();

  bus.begin(tbeam_supreme::sdSck(), tbeam_supreme::sdMiso(), tbeam_supreme::sdMosi(), tbeam_supreme::sdCs());
  digitalWrite(tbeam_supreme::sdCs(), HIGH);
  delay(1);
  for (int i = 0; i < 10; ++i) {
    bus.transfer(0xFF);
  }

  uint32_t t0 = millis();
  bool ok = SD.begin(tbeam_supreme::sdCs(), bus, hz);
  uint32_t dt = millis() - t0;
  if (!ok) {
    logf("Mount FAIL bus=%s hz=%lu dt=%lums", busName, (unsigned long)hz, (unsigned long)dt);
    return false;
  }

  uint8_t type = SD.cardType();
  if (type == CARD_NONE) {
    SD.end();
    logf("Mount FAIL bus=%s hz=%lu dt=%lums cardType=NONE", busName, (unsigned long)hz, (unsigned long)dt);
    return false;
  }

  uint64_t mb = SD.cardSize() / (1024ULL * 1024ULL);
  logf("Mount OK   bus=%s hz=%lu dt=%lums type=%u size=%lluMB",
       busName,
       (unsigned long)hz,
       (unsigned long)dt,
       (unsigned)type,
       mb);
  return true;
}

static MountMatrixResult runMountMatrix() {
  const uint32_t freqs[] = {400000, 1000000, 4000000, 10000000};
  MountMatrixResult result{};

  for (size_t i = 0; i < (sizeof(freqs) / sizeof(freqs[0])); ++i) {
    result.attempts++;
    if (tryMount(g_spiH, "HSPI", freqs[i])) {
      result.anySuccess = true;
      result.successBus = "HSPI";
      result.successHz = freqs[i];
      return result;
    }
  }

  for (size_t i = 0; i < (sizeof(freqs) / sizeof(freqs[0])); ++i) {
    result.attempts++;
    if (tryMount(g_spiF, "FSPI", freqs[i])) {
      result.anySuccess = true;
      result.successBus = "FSPI";
      result.successHz = freqs[i];
      return result;
    }
  }

  return result;
}

static void emitVendorReport(const MountMatrixResult& mm,
                             const ProbeSummary& ph,
                             const ProbeSummary& pf,
                             float vbusV,
                             float battV,
                             bool bldo1,
                             bool battPresent) {
  logf("REPORT node=%s cycle=%lu fw=%s", NODE_LABEL, (unsigned long)g_diagCycleCount, FW_BUILD_UTC);
  logf("REPORT test_note=%s", DIAG_TEST_NOTE);
  logf("REPORT power bldo1=%u vbus=%.3fV batt=%.3fV batt_present=%u",
       bldo1 ? 1U : 0U,
       vbusV,
       battV,
       battPresent ? 1U : 0U);
  logf("REPORT spi_probe hspi(ff=%u zero=%u other=%u) fspi(ff=%u zero=%u other=%u)",
       (unsigned)ph.ffCount,
       (unsigned)ph.zeroCount,
       (unsigned)ph.otherCount,
       (unsigned)pf.ffCount,
       (unsigned)pf.zeroCount,
       (unsigned)pf.otherCount);
  if (mm.anySuccess) {
    logf("REPORT mount_matrix status=PASS attempts=%u first_success=%s@%luHz",
         (unsigned)mm.attempts,
         mm.successBus,
         (unsigned long)mm.successHz);
    logf("REPORT verdict=SD interface operational in this cycle");
    return;
  }

  logf("REPORT mount_matrix status=FAIL attempts=%u first_success=none",
       (unsigned)mm.attempts);

  if (bldo1 && vbusV > 4.5f && ph.ffCount == 8 && pf.ffCount == 8) {
    logf("REPORT verdict=Power looks good; SPI lines idle high; no card response on any bus/frequency; likely socket/interconnect/baseboard hardware fault");
  } else if (!bldo1) {
    logf("REPORT verdict=SD rail appears off; investigate PMU/BLDO1 control path");
  } else {
    logf("REPORT verdict=No card response; check SD socket, board interconnect, signal integrity, and card seating");
  }
}

static bool runFileIoValidation(uint32_t cycleNo) {
  if (!SD.exists("/diag")) {
    if (!SD.mkdir("/diag")) {
      logf("I/O FAIL: cannot create /diag");
      return false;
    }
  }

  const char* path = "/diag/sd_diag_probe.log";
  File f = SD.open(path, FILE_APPEND);
  if (!f) {
    logf("I/O FAIL: cannot open %s", path);
    return false;
  }

  float vbusV = 0.0f, battV = 0.0f;
  bool bldo1 = false, battPresent = false;
  readPmu(vbusV, battV, bldo1, battPresent);

  uint32_t t0 = millis();
  f.printf("cycle=%lu ms=%lu bldo1=%u vbus=%.3f batt=%.3f batt_present=%u mounted=%u\n",
           (unsigned long)cycleNo,
           (unsigned long)millis(),
           bldo1 ? 1U : 0U,
           vbusV,
           battV,
           battPresent ? 1U : 0U,
           g_sd.isMounted() ? 1U : 0U);
  f.flush();
  f.close();

  uint32_t writeMs = millis() - t0;

  File r = SD.open(path, FILE_READ);
  if (!r) {
    logf("I/O FAIL: reopen for read failed");
    return false;
  }

  size_t size = (size_t)r.size();
  if (size == 0) {
    r.close();
    logf("I/O FAIL: file size is zero");
    return false;
  }

  r.seek(size > 120 ? size - 120 : 0);
  String tail = r.readString();
  r.close();

  if (tail.indexOf(String("cycle=") + cycleNo) < 0) {
    logf("I/O FAIL: verification token missing for cycle=%lu", (unsigned long)cycleNo);
    return false;
  }

  logf("I/O OK: append+flush+readback size=%uB write=%lums", (unsigned)size, (unsigned long)writeMs);
  return true;
}

static void onSdEvent(SdEvent event, const char* message) {
  logf("SD event: %s", message ? message : "(null)");

  if (event == SdEvent::NO_CARD) {
    oledShowLines("SD Diagnostics", "NO CARD", "Insert/reseat card");
  } else if (event == SdEvent::CARD_MOUNTED) {
    oledShowLines("SD Diagnostics", "CARD MOUNTED", "Running checks");
  } else if (event == SdEvent::CARD_REMOVED) {
    oledShowLines("SD Diagnostics", "CARD REMOVED", "Check socket/fit");
  }
}

static void emitHeartbeat() {
  float vbusV = 0.0f, battV = 0.0f;
  bool bldo1 = false, battPresent = false;
  readPmu(vbusV, battV, bldo1, battPresent);

  PinSnapshot p = readPins();
  logf("HB mounted=%u BLDO1=%u VBUS=%.3fV VBAT=%.3fV batt_present=%u pins cs=%d sck=%d miso=%d mosi=%d",
       g_sd.isMounted() ? 1U : 0U,
       bldo1 ? 1U : 0U,
       vbusV,
       battV,
       battPresent ? 1U : 0U,
       p.cs,
       p.sck,
       p.miso,
       p.mosi);

  char l1[28], l2[28], l3[28], l4[28], l5[28];
  snprintf(l1, sizeof(l1), "%s SD DIAG", NODE_LABEL);
  snprintf(l2, sizeof(l2), "mounted:%s bldo1:%u", g_sd.isMounted() ? "yes" : "no", bldo1 ? 1U : 0U);
  snprintf(l3, sizeof(l3), "VBUS:%.2f VBAT:%.2f", vbusV, battV);
  snprintf(l4, sizeof(l4), "MISO:%d CS:%d", p.miso, p.cs);
  snprintf(l5, sizeof(l5), "%s | %s", g_lastDiagLine1, g_lastDiagLine2);
  oledShowLines(l1, l2, l3, l4, l5);
}

static void runDiagnosticCycle() {
  g_diagCycleCount++;
  logf("========== DIAG CYCLE %lu START =========", (unsigned long)g_diagCycleCount);

  float vbusV = 0.0f, battV = 0.0f;
  bool bldo1 = false, battPresent = false;
  readPmu(vbusV, battV, bldo1, battPresent);
  logf("Power baseline: BLDO1=%u VBUS=%.3fV VBAT=%.3fV batt_present=%u",
       bldo1 ? 1U : 0U,
       vbusV,
       battV,
       battPresent ? 1U : 0U);

  logPins("diag-start");
  g_lastProbeH = runIdleProbeOnBus(g_spiH, "HSPI");
  g_lastProbeF = runIdleProbeOnBus(g_spiF, "FSPI");

  MountMatrixResult mm = runMountMatrix();
  if (!mm.anySuccess) {
    snprintf(g_lastDiagLine1, sizeof(g_lastDiagLine1), "Mount scan: FAIL");
    snprintf(g_lastDiagLine2, sizeof(g_lastDiagLine2), "No bus/freq worked");
    SD.end();
  } else {
    bool ioOk = runFileIoValidation(g_diagCycleCount);
    snprintf(g_lastDiagLine1, sizeof(g_lastDiagLine1), "Mount scan: OK");
    snprintf(g_lastDiagLine2, sizeof(g_lastDiagLine2), "File I/O: %s", ioOk ? "OK" : "FAIL");
    SD.end();
  }

  if ((g_diagCycleCount % kRailRetestEvery) == 0) {
    logf("Rail retest step");
    if (cycleSdRail()) {
      MountMatrixResult remount = runMountMatrix();
      logf("Rail retest remount: %s", remount.anySuccess ? "OK" : "FAIL");
      SD.end();
    }
  }

  emitVendorReport(mm, g_lastProbeH, g_lastProbeF, vbusV, battV, bldo1, battPresent);

  logf("========== DIAG CYCLE %lu END =========", (unsigned long)g_diagCycleCount);
}

void setup() {
  Serial.begin(115200);
  delay(kSerialDelayMs);

  Serial.println("\r\n==================================================");
  Serial.println("Exercise 13: SD Card Diagnostics");
  Serial.println("==================================================");

  logf("Node: %s", NODE_LABEL);
  logf("FW build UTC: %s", FW_BUILD_UTC);
  logf("Test note: %s", DIAG_TEST_NOTE);
  logf("Pins: CS=%d SCK=%d MISO=%d MOSI=%d IMU_CS=%d", tbeam_supreme::sdCs(), tbeam_supreme::sdSck(), tbeam_supreme::sdMiso(), tbeam_supreme::sdMosi(), tbeam_supreme::imuCs());
  logf("PMU I2C: SDA1=%d SCL1=%d", tbeam_supreme::i2cSda(), tbeam_supreme::i2cScl());

  Wire.begin(OLED_SDA, OLED_SCL);
  g_oled.setI2CAddress(OLED_ADDR << 1);
  g_oled.begin();
  oledShowLines("Exercise 13", "SD Card Diagnostics", NODE_LABEL, "Booting...");

  if (!tbeam_supreme::initPmuForPeripherals(g_pmu, &Serial)) {
    logf("WARN: PMU init failed via adapter");
  }

  forceSpiDeselected();
  logPins("boot");

  SdWatcherConfig cfg{};
  cfg.enableSdRailCycle = true;
  cfg.enablePinDumps = true;
  cfg.recoveryRailCycleOnFullScan = true;
  cfg.startupWarmupMs = 1500;
  cfg.pollIntervalAbsentMs = 1000;
  cfg.pollIntervalMountedMs = 2000;
  cfg.fullScanIntervalMs = 8000;
  cfg.votesToPresent = 2;
  cfg.votesToAbsent = 5;

  if (!g_sd.begin(cfg, onSdEvent)) {
    logf("WARN: StartupSdManager begin() failed");
  }

  g_lastMounted = g_sd.isMounted();
  g_lastHeartbeatMs = millis();
  g_lastDiagMs = millis() - kDiagCycleMs + 2000;
}

void loop() {
  g_sd.update();

  if (g_sd.consumeMountedEvent()) {
    g_lastMounted = true;
    logf("Event: mounted");
  }
  if (g_sd.consumeRemovedEvent()) {
    g_lastMounted = false;
    logf("Event: removed");
  }

  uint32_t now = millis();

  if ((uint32_t)(now - g_lastHeartbeatMs) >= kHeartbeatMs) {
    g_lastHeartbeatMs = now;
    emitHeartbeat();
  }

  if ((uint32_t)(now - g_lastDiagMs) >= kDiagCycleMs) {
    g_lastDiagMs = now;
    runDiagnosticCycle();
  }

  delay(kLoopDelayMs);
}