print("Hello, ESP32!")

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()

# Custom function to read temperature in Celsius
def read_temperature():
    raw_temp = i2c.readfrom_mem(0x68, 0x41, 2)
    temp_raw = int.from_bytes(raw_temp, 'big') if raw_temp[0] < 128 else int.from_bytes(raw_temp, 'big') - 65536
    return (temp_raw / 340.0) + 36.53

gyro = ""
accel = ""
temp = None

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

    if new_gyro != gyro or new_accel != accel or new_temp != temp:
        gyro = new_gyro
        accel = new_accel
        temp = new_temp

        print("Gyro:", gyro, "Accel:", accel, "Temp:", round(temp, 1), "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, 1)) + "C", 0, 50)

        oled.show()

    time.sleep(0.1)