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

// XIAO ESP32C3 I2C pins:
// D4 = SDA = GPIO6
// D5 = SCL = GPIO7
static const int OLED_SDA = D4;
static const int OLED_SCL = D5;

// Use software I2C first because this matches Seeed's SH1107 Grove OLED example.
// U8g2 SW_I2C argument order is: clock, data, reset.
U8G2_SH1107_SEEED_128X128_1_SW_I2C u8g2(
  U8G2_R0,
  /* clock=*/ OLED_SCL,
  /* data=*/ OLED_SDA,
  /* reset=*/ U8X8_PIN_NONE
);

void i2cScan() {
  Serial.println();
  Serial.println("I2C scan start...");

  int found = 0;

  // Hardware Wire scan. This does not prove U8g2 SW_I2C works,
  // but it proves the display is visible on the bus.
  Wire.begin(OLED_SDA, OLED_SCL);
  Wire.setClock(100000);

  for (uint8_t addr = 1; addr < 127; addr++) {
    Wire.beginTransmission(addr);
    uint8_t error = Wire.endTransmission();

    if (error == 0) {
      Serial.print("FOUND I2C device at 0x");
      if (addr < 16) Serial.print("0");
      Serial.println(addr, HEX);
      found++;
    } else if (error == 4) {
      Serial.print("Unknown error at 0x");
      if (addr < 16) Serial.print("0");
      Serial.println(addr, HEX);
    }
  }

  if (found == 0) {
    Serial.println("NO I2C devices found.");
    Serial.println("Check: SDA=D4/GPIO6, SCL=D5/GPIO7, VCC, GND, Grove cable orientation.");
  } else {
    Serial.print("Total devices found: ");
    Serial.println(found);
    Serial.println("Expected OLED address is commonly 0x3C or 0x3D.");
  }

  Serial.println("I2C scan done.");
  Serial.println();
}

void drawDebugScreen(const char *msg) {
  u8g2.firstPage();
  do {
    u8g2.setFont(u8g2_font_6x12_tf);

    u8g2.drawFrame(0, 0, 128, 128);
    u8g2.drawStr(4, 14, "SH1107 OLED DEBUG");
    u8g2.drawStr(4, 32, msg);

    u8g2.drawStr(4, 52, "SDA: D4 / GPIO6");
    u8g2.drawStr(4, 66, "SCL: D5 / GPIO7");

    u8g2.drawStr(4, 88, "If visible:");
    u8g2.drawStr(4, 102, "I2C + U8g2 OK");

    // Draw high-contrast test shapes
    u8g2.drawBox(90, 90, 28, 28);
    u8g2.drawCircle(104, 50, 15);
  } while (u8g2.nextPage());
}

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

  Serial.println();
  Serial.println("===== XIAO ESP32C3 + SH1107 OLED DEBUG =====");
  Serial.print("OLED_SDA pin value: ");
  Serial.println(OLED_SDA);
  Serial.print("OLED_SCL pin value: ");
  Serial.println(OLED_SCL);

  i2cScan();

  Serial.println("Starting U8g2...");
  u8g2.begin();

  // Force display on, disable power save.
  u8g2.setPowerSave(0);

  Serial.println("Drawing debug screen...");
  drawDebugScreen("u8g2.begin() done");

  Serial.println("Setup complete.");
}

void loop() {
  static uint32_t counter = 0;

  Serial.print("Loop counter: ");
  Serial.println(counter);

  char line[32];
  snprintf(line, sizeof(line), "Counter: %lu", (unsigned long)counter++);

  u8g2.firstPage();
  do {
    u8g2.setFont(u8g2_font_6x12_tf);
    u8g2.drawFrame(0, 0, 128, 128);
    u8g2.drawStr(4, 16, "OLED alive test");
    u8g2.drawStr(4, 36, line);
    u8g2.drawStr(4, 60, "Addr scan in Serial");
    u8g2.drawBox((counter * 8) % 100, 96, 20, 20);
  } while (u8g2.nextPage());

  delay(1000);
}