#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdio.h> // For dtostrf function

#define F_CPU 16000000UL
#define BAUD_RATE 9600
#define BAUD_PRESCALE ((F_CPU / (16UL * BAUD_RATE)) - 1)

float x, x_old, u = 1.0, tau, delt;
char counter_str[16];
volatile uint16_t ms_counter = 0;
volatile uint8_t sec_counter = 0;
float x_values[100]; // Array to store x values
uint8_t x_index = 0; // Index for the x_values array

void init_uart(void) {
    UBRR0H = (BAUD_PRESCALE >> 8);
    UBRR0L = BAUD_PRESCALE;

    UCSR0B = (1 << TXEN0); // Enable transmitter
    UCSR0C = (1 << UCSZ01) | (1 << UCSZ00); // 8-bit data
}

void uart_send_char(char data) {
    while (!(UCSR0A & (1 << UDRE0))); // Wait for empty transmit buffer
    UDR0 = data; // Put data into buffer, sends the data
}

void uart_send_string(const char *str) {
    while (*str) {
        uart_send_char(*str);
        str++;
    }
}

// Timer1 Compare Match A interrupt handler
ISR(TIMER1_COMPA_vect) {
    ms_counter++; // Increment the millisecond counter

    if (ms_counter >= 1000) {
        ms_counter = 0; // Reset the millisecond counter

        sec_counter++; // Increment the second counter
        if (sec_counter >= 10) {
            sec_counter = 0; // Reset the second counter

            // Toggle the value of u every second
            if (u == 1.0) {
                u = 0.0;
            } else {
                u = 1.0;
            }
        }
    }

    x = (delt / tau) * (u - x) + x_old;
    x_old = x;

    // Store x value in the array
    if (x_index < 100) {
        x_values[x_index] = x;
        x_index++;
    }
}

int main(void) {
    tau = 0.025;
    delt = 0.001;

    // Initialize UART
    init_uart();

    // Configure Timer1 for compare match interrupt every 1 ms
    TCCR1B |= (1 << WGM12);    // CTC mode
    OCR1A = 15999;             // 16MHz / 1024 = 15625; 1ms = 16000 - 1
    TIMSK1 |= (1 << OCIE1A);   // Enable Timer1 compare match A interrupt
    TCCR1B |= (1 << CS12) | (1 << CS10); // Start Timer1 with prescaler 1024

    sei(); // Enable global interrupts

    while (1) {
        // Print the x values stored in the array through UART
        for (uint8_t i = 0; i < x_index; i++) {
            dtostrf(x_values[i], 0, 2, counter_str);
            uart_send_string(counter_str);
            uart_send_string("\r\n");
        }

        // Clear the x values array and index
        x_index = 0;

        // Delay before printing the x values again
        _delay_ms(5000); // Delay for 5 seconds
    }

    return 0;
}