import machine
import time
import ustruct
import ssd1306
import neopixel

i2c = machine.I2C(scl=machine.Pin(22), sda=machine.Pin(21))
display = ssd1306.SSD1306_I2C(128, 64, i2c)

led_pin = machine.Pin(6, machine.Pin.OUT)
neo_pixel = neopixel.NeoPixel(led_pin, 1)

mpu_address = 0x68

x_by_gyro = 0.0
y_by_gyro = 0.0
z_by_gyro = 0.0

x_origin = 64
y_origin = 32
view_distance = 150.0

cube_vertex = [
    (-20, -20, 20),
    (20, -20, 20),
    (20, 20, 20),
    (-20, 20, 20),
    (-20, -20, -20),
    (20, -20, -20),
    (20, 20, -20),
    (-20, 20, -20),
]

wireframe = [[0, 0] for _ in range(8)]

def setup():
    global x_by_gyro, y_by_gyro, z_by_gyro

    uart = machine.UART(0, baudrate=115200)

    i2c.writeto(mpu_address, bytearray([0x6B, 0]))

    while True:
        data = i2c.readfrom(mpu_address, 14)
        ax, ay, az, temp, gx, gy, gz = ustruct.unpack_from('>hbb')