from __future__ import annotations

import sys

from PyQt5.QtWidgets import QApplication

from uas.uas_control.base_controller import BaseUASController
from uas.uas_control.command import CommandType, Commands
from uas.uas_control.controllers.robot_dog_controller import SEP, RobotDogController
from uas.uas_control.controllers.uav_controller import UAVController
from uas.uas_control.controllers.usv_controller import USVController
from uas.uas_control.manager import UASManager
from uas.uas_control.protocols.mavlink_adapter import MAVLinkAdapter
from uas.uas_control.protocols.ros_adapter import ROSAdapter
from uas.uas_control.protocols.simulation_adapter import SimulationAdapter


# ═════════════════════════════════════════════════════════════════
# 演示函数
# ═════════════════════════════════════════════════════════════════

def demo_uav_mavlink(app: QApplication):
    print(f"\n{SEP}\n  演示1：无人机 × MAVLink 适配器\n{SEP}")

    adapter = MAVLinkAdapter(
        device_id         = "UAV-001",
        connection_string = "udp:127.0.0.1:14550",
    )
    uav = UAVController("UAV-001", adapter)

    results, errors = [], []
    uav.command_result.connect(lambda r: results.append(r))
    uav.error_occurred.connect(lambda m: errors.append(m))

    assert uav.connect(), "连接应成功"
    print(f"  连接状态  : {uav.is_connected}")
    print(f"  适配器    : {adapter}")

    # 标准起飞流程
    flow = [
        ("解锁",     uav.arm()),
        ("起飞50m",  uav.takeoff(altitude=50.0, speed=3.0)),
        ("前往目标", uav.goto(39.95, 116.45, 100.0, speed=10.0)),
        ("云台俯仰", uav.set_gimbal(pitch=-45.0)),
        ("拍照",     uav.camera_shoot()),
        ("录像开始", uav.camera_record(start=True)),
        ("盘旋",     uav.loiter(radius=80.0)),
        ("录像停止", uav.camera_record(start=False)),
        ("返航",     uav.rtl()),
        ("降落",     uav.land()),
        ("加锁",     uav.disarm()),
    ]
    for name, result in flow:
        mark = "✅" if result.success else "❌"
        print(f"  {mark} {name:8s}: {result.message[:45]}")

    # 遥测
    app.processEvents()
    telem = uav.get_telemetry()
    if telem:
        print(f"\n  遥测快照  : {telem}")

    print(f"  指令历史  : {len(uav.command_history)} 条")
    uav.disconnect()
    print(f"  ✅ MAVLink 适配器演示通过\n")


def demo_robot_dog_ros(app: QApplication):
    print(f"\n{SEP}\n  演示2：机器狗 × ROS2 适配器\n{SEP}")

    adapter = ROSAdapter(
        device_id   = "DOG-001",
        ros_version = 2,
        namespace   = "spot",
        device_type = "robot_dog",
    )
    dog = RobotDogController("DOG-001", adapter)

    results = []
    dog.command_result.connect(lambda r: results.append(r))

    dog.connect()
    print(f"  连接状态  : {dog.is_connected}")
    print(f"  适配器    : {adapter}")

    # 机器狗动作序列
    actions = [
        ("站立",     dog.stand()),
        ("设置高度", dog.set_body_height(0.6)),
        ("行走",     dog.walk(speed=1.5, heading=90.0)),
        ("小跑",     dog.trot(speed=2.5)),
        ("设置步态", dog.set_gait("TROT")),
        ("爬楼梯",   dog.climb_stairs()),
        ("自定义",   dog.custom_command("led_blink", color="red", hz=2)),
        ("坐下",     dog.sit()),
    ]
    for name, result in actions:
        mark = "✅" if result.success else "❌"
        print(f"  {mark} {name:8s}: {result.message[:45]}")

    app.processEvents()
    telem = dog.get_telemetry()
    if telem:
        print(f"\n  遥测快照  : {telem}")

    dog.disconnect()
    print(f"  ✅ ROS2 适配器演示通过\n")


