"""
LLUAV WebSocket Client

Enhanced client with:
  - Automatic login on connect
  - Auto-reconnect with exponential backoff
  - Connection keep-alive (heartbeat)
  - Local command queue (strict request/response serialization)
  - Timeout & retry management per command
  - Periodic task scheduler

Protocol: text-based commands over WebSocket, JSON responses.
"""

import json
import time
import threading
from collections import deque
from concurrent.futures import Future
from typing import Optional, Union, Dict, Any, Callable

try:
    import websocket
    HAS_WEBSOCKET_CLIENT = True
except ImportError:
    HAS_WEBSOCKET_CLIENT = False

try:
    import websockets
    import asyncio
    HAS_WEBSOCKETS = True
except ImportError:
    HAS_WEBSOCKETS = False

from .exceptions import LLUAVConnectionError, LLUAVCommandError

DEFAULT_PORT = 53001
DEFAULT_TIMEOUT = 10.0


class _QueuedCommand:
    """Internal queued command object."""
    __slots__ = ('message', 'deadline', 'future', 'retry_count', 'timeout_val')

    def __init__(self, message: str, timeout: float):
        self.message = message
        self.deadline = time.time() + timeout
        self.future = Future()
        self.retry_count = 0
        self.timeout_val = timeout


# ---------------------------------------------------------------------------
# Synchronous Client
# ---------------------------------------------------------------------------

