#include "SdCardDiag.h"

#include <SPI.h>
#include <Wire.h>
#include <stdarg.h>

#include "driver/gpio.h"
#include "tbeam_supreme_adapter.h"

SdCardDiag::SdCardDiag(Print& out) : out_(out) {}

bool SdCardDiag::begin() {
  printDivider();
  logf("sdcard_diag begin");
  printPinMap();

  pmuReady_ = initPmu();
  printRailStatus();
  return pmuReady_;
}

void SdCardDiag::printReport() {
  printDivider();
  logf("starting SD card diagnostic cycle");

  forceSpiDeselected();
  printRailStatus();
  printPinLevels();

  cycleSdRail();
  delay(1200);
  printRailStatus();

  idleProbe(hspi_, "HSPI");
  idleProbe(fspi_, "FSPI");

  if (!mountCard()) {
    logf("result: no mountable SD card found");
    return;
  }

  printCardDetails();
}

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

void SdCardDiag::printDivider() {
  out_.println("------------------------------------------------------------");
}

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

bool SdCardDiag::initPmu() {
  if (!tbeam_supreme::initPmuForPeripherals(pmu_, &out_)) {
    logf("PMU init failed; SD rail power control unavailable");
    return false;
  }

  logf("PMU init ok");
  return true;
}

void SdCardDiag::cycleSdRail(uint32_t offMs, uint32_t onSettleMs) {
  if (!pmu_) {
    logf("rail cycle skipped: pmu unavailable");
    return;
  }

  forceSpiDeselected();
  SD.end();
  hspi_.end();
  fspi_.end();

  pmu_->disablePowerOutput(XPOWERS_BLDO1);
  delay(offMs);
  pmu_->setPowerChannelVoltage(XPOWERS_BLDO1, 3300);
  pmu_->enablePowerOutput(XPOWERS_BLDO1);
  delay(onSettleMs);

  logf("cycled SD rail off=%lums settle=%lums",
       (unsigned long)offMs,
       (unsigned long)onSettleMs);
}

void SdCardDiag::printRailStatus() {
  if (!pmu_) {
    logf("PMU: unavailable");
    return;
  }

  const float vbusV = pmu_->getVbusVoltage() / 1000.0f;
  const float battV = pmu_->getBattVoltage() / 1000.0f;
  const bool battPresent = pmu_->isBatteryConnect();
  const bool sdRailOn = pmu_->isPowerChannelEnable(XPOWERS_BLDO1);

  logf("PMU: SD_BLDO1=%s VBUS=%.3fV BATT=%.3fV battery=%s",
       boolToOnOff(sdRailOn),
       vbusV,
       battV,
       battPresent ? "present" : "absent");
}

void SdCardDiag::printPinMap() {
  logf("Pins: SD_CS=%d SD_SCK=%d SD_MISO=%d SD_MOSI=%d IMU_CS=%d",
       tbeam_supreme::sdCs(),
       tbeam_supreme::sdSck(),
       tbeam_supreme::sdMiso(),
       tbeam_supreme::sdMosi(),
       tbeam_supreme::imuCs());
}

void SdCardDiag::printPinLevels() {
  logf("Levels: CS=%d SCK=%d MISO=%d MOSI=%d",
       gpio_get_level((gpio_num_t)tbeam_supreme::sdCs()),
       gpio_get_level((gpio_num_t)tbeam_supreme::sdSck()),
       gpio_get_level((gpio_num_t)tbeam_supreme::sdMiso()),
       gpio_get_level((gpio_num_t)tbeam_supreme::sdMosi()));
}

SdCardDiag::ProbeBytes SdCardDiag::idleProbe(SPIClass& bus, const char* busName) {
  ProbeBytes 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 (uint8_t i = 0; i < 8; ++i) {
    const uint8_t value = bus.transfer(0xFF);
    out.bytes[i] = value;
    if (value == 0xFF) out.ffCount++;
    else if (value == 0x00) out.zeroCount++;
    else out.otherCount++;
  }

  logf("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.bytes[0],
       out.bytes[1],
       out.bytes[2],
       out.bytes[3],
       out.bytes[4],
       out.bytes[5],
       out.bytes[6],
       out.bytes[7]);

  return out;
}