def demo_protocol_hotswap(app: QApplication):
    print(f"\n{SEP}\n  演示3：协议热切换（MAVLink → ROS2）\n{SEP}")

    # 初始使用 MAVLink
    mav_adapter = MAVLinkAdapter("UAV-002", "udp:127.0.0.1:14551")
    uav = UAVController("UAV-002", mav_adapter)
    uav.connect()
    r1 = uav.arm()
    print(f"  MAVLink 解锁: {'✅' if r1.success else '❌'} {r1.message[:40]}")
    print(f"  当前适配器  : {uav._adapter.__class__.__name__}")

    # 热切换到 ROS2
    ros_adapter = ROSAdapter(
        "UAV-002", ros_version=2,
        namespace="uav2", device_type="controller"
    )
    uav.switch_adapter(ros_adapter)
    uav.connect()
    r2 = uav.goto(39.9, 116.4, 80.0)
    print(f"  ROS2 GOTO  : {'✅' if r2.success else '❌'} {r2.message[:40]}")
    print(f"  切换后适配器: {uav._adapter.__class__.__name__}")

    # 再切换回仿真适配器
    sim_adapter = SimulationAdapter("UAV-002", device_type="controller")
    uav.switch_adapter(sim_adapter)
    uav.connect()
    r3 = uav.takeoff(100.0)
    print(f"  仿真 起飞  : {'✅' if r3.success else '❌'} {r3.message[:40]}")
    print(f"  最终适配器  : {uav._adapter.__class__.__name__}")

    uav.disconnect()
    print(f"  ✅ 协议热切换演示通过\n")


def demo_multi_device_manager(app: QApplication):
    print(f"\n{SEP}\n  演示4：多设备统一管理（UASManager）\n{SEP}")

    manager = UASManager()
    alerts  = []
    manager.alert.connect(lambda did, msg: alerts.append(f"[{did}] {msg}"))

    # 注册 3 架无人机 + 2 只机器狗 + 1 艘无人船
    devices = [
        UAVController("UAV-A",
                      SimulationAdapter("UAV-A", device_type="controller")),
        UAVController("UAV-B",
                      SimulationAdapter("UAV-B", device_type="controller")),
        UAVController("UAV-C",
                      MAVLinkAdapter("UAV-C", "udp:127.0.0.1:14552")),
        RobotDogController("DOG-A",
            SimulationAdapter("DOG-A", device_type="robot_dog")),
        RobotDogController("DOG-B",
            ROSAdapter("DOG-B", ros_version=2,
                       namespace="dog_b", device_type="robot_dog")),
        USVController("USV-A",
            SimulationAdapter("USV-A", device_type="controller")),
    ]
    for d in devices:
        manager.register(d)

    print(f"  已注册设备: {manager.device_count} 台")

    # 批量连接
    conn_results = manager.connect_all()
    ok_count = sum(1 for v in conn_results.values() if v)
    print(f"  批量连接  : {ok_count}/{len(conn_results)} 成功")

    # 广播：所有无人机解锁
    print(f"\n  广播：所有无人机解锁")
    arm_results = manager.broadcast(
        lambda did: Commands.arm(did), device_type="controller"
    )
    for did, r in arm_results.items():
        print(f"    {'✅' if r.success else '❌'} {did}: {r.message[:35]}")

    # 广播：所有机器狗站立
    print(f"\n  广播：所有机器狗站立")
    stand_results = manager.broadcast(
        lambda did: Commands.stand(did), device_type="robot_dog"
    )
    for did, r in stand_results.items():
        print(f"    {'✅' if r.success else '❌'} {did}: {r.message[:35]}")

    # 单独控制
    print(f"\n  单独控制 UAV-A 起飞")
    uav_a: UAVController = manager.get("UAV-A")
    r = uav_a.takeoff(altitude=80.0)
    print(f"    {'✅' if r.success else '❌'} UAV-A 起飞: {r.message[:40]}")

    print(f"\n  单独控制 DOG-A 行走")
    dog_a: RobotDogController = manager.get("DOG-A")
    r = dog_a.walk(speed=2.0, heading=45.0)
    print(f"    {'✅' if r.success else '❌'} DOG-A 行走: {r.message[:40]}")

    # 遥测快照
    app.processEvents()
    print(f"\n  遥测快照（所有设备）:")
    for did, telem in manager.get_all_telemetry().items():
        if telem:
            print(f"    {did:8s} bat={telem.battery_percent:3d}% "
                  f"spd={telem.ground_speed:.1f}m/s "
                  f"pos=({telem.latitude:.4f},{telem.longitude:.4f})")

    # 协议热切换（通过 Manager）
    print(f"\n  UAV-A 协议热切换: Simulation → MAVLink")
    new_adapter = MAVLinkAdapter("UAV-A", "udp:127.0.0.1:14553")
    manager.switch_protocol("UAV-A", new_adapter)
    r = uav_a.goto(39.95, 116.45, 100.0)
    print(f"    {'✅' if r.success else '❌'} UAV-A GOTO (MAVLink): "
          f"{r.message[:35]}")

    # 紧急停止所有
    print(f"\n  触发：紧急停止所有设备")
    stop_results = manager.emergency_stop_all()
    ok = sum(1 for r in stop_results.values() if r.success)
    print(f"    紧急停止: {ok}/{len(stop_results)} 成功")

    # 状态报告
    print(f"\n{manager.status_report()}")

    manager.disconnect_all()
    print(f"  ✅ 多设备管理演示通过\n")


