"""
DroneLink — Serial Communication Interface
============================================
Handles serial port connection, packet framing, and
command sending to drone flight controllers and gimbals.

Supports custom protocols over UART/USB-Serial as well as
MAVLink heartbeat sending (optional pymavlink dependency).

Author: ByIbos
License: MIT
"""

import struct
import time


class DroneLink:
    """
    Serial communication link to a drone or gimbal controller.

    Opens a serial port and provides methods for sending structured
    command packets. Supports both raw byte sending and a simple
    packet protocol with header, command ID, payload, and checksum.

    Packet format:
        [0xAA] [0x55] [cmd_id] [length] [payload...] [checksum]

    Args:
        port: Serial port name (e.g., "COM3", "/dev/ttyUSB0").
        baudrate: Communication speed (default: 115200).
        timeout: Read timeout in seconds (default: 1.0).

    Example:
        >>> link = DroneLink("COM3", baudrate=115200)
        >>> link.send_command(0x01, [100, 200])  # Custom command
        >>> link.send_raw(b"\\x01\\x02\\x03")
        >>> link.close()
    """

    CMD_HEARTBEAT = 0x00
    CMD_GIMBAL_ANGLE = 0x01
    CMD_GIMBAL_SPEED = 0x02
    CMD_FLIGHT_MODE = 0x10
    CMD_ARM = 0x11
    CMD_DISARM = 0x12

    HEADER = bytes([0xAA, 0x55])

    def __init__(self, port, baudrate=115200, timeout=1.0):
        self.port = port
        self.baudrate = baudrate
        self.timeout = timeout
        self.serial = None
        self.connected = False
        self._last_heartbeat = 0

        self._connect()

    def _connect(self):
        """Open the serial port."""
        try:
            import serial
            self.serial = serial.Serial(
                port=self.port,
                baudrate=self.baudrate,
                timeout=self.timeout,
            )
            self.connected = True
            print(f"[DroneLink] Connected to {self.port} @ {self.baudrate} baud")
        except ImportError:
            raise ImportError(
                "pyserial is required: pip install pyserial"
            )
        except Exception as e:
            self.connected = False
            print(f"[DroneLink] Connection failed: {e}")

    def send_command(self, cmd_id, payload=None):
        """
        Send a structured command packet.

        Args:
            cmd_id: Command identifier byte (0x00-0xFF).
            payload: List of bytes or bytearray (default: empty).

        Returns:
            bool: True if sent successfully.
        """
        if not self.connected or self.serial is None:
            return False

        if payload is None:
            payload = []

        data = list(payload)
        length = len(data)
        packet = self.HEADER + bytes([cmd_id, length]) + bytes(data)

        # Simple checksum (XOR of all bytes after header)
        checksum = 0
        for b in packet[2:]:
            checksum ^= b
        packet += bytes([checksum])

        try:
            self.serial.write(packet)
            return True
        except Exception as e:
            print(f"[DroneLink] Send error: {e}")
            return False

    def send_raw(self, data):
        """
        Send raw bytes without framing.

        Args:
            data: bytes or bytearray to send.
        """
        if self.connected and self.serial:
            self.serial.write(data)

    def send_heartbeat(self):
        """Send a heartbeat packet (CMD_HEARTBEAT)."""
        self._last_heartbeat = time.time()
        return self.send_command(self.CMD_HEARTBEAT)

    def read_response(self, size=64):
        """
        Read response bytes from the serial port.

        Args:
            size: Maximum bytes to read.

        Returns:
            bytes: Received data (may be empty if timeout).
        """
        if self.connected and self.serial:
            return self.serial.read(size)
        return b""

    def close(self):
        """Close the serial connection."""
        if self.serial:
            self.serial.close()
            self.connected = False
            print("[DroneLink] Connection closed.")

    def __del__(self):
        self.close()

    def __repr__(self):
        status = "connected" if self.connected else "disconnected"
        return f"DroneLink(port={self.port}, baud={self.baudrate}, {status})"
