# -*- coding: utf-8 -*-
"""
无人系统控制器实现 + 多设备管理器 + 完整演示 — 完整版
"""

from __future__ import annotations

import logging

from uas.uas_control import command_registry
from uas.uas_control.base_controller import BaseUASController
from uas.uas_control.command import Command, CommandResult, CommandType, Commands
from uas.uas_control.telemetry import BaseTelemetry, RobotDogTelemetry

logging.basicConfig(
    level  = logging.INFO,
    format = "%(asctime)s [%(levelname)-8s] %(name)s - %(message)s",
)
logger = logging.getLogger("main")

SEP  = "═" * 65
SEP2 = "─" * 65




# ═════════════════════════════════════════════════════════════════
# 机器狗控制器
# ═════════════════════════════════════════════════════════════════

class RobotDogController(BaseUASController):
    """
    机器狗控制器

    扩展机器狗专有 API：
      sit / stand / walk / trot / set_gait
      set_body_height / climb_stairs
    """

    @property
    def device_type(self) -> str:
        return "robot_dog"

    # ── 机器狗专有 API ────────────────────────────────────────────
    @command_registry.register
    def sit(self) -> CommandResult:
        return self.send(Commands.sit(self.device_id))

    @command_registry.register
    def stand(self) -> CommandResult:
        return self.send(Commands.stand(self.device_id))

    @command_registry.register
    def walk(self, speed: float = 1.0,
             heading: float = 0.0) -> CommandResult:
        return self.send(Commands.walk(self.device_id, speed, heading))

    @command_registry.register
    def trot(self, speed: float = 2.0) -> CommandResult:
        return self.send(Command(
            command_type = CommandType.TROT,
            target_id    = self.device_id,
            params       = {"speed": speed},
        ))

    @command_registry.register
    def set_gait(self, gait: str) -> CommandResult:
        """设置步态: WALK / TROT / BOUND / CRAWL"""
        return self.send(Command(
            command_type = CommandType.SET_GAIT,
            target_id    = self.device_id,
            params       = {"gait": gait},
        ))

    @command_registry.register
    def set_body_height(self, height: float) -> CommandResult:
        return self.send(Commands.custom(
            self.device_id, "set_body_height", height=height
        ))

    @command_registry.register
    def climb_stairs(self) -> CommandResult:
        return self.send(Command(
            command_type = CommandType.CLIMB_STAIRS,
            target_id    = self.device_id,
        ))

    # ── 遥测钩子 ──────────────────────────────────────────────────

    def _on_telemetry(self, telem: BaseTelemetry) -> None:
        if not isinstance(telem, RobotDogTelemetry):
            return
        if 0 < telem.battery_percent < 15:
            self.error_occurred.emit(
                f"{self.device_id}: 电量严重不足 {telem.battery_percent}%"
            )
        if telem.obstacle_detected:
            self.error_occurred.emit(
                f"{self.device_id}: 检测到障碍物，"
                f"距离={telem.nearest_obstacle_m:.1f}m"
            )