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


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 msg_demande_donnees(carte_id: bytes, registre : int, charge_utile : bytes):
    pass
    
def led_on():
    msg_envoi_donnees(b'D', 0x00, b'\x06')

def led_off():
    msg_envoi_donnees(b'D', 0x00, b'\x36') 

def lecture_pos():
    ser.write(b'\xFF\xFF\x05dP\x00\x0c\x00')
    nb_read = ser.in_waiting
    data = ser.read(nb_read)
    print("nb_read={}".format(nb_read))
    list_message = data.split(b"\xff\xffP")
    print("nb_read={}, list_message={}".format(nb_read, len(list_message)))
    if len(list_message)>1:
        if len(list_message[1])>12:
                int_value = struct.unpack('fff',list_message[1][1:13])
                return (int_value[0],int_value[1],int_value[2])

def lecture_abscisse():
    ser.write(b'\xFF\xFF\x05dP\x8C\x04\x00')
    while ser.in_waiting == 0:
        pass
    data = ser.read(ser.in_waiting)
    list_message = data.split(b"\xff\xffP")
    if len(list_message)>1:
        if len(list_message[1])>4:
                int_value = struct.unpack('f',list_message[1][1:5])
                return int_value[0]

def lecture_pos_consigne():
    ser.write(b'\xFF\xFF\x05dP\x00\x08\x00')
    while ser.in_waiting == 0:
        pass
    data = ser.read(ser.in_waiting)
    list_message = data.split(b"\xff\xffP")
    if len(list_message)>1:
        if len(list_message[1])>12:
                int_value = struct.unpack('ff',list_message[1][1:9])
                return (int_value[0],int_value[1])
        
def lecture_propulsion():
    ser.write(b'\xFF\xFF\x05dP\x80\x18\x00')
    nb_read = ser.in_waiting
    data = ser.read(nb_read)
    # print("nb_read={}".format(nb_read))
    list_message = data.split(b"\xff\xffP")
    # print("nb_read={}, list_message={}".format(nb_read, len(list_message)))
    if len(list_message)>1:
        if len(list_message[1])>25:
                struct_value = struct.unpack('ffffff',list_message[1][1:25])
                return (struct_value[0],struct_value[1],struct_value[2], struct_value[3], struct_value[4], struct_value[5])
    
if __name__ == "__main__":

    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # UDP
    sock.sendto(bytes(">c:1", "utf-8"), ("127.0.0.1", 47269))


    ser = serial.Serial('/dev/ttyACM0' , 115200)
    traj = struct.pack('bffff', 0, 0.0, 0.0, 500. , 0.)
    # traj = struct.pack('bffffffff', 2, 
    #                    0.0, 0.0, 
    #                    150. , 0.,
    #                    50. , 200.,
    #                    200. , 200.,)
    msg_envoi_donnees(b'P', 0x22, traj)

    while True:
        pos = None
        #données_propulsion= None
        données_propulsion = lecture_propulsion()
        #abscisse = lecture_abscisse()
        #pos_consigne = lecture_pos_consigne()

        # if pos is not None and abscisse is not None and pos_consigne is not None :
        #    print(">pos:{:.2f}:{:.2f}:{:.2f}:{:.2f}:{:.2f}".
        #          format(pos[0],pos[1], abscisse, pos_consigne[0], pos_consigne[1] )) 

        if données_propulsion != None : 
            x, y = données_propulsion[0],données_propulsion[1]    
            orientation = données_propulsion[2]
            abscisse = données_propulsion[3]
            x_consigne, y_consigne = données_propulsion[4],données_propulsion[5]    
            print("propulsion\t{:.3f}\t{:.2f}\t{:.2f}\t{:.2f}\t{:.2f}\t{:.2f}\t{:.2f}\t".format(time(), x, y, orientation, abscisse, x_consigne, y_consigne))
        else:
            # print("propulsion=None")
            pass
        
        #abscisse = lecture_abscisse()
        # if abscisse != None:
        #      print(">abscisse:{:.2f}".format(abscisse))

        sleep(0.05)