#include <avr/io.h>
#include <avr/interrupt.h>

#define F_CPU 16000000UL    // 16MHz clock frequency
#define LED_PIN PD0         // LED is connected to Pin D0

volatile uint32_t systemClock = 0;

void setupTimer0() {
	// Configure Timer0 for CTC mode
	TCCR0A |= (1 << WGM01);                 // Set Waveform Generation Mode bits for CTC
	TCCR0B |= (1 << CS01) | (1 << CS00);    // Set prescaler to 64
	OCR0A = 249;                            // Set Output Compare Register to 249 for ~1ms interrupt
	TIMSK0 |= (1 << OCIE0A);                // Enable Timer0 Output Compare A Match interrupt
}

// Timer0 Output Compare A Match Interrupt Service Routine
ISR(TIMER0_COMPA_vect) {
	systemClock++;  // Increment the milliseconds counter
}

uint32_t getSystemClock(){
	cli();
	uint32_t tempSystemClock = systemClock;
	sei();
	return tempSystemClock;
}

void waitFor(uint32_t ms) {
	uint32_t endTime = getSystemClock() + ms;
	while (getSystemClock() != endTime);
}

void waitUntil(uint32_t ms) {
	while (getSystemClock() != ms);
}

void initializeLED() {
	DDRD |= (1 << LED_PIN);  // Set LED_PIN as an output
}

void toggleLED() {
	PORTD ^= (1 << LED_PIN); // Toggle the LED
}

int main(void) {
	initializeLED();    // Initialize the LED pin
	setupTimer0();      // Setup Timer0
	//sei();              // Enable global interrupts

	waitUntil(500);     // Wait until 500 ms have passed
	toggleLED();        // Toggle the LED to turn it on

	while (1) {
		waitFor(500);   // Wait for 500 ms
		toggleLED();    // Toggle the LED
	}
}