############################
##### IMPORT LIBRARIES #####
############################

import machine
from utime import sleep

import ssd1306

import mpu6050

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

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

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

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

# ssd1306 configuration
oled_width= 128
oled_height= 64

# Use ssd1306 and mpu with devices
oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)
mpu = mpu6050.MPU6050(i2c)

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

def main():
    while True:
        # Subroutine here
        sensor_data = mpu_i2c()
        oled_i2c(sensor_data)

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

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

    print("Gyro: x:" + str(round(gyro_data[0], 2)) + ", y:" + str(round(gyro_data[1], 2)) + ", z:" + str(round(gyro_data[2], 2)))
    print("Acceleration: x:" + str(round(accel_data[0], 2)) + ", y:" + str(round(accel_data[1], 2)) + ", z:" + str(round(accel_data[2], 2)))
    print("Temperature: " + str(round(temp_data, 2)))

    return accel_data, temp_data, gyro_data


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

    oled.fill(0)

    # Select coordinate (column no., row no.)
    oled.text("Gyro", 0, 2)
    #oled.text("------------", 0, 10)
    oled.text("x:" + str(round(gyro_data[0], 2)), 0, 15)
    oled.text("y:" + str(round(gyro_data[1], 2)), 0, 25)
    oled.text("z:" + str(round(gyro_data[2], 2)), 0, 35)

    # Select coordinate (column no., row no.)
    oled.text("Accel", 70, 2)
    oled.text("x:" + str(round(accel_data[0], 2)), 70, 15)
    oled.text("y:" + str(round(accel_data[1], 2)), 70, 25)
    oled.text("z:" + str(round(accel_data[2], 2)), 70, 35)

    # Select coordinate (column no., row no.)
    oled.text("temp:"+ str(round(temp_data, 2)), 0, 55)
    oled.show()


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

if __name__ == "__main__":
    main()