#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1 
#define SCREEN_ADDRESS 0x3C // If it doesn't work, try 0x3D

// Initialize the display using the "Wire" I2C bus
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void setup() {
  // 1. Setup Status LEDs (Active LOW on RP2040)
  pinMode(17, OUTPUT); // Red
  pinMode(16, OUTPUT); // Green
  digitalWrite(17, HIGH); // Off
  digitalWrite(16, HIGH); // Off

  // 2. Define I2C Pins for XIAO RP2040 (D4 = SDA, D5 = SCL)
  Wire.setSDA(6);
  Wire.setSCL(7);
  Wire.begin();
  Wire.setClock(100000); // Set to 100kHz for stability

  // 3. Try to initialize the OLED
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    digitalWrite(17, LOW); // Turn on RED if failed
    while(true); // Stop here
  }

  // 4. Success! Turn on Green LED
  digitalWrite(16, LOW); 

  // 5. Show something on the screen
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 20);
  display.println(F("RP2040 OK"));
  display.display();
}

void loop() {
  // Just stay alive
}