CRC_TABLE = [
        0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3,
        0xae, 0xf2, 0xbf, 0x68, 0x25, 0x8b, 0xc6, 0x11, 0x5c, 0xa9, 0xe4, 0x33,
        0x7e, 0xd0, 0x9d, 0x4a, 0x07, 0x5b, 0x16, 0xc1, 0x8c, 0x22, 0x6f, 0xb8,
        0xf5, 0x1f, 0x52, 0x85, 0xc8, 0x66, 0x2b, 0xfc, 0xb1, 0xed, 0xa0, 0x77,
        0x3a, 0x94, 0xd9, 0x0e, 0x43, 0xb6, 0xfb, 0x2c, 0x61, 0xcf, 0x82, 0x55,
        0x18, 0x44, 0x09, 0xde, 0x93, 0x3d, 0x70, 0xa7, 0xea, 0x3e, 0x73, 0xa4,
        0xe9, 0x47, 0x0a, 0xdd, 0x90, 0xcc, 0x81, 0x56, 0x1b, 0xb5, 0xf8, 0x2f,
        0x62, 0x97, 0xda, 0x0d, 0x40, 0xee, 0xa3, 0x74, 0x39, 0x65, 0x28, 0xff,
        0xb2, 0x1c, 0x51, 0x86, 0xcb, 0x21, 0x6c, 0xbb, 0xf6, 0x58, 0x15, 0xc2,
        0x8f, 0xd3, 0x9e, 0x49, 0x04, 0xaa, 0xe7, 0x30, 0x7d, 0x88, 0xc5, 0x12,
        0x5f, 0xf1, 0xbc, 0x6b, 0x26, 0x7a, 0x37, 0xe0, 0xad, 0x03, 0x4e, 0x99,
        0xd4, 0x7c, 0x31, 0xe6, 0xab, 0x05, 0x48, 0x9f, 0xd2, 0x8e, 0xc3, 0x14,
        0x59, 0xf7, 0xba, 0x6d, 0x20, 0xd5, 0x98, 0x4f, 0x02, 0xac, 0xe1, 0x36,
        0x7b, 0x27, 0x6a, 0xbd, 0xf0, 0x5e, 0x13, 0xc4, 0x89, 0x63, 0x2e, 0xf9,
        0xb4, 0x1a, 0x57, 0x80, 0xcd, 0x91, 0xdc, 0x0b, 0x46, 0xe8, 0xa5, 0x72,
        0x3f, 0xca, 0x87, 0x50, 0x1d, 0xb3, 0xfe, 0x29, 0x64, 0x38, 0x75, 0xa2,
        0xef, 0x41, 0x0c, 0xdb, 0x96, 0x42, 0x0f, 0xd8, 0x95, 0x3b, 0x76, 0xa1,
        0xec, 0xb0, 0xfd, 0x2a, 0x67, 0xc9, 0x84, 0x53, 0x1e, 0xeb, 0xa6, 0x71,
        0x3c, 0x92, 0xdf, 0x08, 0x45, 0x19, 0x54, 0x83, 0xce, 0x60, 0x2d, 0xfa,
        0xb7, 0x5d, 0x10, 0xc7, 0x8a, 0x24, 0x69, 0xbe, 0xf3, 0xaf, 0xe2, 0x35,
        0x78, 0xd6, 0x9b, 0x4c, 0x01, 0xf4, 0xb9, 0x6e, 0x23, 0x8d, 0xc0, 0x17,
        0x5a, 0x06, 0x4b, 0x9c, 0xd1, 0x7f, 0x32, 0xe5, 0xa8
    ]

def angle_step(start_angle, end_angle, length):
    if start_angle <= end_angle:
        return (end_angle - start_angle) / length
    return (36000 + end_angle - start_angle) / length

def angle_from_step(start_angle, step, index):
    return (start_angle + (step * index)) % 36000

def calc_crc_from_buffer(buffer, buffer_size):
    crc = 0xD8
    for i in range(0, buffer_size):
        crc = CRC_TABLE[(crc ^ buffer[i]) & 0xff]
    return crc

def time_diff(time_1, time_2):
    if time_1 > time_2:
        time_1 -= 30000
    return time_2 - time_1

class Lidar2d:
    def __init__(self, tx_pin, rx_pin, baudrate=230400):
        self._tx_pin = tx_pin
        self._rx_pin = rx_pin
        self._baudrate = baudrate
        self._uart = busio.UART(self._tx_pin, self._rx_pin, baudrate=baudrate)
        self._prev_time = 0
        self._timestamp = 0
        self.has_update = False
        self._update_time = [0, 0, 0, 0]
        self._latest_data = []

    def update(self, utc):
        data = self._uart.read(2)
        header = data[0]
        verlen = data[1]
        if header != 84 and verlen != 44:
            return
        data = self._uart.read(45)
        speed = int.from_bytes(data[0:2], "little")  # deg / s
        start_angle = int.from_bytes(data[2:4], "little")  # deg
        points = [
            (int.from_bytes(data[4:6], "little"), data[6]),
            (int.from_bytes(data[7:9], "little"), data[9]),
            (int.from_bytes(data[10:12], "little"), data[12]),
            (int.from_bytes(data[13:15], "little"), data[15]),
            (int.from_bytes(data[16:18], "little"), data[18]),
            (int.from_bytes(data[19:21], "little"), data[21]),
            (int.from_bytes(data[22:24], "little"), data[24]),
            (int.from_bytes(data[25:27], "little"), data[27]),
            (int.from_bytes(data[28:30], "little"), data[30]),
            (int.from_bytes(data[31:33], "little"), data[33]),
            (int.from_bytes(data[34:36], "little"), data[36]),
            (int.from_bytes(data[37:39], "little"), data[39]),
        ]
        end_angle = int.from_bytes(data[40:42], "little")
        self._prev_time = self._timestamp
        self._timestamp = int.from_bytes(data[42:44], "little")
        crc_check = data[44]
        if calc_crc_from_buffer(data, 44) != crc_check:
            return
        self.has_update = True
        self._latest_data = []
        self._update_time = utc
        step = angle_step(start_angle, end_angle, 11)
        for idx, distance in enumerate(points):
            angle = int(angle_from_step(start_angle, step, idx) * 100)
            self._latest_data.append((angle, distance[0], distance[1]))

    def get_lidar_message(self, utc):
        if not self.has_update:
            return None
        self.has_update = False
        message = bytearray(b'\xd4\x53\x2a\x6f')
        message += self._update_time[0].to_bytes(1, 'little')
        message += self._update_time[1].to_bytes(1, 'little')
        message += self._update_time[2].to_bytes(1, 'little')
        message += self._update_time[3].to_bytes(4, 'little')
        message += (len(self._latest_data)).to_bytes(1, 'little')
        for idx in range(len(self._latest_data)):
            angle, distance, intensity = self._latest_data[idx]
            message += angle.to_bytes(4, 'little')
            message += distance.to_bytes(2, 'little')
            message += intensity.to_bytes(1, 'little')
        message += b'\x00\x00'
        return message