base_agent/uas/uas_control/manager.py

from __future__ import annotations

import logging
import time
from typing import Dict, List, Optional

from PyQt5.QtCore import QObject, QTimer, pyqtSignal

from uas.uas_control.base_adapter import BaseProtocolAdapter
from uas.uas_control.base_controller import BaseUASController
from uas.uas_control.command import CommandResult, Commands
from uas.uas_control.telemetry import BaseTelemetry


# ═════════════════════════════════════════════════════════════════
# 多设备统一管理器
# ═════════════════════════════════════════════════════════════════

class UASManager(QObject):
    """
    无人系统多设备统一管理器

    职责：
      - 统一注册/注销控制器（支持任意 BaseUASController 子类）
      - 批量连接/断开
      - 广播指令（全部设备 or 按类型筛选）
      - 汇聚遥测数据
      - 设备健康监控（心跳超时检测）
      - 协议热切换（switch_protocol）
    """

    device_added    = pyqtSignal(str)          # device_id
    device_removed  = pyqtSignal(str)          # device_id
    telemetry_all   = pyqtSignal(str, object)  # (device_id, telemetry)
    alert           = pyqtSignal(str, str)     # (device_id, message)

    HEARTBEAT_TIMEOUT_S = 5.0

    def __init__(self, parent: QObject = None):
        super().__init__(parent)
        self._controllers:     Dict[str, BaseUASController] = {}
        self._last_telem_time: Dict[str, float]             = {}

        # 心跳监控定时器
        self._hb_timer = QTimer(self)
        self._hb_timer.timeout.connect(self._check_heartbeats)
        self._hb_timer.start(2000)
        self.logger = logging.getLogger(
            f"{self.__class__.__name__}"
        )

    # ── 设备管理 ──────────────────────────────────────────────────

    def register(self, ctrl: BaseUASController) -> None:
        """注册控制器"""
        did = ctrl.device_id
        self._controllers[did]    = ctrl
        self._last_telem_time[did] = time.time()

        ctrl.telemetry_updated.connect(
            lambda t, _id=did: self._on_telemetry(_id, t)
        )
        ctrl.error_occurred.connect(
            lambda msg, _id=did: self.alert.emit(_id, msg)
        )
        self.device_added.emit(did)
        self.logger.info(f"设备已注册: {did} [{ctrl.device_type}]")

    def unregister(self, device_id: str) -> bool:
        """注销控制器"""
        ctrl = self._controllers.pop(device_id, None)
        if ctrl:
            ctrl.disconnect()
            self._last_telem_time.pop(device_id, None)
            self.device_removed.emit(device_id)
            self.logger.info(f"设备已注销: {device_id}")
            return True
        return False

    def get(self, device_id: str) -> Optional[BaseUASController]:
        return self._controllers.get(device_id)

    def get_all(self) -> List[BaseUASController]:
        return list(self._controllers.values())

    def get_by_type(self, device_type: str) -> List[BaseUASController]:
        return [c for c in self._controllers.values()
                if c.device_type == device_type]

    @property
    def device_count(self) -> int:
        return len(self._controllers)

    # ── 批量操作 ──────────────────────────────────────────────────

    def connect_all(self) -> Dict[str, bool]:
        results = {}
        for did, ctrl in self._controllers.items():
            results[did] = ctrl.connect()
        return results

    def disconnect_all(self) -> None:
        for ctrl in self._controllers.values():
            ctrl.disconnect()

    def broadcast(
        self,
        cmd_builder,
        device_type: Optional[str] = None,
    ) -> Dict[str, CommandResult]:
        """
        广播指令
        :param cmd_builder: callable(device_id) → Command
        :param device_type: 仅发送给指定类型（None=全部）
        """
        targets = (self.get_by_type(device_type)
                   if device_type else self.get_all())
        return {
            ctrl.device_id: ctrl.send(cmd_builder(ctrl.device_id))
            for ctrl in targets
        }

    def emergency_stop_all(self) -> Dict[str, CommandResult]:
        """紧急停止所有设备"""
        self.logger.warning("⚠️  紧急停止所有设备！")
        return self.broadcast(lambda did: Commands.emergency_stop(did))

    def switch_protocol(
        self,
        device_id:   str,
        new_adapter: BaseProtocolAdapter,
    ) -> bool:
        """
        为指定设备热切换协议适配器
        例：MAVLink → ROS，无需重启控制器
        """
        ctrl = self.get(device_id)
        if ctrl is None:
            self.logger.error(f"设备不存在: {device_id}")
            return False
        ctrl.switch_adapter(new_adapter)
        self.logger.info(
            f"协议热切换完成: {device_id} → "
            f"{new_adapter.__class__.__name__}"
        )
        return True

    # ── 遥测汇聚 ──────────────────────────────────────────────────

    def get_all_telemetry(self) -> Dict[str, Optional[BaseTelemetry]]:
        return {did: ctrl.get_telemetry()
                for did, ctrl in self._controllers.items()}

    def _on_telemetry(self, device_id: str,
                       telem: BaseTelemetry) -> None:
        self._last_telem_time[device_id] = time.time()
        self.telemetry_all.emit(device_id, telem)

    # ── 心跳监控 ──────────────────────────────────────────────────

    def _check_heartbeats(self) -> None:
        now = time.time()
        for did, last_t in list(self._last_telem_time.items()):
            gap = now - last_t
            if gap > self.HEARTBEAT_TIMEOUT_S:
                self.alert.emit(
                    did,
                    f"心跳超时 {gap:.1f}s，设备可能失联"
                )

    def status_report(self) -> str:
        """生成所有设备状态摘要"""
        lines = [f"{'─'*65}", f"  UASManager 状态报告  设备数={self.device_count}",
                 f"{'─'*65}"]
        for did, ctrl in self._controllers.items():
            telem = ctrl.get_telemetry()
            conn  = "✅已连接" if ctrl.is_connected else "❌未连接"
            bat   = f"🔋{telem.battery_percent}%" if telem else "🔋--"
            lines.append(
                f"  {did:12s} [{ctrl.device_type:10s}] "
                f"{conn}  {bat}  "
                f"adapter={ctrl._adapter.__class__.__name__}"
            )
        lines.append(f"{'─'*65}")
        return "\n".join(lines)

    def __repr__(self) -> str:
        return (f"UASManager("
                f"devices={self.device_count}, "
                f"ids={list(self._controllers.keys())})")