"""
A lightweight MicroPython implementation for interfacing with an MPU-6050 via I2C. 
Author: Tim Hanewich - https://github.com/TimHanewich
Version: 1.0
License: MIT License
"""

import machine
import time

class MPU6050:
    """Class for reading gyro rates and acceleration data from an MPU-6050 module via I2C."""
    
    def __init__(self, i2c: machine.I2C, address: int = 0x68):
        """
        Creates a new MPU6050 class for reading gyro rates and acceleration data.
        :param i2c: A setup I2C module of the machine module.
        :param address: The I2C address of the MPU-6050 you are using (0x68 is the default).
        """
        self.address = address
        self.i2c = i2c
        
    def wake(self) -> None:
        """Wake up the MPU-6050."""
        self.i2c.writeto_mem(self.address, 0x6B, bytes([0x01]))

    def sleep(self) -> None:
        """Places MPU-6050 in sleep mode (low power consumption)."""
        self.i2c.writeto_mem(self.address, 0x6B, bytes([0x40]))
        
    def who_am_i(self) -> int:
        """Returns the address of the MPU-6050 (ensure it is working)."""
        return self.i2c.readfrom_mem(self.address, 0x75, 1)[0]
    
    def read_temperature(self) -> float:
        """Reads the temperature, in celsius, of the onboard temperature sensor of the MPU-6050."""
        data = self.i2c.readfrom_mem(self.address, 0x41, 2)
        raw_temp: float = self._translate_pair(data[0], data[1])
        temp: float = (raw_temp / 340.0) + 36.53
        return temp

    def read_gyro_data(self) -> tuple[float, float, float]:
        """Read the gyroscope data, in a (x, y, z) tuple."""
        data = self.i2c.readfrom_mem(self.address, 0x43, 6)  # Read 6 bytes (gyro data)
        modifier = 131.0  # Default gyro modifier for ±250°/s
        x = self._translate_pair(data[0], data[1]) / modifier
        y = self._translate_pair(data[2], data[3]) / modifier
        z = self._translate_pair(data[4], data[5]) / modifier
        return (x, y, z)
                
    def read_accel_data(self) -> tuple[float, float, float]:
        """Read the accelerometer data, in a (x, y, z) tuple."""
        data = self.i2c.readfrom_mem(self.address, 0x3B, 6)  # Read 6 bytes (accel data)
        modifier = 16384.0  # Default accel modifier for ±2g
        x = self._translate_pair(data[0], data[1]) / modifier
        y = self._translate_pair(data[2], data[3]) / modifier
        z = self._translate_pair(data[4], data[5]) / modifier
        return (x, y, z)
        
    #### UTILITY FUNCTIONS ####
    def _translate_pair(self, high: int, low: int) -> int:
        """Converts a byte pair to a usable value."""
        value = (high << 8) + low
        if value >= 0x8000:
            value = -((65535 - value) + 1)
        return value   

# Configuración del I2C
i2c = machine.I2C(1, sda=machine.Pin(14), scl=machine.Pin(15))  # SDA: Pin 14, SCL: Pin 15

# Configuración del MPU6050
mpu = MPU6050(i2c)

# Despertar el MPU6050
mpu.wake()

# Bucle principal
while True:
    # Leer datos del giroscopio y acelerómetro
    gyro = mpu.read_gyro_data()
    accel = mpu.read_accel_data()

    # Imprimir los datos
    print(f"Gyro: X={gyro[0]:.2f}, Y={gyro[1]:.2f}, Z={gyro[2]:.2f}")
    print(f"Accel: X={accel[0]:.2f}, Y={accel[1]:.2f}, Z={accel[2]:.2f}")
    print("-" * 30)

    # Esperar un breve tiempo antes de la siguiente lectura
    time.sleep(0.1)