from machine import Pin, I2C, SoftI2C
from OLED_1inch5 import OLED_1inch5
from QMI8658 import QMI8658
from time import sleep, ticks_ms, ticks_diff

print("=== LAB 10 REAL VERSION ===")

# ================= ADDRESSES =================
OLED_ADDR = 0x3D
QMI8658_ADDR = 0x6B

# ================= I2C INIT =================
OLED_i2c = SoftI2C(sda=Pin(6), scl=Pin(7), freq=1_000_000)
sleep(0.1)

Sensors_i2c = I2C(id=0, sda=Pin(8), scl=Pin(9), freq=100_000)
sleep(0.1)

# ================= OBJECTS =================
OLED = OLED_1inch5(OLED_ADDR, OLED_i2c)
qmi8658 = QMI8658(Sensors_i2c, QMI8658_ADDR)

# ================= BUTTON =================
button = Pin(3, Pin.IN, Pin.PULL_UP)

running = True
btn_flag = False
last_press = 0

def button_irq(pin):
    global btn_flag, last_press
    now = ticks_ms()
    if ticks_diff(now, last_press) > 300:
        btn_flag = True
        last_press = now

button.irq(trigger=Pin.IRQ_FALLING, handler=button_irq)

# ================= DRAW CIRCLE =================
def draw_circle(x0, y0, r, color):
    for x in range(-r, r):
        for y in range(-r, r):
            if x*x + y*y <= r*r:
                OLED.pixel(x0 + x, y0 + y, color)

# ================= MAIN LOOP =================
while True:

    if btn_flag:
        btn_flag = False
        running = not running

    if not running:
        OLED.fill(0)
        OLED.text("PROGRAM OFF", 40, 60, 15)
        OLED.show()
        sleep(0.3)
        continue

    xyz = qmi8658.Read_XYZ()

    acc_x = xyz[0]
    acc_y = xyz[1]
    acc_z = xyz[2]

    gyr_x = xyz[3]
    gyr_y = xyz[4]
    gyr_z = xyz[5]

    # центр дисплея 128x128
    center_x = 64
    center_y = 64

    scale = 30

    circle_x = int(center_x + acc_x * scale)
    circle_y = int(center_y - acc_y * scale)

    # обмеження меж
    if circle_x < 8: circle_x = 8
    if circle_x > 120: circle_x = 120
    if circle_y < 8: circle_y = 8
    if circle_y > 120: circle_y = 120

    OLED.fill(0)

    # ===== Вивід даних =====
    OLED.text("ACC_X={:+.2f}".format(acc_x), 1, 5, 15)
    OLED.text("ACC_Y={:+.2f}".format(acc_y), 1, 15, 15)
    OLED.text("ACC_Z={:+.2f}".format(acc_z), 1, 25, 15)

    OLED.text("GYR_X={:+.2f}".format(gyr_x), 1, 45, 15)
    OLED.text("GYR_Y={:+.2f}".format(gyr_y), 1, 55, 15)
    OLED.text("GYR_Z={:+.2f}".format(gyr_z), 1, 65, 15)

    # ===== Малюємо коло =====
    draw_circle(circle_x, circle_y, 6, 12)

    OLED.show()

    print("ACC_X={:+.2f} ACC_Y={:+.2f} ACC_Z={:+.2f}".format(acc_x, acc_y, acc_z))
    print("GYR_X={:+.2f} GYR_Y={:+.2f} GYR_Z={:+.2f}\n".format(gyr_x, gyr_y, gyr_z))

    sleep(0.2)