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

// OLED scherm instellingen
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// Pin definities Wemos D1 R32
const int autoSwitch = 16;       // GPIO16 - auto aan/uit
const int leftSwitch = 17;       // GPIO17 - links richting
const int rightSwitch = 5;       // GPIO5  - rechts richting
const int hazardSwitch = 18;     // GPIO18 - gevaarslichten
const int headlightsSwitch = 19; // GPIO19 - koplampen
const int fogSwitch = 21;        // GPIO21 - mistlampen

const int speedUpButton = 22;    // GPIO22 - snelheid omhoog
const int speedDownButton = 23;  // GPIO23 - snelheid omlaag
const int hornButton = 4;        // GPIO4  - claxon knop
const int beamToggleButton = 15; // GPIO15 - schakelen koplamp/verstraler
const int buzzerPin = 2;         // GPIO2  - passieve buzzer

int speed = 0;
bool highBeams = false;

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

  // Pins als input met interne pullup waar nodig
  pinMode(autoSwitch, INPUT_PULLUP);
  pinMode(leftSwitch, INPUT_PULLUP);
  pinMode(rightSwitch, INPUT_PULLUP);
  pinMode(hazardSwitch, INPUT_PULLUP);
  pinMode(headlightsSwitch, INPUT_PULLUP);
  pinMode(fogSwitch, INPUT_PULLUP);
  pinMode(speedUpButton, INPUT_PULLUP);
  pinMode(speedDownButton, INPUT_PULLUP);
  pinMode(hornButton, INPUT_PULLUP);
  pinMode(beamToggleButton, INPUT_PULLUP);
  pinMode(buzzerPin, OUTPUT);

  // OLED initialisatie
  Wire.begin(21, 22); // SDA=21, SCL=22
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  display.clearDisplay();
  display.display();
}

void loop() {
  // Auto aan/uit status (actief laag)
  bool autoOn = digitalRead(autoSwitch) == LOW;

  if (autoOn) {
    if (digitalRead(speedUpButton) == LOW && speed < 180) {
      speed += 5;
      delay(150); // debounce
    }
    if (digitalRead(speedDownButton) == LOW && speed > 0) {
      speed -= 5;
      delay(150);
    }
    if (digitalRead(beamToggleButton) == LOW) {
      highBeams = !highBeams;
      delay(300);
    }

    // Claxon aan/uit met 250 Hz toon
    if (digitalRead(hornButton) == LOW) {
      tone(buzzerPin, 250);
    } else {
      noTone(buzzerPin);
    }

    // OLED display update
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(0,0);
    display.print("Snelheid:");

    display.setTextSize(3);
    display.setCursor(0, 20);
    display.print(speed);
    display.print(" km/u");

    display.setTextSize(1);
    display.setCursor(0, 55);
    display.print(highBeams ? "Verstralers" : "Koplampen");

    // Weergeven status schakelaars (kan verder uitgebreid)
    display.setCursor(80, 0);
    if (digitalRead(leftSwitch) == LOW) display.print("L ");
    if (digitalRead(rightSwitch) == LOW) display.print("R ");
    if (digitalRead(hazardSwitch) == LOW) display.print("HAZ ");
    if (digitalRead(headlightsSwitch) == LOW) display.print("HL ");
    if (digitalRead(fogSwitch) == LOW) display.print("MST ");

    display.display();

  } else {
    noTone(buzzerPin); // claxon uit als auto uit is
    display.clearDisplay();
    display.setTextSize(2);
    display.setCursor(20, 20);
    display.print("Auto uit");
    display.display();
  }

  delay(100);
}