/*
 * This code utilizes the AVR libraries for I2C and UART communication.
 * Make sure to connect the SDA and SCL pins of the ATmega328P to the
 * corresponding pins of your I2C temperature sensor. Additionally, you might
 * need pull-up resistors for the I2C lines.
 */

#define LED_PIN PB5  // Define the pin connected to the LED

#include <avr/io.h>
#include <util/delay.h>
#include <math.h>

#include "MPU6050.h"
#include "i2c.h"
#include "uart.h"

void blink() {
    // Set the LED pin as output
    DDRB |= (1 << LED_PIN);

    while (1) {
        // Turn on the LED by setting the pin high
        PORTB |= (1 << LED_PIN);
        // Delay for 500 milliseconds
        _delay_ms(500);

        // Turn off the LED by setting the pin low
        PORTB &= ~(1 << LED_PIN);
        // Delay for 500 milliseconds
        _delay_ms(500);
    }
}

int main(void) {
    initUART();
    UART_println("ATmega328P Initialized!");
    I2C_init();
    UART_println("I2C Initialized!");
    MPU6050_Init();

    while(1) {
        UART_println("Hello, World!");
        _delay_ms(1000);
    }

    return 0;
}

// int main() {
//     int16_t accel_data[3]; // Array to store accelerometer data (X, Y, Z)
//     int16_t gyro_data[3]; // Array to store gyroscope data (X, Y, Z)

//     // Initialize MPU-6050
//     i2c_init();
//     MPU6050_Init();

//     while (1) {
//         // Read accelerometer data
//         MPU6050_Read_Accel(accel_data);
//         // Read gyroscope data
//         MPU6050_Read_Gyro(gyro_data);

//         // Print accelerometer and gyroscope data
//         printf("Accelerometer (mg): X=%d, Y=%d, Z=%d\n", accel_data[0], accel_data[1], accel_data[2]);
//         printf("Gyroscope (°/s): X=%d, Y=%d, Z=%d\n", gyro_data[0], gyro_data[1], gyro_data[2]);

//         _delay_ms(1000); // Delay for 1 second
//     }

//     return 0;
// }