#include <Arduino.h>
#define DEBOUNCE_DELAY 50      
#define BLINK_INTERVAL 500     

unsigned long lastBlinkTime = 0;
bool blinkState = false;

bool isPressed(uint8_t pinMask, volatile uint8_t* pinReg) {
  static unsigned long lastDebounceTime[8] = {0};   
  static bool lastStableState[8] = {true};          
  static bool lastReadState[8] = {true};            

  uint8_t pinIndex = 0;
  while ((pinMask >> pinIndex) != 1) pinIndex++;    // bepaalt welk bit van toepassing is

  bool reading = (*pinReg & pinMask);               // leest huidige status van de pin

  if (reading != lastReadState[pinIndex]) {
    lastDebounceTime[pinIndex] = millis();          // bij verandering: debounce timer starten
    lastReadState[pinIndex] = reading;
  }

  if ((millis() - lastDebounceTime[pinIndex]) > DEBOUNCE_DELAY) {
    lastStableState[pinIndex] = reading;            // status pas bijwerken na stabiele periode
  }

  return lastStableState[pinIndex];                 
}

void setup() {
  DDRD |= _BV(DDD5) | _BV(DDD6);
  Serial.begin(9600);   
}

void loop() {
  // Knopstatussen uitlezen met debounce
  bool Button1Pressed = isPressed(_BV(PINB2), &PINB);   // D10
  bool Button2Pressed = isPressed(_BV(PINB3), &PINB);   // D11

  if (Button1Pressed && !Button2Pressed) {
    // Alleen Button1 ingedrukt: LED D5 aan, D6 uit
    PORTD |= _BV(PORTD5);
    PORTD &= ~_BV(PORTD6);
  } else if (!Button1Pressed && Button2Pressed) {
    // Alleen Button2 ingedrukt: LED D6 aan, D5 uit en "Hello World!" printen
    PORTD &= ~_BV(PORTD5);
    PORTD |= _BV(PORTD6);
    static unsigned long lastHelloTime = 0;
    if (millis() - lastHelloTime > 100) {
      Serial.println("Hello World!\n");
      lastHelloTime = millis();
    }
  } else if (Button1Pressed && Button2Pressed) {
    // Beide knoppen ingedrukt: LEDs knipperen om en om
    if (millis() - lastBlinkTime >= BLINK_INTERVAL) {
      blinkState = !blinkState;
      lastBlinkTime = millis();
    }

    if (blinkState) {
      PORTD |= _BV(PORTD5);
      PORTD &= ~_BV(PORTD6);
    } else {
      PORTD &= ~_BV(PORTD5);
      PORTD |= _BV(PORTD6);
    }
  } else {
    // Geen knop ingedrukt: beide LEDs uit
    PORTD &= ~(_BV(PORTD5) | _BV(PORTD6));
  }
}