def demo_fault_injection(app: QApplication):
    print(f"\n{SEP}\n  演示5：故障注入与容错\n{SEP}")

    adapter = SimulationAdapter(
        "UAV-FAULT", device_type="controller",
        fail_rate=0.0, latency_ms=10.0
    )
    uav = UAVController("UAV-FAULT", adapter)
    uav.connect()

    errors = []
    uav.error_occurred.connect(lambda m: errors.append(m))

    # 正常发送
    r = uav.arm()
    print(f"  正常模式 ARM: {'✅' if r.success else '❌'} {r.message[:40]}")

    # 注入 80% 故障率
    adapter.inject_fault(0.8)
    print(f"\n  注入故障率 80%，连续发送 10 条指令:")
    success_cnt = 0
    for i in range(10):
        r = uav.goto(39.9 + i*0.001, 116.4, 50.0)
        if r.success:
            success_cnt += 1
        print(f"    [{i+1:02d}] {'✅' if r.success else '❌'} "
              f"{r.message[:40]}")

    print(f"\n  成功率: {success_cnt}/10 "
          f"（期望约 2/10，实际随机）")

    # 恢复正常
    adapter.inject_fault(0.0)
    r = uav.emergency_stop()
    print(f"\n  恢复后 EMERGENCY_STOP: "
          f"{'✅' if r.success else '❌'} {r.message[:40]}")

    # 回调注册
    cb_results = []
    uav.register_callback(
        CommandType.GOTO,
        lambda r: cb_results.append(r)
    )
    uav.goto(39.9, 116.4, 50.0)
    print(f"\n  指令回调触发: {len(cb_results)} 次")
    assert len(cb_results) == 1

    uav.disconnect()
    print(f"  ✅ 故障注入演示通过\n")


def demo_custom_extension(app: QApplication):
    print(f"\n{SEP}\n  演示6：自定义扩展（无人船 USV）\n{SEP}")

    # USVController 继承 BaseUASController，
    # 复用 SimulationAdapter，无需修改任何底层代码
    adapter = SimulationAdapter("USV-001", device_type="controller")
    usv = USVController("USV-001", adapter)
    usv.connect()

    actions = [
        ("差速推进", usv.set_throttle(left=0.8, right=0.6)),
        ("前往目标", usv.goto(22.5, 113.9, 0.0, speed=5.0)),
        ("水面巡逻", usv.patrol([
            (22.50, 113.90, 0.0, 5.0),
            (22.52, 113.92, 0.0, 5.0),
            (22.54, 113.90, 0.0, 5.0),
        ])),
        ("抛锚",     usv.anchor()),
        ("紧急停止", usv.emergency_stop()),
    ]
    for name, result in actions:
        mark = "✅" if result.success else "❌"
        print(f"  {mark} {name:8s}: {result.message[:45]}")

    print(f"\n  USVController 继承关系验证:")
    print(f"    isinstance(usv, BaseUASController) = "
          f"{isinstance(usv, BaseUASController)}")
    print(f"    device_type = {usv.device_type}")
    print(f"    adapter     = {usv._adapter.__class__.__name__}")

    usv.disconnect()
    print(f"  ✅ 自定义扩展演示通过\n")


# ═════════════════════════════════════════════════════════════════
# 主入口
# ═════════════════════════════════════════════════════════════════

def main():
    app = QApplication(sys.argv)

    demo_uav_mavlink(app)
    demo_robot_dog_ros(app)
    demo_protocol_hotswap(app)
    demo_multi_device_manager(app)
    demo_fault_injection(app)
    demo_custom_extension(app)

    print(f"{SEP}")
    print(f"  ✅ 所有演示通过！无人系统控制层模块运行正常")
    print(f"  模块统计:")
    print(f"    协议适配器  : MAVLinkAdapter / ROSAdapter / SimulationAdapter")
    print(f"    控制器      : UAVController / RobotDogController / USVController")
    print(f"    管理器      : UASManager（支持 {6} 台设备并发）")
    print(f"    支持指令数  : {len(CommandType)} 种")
    print(f"{SEP}\n")


if __name__ == "__main__":
    main()