#!/usr/bin/env python3

import serial
import struct
import threading
import time

PORT = "/dev/ttyACM0"
BAUDRATE = 115200

COMMAND_PREFIX = 0x69

COMMAND_ERROR = 0
COMMAND_ACK = 1
COMMAND_NACK = 2
LED = 3

DEVICE_ID = 0


def calculate_crc(msg: bytes) -> int:
    s = sum(msg) & 0xFF
    return (-s) & 0xFF


def make_packet(command: int, data: bytes = b"") -> bytes:
    length = len(data)

    pkt = bytearray()
    pkt.append(COMMAND_PREFIX)
    pkt.append(length)
    pkt.append(DEVICE_ID)
    pkt.append(command)
    pkt.append(0)              # CRC placeholder
    pkt.extend(data)

    pkt[4] = calculate_crc(pkt[:4] + pkt[5:])

    return bytes(pkt)


def verify_crc(packet: bytes) -> bool:
    crc = packet[4]
    calc = calculate_crc(packet[:4] + packet[5:])
    return crc == calc


def packet_size(buf: bytes):
    if len(buf) < 2:
        return None
    return 5 + buf[1]


def reader(ser):
    rx = bytearray()

    while True:
        data = ser.read(64)
        if not data:
            continue

        rx.extend(data)

        while rx:
            # Binary packet?
            if rx[0] == COMMAND_PREFIX:
                size = packet_size(rx)
                if size is None or len(rx) < size:
                    break

                pkt = bytes(rx[:size])
                del rx[:size]

                if not verify_crc(pkt):
                    print("RX: Bad CRC:", pkt.hex())
                    continue

                length = pkt[1]
                dev_id = pkt[2]
                cmd = pkt[3]

                if cmd == COMMAND_ACK:
                    print(f"<-- ACK (device={dev_id})")

                elif cmd == COMMAND_NACK:
                    print(f"<-- NACK (device={dev_id})")

                elif cmd == COMMAND_ERROR:
                    print(f"<-- ERROR (device={dev_id})")

                else:
                    print(f"<-- Command {cmd} len={length}")

            else:
                # ASCII log output
                idx = rx.find(b'\n')
                if idx == -1:
                    break

                line = rx[:idx + 1]
                del rx[:idx + 1]

                try:
                    print("[LOG]", line.decode().rstrip())
                except UnicodeDecodeError:
                    print("[RAW]", line.hex())


def main():
    ser = serial.Serial(PORT, BAUDRATE, timeout=0.05)

    threading.Thread(target=reader, args=(ser,), daemon=True).start()

    for i in range(10):
        print(f"--> Sending LED command {i + 1}")
        ser.write(make_packet(LED))
        time.sleep(0.5)

    print("Done.")
    time.sleep(2)


if __name__ == "__main__":
    main()