import machine
import utime, sys
from stm32_ssd1306 import SSD1306, SSD1306_I2C
from stm32_alphabot_v2 import AlphaBot_v2
import neopixel
import _thread
import os
import buzzer

import binascii
import uasyncio as asyncio
import RobotBleServer
#import base64


from Interpreteur import StartCPU

motorSpeedFactor = 50

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

oled.fill(0)
oled.show()

def motorCallback(motG, motD):
    sMotG = 1 - ((motG >> 2) & 0b10)
    sMotD = 1 - ((motD >> 2) & 0b10)
    motG = ((motG & 0b0111) * sMotG) * motorSpeedFactor / 7
    motD = ((motD & 0b0111) * sMotD) * motorSpeedFactor / 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()








# to know COM port used when connected on PC:
# python -m serial.tools.list_ports

# in this example, robot will send back to PC the checksum of each message received


robotName = 'Nogard'
toSend = []

def onMsgToRobot(data:str|bytes):
    """Function to call when a message sent by PC is received
    :param data: message received"""
    checksum = binascii.crc32(data)
    print('received', data, '=>', checksum)
    #data = data.encode("ascii")
    #data = base64.decodebytes(data)
    print(data)

    

    StartCPU(data, motorCallback)
    alphabot.stop()


async def robotMainTask(bleConnection):
    """Main function for robot activities
    :param bleConnection: object to check BLE connection and send messages"""
    while True:
        await asyncio.sleep(0.1)
        #print('connection', bleConnection.connection)
        if not bleConnection.connection: continue
        if toSend == []: continue
        while not toSend == []:
            data = toSend.pop(0)
            bleConnection.sendMessage(data)
            print('sent', data)

# Run tasks
async def main():
    print('Start main')
    bleConnection = RobotBleServer.RobotBleServer(robotName=robotName, onMsgReceived=onMsgToRobot)
    asyncio.create_task(robotMainTask(bleConnection))
    await bleConnection.communicationTask()

asyncio.run(main())