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

// OLED-skärmen
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1 // Skärmen har ingen resetpin
#define OLED_ADDRESS 0x3C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// LED-pins
const int ledRed = 2;     // Systemstatus
const int ledYellow = 3;  // Mätningsstatus
const int ledGreen = 4;   // Skickastatus
const int ledBlue = 5;    // Extra funktion

// Knapp-pins
const int btnOnOff = 6;
const int btnMeasure = 7;
const int btnSend = 8;
const int btnExtra = 9;

// Systemets status
bool isSystemOn = false;
bool isMeasuring = false;
bool isSending = false;

void setup() {
  // Initiera seriekommunikation för debugging
  Serial.begin(9600);

  // Initiera OLED-skärmen
  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDRESS)) {
    Serial.println("OLED init misslyckades");
    for (;;);
  }
  display.clearDisplay();

  // Ställ in LED-pins som utgångar
  pinMode(ledRed, OUTPUT);
  pinMode(ledYellow, OUTPUT);
  pinMode(ledGreen, OUTPUT);
  pinMode(ledBlue, OUTPUT);

  // Ställ in knapp-pins som ingångar med pullup
  pinMode(btnOnOff, INPUT_PULLUP);
  pinMode(btnMeasure, INPUT_PULLUP);
  pinMode(btnSend, INPUT_PULLUP);
  pinMode(btnExtra, INPUT_PULLUP);

  // Initiera systemstatus på OLED och LEDs
  updateDisplay("System OFF");
  digitalWrite(ledRed, LOW);
}

void loop() {
  // Läs knappstatus
  bool onOffPressed = !digitalRead(btnOnOff);
  bool measurePressed = !digitalRead(btnMeasure);
  bool sendPressed = !digitalRead(btnSend);
  bool extraPressed = !digitalRead(btnExtra);

  // Hantera On/Off-knappen
  if (onOffPressed) {
    delay(200); // Debounce
    isSystemOn = !isSystemOn;
    if (isSystemOn) {
      updateDisplay("System ON");
      digitalWrite(ledRed, HIGH);
    } else {
      updateDisplay("System OFF");
      digitalWrite(ledRed, LOW);
      resetLEDs();
    }
  }

  // Hantera Mätningsknappen
  if (isSystemOn && measurePressed && !isMeasuring) {
    delay(200); // Debounce
    isMeasuring = true;
    digitalWrite(ledYellow, HIGH);
    updateDisplay("Measuring...");
    delay(3000); // Simulera mätning
    isMeasuring = false;
    digitalWrite(ledYellow, LOW);
    digitalWrite(ledYellow, HIGH); // Lysa konstant efter mätning
    updateDisplay("Measurement Done");
  }

  // Hantera Skickaknappen
  if (isSystemOn && sendPressed && !isSending) {
    delay(200); // Debounce
    isSending = true;
    digitalWrite(ledGreen, HIGH);
    updateDisplay("Sending...");
    delay(3000); // Simulera skickning
    digitalWrite(ledGreen, LOW);
    updateDisplay("Sent");
    delay(5000); // Låt LED lysa i 5 sekunder
    isSending = false;
    digitalWrite(ledGreen, LOW);
  }

  // Hantera Extra-knappen
  if (isSystemOn && extraPressed) {
    delay(200); // Debounce
    updateDisplay("Extra Function");
    digitalWrite(ledBlue, HIGH);
    delay(1000); // Simulera funktion
    digitalWrite(ledBlue, LOW);
  }
}

// Funktion för att uppdatera skärmen
void updateDisplay(const char* message) {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println(message);
  display.display();
}

// Funktion för att stänga av alla LEDs
void resetLEDs() {
  digitalWrite(ledRed, LOW);
  digitalWrite(ledYellow, LOW);
  digitalWrite(ledGreen, LOW);
  digitalWrite(ledBlue, LOW);
}