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

// ===== OLED inställningar =====
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 32
#define OLED_ADDR 0x3C

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// ===== Pins (samma som din Rust-kod) =====
#define BUTTON1 7
#define BUTTON2 8
#define LED_PIN 25   // inbyggd LED på Pico

void setup() {
  // Starta seriell (valfri)
  Serial.begin(115200);

  // Knappar (pull-up)
  pinMode(BUTTON1, INPUT_PULLUP);
  pinMode(BUTTON2, INPUT_PULLUP);

  // LED
  pinMode(LED_PIN, OUTPUT);

  // I2C (samma pins som din Rust-kod)
  Wire.setSDA(20);
  Wire.setSCL(21);
  Wire.begin();

  // Starta display
  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
    Serial.println("OLED fel!");
    while (true); // stanna om fel
  }

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
}

void loop() {
  // Läs knappar (LOW = tryckt)
  bool button1Pressed = (digitalRead(BUTTON1) == LOW);
  bool button2Pressed = (digitalRead(BUTTON2) == LOW);

  // LED logik
  if (button1Pressed || button2Pressed) {
    digitalWrite(LED_PIN, HIGH);
  } else {
    digitalWrite(LED_PIN, LOW);
  }

  // ===== Display =====
  display.clearDisplay();

  display.setCursor(0, 0);
  display.println("System igang");

  display.setCursor(0, 10);
  display.print("Knapp1: ");
  display.println(button1Pressed ? "TRYCKT" : "UPPE");

  display.setCursor(0, 20);
  display.print("Knapp2: ");
  display.println(button2Pressed ? "TRYCKT" : "UPPE");

  display.display();

  delay(100); // liten paus
}