// 20260401 Codex

#include <Arduino.h>
#include <SD.h>
#include <SPI.h>
#include <Wire.h>
#include <U8g2lib.h>
#include <driver/gpio.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 kSpiHz = 400000;
static const uint32_t kReadyHeartbeatMs = 2000;

enum class RawState : uint8_t {
  PMU_FAIL = 0,
  RAIL_OFF,
  BUS_FLOAT,
  BUS_LOW,
  CMD0_TIMEOUT,
  CMD0_NOT_IDLE,
  CMD8_TIMEOUT,
  CMD8_BAD_R1,
  ACMD41_TIMEOUT,
  CMD58_TIMEOUT,
  READY
};

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 bytes[8] = {0};
};

struct RawSnapshot {
  RawState state = RawState::PMU_FAIL;
  bool pmuOk = false;
  bool railOn = false;
  PinSnapshot pins{};
  ProbeSummary idle{};
  uint8_t cmd0 = 0xFF;
  uint8_t cmd8r1 = 0xFF;
  uint8_t cmd8data[4] = {0xFF, 0xFF, 0xFF, 0xFF};
  uint8_t acmd41 = 0xFF;
  uint8_t cmd58r1 = 0xFF;
  uint8_t ocr[4] = {0xFF, 0xFF, 0xFF, 0xFF};
};

static XPowersLibInterface* g_pmu = nullptr;
static U8G2_SH1106_128X64_NONAME_F_HW_I2C g_oled(U8G2_R0, U8X8_PIN_NONE);
static SPIClass g_spi(HSPI);
static uint32_t g_sampleCount = 0;
static uint32_t g_markCount = 0;
static char g_inputLine[32] = {0};
static uint8_t g_inputLen = 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 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 beginBus() {
  SD.end();
  g_spi.end();
  delay(2);
  forceSpiDeselected();
  g_spi.begin(
    tbeam_supreme::sdSck(),
    tbeam_supreme::sdMiso(),
    tbeam_supreme::sdMosi(),
    tbeam_supreme::sdCs()
  );
  digitalWrite(tbeam_supreme::sdCs(), HIGH);
}

static ProbeSummary idleProbe() {
  ProbeSummary out;
  beginBus();
  delay(1);
  for (int i = 0; i < 8; ++i) {
    uint8_t b = g_spi.transfer(0xFF);
    out.bytes[i] = b;
    if (b == 0xFF) out.ffCount++;
    else if (b == 0x00) out.zeroCount++;
    else out.otherCount++;
  }
  return out;
}

static uint8_t waitR1(uint16_t tries = 16) {
  for (uint16_t i = 0; i < tries; ++i) {
    uint8_t r = g_spi.transfer(0xFF);
    if ((r & 0x80) == 0) {
      return r;
    }
  }
  return 0xFF;
}

static uint8_t sendCommand(uint8_t cmd, uint32_t arg, uint8_t crc) {
  g_spi.transfer(0xFF);
  digitalWrite(tbeam_supreme::sdCs(), LOW);
  g_spi.transfer(0x40 | cmd);
  g_spi.transfer((arg >> 24) & 0xFF);
  g_spi.transfer((arg >> 16) & 0xFF);
  g_spi.transfer((arg >> 8) & 0xFF);
  g_spi.transfer(arg & 0xFF);
  g_spi.transfer(crc);
  uint8_t r1 = waitR1();
  return r1;
}

static void endCommand() {
  digitalWrite(tbeam_supreme::sdCs(), HIGH);
  g_spi.transfer(0xFF);
}

static const char* stateToString(RawState state) {
  switch (state) {
    case RawState::PMU_FAIL: return "PMU_FAIL";
    case RawState::RAIL_OFF: return "RAIL_OFF";
    case RawState::BUS_FLOAT: return "BUS_FLOAT";
    case RawState::BUS_LOW: return "BUS_LOW";
    case RawState::CMD0_TIMEOUT: return "CMD0_TO";
    case RawState::CMD0_NOT_IDLE: return "CMD0_BAD";
    case RawState::CMD8_TIMEOUT: return "CMD8_TO";
    case RawState::CMD8_BAD_R1: return "CMD8_BAD";
    case RawState::ACMD41_TIMEOUT: return "ACMD41_TO";
    case RawState::CMD58_TIMEOUT: return "CMD58_TO";
    case RawState::READY: return "READY";
    default: return "UNKNOWN";
  }
}

