print("Hello, ESP32!")
#############################
###### IMPORT LIBRARIES ####
#############################

import machine
from utime import sleep

from ssd1306 import SSD1306_I2C
import mpu6050


#############################
###### PIN CONFIGURATIONS ##
#############################

# Configure the pin as output
# Replace the x with SDA Pin x number here
sda = machine.Pin(21)

# Replace the y with SCL Pin y number here
scl = machine.Pin(22)

# i2c configuration
i2c = machine.SoftI2C(sda=sda, scl=scl, freq=100000)

# use i2c with devices
oled = SSD1306_I2C(128, 64, i2c)
mpu = mpu6050.MPU6050(i2c)


#############################
###### MAIN ROUTINE ########
#############################

def main():

    while True:
        # subroutine here
        sensor_data = mpu_i2c()
        lcd_i2c(sensor_data)
        sleep(1)


#############################
###### SUBROUTINES #########
#############################

def mpu_i2c():

    accel_data = mpu.read_accel_data()
    temp_data = mpu.read_temperature()

    print("acceleration_x: ", accel_data[0])
    print("acceleration_y: ", accel_data[1])
    print("acceleration_z: ", accel_data[2])
    print("temperature: ", temp_data)

    return accel_data, temp_data


def lcd_i2c(sensor_data):

    accel_data = sensor_data[0]
    temp_data = sensor_data[1]

    # clear display
    oled.fill(0)

    # display x, y, z, and temperature
    oled.text("X:{:.2f}".format(accel_data[0]), 0, 0)
    oled.text("Y:{:.2f}".format(accel_data[1]), 0, 16)
    oled.text("Z:{:.2f}".format(accel_data[2]), 0, 32)
    oled.text("T:{:.1f}C".format(temp_data), 0, 48)

    oled.show()


#############################
###### EXECUTE MAIN ROUTINE
#############################

if __name__ == '__main__':
    main()