import struct
import serial
from time import sleep


def send_msg(message : bytes):
    message_binaire = struct.pack('BBB{}sB'.format(message.__len__()), 0xFF, 0xFF, message.__len__() + 1, message, 0x00)
    print(message_binaire)
    ser.write(message_binaire)

def msg_envoi_donnees(carte_id: bytes, registre : int, charge_utile : bytes):
    send_msg(struct.pack('ccBB{}s'.format(charge_utile.__len__()), b'r', carte_id, registre, charge_utile.__len__(), charge_utile))
    
def led_on():
    msg_envoi_donnees(b'D', 0x00, b'\x06')

def led_off():
    msg_envoi_donnees(b'D', 0x00, b'\x36') 
    
if __name__ == "__main__":
    ser = serial.Serial('/dev/ttyACM0' , 115200)
    traj = struct.pack('bffff', 0, 0.0, 0.0, 100. , 0.)
    msg_envoi_donnees(b'P', 0x22, traj)

    while True:
        
        led_off()
        sleep(0.5)
        led_on()
        sleep(0.5)