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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins on STM32)
#define OLED_RESET    -1  // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void setup() {
  // Initialize I2C communication as master
  Serial.begin(9600);
  Wire.begin();
  pinMode(PB5, OUTPUT);
  digitalWrite(PB5, LOW);
  
  // Initialize SSD1306 OLED display
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {  // Address 0x3C for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  Serial.println("Start");
  // Clear the display buffer
  digitalWrite(PB5, HIGH);
  Wire.beginTransmission(0x3C);
  Wire.write("Test");
  Wire.endTransmission();
  //display.clearDisplay();
  digitalWrite(PB5, LOW);
  Serial.println("Ready");
  delay(5000);
  
  // Set text size, color and print message
  display.setTextSize(1);      // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE); // White text
  display.setCursor(0, 0);     // Start at top-left corner
  
  // Display message
  display.println("Hello, STM32!");
  
  // Display buffer on the screen
  display.display();
}

void loop() {
  // Nothing to do here
}