// 20260401 Codex

#include <Arduino.h>
#include <esp_system.h>
#include <SPI.h>
#include <SD.h>
#include <Wire.h>
#include <U8g2lib.h>

#include "tbeam_supreme_adapter.h"

#ifndef NODE_LABEL
#define NODE_LABEL "NODE"
#endif

#ifndef OLED_SDA
#define OLED_SDA 17
#endif

#ifndef OLED_SCL
#define OLED_SCL 18
#endif

static const uint32_t kSerialDelayMs = 1500;
static const uint32_t kPollIntervalMs = 200;
static const uint32_t kSdFreqHz = 400000;
static const uint32_t kBootSettleMs = 2000;
static const uint32_t kSdRailOffMs = 300;
static const uint32_t kSdRailOnSettleMs = 1200;
static const uint8_t kOutVotesBeforeRailCycle = 10;
static const uint32_t kMinRailCycleGapMs = 5000;

static const uint32_t kDelayedRetryOffsetsMs[] = {
  1000,
  3000,
  7000,
  15000
};

static XPowersLibInterface* g_pmu = nullptr;
static U8G2_SH1106_128X64_NONAME_F_HW_I2C g_oled(U8G2_R0, U8X8_PIN_NONE);
static SPIClass g_sdSpi(HSPI);
static uint32_t g_bootMs = 0;
static size_t g_nextDelayedRetry = 0;
static uint32_t g_lastRailCycleMs = 0;
static uint8_t g_consecutiveOut = 0;

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

static void oledShowStatus(const char* status, uint32_t sampleCount) {
  char line1[24];
  char line2[24];
  char line3[24];

  snprintf(line1, sizeof(line1), "%s TF CARD", NODE_LABEL);
  snprintf(line2, sizeof(line2), "%s", status);
  snprintf(line3, sizeof(line3), "sample %lu", (unsigned long)sampleCount);

  g_oled.clearBuffer();
  g_oled.setFont(u8g2_font_6x12_tf);
  g_oled.drawUTF8(0, 14, line1);
  g_oled.setFont(u8g2_font_logisoso20_tf);
  g_oled.drawUTF8(0, 42, status);
  g_oled.setFont(u8g2_font_6x12_tf);
  g_oled.drawUTF8(0, 62, line3);
  g_oled.sendBuffer();
}

static void oledShowBootPhase(const char* line2, const char* line3) {
  char line1[24];

  snprintf(line1, sizeof(line1), "%s TF CARD", NODE_LABEL);

  g_oled.clearBuffer();
  g_oled.setFont(u8g2_font_6x12_tf);
  g_oled.drawUTF8(0, 14, line1);
  if (line2) g_oled.drawUTF8(0, 32, line2);
  if (line3) g_oled.drawUTF8(0, 50, line3);
  g_oled.sendBuffer();
}

static bool cardReadable() {
  SD.end();
  g_sdSpi.end();
  delay(2);

  forceSpiDeselected();
  g_sdSpi.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) {
    g_sdSpi.transfer(0xFF);
  }

  if (!SD.begin(tbeam_supreme::sdCs(), g_sdSpi, kSdFreqHz)) {
    return false;
  }

  if (SD.cardType() == CARD_NONE) {
    SD.end();
    return false;
  }

  File root = SD.open("/", FILE_READ);
  bool ok = (bool)root;
  if (root) {
    root.close();
  }

  SD.end();
  return ok;
}

static bool cycleSdRail() {
  if (!g_pmu) {
    Serial.println("rail cycle skipped: no PMU");
    return false;
  }

  SD.end();
  g_sdSpi.end();
  forceSpiDeselected();

  g_pmu->disablePowerOutput(XPOWERS_BLDO1);
  delay(kSdRailOffMs);
  g_pmu->setPowerChannelVoltage(XPOWERS_BLDO1, 3300);
  g_pmu->enablePowerOutput(XPOWERS_BLDO1);
  delay(kSdRailOnSettleMs);
  forceSpiDeselected();

  g_lastRailCycleMs = millis();
  Serial.printf("rail cycle: off=%lu on_settle=%lu\r\n",
                (unsigned long)kSdRailOffMs,
                (unsigned long)kSdRailOnSettleMs);
  return true;
}