bool SdCardDiag::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 (uint8_t i = 0; i < 10; ++i) {
    bus.transfer(0xFF);
  }

  const uint32_t startMs = millis();
  const bool ok = SD.begin(tbeam_supreme::sdCs(), bus, hz);
  const uint32_t elapsedMs = millis() - startMs;

  if (!ok) {
    logf("mount fail bus=%s hz=%lu dt=%lums",
         busName,
         (unsigned long)hz,
         (unsigned long)elapsedMs);
    return false;
  }

  const 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)elapsedMs);
    return false;
  }

  activeSpi_ = &bus;
  activeBusName_ = busName;
  activeHz_ = hz;

  logf("mount ok bus=%s hz=%lu dt=%lums type=%s",
       busName,
       (unsigned long)hz,
       (unsigned long)elapsedMs,
       cardTypeToString(type));
  return true;
}

bool SdCardDiag::mountCard() {
  activeSpi_ = nullptr;
  activeBusName_ = "none";
  activeHz_ = 0;

  const uint32_t freqs[] = {400000, 1000000, 4000000, 10000000};
  for (uint8_t i = 0; i < (sizeof(freqs) / sizeof(freqs[0])); ++i) {
    if (tryMount(hspi_, "HSPI", freqs[i])) {
      return true;
    }
  }
  for (uint8_t i = 0; i < (sizeof(freqs) / sizeof(freqs[0])); ++i) {
    if (tryMount(fspi_, "FSPI", freqs[i])) {
      return true;
    }
  }

  return false;
}

void SdCardDiag::printCardDetails() {
  const uint8_t type = SD.cardType();
  const uint64_t cardBytes = SD.cardSize();
  const uint64_t totalBytes = SD.totalBytes();
  const uint64_t usedBytes = SD.usedBytes();

  logf("card: type=%s size=%llu MB fs_total=%llu MB fs_used=%llu MB fs_free=%llu MB",
       cardTypeToString(type),
       (unsigned long long)(cardBytes / (1024ULL * 1024ULL)),
       (unsigned long long)(totalBytes / (1024ULL * 1024ULL)),
       (unsigned long long)(usedBytes / (1024ULL * 1024ULL)),
       (unsigned long long)((totalBytes >= usedBytes ? (totalBytes - usedBytes) : 0) / (1024ULL * 1024ULL)));
  logf("mount: bus=%s hz=%lu",
       activeBusName_,
       (unsigned long)activeHz_);
  printCardIdentity();

  File root = SD.open("/");
  if (!root || !root.isDirectory()) {
    logf("root open failed after successful mount");
    if (root) {
      root.close();
    }
    return;
  }

  DirSummary summary;
  summarizeDirectory(root, summary, 0);
  root.close();

  logf("root summary: files=%lu dirs=%lu bytes=%llu max_depth=%u",
       (unsigned long)summary.fileCount,
       (unsigned long)summary.dirCount,
       (unsigned long long)summary.fileBytes,
       (unsigned)summary.maxDepth);

  root = SD.open("/");
  if (!root || !root.isDirectory()) {
    logf("root reopen failed for sample listing");
    if (root) {
      root.close();
    }
    return;
  }

  printSampleEntries(root, 8);
  root.close();
}

