#include <Arduino.h>

#define BUTTON_D10_PIN (1 << 2) // PB2 (D10) - SLOW DOWN
#define BUTTON_D11_PIN (1 << 3) // PB3 (D11) - SPEED UP

#define INTERNAL_LED_PIN (1 << 5) // PB5 (D13)
#define LED_D5_PIN (1 << 5)       // PD5 (D5)
#define LED_D6_PIN (1 << 6)       // PD6 (D6)
#define LED_D8_PIN (1 << 0)       // PB0 (D8)
#define LED_D9_PIN (1 << 1)       // PB1 (D9)

#define LED_PORTD_SHIFT 5

#define LED_PORTB_SHIFT 2

#define LSN_LOW (0b11)
#define LSN_HIGH (0b1100)

#define DEBOUNCE_DELAY 50

unsigned long currentTime = 0;
unsigned long lastDebounceD10 = 0;
unsigned long lastDebounceD11 = 0;

volatile bool buttonD10Flag = false;
volatile bool buttonD11Flag = false;

volatile int timerCounter = 0;

bool buttonD10State = false;
bool buttonD11State = false;

volatile bool pastButtonD10State = false;
volatile bool pastButtonD11State = false;

// Track last prescaler to avoid unnecessary writes
uint8_t lastPrescaler = 0;

// --- Prescaler Setting Function (atomic, only if changed) ---
void setPrescaler(uint8_t prescalerBits) {
  if (lastPrescaler != prescalerBits) {
    cli();
    TCCR1B &= ~((1 << CS12) | (1 << CS11) | (1 << CS10));
    TCCR1B |= prescalerBits;
    sei();
    lastPrescaler = prescalerBits;
  }
}

ISR(TIMER1_COMPA_vect)
{
  if (timerCounter > 0x0F) timerCounter = 0;
  if ((timerCounter % 4) == 0) PORTB ^= INTERNAL_LED_PIN;
  timerCounter++;
}

ISR(PCINT0_vect)
{
  bool currentD10State = (PINB & BUTTON_D10_PIN) == 0;
  if (pastButtonD10State != currentD10State) buttonD10Flag = true;
  pastButtonD10State = currentD10State;

  bool currentD11State = (PINB & BUTTON_D11_PIN) == 0;
  if (pastButtonD11State != currentD11State) buttonD11Flag = true;
  pastButtonD11State = currentD11State;
}

void setup() {
  DDRD |= (LED_D5_PIN) | (LED_D6_PIN);
  DDRB |= (LED_D8_PIN) | (LED_D9_PIN) | INTERNAL_LED_PIN;
  DDRB &= ~(BUTTON_D10_PIN | BUTTON_D11_PIN);
  PORTB |= (BUTTON_D10_PIN | BUTTON_D11_PIN);

  cli();
  TCCR1A = 0;
  TCCR1B = 0;
  TCNT1 = 0;
  OCR1A = 31249; // 4Hz with prescaler 64
  TCCR1B |= (1 << WGM12); // CTC mode
  setPrescaler((1 << CS11) | (1 << CS10)); // Start with prescaler 64
  TIMSK1 |= (1 << OCIE1A);

  PCICR |= (1 << PCIE0);
  PCMSK0 |= (1 << PCINT2) | (1 << PCINT3);

  sei();

  Serial.begin(9600);
}

void loop() {
  currentTime = millis();

  // Debounce D10
  if (buttonD10Flag) {
    buttonD10Flag = false;
    if (currentTime - lastDebounceD10 > DEBOUNCE_DELAY) {
      lastDebounceD10 = currentTime;
      buttonD10State = (PINB & BUTTON_D10_PIN) == 0;
    }
  }

  // Debounce D11
  if (buttonD11Flag) {
    buttonD11Flag = false;
    if (currentTime - lastDebounceD11 > DEBOUNCE_DELAY) {
      lastDebounceD11 = currentTime;
      buttonD11State = (PINB & BUTTON_D11_PIN) == 0;
    }
  }

  // Set prescaler based on button state (only if changed)
  if (buttonD10State) {
    setPrescaler((1 << CS12)); // 256
    Serial.println("a");
  } else if (buttonD11State) {
    setPrescaler((1 << CS11)); // 8
    Serial.println("b");
  } else {
    setPrescaler((1 << CS11) | (1 << CS10)); // 64 (default)
    Serial.println("None");
  }

  // Update LEDs for counter value (4 bits: D5, D6, D8, D9)
  PORTD &= ~(LSN_LOW << LED_PORTD_SHIFT);
  PORTD |= ((timerCounter & LSN_LOW) << LED_PORTD_SHIFT);

  PORTB &= ~(LSN_LOW);
  PORTB |= ((timerCounter & LSN_HIGH) >> LED_PORTB_SHIFT);
}