# IMPORT LIBRARIES
import machine
from utime import sleep
from ssd1306 import SSD1306_I2C
import mpu6050

# PIN CONFIGURATION
sda = machine.Pin(21)
scl = machine.Pin(22)
i2c = machine.SoftI2C(sda=sda, scl=scl, freq=100000)

# INITIALIZE DEVICES
oled_width = 128  # Width of the OLED display
oled_height = 64  # Height of the OLED display
oled = SSD1306_I2C(oled_width, oled_height, i2c)

mpu = mpu6050.MPU6050(i2c)

# MAIN ROUTINE
def main():
    while True:
        sensor_data = mpu_i2c()
        oled_display(sensor_data)

def mpu_i2c():
    accel_data = mpu.read_accel_data()
    gyro_data = mpu.read_gyro_data()
    temp_data = mpu.read_temperature()

    print("Acceleration (g): x={:.2f}, y={:.2f}, z={:.2f}".format(accel_data[0], accel_data[1], accel_data[2]))
    print("Rotation (°/s): x={:.2f}, y={:.2f}, z={:.2f}".format(gyro_data[0], gyro_data[1], gyro_data[2]))
    print("Temperature (°C): {:.2f}".format(temp_data))

    return accel_data, gyro_data, temp_data

def oled_display(sensor_data):
    accel_data = sensor_data[0]
    gyro_data = sensor_data[1]
    temp_data = sensor_data[2]

    # Clear the OLED display
    oled.fill(0)
    
    # Display acceleration data
    oled.text("Acc X:{:.2f}g".format(accel_data[0]), 0, 0)
    oled.text("Acc Y:{:.2f}g".format(accel_data[1]), 0, 10)
    oled.text("Acc Z:{:.2f}g".format(accel_data[2]), 0, 20)
    
    # Display rotation data
    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:{:.2f}C".format(temp_data), 0, 60)

    # Refresh the OLED display
    oled.show()

if __name__ == '__main__':
    main()