#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
}
void leitura(){
  int16_t aceleracao_bruto[3], giro_bruto[3], temperatura_bruto;
  float aceleracao[3], giro[3], temperatura;
  uint8_t buffer[6];
  // 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
  }

  // Ler temperatura
  mpu9250_ler(0x41, buffer, 2);
  temperatura_bruto = (int16_t)((buffer[0] << 8) | buffer[1]);
  temperatura = (temperatura_bruto / 340.0) + 36.53;

  // 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]);
  printf("Temperatura: %.2f °C\n", temperatura);
}

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);

    if (id != 0x71 && id != 0x68) {
        printf("Falha ao detectar MPU-9250!\n");
        return 1;
    }

    while (1) {
       leitura();
       sleep_ms(500);
    }
}