class LLUAVClient:
    """
    Synchronous WebSocket client for LLUAV drone firmware.

    Features:
      - Automatic login after connect
      - Auto-reconnect with exponential backoff
      - Heartbeat keep-alive
      - Strict command queue (one command in flight at a time)
      - Per-command timeout and retry
      - Periodic task scheduler

    Requires `websocket-client`:
        pip install websocket-client

    Usage:
        client = LLUAVClient("192.168.1.100", password="your_password")
        client.connect()
        print(client.get_state())
        client.takeoff()
        client.disconnect()
    """

    def __init__(self, host: str = "localhost", port: int = DEFAULT_PORT,
                 password: Optional[str] = None, timeout: float = DEFAULT_TIMEOUT,
                 auto_reconnect: bool = True, reconnect_interval: float = 1.0,
                 max_reconnect_interval: float = 30.0, max_retries: int = 3,
                 heartbeat_interval: float = 5.0, heartbeat_cmd: str = "state"):
        if not HAS_WEBSOCKET_CLIENT:
            raise ImportError("Please install websocket-client: pip install websocket-client")
        self.host = host
        self.port = port
        self.password = password
        self.timeout = timeout
        self.auto_reconnect = auto_reconnect
        self.reconnect_interval = reconnect_interval
        self.max_reconnect_interval = max_reconnect_interval
        self.max_retries = max_retries
        self.heartbeat_interval = heartbeat_interval
        self.heartbeat_cmd = heartbeat_cmd

        self.ws: Optional[websocket.WebSocket] = None
        self._connected = False
        self._shutdown = True
        self._queue: deque[_QueuedCommand] = deque()
        self._queue_lock = threading.Lock()
        self._queue_cv = threading.Condition(self._queue_lock)
        self._periodic_tasks: Dict[str, tuple] = {}  # name -> (interval, cmd, args, last_run)
        self._periodic_lock = threading.Lock()

        self._worker_thread: Optional[threading.Thread] = None
        self._heartbeat_thread: Optional[threading.Thread] = None
        self._periodic_thread: Optional[threading.Thread] = None

    @property
    def url(self) -> str:
        return f"ws://{self.host}:{self.port}"

    # ---------------- Connection Management ----------------

    def connect(self) -> None:
        """Establish WebSocket connection and start background threads."""
        if not self._shutdown:
            return  # Already connected or connecting
        self._shutdown = False
        self._do_connect()
        self._start_threads()

    def _do_connect(self) -> None:
        try:
            self.ws = websocket.create_connection(self.url, timeout=self.timeout)
            self._connected = True
        except Exception as e:
            self._connected = False
            self._shutdown = True
            raise LLUAVConnectionError(f"Failed to connect to {self.url}: {e}")
        if self.password:
            self._do_login()

    def _do_login(self) -> None:
        try:
            self.ws.send(f"login {self.password}")
            raw = self.ws.recv()
            resp = json.loads(raw)
        except Exception as e:
            self._connected = False
            self._shutdown = True
            try:
                self.ws.close()
            except Exception:
                pass
            raise LLUAVConnectionError(f"Login failed: {e}")
        if resp.get("ret", 0) != 0:
            self._connected = False
            self._shutdown = True
            try:
                self.ws.close()
            except Exception:
                pass
            raise LLUAVCommandError(resp.get("msg", "Login failed"), resp.get("ret"), resp)

    def disconnect(self) -> None:
        """Close connection, stop all threads and clear queues."""
        self._shutdown = True
        with self._queue_lock:
            self._queue_cv.notify_all()
        self._close_ws()
        # Cancel pending commands
        with self._queue_lock:
            while self._queue:
                cmd = self._queue.popleft()
                if not cmd.future.done():
                    cmd.future.set_exception(LLUAVConnectionError("Disconnected by user"))
        for t in (self._worker_thread, self._heartbeat_thread, self._periodic_thread):
            if t and t.is_alive():
                t.join(timeout=2.0)
        self._worker_thread = None
        self._heartbeat_thread = None
        self._periodic_thread = None

    def _close_ws(self) -> None:
        self._connected = False
        if self.ws:
            try:
                self.ws.close()
            except Exception:
                pass
            self.ws = None

    def is_connected(self) -> bool:
        return self._connected and self.ws is not None and self.ws.connected

    # ---------------- Background Threads ----------------

    def _start_threads(self) -> None:
        self._worker_thread = threading.Thread(target=self._worker_loop, daemon=True)
        self._worker_thread.start()
        if self.heartbeat_interval > 0:
            self._heartbeat_thread = threading.Thread(target=self._heartbeat_loop, daemon=True)
            self._heartbeat_thread.start()
        self._periodic_thread = threading.Thread(target=self._periodic_loop, daemon=True)
        self._periodic_thread.start()

    def _worker_loop(self) -> None:
        """Main worker: dequeue commands, send, recv, handle reconnect."""
        while not self._shutdown:
            cmd: Optional[_QueuedCommand] = None
            with self._queue_lock:
                if not self._queue:
                    self._queue_cv.wait(timeout=0.5)
                    continue
                cmd = self._queue[0]  # peek

            if cmd is None:
                continue

            # Check deadline
            if time.time() > cmd.deadline:
                with self._queue_lock:
                    if self._queue and self._queue[0] is cmd:
                        self._queue.popleft()
                if not cmd.future.done():
                    cmd.future.set_exception(LLUAVCommandError("Command timeout"))
                continue

            # Ensure connection
            if not self.is_connected():
                if self.auto_reconnect and not self._shutdown:
                    if not self._try_reconnect():
                        time.sleep(0.5)
                        continue
                else:
                    with self._queue_lock:
                        if self._queue and self._queue[0] is cmd:
                            self._queue.popleft()
                    if not cmd.future.done():
                        cmd.future.set_exception(LLUAVConnectionError("Not connected"))
                    continue

            # Send and receive
            try:
                self.ws.send(cmd.message)
                raw = self.ws.recv()
                resp = json.loads(raw)
            except Exception as e:
                self._connected = False
                if self.auto_reconnect and not self._shutdown:
                    cmd.retry_count += 1
                    if cmd.retry_count > self.max_retries:
                        with self._queue_lock:
                            if self._queue and self._queue[0] is cmd:
                                self._queue.popleft()
                        if not cmd.future.done():
                            cmd.future.set_exception(LLUAVConnectionError(f"Max retries exceeded: {e}"))
                    else:
                        time.sleep(0.5)
                else:
                    with self._queue_lock:
                        if self._queue and self._queue[0] is cmd:
                            self._queue.popleft()
                    if not cmd.future.done():
                        cmd.future.set_exception(LLUAVConnectionError(str(e)))
                continue

            # Success
            with self._queue_lock:
                if self._queue and self._queue[0] is cmd:
                    self._queue.popleft()

            if resp.get("ret", 0) != 0:
                if not cmd.future.done():
                    cmd.future.set_exception(LLUAVCommandError(
                        resp.get("msg", f"Command '{cmd.message}' failed"),
                        resp.get("ret"), resp))
            else:
                if not cmd.future.done():
                    cmd.future.set_result(resp)

    def _try_reconnect(self) -> bool:
        delay = self.reconnect_interval
        while not self._shutdown and not self.is_connected():
            try:
                self.ws = websocket.create_connection(self.url, timeout=self.timeout)
                self._connected = True
                if self.password:
                    self.ws.send(f"login {self.password}")
                    raw = self.ws.recv()
                    resp = json.loads(raw)
                    if resp.get("ret", 0) != 0:
                        self.ws.close()
                        self._connected = False
                        return False
                return True
            except Exception:
                self._connected = False
                time.sleep(delay)
                delay = min(delay * 2, self.max_reconnect_interval)
        return self.is_connected()

    def _heartbeat_loop(self) -> None:
        """Enqueue a lightweight command periodically to keep connection alive."""
        while not self._shutdown:
            time.sleep(self.heartbeat_interval)
            if self._shutdown:
                break
            if self.is_connected():
                self._enqueue_fire_and_forget(self.heartbeat_cmd)

    def _periodic_loop(self) -> None:
        """Check periodic tasks and enqueue them when due."""
        while not self._shutdown:
            time.sleep(0.2)
            if self._shutdown:
                break
            now = time.time()
            with self._periodic_lock:
                tasks = list(self._periodic_tasks.items())
            for name, (interval, cmd, args, last_run) in tasks:
                if now - last_run >= interval:
                    self._enqueue_fire_and_forget(cmd, args)
                    with self._periodic_lock:
                        if name in self._periodic_tasks:
                            self._periodic_tasks[name] = (interval, cmd, args, now)

    # ---------------- Queue API ----------------

    def _enqueue(self, cmd: str, args: str = "", timeout: Optional[float] = None) -> Future:
        """Enqueue a command and return a Future."""
        if timeout is None:
            timeout = self.timeout
        message = f"{cmd} {args}".strip()
        qcmd = _QueuedCommand(message, timeout)
        with self._queue_lock:
            self._queue.append(qcmd)
            self._queue_cv.notify()
        return qcmd.future

    def _enqueue_fire_and_forget(self, cmd: str, args: str = "") -> None:
        """Enqueue a command without waiting for the result (used by heartbeat/periodic)."""
        message = f"{cmd} {args}".strip()
        qcmd = _QueuedCommand(message, self.timeout)
        # Consume the future silently to avoid broken-promise logs
        qcmd.future.add_done_callback(lambda f: None)
        with self._queue_lock:
            self._queue.append(qcmd)
            self._queue_cv.notify()

    def send_cmd(self, cmd: str, args: str = "", timeout: Optional[float] = None, block: bool = True) -> Any:
        """
        Send a command through the local queue.

        Args:
            cmd: Command name (e.g. 'takeoff')
            args: Command arguments
            timeout: Override default timeout (seconds)
            block: If True, block until response and return dict.
                   If False, return a concurrent.futures.Future.
        """
        future = self._enqueue(cmd, args, timeout)
        if block:
            return future.result()
        return future

    # ---------------- Periodic Tasks ----------------

    def add_periodic_task(self, name: str, interval: float, cmd: str, args: str = "") -> None:
        """
        Register a periodic task that will be enqueued at the given interval.

        Args:
            name: Unique task identifier
            interval: Interval in seconds
            cmd: Command to send
            args: Command arguments
        """
        with self._periodic_lock:
            self._periodic_tasks[name] = (interval, cmd, args, time.time())

    def remove_periodic_task(self, name: str) -> None:
        """Remove a previously registered periodic task."""
        with self._periodic_lock:
            self._periodic_tasks.pop(name, None)

    # ---------------- High-level APIs ----------------

    def login(self, password: str) -> Dict[str, Any]:
        return self.send_cmd("login", password)

    def admin_lock(self) -> Dict[str, Any]:
        return self.send_cmd("admin_lock")

    def admin_unlock(self) -> Dict[str, Any]:
        return self.send_cmd("admin_unlock")

    def get_state(self) -> Dict[str, Any]:
        return self.send_cmd("state")

    def get_param(self, name: Optional[str] = None) -> Dict[str, Any]:
        return self.send_cmd("param" if name is None else "get_param", name or "")

    def get_version(self) -> Dict[str, Any]:
        return self.send_cmd("ver")

    def get_sysinfo(self) -> Dict[str, Any]:
        return self.send_cmd("sysinfo")

    def get_disk_usage(self) -> Dict[str, Any]:
        return self.send_cmd("diskusage")

    def get_statics(self) -> Dict[str, Any]:
        return self.send_cmd("statics")

    def get_machine_id(self) -> Dict[str, Any]:
        return self.send_cmd("machineid")

    def get_hud1(self) -> Dict[str, Any]:
        return self.send_cmd("hud1")

    def get_lidar_pose(self) -> Dict[str, Any]:
        return self.send_cmd("lidarpose")

    def get_apron_state(self) -> Dict[str, Any]:
        return self.send_cmd("apron_state")

    def get_mavlink_msgs(self) -> Dict[str, Any]:
        return self.send_cmd("mavlink_msgs")

    def get_submap_info(self) -> Dict[str, Any]:
        return self.send_cmd("get_submap_info")

    def takeoff(self) -> Dict[str, Any]:
        return self.send_cmd("takeoff")

    def land(self) -> Dict[str, Any]:
        return self.send_cmd("land")

    def rtl(self) -> Dict[str, Any]:
        return self.send_cmd("rtl")

    def mission_rtl(self) -> Dict[str, Any]:
        return self.send_cmd("mrtl")

    def tether_land(self) -> Dict[str, Any]:
        return self.send_cmd("tether_land")

    def set_sp_ned(self, x: float, y: float, z: float, yaw: float) -> Dict[str, Any]:
        return self.send_cmd("set_sp_ned", f"{x}:{y}:{z}:{yaw}")

    def relocate_pos(self, x: float, y: float, z: float, yaw: float) -> Dict[str, Any]:
        """Relocate position (requires map_locate enabled). Format: x,y,z,yaw"""
        return self.send_cmd("relocate_pos", f"{x},{y},{z},{yaw}")

    def tunnel_fly(self, enable: bool) -> Dict[str, Any]:
        return self.send_cmd("tunnelfly", "1" if enable else "0")

    def disarm(self) -> Dict[str, Any]:
        return self.send_cmd("disarm")

    def sleep(self) -> Dict[str, Any]:
        return self.send_cmd("sleep")

    def wakeup(self) -> Dict[str, Any]:
        return self.send_cmd("wakeup")

    def auto_sleep_enable(self, enable: bool) -> Dict[str, Any]:
        return self.send_cmd("autosleep_enable", "1" if enable else "0")

    def zyhk(self, heights: list, land_x: float = 0.0,
             land_y: float = 0.0, land_yaw: float = 0.0) -> Dict[str, Any]:
        hstr = ",".join(str(h) for h in heights)
        args = hstr
        if land_x or land_y or land_yaw:
            args += f" {land_x},{land_y},{land_yaw}"
        return self.send_cmd("zyhk", args)

    def mission(self, ext_id: int, path_or_json: str) -> Dict[str, Any]:
        return self.send_cmd("mission", f"{ext_id}:{path_or_json}")

    def start_plan(self, plan_str: str) -> Dict[str, Any]:
        return self.send_cmd("plan", plan_str)

    def unlock_plan(self) -> Dict[str, Any]:
        return self.send_cmd("unlock_plan")

    def mission_wp_list(self) -> Dict[str, Any]:
        return self.send_cmd("mw_list")

    def mission_wp_exec(self, ext_id: int, wp_ids: str) -> Dict[str, Any]:
        return self.send_cmd("mw_exec", f"{ext_id}:{wp_ids}")

    def ai1_mission(self, task_id: str, mission_file: str) -> Dict[str, Any]:
        return self.send_cmd("ai1_mission", f"{task_id}:{mission_file}")

    def obs_mission(self, enable: bool) -> Dict[str, Any]:
        return self.send_cmd("obs_mission", "1" if enable else "0")

    def dump_mission_result(self, ext_id: Optional[int] = None) -> Dict[str, Any]:
        return self.send_cmd("dump_missionresult", str(ext_id) if ext_id is not None else "")

    def dump_mission_list(self) -> Dict[str, Any]:
        return self.send_cmd("dump_missionlist")

    def abort_report(self, abort_id: int = -1) -> Dict[str, Any]:
        return self.send_cmd("abort_report", str(abort_id))

    def set_taskname(self, name: str) -> Dict[str, Any]:
        return self.send_cmd("set_taskname", name)

    def save_map(self, path: str) -> Dict[str, Any]:
        return self.send_cmd("savemap", path)

    def set_map_locate(self, enable: bool) -> Dict[str, Any]:
        return self.send_cmd("set_maplocate", "1" if enable else "0")

    def set_map_path(self, path: str) -> Dict[str, Any]:
        return self.send_cmd("set_mappath", path)

    def list_maps(self) -> Dict[str, Any]:
        return self.send_cmd("list_maps")

    def rm_map(self, name: str) -> Dict[str, Any]:
        return self.send_cmd("rm_map", name)

    def capture(self) -> Dict[str, Any]:
        return self.send_cmd("capture")

    def snapshot(self) -> Dict[str, Any]:
        return self.send_cmd("snapshot")

    def record_toggle(self) -> Dict[str, Any]:
        return self.send_cmd("record")

    def savebag(self, start: bool) -> Dict[str, Any]:
        return self.send_cmd("savebag", "1" if start else "0")

    def siyi_reboot(self) -> Dict[str, Any]:
        return self.send_cmd("siyi_reboot")

    def siyi_format(self) -> Dict[str, Any]:
        return self.send_cmd("siyi_format")

    def siyi_center(self) -> Dict[str, Any]:
        return self.send_cmd("siyi_center")

    def siyi_init(self) -> Dict[str, Any]:
        return self.send_cmd("siyi_init")

    def siyi_downvideo(self, vid: int = -1) -> Dict[str, Any]:
        return self.send_cmd("siyi_downvideo", str(vid))

    def siyi_sync(self) -> Dict[str, Any]:
        return self.send_cmd("siyi_sync")

    def led_on(self) -> Dict[str, Any]:
        return self.send_cmd("ledon")

    def led_off(self) -> Dict[str, Any]:
        return self.send_cmd("ledoff")

    def led_enable(self) -> Dict[str, Any]:
        return self.send_cmd("ledenable")

    def led_disable(self) -> Dict[str, Any]:
        return self.send_cmd("leddisable")

    def led_pwm(self, pwm: int) -> Dict[str, Any]:
        return self.send_cmd("ledpwm", str(pwm))

    def led_blink(self) -> Dict[str, Any]:
        return self.send_cmd("led_blink")

    def set_param(self, name: str, value: Union[str, int, float]) -> Dict[str, Any]:
        return self.send_cmd("set_param", f"{name}:{value}")

    def get_param_value(self, name: str) -> Dict[str, Any]:
        return self.send_cmd("get_param", name)

    def reset_params(self) -> Dict[str, Any]:
        return self.send_cmd("reset_param")

    def save_params(self, force: bool = False) -> Dict[str, Any]:
        return self.send_cmd("save_param", "1" if force else "0")

    def change_password(self, new_password: str) -> Dict[str, Any]:
        return self.send_cmd("passwd", new_password)

    def set_apron_vol(self, voltage: float) -> Dict[str, Any]:
        return self.send_cmd("apron_vol", str(voltage))

    def set_apron_amp(self, amperage: float) -> Dict[str, Any]:
        return self.send_cmd("apron_amp", str(amperage))

    def reboot_node(self) -> Dict[str, Any]:
        return self.send_cmd("reboot_node")

    def reboot_ap(self) -> Dict[str, Any]:
        return self.send_cmd("reboot_ap")

    def reboot_system(self) -> Dict[str, Any]:
        return self.send_cmd("reboot_system")

    def poweroff_system(self) -> Dict[str, Any]:
        return self.send_cmd("poweroff_system")

    def prepare_poweroff(self) -> Dict[str, Any]:
        return self.send_cmd("prepare_poweroff")

    def reboot_dlo(self) -> Dict[str, Any]:
        return self.send_cmd("reboot_dlo")

    def restart_dlo(self) -> Dict[str, Any]:
        return self.send_cmd("restart_dlo")

    def kill_livox(self, script: str = "") -> Dict[str, Any]:
        return self.send_cmd("kill_livox", script)

    def lidar_onoff(self, on: bool) -> Dict[str, Any]:
        return self.send_cmd("lidaronoff", "1" if on else "0")

    def lidar_warn(self) -> Dict[str, Any]:
        return self.send_cmd("lidarwarn")

    def utg_get(self) -> Dict[str, Any]:
        return self.send_cmd("utg_get")

    def utg_set(self, speed: int) -> Dict[str, Any]:
        return self.send_cmd("utg_set", str(speed))

    def grabber_ctrl(self, pwm: int) -> Dict[str, Any]:
        return self.send_cmd("grabber_ctrl", str(pwm))

    def ara8_center(self) -> Dict[str, Any]:
        return self.send_cmd("ara8_center")

    def ara8_pitch(self, angle: int) -> Dict[str, Any]:
        return self.send_cmd("ara8_pitch", str(angle))

    def cv_test(self) -> Dict[str, Any]:
        return self.send_cmd("cv_test")

    def preflight_check(self) -> Dict[str, Any]:
        return self.send_cmd("preflight_check")

    def preflight_enable(self, enable: bool) -> Dict[str, Any]:
        return self.send_cmd("preflight_enable", "1" if enable else "0")

    def clear_bags(self, min_gb: int = -1) -> Dict[str, Any]:
        return self.send_cmd("clear_bags", str(min_gb))

    def clear_images(self, min_gb: int = -1) -> Dict[str, Any]:
        return self.send_cmd("clear_images", str(min_gb))

    def list_imagedir(self, size: int = -1) -> Dict[str, Any]:
        return self.send_cmd("list_imagedir", str(size))

    def rm_imagedir(self, name: str) -> Dict[str, Any]:
        return self.send_cmd("rm_imagedir", name)

    def test_wp(self, x: float, y: float, z: float) -> Dict[str, Any]:
        return self.send_cmd("test_wp", f"{x}:{y}:{z}")

    def test_wp_list(self, file_path: str) -> Dict[str, Any]:
        return self.send_cmd("test_listwp", file_path)

    def show_msg(self, count: int = -1) -> Dict[str, Any]:
        return self.send_cmd("showmsg", str(count))

    def save_msg(self) -> Dict[str, Any]:
        return self.send_cmd("savemsg")

    def bms_setpower(self, on: bool) -> Dict[str, Any]:
        return self.send_cmd("bms_setpower", "1" if on else "0")

    def bms_setcharge(self, on: bool) -> Dict[str, Any]:
        return self.send_cmd("bms_setcharge", "1" if on else "0")

    def show_setpoint(self) -> Dict[str, Any]:
        return self.send_cmd("show_sp")

    def show_ap_guid2(self) -> Dict[str, Any]:
        return self.send_cmd("show_apguid2")

    def help(self) -> Dict[str, Any]:
        return self.send_cmd("help")

    def __enter__(self):
        self.connect()
        return self

    def __exit__(self, exc_type, exc_val, exc_tb):
        self.disconnect()
        return False


