import serial
import struct
import time

def calculate_crc(data):
    crc = 0xFFFF
    for pos in data:
        crc ^= pos
        for _ in range(8):
            if (crc & 1) != 0:
                crc >>= 1
                crc ^= 0xA001
            else:
                crc >>= 1
    return crc

def send_command(ser, command):
    # Calcular o checksum
    crc = calculate_crc(command)
    command.append(crc & 0xFF)  # CRC Low byte
    command.append((crc >> 8) & 0xFF)  # CRC High byte

    # Enviar o comando via serial
    ser.write(command)

    # Ler a resposta
    response = ser.read(13)  # Ler a resposta completa, 13 bytes no caso de todos os sensores
    return response

def parse_response(response):
    if len(response) < 11:
        print(f"Resposta incompleta ou inválida. Tamanho recebido: {len(response)}")
        return None

    sensor_values = []
    try:
        for i in range(3, 11, 2):
            value = (response[i] << 8) + response[i + 1]
            sensor_values.append(value)
    except IndexError:
        print("Erro ao processar a resposta. Dados insuficientes.")
        sensor_values.append(-1)
    
    return sensor_values

def main():
    # Configurar a porta serial para COM29
    ser = serial.Serial('COM29', 9600, timeout=1)

    # Comando para requisitar dados de todos os sensores (1, 2, 3, e 4)
    command_all_sensors = [0x01, 0x03, 0x01, 0x06, 0x00, 0x04]

    try:
        while True:
            # Enviar o comando e receber a resposta
            response_all = send_command(ser, command_all_sensors)

            print(response_all.hex())

            # Converter a resposta para milímetros
            sensor_values_all = parse_response(response_all)

            # Exibir a saída formatada
            if sensor_values_all and len(sensor_values_all) == 4:
                print(f"S1: {sensor_values_all[0]} mm S2: {sensor_values_all[1]} mm S3: {sensor_values_all[2]} mm S4: {sensor_values_all[3]} mm")
            else:
                print("Dados incompletos ou inválidos recebidos dos sensores.")

            # Esperar 1 segundo antes da próxima requisição
            time.sleep(1)

    except KeyboardInterrupt:
        print("Parando a execução...")

    finally:
        # Fechar a conexão serial
        ser.close()

if __name__ == "__main__":
    main()