#include <stdint.h>
#include <avr/io.h>
#include <util/delay.h>
#include "MPU6050.h"
#include "i2c.h"
#include "uart.h"

// Function to initialize MPU-6050
void MPU6050_Init() {
    DEBUG("MPU6050_init: Initializing...");
    // Wake up MPU-6050 by writing to PWR_MGMT_1 register
    if(!I2C_start(MPU6050_ADDR << 1 | TW_WRITE)) {
        DEBUG("MPU6050_init: Failed to start I2C communication");
        return;
    }

    // These two seem to be causing problems
    DEBUG("MPU6050_init: Sending PWR_MGMT_1 register address");
    if(!I2C_write(MPU6050_REG_PWR_MGMT_1)) {
        DEBUG("MPU6050_init: Failed to write to PWR_MGMT_1 register");
        return;
    }

    DEBUG("Writing 0x00 to PWR_MGMT_1 register");
    if(!I2C_write(0x00)) { // Clear sleep mode bit
        DEBUG("MPU6050_init: Failed to write to PWR_MGMT_1 register");
        return;
    }

    DEBUG("MPU6050_init: Stopping...");
    I2C_stop();
    DEBUG("MPU6050_init: Initialized!");
}

// Function to read accelerometer data from MPU-6050
void MPU6050_Read_Accel(int16_t* accel_data) {
    uint8_t buffer[6]; // Buffer to store accelerometer data

    // Read accelerometer data starting from ACCEL_XOUT_H register
    if(!I2C_start(MPU6050_ADDR << 1 | TW_WRITE)) {
        DEBUG("MPU6050_Read_Accel: Failed to start I2C communication: %d", MPU6050_ADDR << 1 | TW_WRITE);
        return;
    }

    DEBUG("MPU6050_Read_Accel: Sending ACCEL_XOUT_H register address");
    if(I2C_write(MPU6050_REG_ACCEL_XOUT_H)) {
        DEBUG("MPU6050_Read_Accel: Failed to write to ACCEL_XOUT_H register");
        return;
    }

    DEBUG("MPU6050_Read_Accel: Reading accelerometer data");
    I2C_start(MPU6050_ADDR << 1 | TW_READ);
    for (int i = 0; i < 6; i++) {
        DEBUG("MPU6050_Read_Accel: Reading byte %d", i);
        buffer[i] = I2C_read(i < 5); // Read 6 bytes with ACK for all except last byte
    }
    DEBUG("MPU6050_Read_Accel: Stopping...");
    I2C_stop();

    // Combine high and low bytes for each axis
    accel_data[0] = (buffer[0] << 8) | buffer[1]; // X-axis
    accel_data[1] = (buffer[2] << 8) | buffer[3]; // Y-axis
    accel_data[2] = (buffer[4] << 8) | buffer[5]; // Z-axis
}

// Function to read gyroscope data from MPU-6050
void MPU6050_Read_Gyro(int16_t* gyro_data) {
    // TODO: Implement this function
}