bool SdCardDiag::readCardIdentity(CardIdentity& identity) {
  identity = CardIdentity{};

  if (!activeSpi_) {
    logf("CID read skipped: no active SPI bus");
    return false;
  }

  const uint32_t spiHz = (activeHz_ > 0 && activeHz_ < 4000000UL) ? activeHz_ : 4000000UL;
  uint8_t cmd[6] = {0x40 | 10, 0x00, 0x00, 0x00, 0x00, 0x01};
  cmd[5] = (uint8_t)((crc7(cmd, 5) << 1) | 0x01);

  activeSpi_->beginTransaction(SPISettings(spiHz, MSBFIRST, SPI_MODE0));
  digitalWrite(tbeam_supreme::sdCs(), HIGH);
  activeSpi_->transfer(0xFF);
  digitalWrite(tbeam_supreme::sdCs(), LOW);

  if (!waitForSpiByte(0xFF, 250)) {
    digitalWrite(tbeam_supreme::sdCs(), HIGH);
    activeSpi_->transfer(0xFF);
    activeSpi_->endTransaction();
    logf("CID read failed: card never became ready");
    return false;
  }

  for (uint8_t i = 0; i < sizeof(cmd); ++i) {
    activeSpi_->transfer(cmd[i]);
  }

  uint8_t r1 = 0xFF;
  uint32_t startMs = millis();
  while ((millis() - startMs) < 250) {
    r1 = activeSpi_->transfer(0xFF);
    if ((r1 & 0x80) == 0) {
      break;
    }
  }
  if (r1 != 0x00) {
    digitalWrite(tbeam_supreme::sdCs(), HIGH);
    activeSpi_->transfer(0xFF);
    activeSpi_->endTransaction();
    logf("CID read failed: CMD10 R1=0x%02X", r1);
    return false;
  }

  if (!waitForSpiByte(0xFE, 250)) {
    digitalWrite(tbeam_supreme::sdCs(), HIGH);
    activeSpi_->transfer(0xFF);
    activeSpi_->endTransaction();
    logf("CID read failed: no data token");
    return false;
  }

  for (uint8_t i = 0; i < sizeof(identity.raw); ++i) {
    identity.raw[i] = activeSpi_->transfer(0xFF);
  }
  activeSpi_->transfer(0xFF);
  activeSpi_->transfer(0xFF);

  digitalWrite(tbeam_supreme::sdCs(), HIGH);
  activeSpi_->transfer(0xFF);
  activeSpi_->endTransaction();

  identity.mid = identity.raw[0];
  identity.oid[0] = (char)identity.raw[1];
  identity.oid[1] = (char)identity.raw[2];
  identity.oid[2] = '\0';
  memcpy(identity.pnm, &identity.raw[3], 5);
  identity.pnm[5] = '\0';
  identity.prvMajor = (identity.raw[8] >> 4) & 0x0F;
  identity.prvMinor = identity.raw[8] & 0x0F;
  identity.psn = ((uint32_t)identity.raw[9] << 24) |
                 ((uint32_t)identity.raw[10] << 16) |
                 ((uint32_t)identity.raw[11] << 8) |
                 (uint32_t)identity.raw[12];
  const uint16_t mdt = (uint16_t)(((identity.raw[13] & 0x0F) << 8) | identity.raw[14]);
  identity.mdtYear = 2000 + ((mdt >> 4) & 0xFF);
  identity.mdtMonth = mdt & 0x0F;
  identity.valid = true;
  return true;
}

void SdCardDiag::printCardIdentity() {
  CardIdentity identity;
  if (!readCardIdentity(identity)) {
    logf("CID: unavailable");
    return;
  }

  logf("CID: mid=0x%02X manufacturer_guess=%s oem=%s product=%s revision=%u.%u serial=0x%08llX date=%04u-%02u",
       identity.mid,
       manufacturerNameFromMid(identity.mid),
       identity.oid,
       identity.pnm,
       (unsigned)identity.prvMajor,
       (unsigned)identity.prvMinor,
       (unsigned long long)identity.psn,
       (unsigned)identity.mdtYear,
       (unsigned)identity.mdtMonth);
  logf("CID raw: %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X %02X",
       identity.raw[0],
       identity.raw[1],
       identity.raw[2],
       identity.raw[3],
       identity.raw[4],
       identity.raw[5],
       identity.raw[6],
       identity.raw[7],
       identity.raw[8],
       identity.raw[9],
       identity.raw[10],
       identity.raw[11],
       identity.raw[12],
       identity.raw[13],
       identity.raw[14],
       identity.raw[15]);
}

