import time
import serial


def hex_to_int(data):
    data_length = data[2]
    hex_distance = data[3:3+data_length]

    return int.from_bytes(hex_distance, byteorder='big')

for i in range(1000):
    ser = serial.Serial(
        port="/dev/ttyS0",
        baudrate=19200,
        timeout=1,
        bytesize=serial.EIGHTBITS,
        parity=serial.PARITY_NONE,
        stopbits=serial.STOPBITS_ONE
    )

    req = b'\x01\x04\x00\x00\x00\x01\x31\xca'
    time.sleep(0.3)
    ser.write(req)
    result = ser.read(6)

    distance = hex_to_int(result)
    print(f'Laser Distance: {distance}mm')

    ser.close()

