#!/usr/bin/env python3

import struct
import serial

# -----------------------------------------------------------------------------
# Configuration
# -----------------------------------------------------------------------------

PORT = "/dev/ttyACM1"
BAUDRATE = 115200

COMMAND_PREFIX = 0x69
COMMAND_ID = 0x00

COMMAND_LED = 2

MAX_BRIGHTNESS = 255
MIN_TRACK_SPEED = 1
MAX_TRACK_SPEED = 1000

# -----------------------------------------------------------------------------
# LED Track
# -----------------------------------------------------------------------------

track = [
    (MAX_BRIGHTNESS,   0,   0,   0),   # Red
    (MAX_BRIGHTNESS, MAX_BRIGHTNESS,   0,   0),   # Yellow
    (0,   MAX_BRIGHTNESS,   0,   0),   # Green
    (0,   MAX_BRIGHTNESS, MAX_BRIGHTNESS,   0),   # Cyan
    (0,     0, MAX_BRIGHTNESS,   0),   # Blue
]

# -----------------------------------------------------------------------------
# Build payload
# speed + length (amount of colors in track) + track (lines of colors in uint8_t)
# -----------------------------------------------------------------------------

payload = bytearray()

# Track speed
speed = 10
payload.append((speed & 0xff))
payload.append(((speed >> 8) & 0xff))

# Number of colors
payload.append(len(track))

# Colors
for color in track:
    payload += struct.pack("BBBB", *color)

# Length is:
# payload
length = len(payload)

# -----------------------------------------------------------------------------
# Build packet with placeholder CRC
# -----------------------------------------------------------------------------

packet = bytearray([
    COMMAND_PREFIX,
    length,
    COMMAND_ID,
    COMMAND_LED,
    0x00,      # Placeholder CRC
])

packet += payload

# -----------------------------------------------------------------------------
# Calculate CRC (matches command_calculate_crc())
#
# Sum every byte except the CRC byte itself.
# -----------------------------------------------------------------------------

crc_sum = 0

for i, byte in enumerate(packet):
    if i == 4:      # Skip CRC field
        continue
    crc_sum += byte

packet[4] = 0x100 - (crc_sum & 0xFF)
# Equivalent to:
# packet[4] = (0x100 - (crc_sum & 0xFF)) & 0xFF

# -----------------------------------------------------------------------------
# Send
# -----------------------------------------------------------------------------

print(f"Packet ({len(packet)} bytes):")
print(packet.hex(" "))

with serial.Serial(PORT, BAUDRATE, timeout=1) as ser:
    ser.write(packet)
    ser.flush()

print("Done.")