void SdCardDiag::summarizeDirectory(File dir, DirSummary& summary, uint8_t depth) {
  File entry = dir.openNextFile();
  while (entry) {
    if (depth > summary.maxDepth) {
      summary.maxDepth = depth;
    }

    if (entry.isDirectory()) {
      summary.dirCount++;
      summarizeDirectory(entry, summary, depth + 1);
    } else {
      summary.fileCount++;
      summary.fileBytes += entry.size();
      if (depth > summary.maxDepth) {
        summary.maxDepth = depth;
      }
    }

    entry.close();
    entry = dir.openNextFile();
  }
}

void SdCardDiag::printSampleEntries(File dir, uint8_t maxEntries) {
  uint8_t index = 0;
  File entry = dir.openNextFile();
  if (!entry) {
    logf("root entries: <empty>");
    return;
  }

  while (entry && index < maxEntries) {
    logf("root[%u]: %s %s %llu bytes",
         (unsigned)index,
         entry.isDirectory() ? "DIR " : "FILE",
         entry.name(),
         (unsigned long long)entry.size());
    entry.close();
    ++index;
    entry = dir.openNextFile();
  }

  if (entry) {
    entry.close();
    logf("root entries: showing first %u only", (unsigned)maxEntries);
  }
}

uint8_t SdCardDiag::crc7(const uint8_t* data, size_t len) const {
  uint8_t crc = 0;
  for (size_t i = 0; i < len; ++i) {
    uint8_t value = data[i];
    for (uint8_t bit = 0; bit < 8; ++bit) {
      crc <<= 1;
      if (((value & 0x80) ^ (crc & 0x80)) != 0) {
        crc ^= 0x09;
      }
      value <<= 1;
    }
  }
  return crc & 0x7F;
}

bool SdCardDiag::waitForSpiByte(uint8_t expected, uint32_t timeoutMs) {
  const uint32_t startMs = millis();
  while ((millis() - startMs) < timeoutMs) {
    if (activeSpi_->transfer(0xFF) == expected) {
      return true;
    }
  }
  return false;
}

const char* SdCardDiag::manufacturerNameFromMid(uint8_t mid) const {
  // Provenance:
  // - Official SD CID field layout comes from the SD Association simplified spec:
  //   "Part 1 Physical Layer Simplified Specification", CID table section.
  //   MID is an 8-bit manufacturer ID assigned by SD-3C, LLC.
  // - SD-3C does not publish a public authoritative MID->vendor table.
  // - The mapping below is therefore best-effort and community-derived from
  //   observed card CIDs, cross-checked 2026-04-13 against:
  //   https://www.it-sd.com/articles/secure-digital-card-registers/
  //   https://www.cameramemoryspeed.com/sd-memory-card-faq/reading-sd-card-cid-serial-psn-internal-numbers/
  //   https://zeroalpha.com.au/services/data-recovery-blog/sd/secure-digital-sd-memory-card-technology-explained
  // Use the printed MID/OID/PNM/CID raw values as the authoritative log data.
  switch (mid) {
    case 0x01: return "Panasonic";
    case 0x02: return "Toshiba/Kioxia";
    case 0x03: return "SanDisk";
    case 0x1B: return "Samsung";
    case 0x1D: return "ADATA";
    case 0x1F: return "Kingston";
    case 0x27: return "Phison";
    case 0x28: return "Lexar";
    case 0x31: return "Silicon Power";
    case 0x41: return "Kingston/ATP";
    case 0x74: return "Transcend";
    case 0x76: return "Patriot";
    case 0x82: return "Sony";
    case 0x89: return "Delkin";
    default: return "Unknown";
  }
}

const char* SdCardDiag::cardTypeToString(uint8_t type) const {
  switch (type) {
    case CARD_MMC:
      return "MMC";
    case CARD_SD:
      return "SDSC";
    case CARD_SDHC:
      return "SDHC/SDXC";
    default:
      return "UNKNOWN";
  }
}

const char* SdCardDiag::boolToOnOff(bool value) const {
  return value ? "ON" : "OFF";
}
