import machine
from utime import sleep
from ssd1306 import SSD1306_I2C
import mpu6050

# I2C Configuration
sda = machine.Pin(21)
scl = machine.Pin(22)

i2c = machine.SoftI2C(sda=sda, scl=scl, freq=10000)  # Standard frequency
oled = SSD1306_I2C(128, 64, i2c)  # SSD1306 resolution 128x64
mpu = mpu6050.MPU6050(i2c)  # MPU6050 initialization

# Main routine
def main():
    while True:
        sensor_data = mpu_i2c()
        display_oled(sensor_data)
        sleep(1)  # Delay to avoid rapid updates

# MPU6050 Data Reading
def mpu_i2c():
    accel_data = mpu.read_accel_data()  # Get acceleration data
    gyro_data = mpu.read_gyro_data()   # Get gyroscope (rotation) data
    temp_data = mpu.read_temperature() # Get temperature data

    print("Acceleration X:", accel_data[0])
    print("Acceleration Y:", accel_data[1])
    print("Acceleration Z:", accel_data[2])
    print("Gyro X:", gyro_data[0])
    print("Gyro Y:", gyro_data[1])
    print("Gyro Z:", gyro_data[2])
    print("Temperature:", temp_data)

    return accel_data, gyro_data, temp_data

# OLED Data Display
def display_oled(sensor_data):
    accel_data = sensor_data[0]
    gyro_data = sensor_data[1]
    temp_data = sensor_data[2]

    oled.fill(0)  # Clear the screen

    # Display Acceleration
    oled.text("Accel X: {:.2f}".format(accel_data[0]), 0, 0)
    oled.text("Accel Y: {:.2f}".format(accel_data[1]), 0, 10)
    oled.text("Accel Z: {:.2f}".format(accel_data[2]), 0, 20)

    # Display Gyroscope (Rotation)
    oled.text("Gyro X: {:.2f}".format(gyro_data[0]), 0, 30)
    oled.text("Gyro Y: {:.2f}".format(gyro_data[1]), 0, 40)
    oled.text("Gyro Z: {:.2f}".format(gyro_data[2]), 0, 50)

    # Display Temperature
    oled.text("Temp: {:.1f}C".format(temp_data), 0, 60)

    oled.show()  # Refresh the display

# Entry point
if __name__ == '__main__':
    main()