# ---------------------------------------------------------------------------
# Async Client
# ---------------------------------------------------------------------------

class _AsyncQueuedCommand:
    __slots__ = ('message', 'deadline', 'future', 'retry_count', 'timeout_val')

    def __init__(self, message: str, timeout: float):
        self.message = message
        self.deadline = time.time() + timeout
        self.future = asyncio.get_event_loop().create_future()
        self.retry_count = 0
        self.timeout_val = timeout


class LLUAVAsyncClient:
    """
    Asynchronous WebSocket client for LLUAV drone firmware.

    Features:
      - Automatic login after connect
      - Auto-reconnect with exponential backoff
      - Heartbeat keep-alive
      - Strict command queue (one command in flight at a time)
      - Per-command timeout and retry
      - Periodic task scheduler

    Requires `websockets`:
        pip install websockets

    Usage:
        client = LLUAVAsyncClient("192.168.1.100", password="your_password")
        await client.connect()
        state, ver = await asyncio.gather(client.get_state(), client.get_version())
        await client.disconnect()
    """

    def __init__(self, host: str = "localhost", port: int = DEFAULT_PORT,
                 password: Optional[str] = None, timeout: float = DEFAULT_TIMEOUT,
                 auto_reconnect: bool = True, reconnect_interval: float = 1.0,
                 max_reconnect_interval: float = 30.0, max_retries: int = 3,
                 heartbeat_interval: float = 5.0, heartbeat_cmd: str = "state"):
        if not HAS_WEBSOCKETS:
            raise ImportError("Please install websockets: pip install websockets")
        self.host = host
        self.port = port
        self.password = password
        self.timeout = timeout
        self.auto_reconnect = auto_reconnect
        self.reconnect_interval = reconnect_interval
        self.max_reconnect_interval = max_reconnect_interval
        self.max_retries = max_retries
        self.heartbeat_interval = heartbeat_interval
        self.heartbeat_cmd = heartbeat_cmd

        self.ws = None
        self._connected = False
        self._shutdown = True
        self._queue: asyncio.Queue[_AsyncQueuedCommand] = asyncio.Queue()
        self._periodic_tasks: Dict[str, tuple] = {}
        self._periodic_lock = asyncio.Lock()

        self._worker_task: Optional[asyncio.Task] = None
        self._heartbeat_task: Optional[asyncio.Task] = None
        self._periodic_task: Optional[asyncio.Task] = None

    @property
    def url(self) -> str:
        return f"ws://{self.host}:{self.port}"

    # ---------------- Connection Management ----------------

    async def connect(self) -> None:
        if not self._shutdown:
            return
        self._shutdown = False
        await self._do_connect()
        self._worker_task = asyncio.create_task(self._worker_loop())
        if self.heartbeat_interval > 0:
            self._heartbeat_task = asyncio.create_task(self._heartbeat_loop())
        self._periodic_task = asyncio.create_task(self._periodic_loop())

    async def _do_connect(self) -> None:
        try:
            self.ws = await asyncio.wait_for(
                websockets.connect(self.url, open_timeout=self.timeout),
                timeout=self.timeout + 5.0
            )
            self._connected = True
        except Exception as e:
            self._connected = False
            self._shutdown = True
            raise LLUAVConnectionError(f"Failed to connect to {self.url}: {e}")
        if self.password:
            await self._do_login()

    async def _do_login(self) -> None:
        try:
            await self.ws.send(f"login {self.password}")
            raw = await asyncio.wait_for(self.ws.recv(), timeout=self.timeout)
            resp = json.loads(raw)
        except Exception as e:
            self._connected = False
            self._shutdown = True
            try:
                await self.ws.close()
            except Exception:
                pass
            raise LLUAVConnectionError(f"Login failed: {e}")
        if resp.get("ret", 0) != 0:
            self._connected = False
            self._shutdown = True
            try:
                await self.ws.close()
            except Exception:
                pass
            raise LLUAVCommandError(resp.get("msg", "Login failed"), resp.get("ret"), resp)

    async def disconnect(self) -> None:
        self._shutdown = True
        # Cancel queued commands
        while not self._queue.empty():
            try:
                cmd = self._queue.get_nowait()
                if not cmd.future.done():
                    cmd.future.set_exception(LLUAVConnectionError("Disconnected by user"))
            except asyncio.QueueEmpty:
                break
        await self._close_ws()
        for t in (self._worker_task, self._heartbeat_task, self._periodic_task):
            if t and not t.done():
                t.cancel()
                try:
                    await t
                except asyncio.CancelledError:
                    pass
        self._worker_task = None
        self._heartbeat_task = None
        self._periodic_task = None

    async def _close_ws(self) -> None:
        self._connected = False
        if self.ws:
            try:
                await self.ws.close()
            except Exception:
                pass
            self.ws = None

    def is_connected(self) -> bool:
        return self._connected and self.ws is not None and self.ws.open

    # ---------------- Background Tasks ----------------

    async def _worker_loop(self) -> None:
        while not self._shutdown:
            try:
                cmd = await asyncio.wait_for(self._queue.get(), timeout=0.5)
            except asyncio.TimeoutError:
                continue

            if time.time() > cmd.deadline:
                if not cmd.future.done():
                    cmd.future.set_exception(LLUAVCommandError("Command timeout"))
                continue

            if not self.is_connected():
                if self.auto_reconnect and not self._shutdown:
                    if not await self._try_reconnect():
                        await asyncio.sleep(0.5)
                        await self._queue.put(cmd)
                        continue
                else:
                    if not cmd.future.done():
                        cmd.future.set_exception(LLUAVConnectionError("Not connected"))
                    continue

            try:
                await self.ws.send(cmd.message)
                raw = await asyncio.wait_for(self.ws.recv(), timeout=cmd.timeout_val)
                resp = json.loads(raw)
            except Exception as e:
                self._connected = False
                if self.auto_reconnect and not self._shutdown:
                    cmd.retry_count += 1
                    if cmd.retry_count <= self.max_retries:
                        await self._queue.put(cmd)
                        await asyncio.sleep(0.5)
                    else:
                        if not cmd.future.done():
                            cmd.future.set_exception(LLUAVConnectionError(f"Max retries exceeded: {e}"))
                else:
                    if not cmd.future.done():
                        cmd.future.set_exception(LLUAVConnectionError(str(e)))
                continue

            if resp.get("ret", 0) != 0:
                if not cmd.future.done():
                    cmd.future.set_exception(LLUAVCommandError(
                        resp.get("msg", f"Command '{cmd.message}' failed"),
                        resp.get("ret"), resp))
            else:
                if not cmd.future.done():
                    cmd.future.set_result(resp)

    async def _try_reconnect(self) -> bool:
        delay = self.reconnect_interval
        while not self._shutdown and not self.is_connected():
            try:
                self.ws = await websockets.connect(self.url, open_timeout=self.timeout)
                self._connected = True
                if self.password:
                    await self.ws.send(f"login {self.password}")
                    raw = await asyncio.wait_for(self.ws.recv(), timeout=self.timeout)
                    resp = json.loads(raw)
                    if resp.get("ret", 0) != 0:
                        await self.ws.close()
                        self._connected = False
                        return False
                return True
            except Exception:
                self._connected = False
                await asyncio.sleep(delay)
                delay = min(delay * 2, self.max_reconnect_interval)
        return self.is_connected()

    async def _heartbeat_loop(self) -> None:
        while not self._shutdown:
            await asyncio.sleep(self.heartbeat_interval)
            if self._shutdown:
                break
            if self.is_connected():
                await self._enqueue_fire_and_forget(self.heartbeat_cmd)

    async def _periodic_loop(self) -> None:
        while not self._shutdown:
            await asyncio.sleep(0.2)
            if self._shutdown:
                break
            now = time.time()
            async with self._periodic_lock:
                tasks = list(self._periodic_tasks.items())
            for name, (interval, cmd, args, last_run) in tasks:
                if now - last_run >= interval:
                    await self._enqueue_fire_and_forget(cmd, args)
                    async with self._periodic_lock:
                        if name in self._periodic_tasks:
                            self._periodic_tasks[name] = (interval, cmd, args, now)

    # ---------------- Queue API ----------------

    async def _enqueue(self, cmd: str, args: str = "", timeout: Optional[float] = None) -> asyncio.Future:
        if timeout is None:
            timeout = self.timeout
        message = f"{cmd} {args}".strip()
        qcmd = _AsyncQueuedCommand(message, timeout)
        await self._queue.put(qcmd)
        return qcmd.future

    async def _enqueue_fire_and_forget(self, cmd: str, args: str = "") -> None:
        message = f"{cmd} {args}".strip()
        qcmd = _AsyncQueuedCommand(message, self.timeout)
        qcmd.future.add_done_callback(lambda f: None)
        await self._queue.put(qcmd)

    async def send_cmd(self, cmd: str, args: str = "", timeout: Optional[float] = None) -> Dict[str, Any]:
        future = await self._enqueue(cmd, args, timeout)
        return await future

    # ---------------- Periodic Tasks ----------------

    async def add_periodic_task(self, name: str, interval: float, cmd: str, args: str = "") -> None:
        async with self._periodic_lock:
            self._periodic_tasks[name] = (interval, cmd, args, time.time())

    async def remove_periodic_task(self, name: str) -> None:
        async with self._periodic_lock:
            self._periodic_tasks.pop(name, None)

    # ---------------- High-level APIs ----------------

    async def login(self, password: str) -> Dict[str, Any]:
        return await self.send_cmd("login", password)

    async def admin_lock(self) -> Dict[str, Any]:
        return await self.send_cmd("admin_lock")

    async def admin_unlock(self) -> Dict[str, Any]:
        return await self.send_cmd("admin_unlock")

    async def get_state(self) -> Dict[str, Any]:
        return await self.send_cmd("state")

    async def get_param(self, name: Optional[str] = None) -> Dict[str, Any]:
        return await self.send_cmd("param" if name is None else "get_param", name or "")

    async def get_version(self) -> Dict[str, Any]:
        return await self.send_cmd("ver")

    async def get_sysinfo(self) -> Dict[str, Any]:
        return await self.send_cmd("sysinfo")

    async def get_disk_usage(self) -> Dict[str, Any]:
        return await self.send_cmd("diskusage")

    async def get_statics(self) -> Dict[str, Any]:
        return await self.send_cmd("statics")

    async def get_machine_id(self) -> Dict[str, Any]:
        return await self.send_cmd("machineid")

    async def get_hud1(self) -> Dict[str, Any]:
        return await self.send_cmd("hud1")

    async def get_lidar_pose(self) -> Dict[str, Any]:
        return await self.send_cmd("lidarpose")

    async def get_apron_state(self) -> Dict[str, Any]:
        return await self.send_cmd("apron_state")

    async def get_mavlink_msgs(self) -> Dict[str, Any]:
        return await self.send_cmd("mavlink_msgs")

    async def get_submap_info(self) -> Dict[str, Any]:
        return await self.send_cmd("get_submap_info")

    async def takeoff(self) -> Dict[str, Any]:
        return await self.send_cmd("takeoff")

    async def land(self) -> Dict[str, Any]:
        return await self.send_cmd("land")

    async def rtl(self) -> Dict[str, Any]:
        return await self.send_cmd("rtl")

    async def mission_rtl(self) -> Dict[str, Any]:
        return await self.send_cmd("mrtl")

    async def tether_land(self) -> Dict[str, Any]:
        return await self.send_cmd("tether_land")

    async def set_sp_ned(self, x: float, y: float, z: float, yaw: float) -> Dict[str, Any]:
        return await self.send_cmd("set_sp_ned", f"{x}:{y}:{z}:{yaw}")

    async def relocate_pos(self, x: float, y: float, z: float, yaw: float) -> Dict[str, Any]:
        """Relocate position (requires map_locate enabled). Format: x,y,z,yaw"""
        return await self.send_cmd("relocate_pos", f"{x},{y},{z},{yaw}")

    async def tunnel_fly(self, enable: bool) -> Dict[str, Any]:
        return await self.send_cmd("tunnelfly", "1" if enable else "0")

    async def disarm(self) -> Dict[str, Any]:
        return await self.send_cmd("disarm")

    async def sleep(self) -> Dict[str, Any]:
        return await self.send_cmd("sleep")

    async def wakeup(self) -> Dict[str, Any]:
        return await self.send_cmd("wakeup")

    async def auto_sleep_enable(self, enable: bool) -> Dict[str, Any]:
        return await self.send_cmd("autosleep_enable", "1" if enable else "0")

    async def zyhk(self, heights: list, land_x: float = 0.0,
                   land_y: float = 0.0, land_yaw: float = 0.0) -> Dict[str, Any]:
        hstr = ",".join(str(h) for h in heights)
        args = hstr
        if land_x or land_y or land_yaw:
            args += f" {land_x},{land_y},{land_yaw}"
        return await self.send_cmd("zyhk", args)

    async def mission(self, ext_id: int, path_or_json: str) -> Dict[str, Any]:
        return await self.send_cmd("mission", f"{ext_id}:{path_or_json}")

    async def start_plan(self, plan_str: str) -> Dict[str, Any]:
        return await self.send_cmd("plan", plan_str)

    async def unlock_plan(self) -> Dict[str, Any]:
        return await self.send_cmd("unlock_plan")

    async def mission_wp_list(self) -> Dict[str, Any]:
        return await self.send_cmd("mw_list")

    async def mission_wp_exec(self, ext_id: int, wp_ids: str) -> Dict[str, Any]:
        return await self.send_cmd("mw_exec", f"{ext_id}:{wp_ids}")

    async def ai1_mission(self, task_id: str, mission_file: str) -> Dict[str, Any]:
        return await self.send_cmd("ai1_mission", f"{task_id}:{mission_file}")

    async def obs_mission(self, enable: bool) -> Dict[str, Any]:
        return await self.send_cmd("obs_mission", "1" if enable else "0")

    async def dump_mission_result(self, ext_id: Optional[int] = None) -> Dict[str, Any]:
        return await self.send_cmd("dump_missionresult", str(ext_id) if ext_id is not None else "")

    async def dump_mission_list(self) -> Dict[str, Any]:
        return await self.send_cmd("dump_missionlist")

    async def abort_report(self, abort_id: int = -1) -> Dict[str, Any]:
        return await self.send_cmd("abort_report", str(abort_id))

    async def set_taskname(self, name: str) -> Dict[str, Any]:
        return await self.send_cmd("set_taskname", name)

    async def save_map(self, path: str) -> Dict[str, Any]:
        return await self.send_cmd("savemap", path)

    async def set_map_locate(self, enable: bool) -> Dict[str, Any]:
        return await self.send_cmd("set_maplocate", "1" if enable else "0")

    async def set_map_path(self, path: str) -> Dict[str, Any]:
        return await self.send_cmd("set_mappath", path)

    async def list_maps(self) -> Dict[str, Any]:
        return await self.send_cmd("list_maps")

    async def rm_map(self, name: str) -> Dict[str, Any]:
        return await self.send_cmd("rm_map", name)

    async def capture(self) -> Dict[str, Any]:
        return await self.send_cmd("capture")

    async def snapshot(self) -> Dict[str, Any]:
        return await self.send_cmd("snapshot")

    async def record_toggle(self) -> Dict[str, Any]:
        return await self.send_cmd("record")

    async def savebag(self, start: bool) -> Dict[str, Any]:
        return await self.send_cmd("savebag", "1" if start else "0")

    async def siyi_reboot(self) -> Dict[str, Any]:
        return await self.send_cmd("siyi_reboot")

    async def siyi_format(self) -> Dict[str, Any]:
        return await self.send_cmd("siyi_format")

    async def siyi_center(self) -> Dict[str, Any]:
        return await self.send_cmd("siyi_center")

    async def siyi_init(self) -> Dict[str, Any]:
        return await self.send_cmd("siyi_init")

    async def siyi_downvideo(self, vid: int = -1) -> Dict[str, Any]:
        return await self.send_cmd("siyi_downvideo", str(vid))

    async def siyi_sync(self) -> Dict[str, Any]:
        return await self.send_cmd("siyi_sync")

    async def led_on(self) -> Dict[str, Any]:
        return await self.send_cmd("ledon")

    async def led_off(self) -> Dict[str, Any]:
        return await self.send_cmd("ledoff")

    async def led_enable(self) -> Dict[str, Any]:
        return await self.send_cmd("ledenable")

    async def led_disable(self) -> Dict[str, Any]:
        return await self.send_cmd("leddisable")

    async def led_pwm(self, pwm: int) -> Dict[str, Any]:
        return await self.send_cmd("ledpwm", str(pwm))

    async def led_blink(self) -> Dict[str, Any]:
        return await self.send_cmd("led_blink")

    async def set_param(self, name: str, value: Union[str, int, float]) -> Dict[str, Any]:
        return await self.send_cmd("set_param", f"{name}:{value}")

    async def get_param_value(self, name: str) -> Dict[str, Any]:
        return await self.send_cmd("get_param", name)

    async def reset_params(self) -> Dict[str, Any]:
        return await self.send_cmd("reset_param")

    async def save_params(self, force: bool = False) -> Dict[str, Any]:
        return await self.send_cmd("save_param", "1" if force else "0")

    async def change_password(self, new_password: str) -> Dict[str, Any]:
        return await self.send_cmd("passwd", new_password)

    async def set_apron_vol(self, voltage: float) -> Dict[str, Any]:
        return await self.send_cmd("apron_vol", str(voltage))

    async def set_apron_amp(self, amperage: float) -> Dict[str, Any]:
        return await self.send_cmd("apron_amp", str(amperage))

    async def reboot_node(self) -> Dict[str, Any]:
        return await self.send_cmd("reboot_node")

    async def reboot_ap(self) -> Dict[str, Any]:
        return await self.send_cmd("reboot_ap")

    async def reboot_system(self) -> Dict[str, Any]:
        return await self.send_cmd("reboot_system")

    async def poweroff_system(self) -> Dict[str, Any]:
        return await self.send_cmd("poweroff_system")

    async def prepare_poweroff(self) -> Dict[str, Any]:
        return await self.send_cmd("prepare_poweroff")

    async def reboot_dlo(self) -> Dict[str, Any]:
        return await self.send_cmd("reboot_dlo")

    async def restart_dlo(self) -> Dict[str, Any]:
        return await self.send_cmd("restart_dlo")

    async def kill_livox(self, script: str = "") -> Dict[str, Any]:
        return await self.send_cmd("kill_livox", script)

    async def lidar_onoff(self, on: bool) -> Dict[str, Any]:
        return await self.send_cmd("lidaronoff", "1" if on else "0")

    async def lidar_warn(self) -> Dict[str, Any]:
        return await self.send_cmd("lidarwarn")

    async def utg_get(self) -> Dict[str, Any]:
        return await self.send_cmd("utg_get")

    async def utg_set(self, speed: int) -> Dict[str, Any]:
        return await self.send_cmd("utg_set", str(speed))

    async def grabber_ctrl(self, pwm: int) -> Dict[str, Any]:
        return await self.send_cmd("grabber_ctrl", str(pwm))

    async def ara8_center(self) -> Dict[str, Any]:
        return await self.send_cmd("ara8_center")

    async def ara8_pitch(self, angle: int) -> Dict[str, Any]:
        return await self.send_cmd("ara8_pitch", str(angle))

    async def cv_test(self) -> Dict[str, Any]:
        return await self.send_cmd("cv_test")

    async def preflight_check(self) -> Dict[str, Any]:
        return await self.send_cmd("preflight_check")

    async def preflight_enable(self, enable: bool) -> Dict[str, Any]:
        return await self.send_cmd("preflight_enable", "1" if enable else "0")

    async def clear_bags(self, min_gb: int = -1) -> Dict[str, Any]:
        return await self.send_cmd("clear_bags", str(min_gb))

    async def clear_images(self, min_gb: int = -1) -> Dict[str, Any]:
        return await self.send_cmd("clear_images", str(min_gb))

    async def list_imagedir(self, size: int = -1) -> Dict[str, Any]:
        return await self.send_cmd("list_imagedir", str(size))

    async def rm_imagedir(self, name: str) -> Dict[str, Any]:
        return await self.send_cmd("rm_imagedir", name)

    async def test_wp(self, x: float, y: float, z: float) -> Dict[str, Any]:
        return await self.send_cmd("test_wp", f"{x}:{y}:{z}")

    async def test_wp_list(self, file_path: str) -> Dict[str, Any]:
        return await self.send_cmd("test_listwp", file_path)

    async def show_msg(self, count: int = -1) -> Dict[str, Any]:
        return await self.send_cmd("showmsg", str(count))

    async def save_msg(self) -> Dict[str, Any]:
        return await self.send_cmd("savemsg")

    async def bms_setpower(self, on: bool) -> Dict[str, Any]:
        return await self.send_cmd("bms_setpower", "1" if on else "0")

    async def bms_setcharge(self, on: bool) -> Dict[str, Any]:
        return await self.send_cmd("bms_setcharge", "1" if on else "0")

    async def show_setpoint(self) -> Dict[str, Any]:
        return await self.send_cmd("show_sp")

    async def show_ap_guid2(self) -> Dict[str, Any]:
        return await self.send_cmd("show_apguid2")

    async def help(self) -> Dict[str, Any]:
        return await self.send_cmd("help")

    async def __aenter__(self):
        await self.connect()
        return self

    async def __aexit__(self, exc_type, exc_val, exc_tb):
        await self.disconnect()
        return False
