#include <Arduino.h>

#define BLINK_INTERVAL 500
#define DEBOUNCE_DELAY 50

volatile bool Button1 = false;
volatile bool Button2 = false;

volatile unsigned long lastDebounceTimeButton1 = 0;
volatile unsigned long lastDebounceTimeButton2 = 0;

bool lastButton1State = true; 
bool lastButton2State = true;

unsigned long lastBlinkTime = 0;
bool blinkState = false;

void setup() {
  // LEDs op D5 en D6 als output
  DDRD |= _BV(DDD5) | _BV(DDD6);

  // Knoppen D10 (PB2) en D11 (PB3) als input met pull-up
  DDRB &= ~(_BV(DDB2) | _BV(DDB3));
  PORTB |= _BV(PORTB2) | _BV(PORTB3);

  // Pin Change Interrupts activeren voor PB2 en PB3
  PCICR |= _BV(PCIE0); // Enable PCINT[7:0] (PORTB)
  PCMSK0 |= _BV(PCINT2) | _BV(PCINT3);

  sei();

  Serial.begin(9600);
}

ISR(PCINT0_vect) {
  unsigned long now = millis();

  // Lees huidige toestand
  bool currentButton1 = PINB & _BV(PINB2);
  bool currentButton2 = PINB & _BV(PINB3);

  // Detecteer falling edge voor button1 (hoog -> laag)
  if (lastButton1State && !currentButton1 && (now - lastDebounceTimeButton1 > DEBOUNCE_DELAY)) {
    Button1 = !Button1; // toggle status
    lastDebounceTimeButton1 = now;
  }

  // Detecteer falling edge voor button 2
  if (lastButton2State && !currentButton2 && (now - lastDebounceTimeButton2 > DEBOUNCE_DELAY)) {
    Button2 = !Button2; // toggle status
    lastDebounceTimeButton2 = now;
  }

  // Update vorige staat
  lastButton1State = currentButton1;
  lastButton2State = currentButton2;
}

void loop() {
  if (Button1 && !Button2) {
    PORTD |= _BV(PORTD5);
    PORTD &= ~_BV(PORTD6);
  } else if (!Button1 && Button2) {
    PORTD &= ~_BV(PORTD5);
    PORTD |= _BV(PORTD6);

    static unsigned long lastHelloTime = 0;
    if (millis() - lastHelloTime > 100) {
      Serial.println("Hello World!");
      lastHelloTime = millis();
    }
  } else if (Button1 && Button2) {
    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 {
    PORTD &= ~(_BV(PORTD5) | _BV(PORTD6));
  }
}