import struct
import serial
from time import sleep
from time import time


ser = serial.Serial('/dev/ttyACM0' , 115200)

def ping(ser):
    # lecture de 1 octet à l'adresse 0.
    ser.write(b'\xFF\xFF\x05dD\x00\x01\x00')
    pass

def lecture_Detection(ser):
    ser.write(b'\xFF\xFF\x05dD\x00\x0C\x00')
    messages, tmp = lire_message(ser, b'')
    for message in messages:
        print("{:.3f} {} {} {} {} {} {}".format(time(), message[2], message[3], message[4], message[5], message[6], message[7]))
        
def check_message(message, offset):
    offset = offset + 3 # on saut '\xFF\xFF + id'
    if(len(message) > 0 + offset):
        taille_message = message[0+offset]
        if(len(message) > taille_message + offset):
            if(message[taille_message+offset] == 0):
                return message[offset-1:taille_message+offset]
    return None

# Renvoi les message lus.
# Les deux octets de début \xFF \xFF ne sont pas contenu dans le message
# octet 0 : id_carte émettrice
# octet 1 : Nombre d'octets reçus
def lire_message(ser, old_data):
    data = old_data
    nb_read = ser.in_waiting
    offset = 0
    valid_offset = 0
    messages = []
    if nb_read > 0:
        data += ser.read(nb_read)
        while offset > -1:
            offset = data.find(b'\xFF\xFF', offset)
            if(offset > -1):
                message = check_message(data, offset)
                if message is not None:
                    messages.append(message)
                    valid_offset = offset
                offset = offset + 1
        data = data[valid_offset:]
    return messages, data


if __name__ == "__main__":
    data = b''
    while 1:
        lecture_Detection(ser)
        sleep(0.01)

    while(1):
        messages, data = lire_message(ser, data)
        for message in messages:
            print(message)