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

#define I2C_PORT i2c0
#define SDA_PIN 4
#define SCL_PIN 5

#define MPU6050_ADDR 0x68

#define REG_PWR_MGMT_1 0x6B
#define REG_ACCEL_XOUT_H 0x3B

#define ACCEL_SENS 16384.0f
#define GYRO_SENS 131.0f

void mpu6050_init() {
    uint8_t buf[2];
    
    buf[0] = REG_PWR_MGMT_1;
    buf[1] = 0x80;
    i2c_write_blocking(I2C_PORT, MPU6050_ADDR, buf, 2, false);
    sleep_ms(100);

    buf[0] = REG_PWR_MGMT_1;
    buf[1] = 0x01;
    i2c_write_blocking(I2C_PORT, MPU6050_ADDR, buf, 2, false);
    sleep_ms(100);
}

int main() {
    stdio_init_all();

    i2c_init(I2C_PORT, 400 * 1000);
    gpio_set_function(SDA_PIN, GPIO_FUNC_I2C);
    gpio_set_function(SCL_PIN, GPIO_FUNC_I2C);
    
    gpio_pull_up(SDA_PIN);
    gpio_pull_up(SCL_PIN);

    sleep_ms(2000);
    printf("Inicializando MPU6050...\n");
    
    mpu6050_init();

    uint8_t buffer[14];
    float roll = 0.0f, pitch = 0.0f, yaw = 0.0f;
    uint32_t last_time = time_us_32();

    while (1) {
        uint8_t val = REG_ACCEL_XOUT_H;
        
        int ret_write = i2c_write_timeout_us(I2C_PORT, MPU6050_ADDR, &val, 1, true, 10000);
        int ret_read  = i2c_read_timeout_us(I2C_PORT, MPU6050_ADDR, buffer, 14, false, 10000);

        if (ret_read == 14 && ret_write == 1) {
            
            int16_t accel_x = (buffer[0] << 8) | buffer[1];
            int16_t accel_y = (buffer[2] << 8) | buffer[3];
            int16_t accel_z = (buffer[4] << 8) | buffer[5];
            
            int16_t gyro_x  = (buffer[8]  << 8) | buffer[9];
            int16_t gyro_y  = (buffer[10] << 8) | buffer[11];
            int16_t gyro_z  = (buffer[12] << 8) | buffer[13];

            float ax = accel_x / ACCEL_SENS;
            float ay = accel_y / ACCEL_SENS;
            float az = accel_z / ACCEL_SENS;
            
            float gx = gyro_x / GYRO_SENS;
            float gy = gyro_y / GYRO_SENS;
            float gz = gyro_z / GYRO_SENS;

            uint32_t current_time = time_us_32();
            float dt = (current_time - last_time) / 1000000.0f;
            last_time = current_time;

            if (dt > 0.1f) dt = 0.01f;

            float accel_pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180.0 / M_PI;
            float accel_roll  = atan2(ay, az) * 180.0 / M_PI;

            pitch = 0.98f * (pitch + gy * dt) + 0.02f * accel_pitch;
            roll  = 0.98f * (roll + gx * dt)  + 0.02f * accel_roll;
            
            yaw += gz * dt;

            printf("X (Roll): %6.2f | Y (Pitch): %6.2f | Z (Yaw): %6.2f\n", roll, pitch, yaw);
            
        } else {
            printf("Error I2C: Recuperando comunicación con MPU6050...\n");
            mpu6050_init();
            last_time = time_us_32();
        }

        sleep_ms(10); 
    }
    return 0;
}