static RawSnapshot captureSnapshot() {
  RawSnapshot snap;
  snap.pins = readPins();
  snap.pmuOk = (g_pmu != nullptr);
  if (!snap.pmuOk) {
    snap.state = RawState::PMU_FAIL;
    return snap;
  }

  snap.railOn = g_pmu->isPowerChannelEnable(XPOWERS_BLDO1);
  if (!snap.railOn) {
    snap.state = RawState::RAIL_OFF;
    return snap;
  }

  snap.idle = idleProbe();
  if (snap.idle.ffCount == 8) {
    snap.state = RawState::BUS_FLOAT;
  } else if (snap.idle.zeroCount == 8) {
    snap.state = RawState::BUS_LOW;
  }

  beginBus();
  delay(1);
  for (int i = 0; i < 10; ++i) {
    g_spi.transfer(0xFF);
  }

  snap.cmd0 = sendCommand(0, 0, 0x95);
  endCommand();
  if (snap.cmd0 == 0xFF) {
    snap.state = RawState::CMD0_TIMEOUT;
    return snap;
  }
  if (snap.cmd0 != 0x01) {
    snap.state = RawState::CMD0_NOT_IDLE;
    return snap;
  }

  snap.cmd8r1 = sendCommand(8, 0x000001AAUL, 0x87);
  if (snap.cmd8r1 == 0xFF) {
    endCommand();
    snap.state = RawState::CMD8_TIMEOUT;
    return snap;
  }
  for (int i = 0; i < 4; ++i) {
    snap.cmd8data[i] = g_spi.transfer(0xFF);
  }
  endCommand();
  if (!(snap.cmd8r1 == 0x01 || snap.cmd8r1 == 0x05)) {
    snap.state = RawState::CMD8_BAD_R1;
    return snap;
  }

  uint8_t ready = 0xFF;
  for (int attempt = 0; attempt < 12; ++attempt) {
    uint8_t r1 = sendCommand(55, 0, 0x65);
    endCommand();
    if (r1 == 0xFF) {
      continue;
    }
    ready = sendCommand(41, 0x40000000UL, 0x77);
    endCommand();
    if (ready == 0x00) {
      break;
    }
    delay(10);
  }
  snap.acmd41 = ready;
  if (snap.acmd41 != 0x00) {
    snap.state = RawState::ACMD41_TIMEOUT;
    return snap;
  }

  snap.cmd58r1 = sendCommand(58, 0, 0xFD);
  if (snap.cmd58r1 == 0xFF) {
    endCommand();
    snap.state = RawState::CMD58_TIMEOUT;
    return snap;
  }
  for (int i = 0; i < 4; ++i) {
    snap.ocr[i] = g_spi.transfer(0xFF);
  }
  endCommand();

  snap.state = RawState::READY;
  return snap;
}

static void printSnapshot(const RawSnapshot& snap) {
  Serial.printf(
    "sample=%lu state=%s rail=%s pins=%d/%d/%d/%d "
    "idle(ff=%u z=%u o=%u %02X %02X %02X %02X) "
    "cmd0=%02X cmd8=%02X [%02X %02X %02X %02X] "
    "acmd41=%02X cmd58=%02X [%02X %02X %02X %02X]\r\n",
    (unsigned long)g_sampleCount,
    stateToString(snap.state),
    snap.railOn ? "ON" : "OFF",
    snap.pins.cs,
    snap.pins.sck,
    snap.pins.miso,
    snap.pins.mosi,
    (unsigned)snap.idle.ffCount,
    (unsigned)snap.idle.zeroCount,
    (unsigned)snap.idle.otherCount,
    snap.idle.bytes[0],
    snap.idle.bytes[1],
    snap.idle.bytes[2],
    snap.idle.bytes[3],
    snap.cmd0,
    snap.cmd8r1,
    snap.cmd8data[0],
    snap.cmd8data[1],
    snap.cmd8data[2],
    snap.cmd8data[3],
    snap.acmd41,
    snap.cmd58r1,
    snap.ocr[0],
    snap.ocr[1],
    snap.ocr[2],
    snap.ocr[3]
  );
}

static void printReadyHeartbeat() {
  Serial.printf("[%10lu] READY heartbeat\r\n", (unsigned long)millis());
}

static bool sameSnapshot(const RawSnapshot& a, const RawSnapshot& b) {
  if (a.state != b.state) return false;
  if (a.railOn != b.railOn) return false;
  if (a.pins.cs != b.pins.cs || a.pins.sck != b.pins.sck ||
      a.pins.miso != b.pins.miso || a.pins.mosi != b.pins.mosi) return false;
  if (a.idle.ffCount != b.idle.ffCount ||
      a.idle.zeroCount != b.idle.zeroCount ||
      a.idle.otherCount != b.idle.otherCount) return false;
  if (a.cmd0 != b.cmd0 || a.cmd8r1 != b.cmd8r1 ||
      a.acmd41 != b.acmd41 || a.cmd58r1 != b.cmd58r1) return false;

  for (int i = 0; i < 4; ++i) {
    if (a.cmd8data[i] != b.cmd8data[i]) return false;
    if (a.ocr[i] != b.ocr[i]) return false;
  }

  return true;
}

