#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include "../lib/usart/src/usart.h"

#define F_CPU 8000000UL // CPU Clock Frequency in Hz

// Define segments for each digit
#define A (1<<PB5)
#define B (1<<PB7)
#define C (1<<PB1)
#define D (1<<PB3)
#define E (1<<PB4)
#define F (1<<PB6)
#define G (1<<PB0)
#define DP (1<<PB2)

// Segment patterns for digits 0 to 9
const uint8_t numbers[] = {
    (A | B | C | D | E | F),   // 0
    (B | C),                   // 1
    (A | B | G | E | D),       // 2
    (A | B | C | D | G),       // 3
    (F | G | B | C),           // 4
    (A | F | G | C | D),       // 5
    (A | F | E | D | C | G),   // 6
    (A | B | C),               // 7
    (A | B | C | D | E | F | G), // 8
    (A | B | C | F | G)        // 9
};

bool decimalpoint[6] = {false, false, false, false, false, false};

// Array to store digits to be displayed
volatile uint8_t digits[] = {0, 0, 0, 0, 0, 0};

// Function to turn on a specific digit
void turnOnDigit(uint8_t digit) {
    decimalpoint[digit] ? PORTB |= DP : PORTB &= ~DP;
    switch (digit) {
        case 0:
            PORTA |= (1<<PA1);
            break;
        case 1:
            PORTA |= (1<<PA0);
            break;
        case 2:
            PORTD |= (1<<PD2);
            break;
        case 3:
            PORTD |= (1<<PD3);
            break;
        case 4:
            PORTD |= (1<<PD4);
            break;
        case 5:
            PORTD |= (1<<PD5);
            break;
        default:
            break;
    }
}

// Timer/Counter1 Compare Match A interrupt
ISR(TIMER1_COMPA_vect) {
    static uint8_t currentDigit = 0;
    PORTA &= ~((1<<PA1) | (1<<PA0));
    PORTD &= ~((1<<PD2) | (1<<PD3) | (1<<PD4) | (1<<PD5));
    PORTB = numbers[digits[currentDigit]];
    turnOnDigit(currentDigit); // Turn on the corresponding digit
    currentDigit = (currentDigit + 1) % 6; // Move to the next digit
}

int main() {
    // Set PORTB as output for segments
    DDRB = 0xFF;
    // Set PORTA and PORTD as output for digit control
    DDRA |= (1<<PA0) | (1<<PA1);
    DDRD |= (1<<PD2) | (1<<PD3) | (1<<PD4) | (1<<PD5);

    // Initialize Timer/Counter1 in CTC mode
    TCCR1B |= (1 << WGM12);
    // Set prescaler to 1024
    TCCR1B |= (1 << CS12) | (0 << CS11) | (1 << CS10);
    // Set compare value for 1ms interrupt at 8MHz CPU clock
    OCR1A = 7;
    // Enable Timer/Counter1 Compare Match A interrupt
    TIMSK |= (1 << OCIE1A);

    usart::init(9600); // Initialize USART with baud rate 9600

    // Enable global interrupts
    sei();

    uint8_t n = 0;
    uint8_t d = 0;
    usart::printf("Started");
    while (1) {
        char c =  usart::get();
        if(c == '\r') {
            usart::put('\n');
            usart::put('\r');
        } else {
            usart::put(c);
        }
        if(c == 'n') {
            n = 0;
            d = 0;
            for(int i = 0; i < 6; i++) {
                digits[i] = 0;
                decimalpoint[i] = false;
            }
        }
        if(c == '.' && n > 0) {
            decimalpoint[n-1] = true;
        }
        if(c >= '0' && c <= '9' && n < 6) {
            digits[n++] = c - '0';
        }
    }

    return 0;
}