from machine import Pin, I2C
import time

# MPU6050 registers
MPU6050_ADDR = 0x68
PWR_MGMT_1 = 0x6B
TEMP_OUT_H = 0x41
ACCEL_XOUT_H = 0x3B
GYRO_XOUT_H = 0x43

# Function to initialize I2C communication
def init_i2c():
    i2c = I2C(0, scl=Pin(9), sda=Pin(8), freq=400000)  # Pico's I2C0
    return i2c

# Function to write a byte to a register
def write_byte(i2c, addr, reg, data):
    i2c.writeto_mem(addr, reg, bytearray([data]))

# Function to read a 16-bit signed value from a register
def read_word(i2c, addr, reg):
    data = i2c.readfrom_mem(addr, reg, 2)
    value = (data[0] << 8) | data[1]
    return value if value < 0x8000 else value - 0x10000  # Convert to signed

# Function to initialize MPU6050
def init_mpu6050(i2c):
    write_byte(i2c, MPU6050_ADDR, PWR_MGMT_1, 0)  # Wake up the MPU6050

# Function to read accelerometer values
def read_accel(i2c):
    accel_x = read_word(i2c, MPU6050_ADDR, ACCEL_XOUT_H)
    accel_y = read_word(i2c, MPU6050_ADDR, ACCEL_XOUT_H + 2)
    accel_z = read_word(i2c, MPU6050_ADDR, ACCEL_XOUT_H + 4)
    return accel_x, accel_y, accel_z

# Function to read gyroscope values
def read_gyro(i2c):
    gyro_x = read_word(i2c, MPU6050_ADDR, GYRO_XOUT_H)
    gyro_y = read_word(i2c, MPU6050_ADDR, GYRO_XOUT_H + 2)
    gyro_z = read_word(i2c, MPU6050_ADDR, GYRO_XOUT_H + 4)
    return gyro_x, gyro_y, gyro_z

# Function to read temperature
def read_temp(i2c):
    temp_raw = read_word(i2c, MPU6050_ADDR, TEMP_OUT_H)
    temp_celsius = (temp_raw / 340.0) + 36.53  # MPU6050 temperature conversion
    return temp_celsius

# Main loop
def main():
    i2c = init_i2c()
    init_mpu6050(i2c)

    while True:
        accel_x, accel_y, accel_z = read_accel(i2c)
        gyro_x, gyro_y, gyro_z = read_gyro(i2c)
        temp_celsius = read_temp(i2c)
        print("Accelerometer X:", accel_x, " Y:", accel_y, " Z:", accel_z)
        print("Gyroscope X:", gyro_x, " Y:", gyro_y, " Z:", gyro_z)
        print("Temperature:", temp_celsius, "C")
        time.sleep(1)

if __name__ == "__main__":
    main()