#!/usr/bin/env python3

import struct
import serial

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

PORT = "/dev/ttyACM1"
BAUDRATE = 115200

COMMAND_PREFIX = 0x69
COMMAND_ID = 0x00

COMMAND_GET_POLAR = 8

# -----------------------------------------------------------------------------
# Build empty payload
# -----------------------------------------------------------------------------

payload = bytearray()
length = len(payload)

# -----------------------------------------------------------------------------
# Build packet
# -----------------------------------------------------------------------------

packet = bytearray([
    COMMAND_PREFIX,
    length,
    COMMAND_ID,
    COMMAND_GET_POLAR,
    0x00,      # CRC placeholder
])

packet += payload

# -----------------------------------------------------------------------------
# Calculate CRC
# -----------------------------------------------------------------------------

crc_sum = 0

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

packet[4] = (0x100 - (crc_sum & 0xFF)) & 0xFF

print("Request:")
print(packet.hex(" "))

# -----------------------------------------------------------------------------
# Send request and receive response
# -----------------------------------------------------------------------------

print("TX bytearray:")
print(packet)

print("TX hex:")
print(packet.hex(" "))

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

    # Read header
    header = ser.read(5)

    if len(header) != 5:
        raise RuntimeError("Timeout waiting for response header")

    prefix, length, cmd_id, command, crc = header

    # Read payload
    payload = ser.read(length)

    if len(payload) != length:
        raise RuntimeError("Timeout waiting for response payload")

# Combine header + payload into a single bytearray
response = bytearray(header)
response.extend(payload)

print("\nRX bytearray:")
print(response)

print("RX hex:")
print(response.hex(" "))

print(f"\nPrefix : 0x{prefix:02X}")
print(f"Length : {length}")
print(f"Cmd ID : {cmd_id}")
print(f"Command: {command}")
print(f"CRC    : 0x{crc:02X}")

if command != COMMAND_GET_POLAR:
    raise RuntimeError(f"Unexpected response command {command}")

if length != 8:
    raise RuntimeError(f"Expected 8-byte payload, got {length}")

theta, r = struct.unpack("<ff", payload)

print(f"\nTheta = {theta:.3f}")
print(f"R     = {r:.3f}")