from machine import Pin, I2C
import ssd1306
import mpu6050
import time

# I2C setting
i2c = I2C(0, scl=Pin(22), sda=Pin(21))

# MPU6050 with I2C
mpu = mpu6050.MPU6050(i2c)

# SSD1306 with I2C
oled_width = 128
oled_height = 64
oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)

# wake up the MPU6050 from sleep
mpu.wake()
gyro = ""
accel = ""
temp_c = ""

while True:
    new_gyro = mpu.read_gyro_data()
    new_accel = mpu.read_accel_data()
    new_temp_c = mpu.read_temperature()

    if(new_gyro != gyro or new_accel != accel or new_temp_c != temp_c):
        gyro = new_gyro
        accel = new_accel
        temp_c = new_temp_c

        print("Gyro: " + str(gyro) + ", Accel:" + str(accel) + "Temp: " + str(temp_c))
        oled.fill(0)
        oled.text("Gyro", 0,2)
        oled.text("-----------------", 0,10)
        oled.text("x:" + str(round(gyro[0],2)), 0, 15)
        oled.text("Y:" + str(round(gyro[1],2)), 0, 25)
        oled.text("Z:" + str(round(gyro[2],2)), 0, 35)
        oled.text("Accel", 70,2)
        oled.text("x:" + str(round(accel[0],2)), 70 ,15)
        oled.text("Y:" + str(round(accel[1],2)), 70 ,25)
        oled.text("Z:" + str(round(accel[2],2)), 70 ,35)
        
        oled.text("temp:" + str(round(temp_c,2)), 0 ,50)
        oled.show()

    time.sleep(0.1)