#include "pico/stdlib.h"
#include "hardware/i2c.h"
#include <math.h>
#include <stdio.h>

// Definições de GPIOs e endereço do MPU6050
#define MPU6050_ADDR 0x68
#define SDA_PIN 4
#define SCL_PIN 5

void inicializar_mpu6050() {
    uint8_t reset[2] = {0x6B, 0x00}; // Endereço do registrador Power Management 1
    i2c_write_blocking(i2c0, MPU6050_ADDR, reset, 2, false);
}

void ler_acelerometro(int16_t *ax, int16_t *ay, int16_t *az) {
    uint8_t buffer[6];
    i2c_write_blocking(i2c0, MPU6050_ADDR, (uint8_t[]){0x3B}, 1, true); // Registrador do acelerômetro
    i2c_read_blocking(i2c0, MPU6050_ADDR, buffer, 6, false);

    *ax = (buffer[0] << 8) | buffer[1];
    *ay = (buffer[2] << 8) | buffer[3];
    *az = (buffer[4] << 8) | buffer[5];
}

float calcular_inclinacao(int16_t ax, int16_t ay, int16_t az) {
    float ax_g = ax / 16384.0; // Converte para g
    float ay_g = ay / 16384.0;
    float az_g = az / 16384.0;

    float inclinacao = atan2(ax_g, sqrt(ay_g * ay_g + az_g * az_g)) * (180.0 / M_PI);
    return inclinacao;
}

int main() {
    stdio_init_all();
    i2c_init(i2c0, 100000); // Inicializa I2C
    gpio_set_function(SDA_PIN, GPIO_FUNC_I2C);
    gpio_set_function(SCL_PIN, GPIO_FUNC_I2C);

    inicializar_mpu6050();

    int16_t ax, ay, az;

    while (true) {
        ler_acelerometro(&ax, &ay, &az);

        float inclinacao = calcular_inclinacao(ax, ay, az);

        printf("Acelerômetro: X=%d, Y=%d, Z=%d\n", ax, ay, az);
        printf("Inclinação no eixo X: %.2f graus\n", inclinacao);

        sleep_ms(1000); // Atualiza a cada 1 segundo
    }
}