import machine
import utime, sys
import json
import gc
from stm32_ssd1306 import SSD1306, SSD1306_I2C
from stm32_vl53l0x import VL53L0X
from stm32_nec import NEC_8, NEC_16
from stm32_alphabot_v2 import AlphaBot_v2
import neopixel
import _thread
import os
import bluetooth
from stm32_ble_uart import BLEUART

import buzzer

from Interpreteur import StartCPU

alphabot = AlphaBot_v2()
oled = SSD1306_I2C(128, 64, alphabot.i2c)

ble = bluetooth.BLE()
uart = BLEUART(ble, name="Nogard")
print("BLE UART : Nogard")

oled.fill(0)
oled.show()

def motorCallback(motG, motD):
    sMotG = 1 - ((motG >> 2) & 0b10)
    sMotD = 1 - ((motD >> 2) & 0b10)
    motG = ((motG & 0b0111) * sMotG) * 100 / 7
    motD = ((motD & 0b0111) * sMotD) * 100 / 7
    print("Mot G :", motG)
    print("Mot D :", motD)
    alphabot.setMotors(left=motG, right=motD)

while True:
    joystickButton = alphabot.getJoystickValue()
    if (joystickButton == "center"):
        oled.text("Coucou !", 0, 0)
        oled.show()
        utime.sleep(1)
        oled.fill(0)
        oled.show()
    if (joystickButton == "left"):
        alphabot.setMotors(left=-100, right=100)
        utime.sleep(1)
        alphabot.stop()
    if (joystickButton == "right"):
        StartCPU("./out.bin", motorCallback)
        alphabot.stop()