#include "SystemStartup.h"

#include <Wire.h>
#include <U8g2lib.h>

#ifndef OLED_SDA
#define OLED_SDA 17
#endif

#ifndef OLED_SCL
#define OLED_SCL 18
#endif

#ifndef OLED_ADDR
#define OLED_ADDR 0x3C
#endif

static const bool kEnableOled = true;

static U8G2_SH1106_128X64_NONAME_F_HW_I2C g_oled(U8G2_R0, /* reset=*/U8X8_PIN_NONE);
static SystemStartup* g_activeSystemStartup = nullptr;

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

SystemStartup::SystemStartup(Print& serial) : serial_(serial), sd_(serial) {}

bool SystemStartup::begin(const SystemStartupConfig& cfg, SystemEventCallback callback) {
  cfg_ = cfg;
  callback_ = callback;
  g_activeSystemStartup = this;

  // Match Exercise 05 behavior: deselect SPI devices immediately at startup.
  forceSpiDeselectedEarly();

  if (kEnableOled) {
    Wire.begin(OLED_SDA, OLED_SCL);
    g_oled.setI2CAddress(OLED_ADDR << 1);
    g_oled.begin();
  }

  emit(SystemEvent::BOOTING, "System startup booting");
  oledShow3("System Startup", "Booting...");

  serial_.printf("Sleeping for %lu ms to allow Serial Monitor connection...\r\n",
                 (unsigned long)cfg_.serialDelayMs);
  delay(cfg_.serialDelayMs);

  return sd_.begin(cfg_.sd, &SystemStartup::onSdEventThunk);
}

void SystemStartup::update() {
  sd_.update();
}

void SystemStartup::onSdEventThunk(SdEvent event, const char* message) {
  if (g_activeSystemStartup != nullptr) {
    g_activeSystemStartup->onSdEvent(event, message);
  }
}

void SystemStartup::onSdEvent(SdEvent event, const char* message) {
  if (event == SdEvent::NO_CARD) {
    oledShow3("SD missing or", "invalid FAT16/32", "Insert/format card");
    emit(SystemEvent::SD_MISSING, message);
  } else if (event == SdEvent::CARD_MOUNTED) {
    oledShow3("SD card ready", "Mounted OK");
    emit(SystemEvent::SD_READY, message);
  } else if (event == SdEvent::CARD_REMOVED) {
    oledShow3("SD card removed", "Please re-insert");
    emit(SystemEvent::SD_REMOVED, message);
  }
}

void SystemStartup::emit(SystemEvent event, const char* message) {
  serial_.printf("[SYSTEM] %s\r\n", message);
  if (callback_ != nullptr) {
    callback_(event, message);
  }
}

void SystemStartup::oledShow3(const char* l1, const char* l2, const char* l3) {
  if (!kEnableOled) {
    return;
  }
  g_oled.clearBuffer();
  g_oled.setFont(u8g2_font_6x10_tf);
  if (l1) g_oled.drawUTF8(0, 16, l1);
  if (l2) g_oled.drawUTF8(0, 32, l2);
  if (l3) g_oled.drawUTF8(0, 48, l3);
  g_oled.sendBuffer();
}