from machine import Pin, I2C
import math
import time

# MPU6050 I2C address
MPU6050_ADDR = 0x68

# MPU6050 Register addresses
PWR_MGMT_1 = 0x6B
ACCEL_XOUT_H = 0x3B
GYRO_XOUT_H = 0x43

# Initialize I2C interface (GPIO4: SDA, GPIO5: SCL)
i2c = I2C(0, scl=Pin(5), sda=Pin(4), freq=400000)

# Initialize MPU6050 by writing to the power management register
i2c.writeto_mem(MPU6050_ADDR, PWR_MGMT_1, b'\x00')

# Function to read raw data from MPU6050
def read_raw_data(addr):
    high = i2c.readfrom_mem(MPU6050_ADDR, addr, 1)[0]
    low = i2c.readfrom_mem(MPU6050_ADDR, addr + 1, 1)[0]
    value = (high << 8) | low
    if value > 32768:
        value = value - 65536
    return value

# Function to calculate and return pitch, roll, yaw angles
def get_sensor_data():
    # Read accelerometer raw values
    acc_x = read_raw_data(ACCEL_XOUT_H)
    acc_y = read_raw_data(ACCEL_XOUT_H + 2)
    acc_z = read_raw_data(ACCEL_XOUT_H + 4)
    
    # Read gyroscope raw values
    gyro_x = read_raw_data(GYRO_XOUT_H)
    gyro_y = read_raw_data(GYRO_XOUT_H + 2)
    gyro_z = read_raw_data(GYRO_XOUT_H + 4)
    
    # Convert accelerometer raw values to g's
    acc_x = acc_x / 16384.0
    acc_y = acc_y / 16384.0
    acc_z = acc_z / 16384.0
    
    # Convert gyroscope raw values to degrees/sec
    gyro_x = gyro_x / 131.0
    gyro_y = gyro_y / 131.0
    gyro_z = gyro_z / 131.0
    
    # Calculate pitch and roll from accelerometer data
    pitch = math.degrees(math.atan2(acc_y, math.sqrt(acc_x ** 2 + acc_z ** 2)))
    roll = math.degrees(math.atan2(-acc_x, acc_z))
    
    # Yaw is derived from gyroscope Z-axis data
    yaw = gyro_z  # Simplified yaw calculation from gyroscope
    
    return acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, pitch, roll, yaw

# Main loop to read and print all the sensor data
while True:
    acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z, pitch, roll, yaw = get_sensor_data()
    
    # Print accelerometer data
    print("Accelerometer (g): X={:.2f}, Y={:.2f}, Z={:.2f}".format(acc_x, acc_y, acc_z))
    
    # Print gyroscope data
    print("Gyroscope (deg/s): X={:.2f}, Y={:.2f}, Z={:.2f}".format(gyro_x, gyro_y, gyro_z))
    
    # Print pitch, roll, and yaw
    print("Pitch: {:.2f}°, Roll: {:.2f}°, Yaw: {:.2f}°".format(pitch, roll, yaw))
    
    # Add a separator for readability
    print("--------------------------------------------------")
    
    time.sleep(0.5)  # Delay of 500 ms before reading the next set of data