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

#define I2C_PORT i2c0
#define MPU9250_ENDERECO 0x68  // Endereço I2C padrão do MPU-9250
#define MAGNETO_ENDERECO 0x0C  // Endereço do magnetômetro AK8963

// Sensibilidades padrão
#define SENSIBILIDADE_GIRO 131.0   // ±250°/s → 131 LSB/°/s
#define SENSIBILIDADE_ACELERACAO 16384.0 // ±2g → 16384 LSB/g
#define GRAVIDADE 9.81            // m/s²

// Função para escrever um registro no MPU-9250
void mpu9250_escrever(uint8_t reg, uint8_t dado) {
    uint8_t buf[] = {reg, dado};
    i2c_write_blocking(I2C_PORT, MPU9250_ENDERECO, buf, 2, false);
}

// Função para ler registros do MPU-9250
void mpu9250_ler(uint8_t reg, uint8_t *buf, uint16_t tamanho) {
    i2c_write_blocking(I2C_PORT, MPU9250_ENDERECO, &reg, 1, true);
    i2c_read_blocking(I2C_PORT, MPU9250_ENDERECO, buf, tamanho, false);
}

// Função para inicializar o MPU-9250
void mpu9250_inicializar() {
    mpu9250_escrever(0x6B, 0x00); // Desativa o modo de suspensão
}

int main() {
  stdio_init_all();

  
  i2c_init(I2C_PORT, 400 * 1000);
  gpio_set_function(8, GPIO_FUNC_I2C);
  gpio_set_function(9, GPIO_FUNC_I2C);
  gpio_pull_up(8);
  gpio_pull_up(9);

  printf("Inicializando MPU-9250...\n");
  mpu9250_inicializar();

  uint8_t id;
  mpu9250_ler(0x75, &id, 1);
  printf("ID do sensor: 0x%X\n", id);

  int16_t aceleracao_bruto[3], giro_bruto[3], temperatura_bruto;
  float aceleracao[3], giro[3], temperatura;
  uint8_t buffer[6];

  while (1) {
    // Ler acelerômetro
    mpu9250_ler(0x3B, buffer, 6);
    for (int i = 0; i < 3; i++) {
        aceleracao_bruto[i] = (int16_t)((buffer[i * 2] << 8) | buffer[i * 2 + 1]);
        aceleracao[i] = (aceleracao_bruto[i] / SENSIBILIDADE_ACELERACAO) * GRAVIDADE; // Em m/s²
    }

    // Ler giroscópio
    mpu9250_ler(0x43, buffer, 6);
    for (int i = 0; i < 3; i++) {
        giro_bruto[i] = (int16_t)((buffer[i * 2] << 8) | buffer[i * 2 + 1]);
        giro[i] = giro_bruto[i] / SENSIBILIDADE_GIRO; // Em °/s
    }

    // Exibir valores corretamente formatados
    printf("Aceleração (m/s²): X=%.2f Y=%.2f Z=%.2f\n", aceleracao[0], aceleracao[1], aceleracao[2]);
    printf("Giro (°/s): X=%.2f Y=%.2f Z=%.2f\n", giro[0], giro[1], giro[2]);

    sleep_ms(500);
  }
}