static void showSnapshot(const RawSnapshot& snap) {
  char l1[24];
  char l2[24];
  char l3[24];
  char l4[24];
  char l5[24];

  snprintf(l1, sizeof(l1), "%s RAW SD", NODE_LABEL);
  snprintf(l2, sizeof(l2), "STATE %s", stateToString(snap.state));
  snprintf(l3, sizeof(l3), "CMD0 %02X C8 %02X A41 %02X", snap.cmd0, snap.cmd8r1, snap.acmd41);
  snprintf(l4, sizeof(l4), "OCR %02X%02X%02X%02X",
           snap.ocr[0], snap.ocr[1], snap.ocr[2], snap.ocr[3]);
  snprintf(l5, sizeof(l5), "IDL %u/%u/%u #%lu",
           (unsigned)snap.idle.ffCount,
           (unsigned)snap.idle.zeroCount,
           (unsigned)snap.idle.otherCount,
           (unsigned long)g_sampleCount);

  g_oled.clearBuffer();
  g_oled.setFont(u8g2_font_5x8_tf);
  g_oled.drawUTF8(0, 12, l1);
  g_oled.drawUTF8(0, 24, l2);
  g_oled.drawUTF8(0, 36, l3);
  g_oled.drawUTF8(0, 48, l4);
  g_oled.drawUTF8(0, 60, l5);
  g_oled.sendBuffer();
}

static void handleSerialInput() {
  auto handleCommand = []() {
    if (g_inputLen == 0) {
      Serial.println();
      return;
    }

    g_inputLine[g_inputLen] = '\0';
    if (strcmp(g_inputLine, "m") == 0 || strcmp(g_inputLine, "M") == 0) {
      g_markCount++;
      Serial.printf("----- MARK %lu -----\r\n", (unsigned long)g_markCount);
    } else if (strcmp(g_inputLine, "ls") == 0 || strcmp(g_inputLine, "LS") == 0) {
      Serial.println("----- LS / -----");
      beginBus();
      for (int i = 0; i < 10; ++i) {
        g_spi.transfer(0xFF);
      }

      if (!SD.begin(tbeam_supreme::sdCs(), g_spi, kSpiHz)) {
        Serial.println("ls: SD.begin failed");
        g_inputLen = 0;
        return;
      }

      File root = SD.open("/", FILE_READ);
      if (!root) {
        Serial.println("ls: open / failed");
        SD.end();
        g_inputLen = 0;
        return;
      }

      File entry = root.openNextFile();
      if (!entry) {
        Serial.println("ls: root empty or unreadable");
      }

      while (entry) {
        Serial.printf("%s%s %lu\r\n",
                      entry.name(),
                      entry.isDirectory() ? "/" : "",
                      (unsigned long)entry.size());
        entry.close();
        entry = root.openNextFile();
      }

      root.close();
      SD.end();
      Serial.println("----- END LS -----");
    } else {
      Serial.printf("unknown command: %s\r\n", g_inputLine);
    }

    g_inputLen = 0;
  };

  while (Serial.available() > 0) {
    int ch = Serial.read();
    if (ch == '\r' || ch == '\n') {
      handleCommand();
    } else if (ch == 0x08 || ch == 0x7F) {
      if (g_inputLen > 0) {
        g_inputLen--;
      }
    } else if (g_inputLen + 1 < sizeof(g_inputLine)) {
      g_inputLine[g_inputLen++] = (char)ch;
    }
  }
}

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

  Wire.begin(OLED_SDA, OLED_SCL);
  g_oled.begin();
  g_oled.clearDisplay();

  tbeam_supreme::initPmuForPeripherals(g_pmu, &Serial);
  forceSpiDeselected();

  Serial.println();
  Serial.println("constantTFCard raw probe");
  Serial.printf("Node: %s\r\n", NODE_LABEL);
  Serial.printf("Poll interval: %lu ms\r\n", (unsigned long)kPollIntervalMs);
  Serial.println("States: PMU_FAIL RAIL_OFF BUS_FLOAT BUS_LOW CMD0_TO CMD0_BAD CMD8_TO CMD8_BAD ACMD41_TO CMD58_TO READY");
  Serial.println("Input: Enter=blank line, m=mark, ls=list root");
}

void loop() {
  static uint32_t lastPollMs = 0;
  static bool haveLastPrinted = false;
  static RawSnapshot lastPrinted{};
  static uint32_t lastReadyHeartbeatMs = 0;

  handleSerialInput();

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

  lastPollMs = now;
  g_sampleCount++;

  RawSnapshot snap = captureSnapshot();
  bool changed = (!haveLastPrinted || !sameSnapshot(snap, lastPrinted));
  if (changed) {
    printSnapshot(snap);
    lastPrinted = snap;
    haveLastPrinted = true;
    if (snap.state == RawState::READY) {
      lastReadyHeartbeatMs = now;
    }
  } else if (snap.state == RawState::READY && now - lastReadyHeartbeatMs >= kReadyHeartbeatMs) {
    printReadyHeartbeat();
    lastReadyHeartbeatMs = now;
  }
  showSnapshot(snap);
}