static void runDelayedRetry(const char* label) {
  bool readable = cardReadable();
  const char* status = readable ? "card IN" : "card OUT";
  Serial.printf("delayed retry %s -> %s\r\n", label, status);
  oledShowStatus(status, 0);
}

static void handleSerialCommands() {
  while (Serial.available() > 0) {
    int ch = Serial.read();
    if (ch == 'r' || ch == 'R') {
      Serial.println("manual command: SD rail reset");
      oledShowBootPhase("manual SD reset", "re-probing");
      if (cycleSdRail()) {
        bool readable = cardReadable();
        Serial.printf("manual SD reset -> %s\r\n", readable ? "card IN" : "card OUT");
        oledShowStatus(readable ? "card IN" : "card OUT", 0);
      }
    } else if (ch == 'b' || ch == 'B') {
      Serial.println("manual command: full reboot");
      Serial.println("restarting now...");
      oledShowBootPhase("manual reboot", "restarting");
      delay(250);
      ESP.restart();
    } else if (ch == '\r' || ch == '\n') {
      continue;
    } else {
      Serial.printf("commands: r=sd reset, b=reboot (got '%c')\r\n", (char)ch);
    }
  }
}

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

  Wire.begin(OLED_SDA, OLED_SCL);
  g_oled.begin();
  g_oled.clearDisplay();
  oledShowBootPhase("BOOT", "init");

  tbeam_supreme::initPmuForPeripherals(g_pmu, &Serial);
  forceSpiDeselected();
  g_bootMs = millis();

  Serial.println();
  Serial.println("constantTFCard");
  Serial.printf("Node: %s\r\n", NODE_LABEL);
  Serial.printf("Polling every %lu ms\r\n", (unsigned long)kPollIntervalMs);
  Serial.printf("Initial settle delay %lu ms\r\n", (unsigned long)kBootSettleMs);
  Serial.println("Commands: r=SD rail reset, b=full reboot");

  oledShowBootPhase("BOOT", "settling SD rail");
  delay(kBootSettleMs);

  runDelayedRetry("after_settle");
}

void loop() {
  static uint32_t lastPollMs = 0;
  static uint32_t sampleCount = 0;

  handleSerialCommands();

  uint32_t now = millis();

  if (g_nextDelayedRetry < (sizeof(kDelayedRetryOffsetsMs) / sizeof(kDelayedRetryOffsetsMs[0])) &&
      now - g_bootMs >= kDelayedRetryOffsetsMs[g_nextDelayedRetry]) {
    char label[20];
    snprintf(label, sizeof(label), "t+%lus", (unsigned long)(kDelayedRetryOffsetsMs[g_nextDelayedRetry] / 1000));
    runDelayedRetry(label);
    g_nextDelayedRetry++;
  }

  if (now - lastPollMs < kPollIntervalMs) {
    delay(5);
    return;
  }

  lastPollMs = now;
  sampleCount++;

  bool readable = cardReadable();
  if (readable) {
    g_consecutiveOut = 0;
  } else if (g_consecutiveOut < 255) {
    g_consecutiveOut++;
  }

  if (!readable &&
      g_consecutiveOut >= kOutVotesBeforeRailCycle &&
      now - g_lastRailCycleMs >= kMinRailCycleGapMs) {
    Serial.printf("persistent OUT: %u polls, forcing SD rail cycle\r\n",
                  (unsigned)g_consecutiveOut);
    oledShowBootPhase("OUT -> rail reset", "re-probing SD");

    if (cycleSdRail()) {
      bool retryReadable = cardReadable();
      readable = retryReadable;
      g_consecutiveOut = retryReadable ? 0 : kOutVotesBeforeRailCycle;
      Serial.printf("after rail cycle -> %s\r\n", retryReadable ? "card IN" : "card OUT");
    }
  }

  const char* status = readable ? "card IN" : "card OUT";

  Serial.println(status);
  oledShowStatus(status, sampleCount);
}
