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

from Interpreteur import StartCPU

motorSpeedFactor = 50
motorDCompensation = 1.04

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) * motorDCompensation
    print("Mot G :", motG)
    print("Mot D :", motD)
    alphabot.setMotors(left=motG, right=motD)

def registerCallback(register, value):
    print(f"Register R{register} changed to {value}")
    pass



# 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)
    print(data)

    StartCPU(data, motorCallback, registerCallback)
    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())