
#!/usr/bin/env python3
# 重构后的主UI窗口 - 分离UI和业务逻辑

from PyQt5.QtWidgets import (QMainWindow, QWidget, QVBoxLayout, QHBoxLayout,
                            QLabel, QSpinBox, QDoubleSpinBox, QPushButton, QScrollArea,
                            QFrame, QGridLayout, QGroupBox, QMessageBox, QTabWidget)
from PyQt5.QtCore import Qt, QTimer, pyqtSignal
from PyQt5.QtGui import QFont
import threading
import time
import rclpy
from px4_msgs.msg import VehicleCommand

from .uav_card import UAVCard
from .px4_swarm_control_v2 import PX4SwarmController
from .formation_controller import FormationController
from .joystick_tab import JoystickTab


class UAVMonitorWindow(QMainWindow):
    """重构后的无人机监控主窗口 - 专注于UI展示"""
    
    def __init__(self, num_uavs=12):
        super().__init__()
        self.num_uavs = num_uavs
        self.uav_cards = {}  # 存储卡片组件
        
        # 初始化ROS 2
        rclpy.init()
        
        # 初始化集群控制器（合并ROS通信与无人机控制）
        self.swarm_controller = PX4SwarmController()
        self.formation_controller = FormationController(self.swarm_controller)
        
        # 设置回调函数
        self._setup_callbacks()
        
        self.setup_ui()
        self.setup_timers()
        
        # 如果无人机数量为0，设置为自动检测模式
        if self.num_uavs == 0:
            print("🔍 设置为自动检测模式，将动态检测连接的无人机")
            self.status_label.setText("自动检测模式 - 等待无人机连接")
        
        # v2版本不需要启动全局offboard线程，由ROS2定时器自动处理
        
        # 添加调试定时器
        self.debug_timer = QTimer()
        self.debug_timer.timeout.connect(self.debug_thread_status)
        self.debug_timer.start(5000)  # 每5秒检查一次
    
    def _setup_callbacks(self):
        """设置ROS回调函数"""
        # 为所有可能的无人机设置订阅器
        for uav_id in range(1, 51):
            callbacks = {
                'vehicle_status': self._vehicle_status_callback,
                'battery_status': self._battery_status_callback,
                'local_position': self._local_position_callback,
                'global_position': self._global_position_callback
            }
            self.swarm_controller.setup_subscribers(uav_id, callbacks)
    
    def _vehicle_status_callback(self, msg, uav_id):
        """无人机状态回调"""
        # 更新卡片数据
        if uav_id in self.uav_cards:
            uav_data = self.swarm_controller.get_uav_data(uav_id)
            print(f"📡 UAV {uav_id}: 状态回调 - 模式={uav_data.get('flight_mode', uav_data.get('flight_mode_str', 'Unknown'))}, 解锁={uav_data.get('is_armed', False)}")
            self.uav_cards[uav_id].update_data(uav_data)
            
        # 检查无人机是否完成降落
        flight_mode = self.swarm_controller.get_uav_data(uav_id).get('flight_mode_str', '')
        if uav_id in self.swarm_controller.landing_uavs and flight_mode != "Land":
            print(f"UAV {uav_id}: 🎉 降落完成，从降落集合中移除")
            self.swarm_controller.landing_uavs.discard(uav_id)
        
        # 检查无人机是否完成返航
        if uav_id in self.swarm_controller.returning_uavs and flight_mode != "Return":
            print(f"UAV {uav_id}: 🎉 返航完成，从返航集合中移除")
            self.swarm_controller.returning_uavs.discard(uav_id)
    
    def _battery_status_callback(self, msg, uav_id):
        """电池状态回调"""
        if uav_id in self.uav_cards:
            uav_data = self.swarm_controller.get_uav_data(uav_id)
            # 兼容旧字段名 'battery' 和新字段名 'battery_voltage'
            voltage = uav_data.get('battery_voltage', uav_data.get('battery', 0))
            print(f"🔋 UAV {uav_id}: 电池回调 - 电压={voltage}V")
            self.uav_cards[uav_id].update_data(uav_data)
    
    def _local_position_callback(self, msg, uav_id):
        """本地位置回调"""
        if uav_id in self.uav_cards:
            uav_data = self.swarm_controller.get_uav_data(uav_id)
            print(f"📍 UAV {uav_id}: 位置回调 - X={uav_data.get('local_x', 0):.2f}, Y={uav_data.get('local_y', 0):.2f}, Z={uav_data.get('local_z', 0):.2f}")
            self.uav_cards[uav_id].update_data(uav_data)
    
    def _global_position_callback(self, msg, uav_id):
        """全局位置回调"""
        if uav_id in self.uav_cards:
            uav_data = self.swarm_controller.get_uav_data(uav_id)
            print(f"🌍 UAV {uav_id}: 全局位置回调")
            self.uav_cards[uav_id].update_data(uav_data)
    
    def setup_ui(self):
        """设置UI界面"""
        self.setWindowTitle("GTdrone集群监控系统")
        self.setGeometry(100, 100, 1400, 900)
        self.showMaximized()  # 启动时最大化窗口
        
        # 中央部件
        central_widget = QWidget()
        self.setCentralWidget(central_widget)
        
        # 主布局 - 使用垂直布局，包含标签页
        main_layout = QVBoxLayout(central_widget)
        
        # 创建标签页控件
        self.tab_widget = QTabWidget()
        
        # 创建无人机监控标签页
        self.uav_monitor_tab = self.create_uav_monitor_tab()
        self.tab_widget.addTab(self.uav_monitor_tab, "无人机监控")
        
        # 创建遥控器设置标签页
        self.joystick_tab = JoystickTab(self.swarm_controller)
        self.tab_widget.addTab(self.joystick_tab, "遥控器设置")
        
        main_layout.addWidget(self.tab_widget)
        
        # 初始化卡片
        self.create_uav_cards()
        
        # 连接按钮信号
        self.connect_button_signals()
        
    def create_uav_monitor_tab(self):
        """创建无人机监控标签页"""
        tab_widget = QWidget()
        layout = QHBoxLayout(tab_widget)
        
        # 左侧控制面板
        left_panel = QWidget()
        left_panel.setMaximumWidth(350)  # 稍微加宽以容纳编队控制
        left_layout = QVBoxLayout(left_panel)
        
        # 标题
        title_label = QLabel("GTDrone集群监控")
        title_font = QFont()
        title_font.setBold(True)
        title_font.setPointSize(16)
        title_label.setFont(title_font)
        title_label.setAlignment(Qt.AlignCenter)
        left_layout.addWidget(title_label)
        
        # 系统状态组
        status_group = QGroupBox("系统状态")
        status_layout = QVBoxLayout(status_group)
        
        # 无人机数量设置
        count_layout = QHBoxLayout()
        count_layout.addWidget(QLabel("集群规模:"))
        self.uav_count_spin = QSpinBox()
        self.uav_count_spin.setRange(1, 50)
        self.uav_count_spin.setValue(self.num_uavs)
        self.uav_count_spin.valueChanged.connect(self.on_uav_count_changed)
        count_layout.addWidget(self.uav_count_spin)
        status_layout.addLayout(count_layout)
        
        # 选中状态
        self.selected_count_label = QLabel("选中: 0 台")
        status_layout.addWidget(self.selected_count_label)
        
        # 连接状态
        self.connected_count_label = QLabel("在线: 0 台")
        status_layout.addWidget(self.connected_count_label)
        
        # 起飞高度设置
        height_layout = QHBoxLayout()
        height_layout.addWidget(QLabel("起飞高度:"))
        self.takeoff_height_spin = QDoubleSpinBox()
        self.takeoff_height_spin.setRange(1.0, 20.0)
        self.takeoff_height_spin.setValue(3.0)  # 显示正值给用户
        self.takeoff_height_spin.setSuffix(" m")
        self.takeoff_height_spin.valueChanged.connect(self.on_takeoff_height_changed)
        height_layout.addWidget(self.takeoff_height_spin)
        status_layout.addLayout(height_layout)
        
        left_layout.addWidget(status_group)
        
        # 控制按钮组
        control_group = QGroupBox("集群控制")
        control_layout = QVBoxLayout(control_group)
        
        # 刷新按钮
        self.refresh_btn = QPushButton("🔄 刷新状态")
        self.refresh_btn.clicked.connect(self.refresh_cards)
        control_layout.addWidget(self.refresh_btn)
        
        # 选中控制
        select_layout = QHBoxLayout()
        self.select_all_btn = QPushButton("✅ 全选")
        self.deselect_all_btn = QPushButton("❌ 全不选")
        select_layout.addWidget(self.select_all_btn)
        select_layout.addWidget(self.deselect_all_btn)
        control_layout.addLayout(select_layout)
        
        # 飞行控制
        flight_control_group = QGroupBox("飞行控制")
        flight_layout = QVBoxLayout(flight_control_group)
        
        self.takeoff_btn = QPushButton("🚀 集群起飞")
        self.hover_btn = QPushButton("⏸️ 集群悬停")
        self.land_btn = QPushButton("🛬 集群降落")
        self.return_btn = QPushButton("🏠 集群返航")
        self.lock_btn = QPushButton("🔒 集群锁桨")
        self.emergency_stop_btn = QPushButton("🛑 紧急停止")
        self.reboot_btn = QPushButton("🔄 飞控重启")
        
        flight_layout.addWidget(self.takeoff_btn)
        flight_layout.addWidget(self.hover_btn)
        flight_layout.addWidget(self.land_btn)
        flight_layout.addWidget(self.return_btn)
        flight_layout.addWidget(self.lock_btn)
        flight_layout.addWidget(self.emergency_stop_btn)
        flight_layout.addWidget(self.reboot_btn)
        
        control_layout.addWidget(flight_control_group)
        
        # 编队控制
        formation_group = QGroupBox("集群编队")
        formation_layout = QVBoxLayout(formation_group)
        
        self.line_formation_btn = QPushButton("📏 一字形")
        self.circle_formation_btn = QPushButton("⭕ 圆形")
        self.square_formation_btn = QPushButton("⬜ 正方形")
        self.triangle_formation_btn = QPushButton("🔺 三角形")
        self.clear_formation_btn = QPushButton("🗑️ 清除编队")
        
        # 编队旋转控制
        rotation_group = QGroupBox("编队旋转")
        rotation_layout = QHBoxLayout(rotation_group)
        
        self.rotate_left_btn = QPushButton("🔄 左转")
        self.rotate_right_btn = QPushButton("🔄 右转")
        
        rotation_layout.addWidget(self.rotate_left_btn)
        rotation_layout.addWidget(self.rotate_right_btn)
        
        formation_layout.addWidget(self.line_formation_btn)
        formation_layout.addWidget(self.circle_formation_btn)
        formation_layout.addWidget(self.square_formation_btn)
        formation_layout.addWidget(self.triangle_formation_btn)
        formation_layout.addWidget(self.clear_formation_btn)
        formation_layout.addWidget(rotation_group)
        
        control_layout.addWidget(formation_group)
        
        # 状态信息
        self.status_label = QLabel("系统就绪")
        control_layout.addWidget(self.status_label)
        
        control_layout.addStretch()
        left_layout.addWidget(control_group)
        
        left_layout.addStretch()
        layout.addWidget(left_panel)
        
        # 右侧卡片区域
        right_panel = QWidget()
        right_layout = QVBoxLayout(right_panel)
        
        # 卡片标题
        cards_title = QLabel("无人机集群状态")
        cards_title_font = QFont()
        cards_title_font.setBold(True)
        cards_title_font.setPointSize(14)
        cards_title.setFont(cards_title_font)
        cards_title.setAlignment(Qt.AlignCenter)
        right_layout.addWidget(cards_title)
        
        # 滚动区域用于显示卡片
        self.scroll_area = QScrollArea()
        self.scroll_area.setWidgetResizable(True)
        
        # 卡片容器
        self.cards_container = QWidget()
        self.cards_layout = QGridLayout(self.cards_container)
        self.cards_layout.setSpacing(10)
        self.scroll_area.setWidget(self.cards_container)
        
        right_layout.addWidget(self.scroll_area)
        layout.addWidget(right_panel)
        
        return tab_widget
        
    def connect_button_signals(self):
        """连接按钮信号"""
        # 飞行控制按钮
        self.takeoff_btn.clicked.connect(lambda: self.on_control_button_clicked("takeoff"))
        self.hover_btn.clicked.connect(lambda: self.on_control_button_clicked("hover"))
        self.land_btn.clicked.connect(lambda: self.on_control_button_clicked("land"))
        self.return_btn.clicked.connect(lambda: self.on_control_button_clicked("return"))
        self.lock_btn.clicked.connect(lambda: self.on_control_button_clicked("lock"))
        self.emergency_stop_btn.clicked.connect(lambda: self.on_control_button_clicked("emergency_stop"))
        self.reboot_btn.clicked.connect(self.on_reboot_clicked)
        
        # 选中控制按钮
        self.select_all_btn.clicked.connect(self.select_all_uavs)
        self.deselect_all_btn.clicked.connect(self.deselect_all_uavs)
        
        # 确保按钮有效
        self.select_all_btn.setEnabled(True)
        self.deselect_all_btn.setEnabled(True)
        
        # 编队控制按钮
        self.line_formation_btn.clicked.connect(lambda: self.on_formation_button_clicked("line"))
        self.circle_formation_btn.clicked.connect(lambda: self.on_formation_button_clicked("circle"))
        self.square_formation_btn.clicked.connect(lambda: self.on_formation_button_clicked("square"))
        self.triangle_formation_btn.clicked.connect(lambda: self.on_formation_button_clicked("triangle"))
        self.clear_formation_btn.clicked.connect(lambda: self.on_formation_button_clicked("clear"))
        
        # 编队旋转按钮
        self.rotate_left_btn.clicked.connect(lambda: self.on_rotation_button_clicked("left"))
        self.rotate_right_btn.clicked.connect(lambda: self.on_rotation_button_clicked("right"))
        
    def on_control_button_clicked(self, action):
        """控制按钮点击处理"""
        print(f"🖱️ 按钮点击: {action}")
        print(f"📋 选中的无人机: {self.swarm_controller.selected_uavs}")
        
        if action == "emergency_stop":
            # 紧急停止只对选中的无人机生效
            if self.swarm_controller.selected_uavs:
                print(f"🛑 执行紧急停止 - 停止选中的 {len(self.swarm_controller.selected_uavs)} 台无人机")
                for uav_id in self.swarm_controller.selected_uavs:
                    self.swarm_controller.publish_vehicle_command(
                        uav_id,
                        VehicleCommand.VEHICLE_CMD_DO_FLIGHTTERMINATION,  # 使用PX4消息包中的紧急停止命令
                        param1=1.0  # param1=1.0 启用紧急停止
                    )
                print("✅ 紧急停止命令已发送到选中的无人机")
            else:
                print("请先选中无人机")
            return
            
        if self.swarm_controller.selected_uavs:
            print(f"🚀 执行集群{action}操作 - 共 {len(self.swarm_controller.selected_uavs)} 台无人机")
            
            # 执行实际的无人机控制
            for uav_id in self.swarm_controller.selected_uavs:
                print(f"  🚁 UAV {uav_id}: 执行 {action}")
                if action == "takeoff":
                    height = self.takeoff_height_spin.value()
                    print(f"    📏 起飞高度: {height} 米")
                    # GUI 中使用非阻塞模式，避免阻塞 Qt 主线程和 ROS 2 定时器
                    self.swarm_controller.takeoff(uav_id, height, blocking=False)
                elif action == "land":
                    self.swarm_controller.land(uav_id)
                elif action == "hover":
                    self.swarm_controller.hover(uav_id)
                elif action == "return":
                    self.swarm_controller.return_home(uav_id)
                elif action == "lock":
                    self.swarm_controller.lock(uav_id)
            
            # v2版本不需要手动启动后台线程，定时器会自动处理
            if action in ["takeoff", "hover"]:
                print(f"  🔄 Offboard定时器已自动启动")
        else:
            print("❌ 请先选中无人机")
            
    def on_reboot_clicked(self):
        """飞控重启按钮点击处理"""
        if not self.swarm_controller.selected_uavs:
            print("请先选中无人机")
            return
            
        # 显示确认对话框
        uav_ids = sorted(self.swarm_controller.selected_uavs)
        uav_text = ", ".join([f"{uav_id}号" for uav_id in uav_ids])
        
        reply = QMessageBox.question(
            self,
            "确认飞控重启",
            f"确认要重启{uav_text}无人机吗？\n\n⚠️ 警告：这是一个有风险的操作，可能导致无人机失控！",
            QMessageBox.Yes | QMessageBox.No,
            QMessageBox.No
        )
        
        if reply == QMessageBox.Yes:
            print(f"🔄 执行飞控重启 - 重启选中的 {len(uav_ids)} 台无人机")
            for uav_id in uav_ids:
                self.execute_reboot(uav_id)
            print("✅ 飞控重启命令已发送到选中的无人机")
        else:
            print("❌ 用户取消飞控重启操作")
            
    def execute_reboot(self, uav_id):
        """执行飞控重启操作"""
        # 使用PX4消息包中的重启命令
        # VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN 命令用于重启或关机
        # 根据PX4官方文档，正确的参数是：
        # param1=1.0 表示重启
        # param2=1.0 表示重启飞控
        # 注意：有些PX4版本可能需要不同的参数组合
        
        # 尝试第一种参数组合：param1=1.0, param2=1.0
        self.swarm_controller.publish_vehicle_command(
            uav_id,
            VehicleCommand.VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN,
            param1=1.0,  # 1=重启
            param2=1.0   # 1=重启飞控
        )
        print(f"UAV {uav_id}: 🔄 发送飞控重启命令 (param1=1.0, param2=1.0)")
        
        # 尝试第二种参数组合：param1=1.0, param2=2.0
        self.swarm_controller.publish_vehicle_command(
            uav_id,
            VehicleCommand.VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN,
            param1=1.0,  # 1=重启
            param2=2.0   # 2=重启飞控（备用参数）
        )
        print(f"UAV {uav_id}: 🔄 发送备用飞控重启命令 (param1=1.0, param2=2.0)")
        
        # 尝试第三种参数组合：param1=2.0, param2=1.0
        self.swarm_controller.publish_vehicle_command(
            uav_id,
            VehicleCommand.VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN,
            param1=2.0,  # 2=重启到bootloader
            param2=1.0   # 1=重启飞控
        )
        print(f"UAV {uav_id}: 🔄 发送bootloader重启命令 (param1=2.0, param2=1.0)")
            
    def on_formation_button_clicked(self, formation_type):
        """编队按钮点击处理"""
        if not self.swarm_controller.selected_uavs:
            print("请先选中无人机")
            return
            
        print(f"设置{formation_type}编队")
        
        # v2版本不需要手动启动后台线程，定时器会自动处理
        print(f"  🔄 编队控制 - Offboard定时器自动运行")
        
        # 获取第一个选中的无人机位置作为参考
        first_uav_id = sorted(self.swarm_controller.selected_uavs)[0]
        uav_data = self.swarm_controller.get_uav_data(first_uav_id)
        
        if uav_data and uav_data.get('connected', False):
            center_position = (
                uav_data.get('local_x', 0),
                uav_data.get('local_y', 0),
                uav_data.get('local_z', 0)
            )
        else:
            center_position = (0, 0, -3.0)  # 默认位置
            
        # 设置编队 - 增加参数使形状更明显
        if formation_type == "line":
            self.formation_controller.set_formation_line(
                self.swarm_controller.selected_uavs,
                spacing=4.0,  # 增加间距使直线更明显
                direction='x',
                center_position=center_position
            )
        elif formation_type == "circle":
            self.formation_controller.set_formation_circle(
                self.swarm_controller.selected_uavs,
                radius=4.0,  # 增加半径使圆形更明显
                center_position=center_position
            )
        elif formation_type == "square":
            self.formation_controller.set_formation_square(
                self.swarm_controller.selected_uavs,
                side_length=6.0,  # 增加边长使正方形更明显
                center_position=center_position
            )
        elif formation_type == "triangle":
            self.formation_controller.set_formation_triangle(
                self.swarm_controller.selected_uavs,
                side_length=5.0,  # 增加边长使三角形更明显
                center_position=center_position
            )
        elif formation_type == "clear":
            self.formation_controller.clear_formation()
            
    def on_rotation_button_clicked(self, direction):
        """编队旋转按钮点击处理"""
        if not self.swarm_controller.selected_uavs:
            print("请先选中无人机")
            return
            
        if self.formation_controller.current_formation is None:
            print("请先设置编队")
            return
            
        # 执行旋转
        if direction == "left":
            self.formation_controller.rotate_formation(-15)  # 左转15度
        elif direction == "right":
            self.formation_controller.rotate_formation(15)   # 右转15度
            
    def on_takeoff_height_changed(self, value):
        """起飞高度改变时的回调"""
        self.swarm_controller.takeoff_height = -value  # NED坐标系
        print(f"起飞高度设置为: {value} 米")
            
    def on_uav_count_changed(self, value):
        """无人机数量改变时的回调"""
        self.num_uavs = value
        self.create_uav_cards()
        self.status_label.setText(f"已更新无人机数量: {value}")
        
    def create_uav_cards(self):
        """创建无人机卡片"""
        # 清除现有卡片
        for card in self.uav_cards.values():
            card.deleteLater()
        self.uav_cards.clear()
        
        # 创建新卡片
        for uav_id in range(1, self.num_uavs + 1):
            card = UAVCard(uav_id)
            card.clicked.connect(self.on_uav_card_clicked)
            self.uav_cards[uav_id] = card
            
            # 确保所有卡片都有正确的UI交互属性
            card.setVisible(True)
            card.setEnabled(True)
            card.setFocusPolicy(Qt.StrongFocus)
            card.setMouseTracking(True)
        
        # 重新排列所有卡片以确保顺序正确
        self.rearrange_cards()
                
    def on_uav_card_clicked(self, uav_id):
        """无人机卡片点击处理"""
        if uav_id in self.swarm_controller.selected_uavs:
            # 如果已经选中，则取消选中
            self.swarm_controller.deselect_uav(uav_id)
            self.uav_cards[uav_id].set_selected(False)
        else:
            # 如果未选中，则选中
            self.swarm_controller.select_uav(uav_id)
            self.uav_cards[uav_id].set_selected(True)
            
        self.update_status_display()
        
    def select_all_uavs(self):
        """全选所有无人机"""
        print("✅ 执行全选操作")
        all_uav_ids = set(self.uav_cards.keys())
        print(f"  可选无人机: {sorted(all_uav_ids)}")
        
        # 使用select_uav方法逐个选中，确保触发所有逻辑
        for uav_id in all_uav_ids:
            self.swarm_controller.select_uav(uav_id)
            if uav_id in self.uav_cards:
                self.uav_cards[uav_id].set_selected(True)
        
        self.update_status_display()
        print(f"  已选中: {sorted(self.swarm_controller.selected_uavs)}")
        
    def deselect_all_uavs(self):
        """取消全选所有无人机"""
        print("❌ 执行取消全选操作")
        selected_copy = list(self.swarm_controller.selected_uavs)
        print(f"  当前选中: {sorted(selected_copy)}")
        
        # 使用deselect_uav方法逐个取消，确保触发所有逻辑
        for uav_id in selected_copy:
            self.swarm_controller.deselect_uav(uav_id)
            if uav_id in self.uav_cards:
                self.uav_cards[uav_id].set_selected(False)
        
        self.update_status_display()
        print(f"  取消后选中: {sorted(self.swarm_controller.selected_uavs)}")
        
    def refresh_cards(self):
        """刷新所有卡片"""
        for uav_id, card in self.uav_cards.items():
            uav_data = self.swarm_controller.get_uav_data(uav_id)
            card.update_data(uav_data)
            
        self.status_label.setText("卡片已刷新")
        
    def update_status_display(self):
        """更新状态显示"""
        uav_data = self.swarm_controller.get_all_uav_data()
        connected_count = sum(1 for data in uav_data.values() if data.get('connected', False))
        self.selected_count_label.setText(f"选中: {len(self.swarm_controller.selected_uavs)} 台")
        self.connected_count_label.setText(f"在线: {connected_count} 台")
        self.status_label.setText(f"已选中 {len(self.swarm_controller.selected_uavs)} 台无人机")
        
    def rearrange_cards(self):
        """重新排列所有卡片，确保按照ID顺序排列"""
        # 清除布局中的所有卡片
        for i in reversed(range(self.cards_layout.count())):
            widget = self.cards_layout.itemAt(i).widget()
            if widget:
                self.cards_layout.removeWidget(widget)
        
        # 确保容器和滚动区域可见且可交互
        self.cards_container.setVisible(True)
        self.cards_container.setEnabled(True)
        self.scroll_area.setVisible(True)
        self.scroll_area.setEnabled(True)
        
        # 按照ID顺序重新添加卡片
        max_cols = 4  # 每行最多4个卡片，避免过于拥挤
        sorted_uav_ids = sorted(self.uav_cards.keys())
        
        for index, uav_id in enumerate(sorted_uav_ids):
            row = index // max_cols
            col = index % max_cols
            card = self.uav_cards[uav_id]
            self.cards_layout.addWidget(card, row, col)
            
            # 确保卡片可见、可点击且可交互
            card.setVisible(True)
            card.setEnabled(True)
            card.setFocusPolicy(Qt.StrongFocus)  # 确保可以接收焦点
            card.setMouseTracking(True)  # 启用鼠标跟踪
            card.show()  # 强制显示卡片
            card.raise_()  # 确保卡片在最前面
            card.update()  # 强制更新卡片
        
        # 确保滚动区域正确显示所有卡片
        self.scroll_area.verticalScrollBar().setValue(0)  # 滚动到顶部
        self.cards_container.update()  # 强制更新容器
        self.scroll_area.update()  # 强制更新滚动区域
        self.cards_container.show()  # 强制显示容器
        self.scroll_area.show()  # 强制显示滚动区域
        
        # 强制刷新整个UI
        self.cards_container.repaint()
        self.scroll_area.repaint()
        self.repaint()
        
        # 延迟刷新，确保UI完全加载
        QTimer.singleShot(100, self.force_ui_refresh)
        
        print(f"📋 重新排列卡片: 共 {len(sorted_uav_ids)} 个卡片，布局为 {max_cols} 列")
        
    def force_ui_refresh(self):
        """强制刷新UI，确保所有卡片都可点击"""
        for uav_id, card in self.uav_cards.items():
            card.update()
            card.repaint()
        self.cards_container.update()
        self.cards_container.repaint()
        self.scroll_area.update()
        self.scroll_area.repaint()
        print("🔄 强制UI刷新完成")
        
    def setup_timers(self):
        """设置定时器"""
        # ROS 2定时器
        self.ros_timer = QTimer()
        self.ros_timer.timeout.connect(self.ros_spin)
        self.ros_timer.start(50)  # 20Hz
        
        # 连接状态检查定时器
        self.connection_timer = QTimer()
        self.connection_timer.timeout.connect(self.check_connections)
        self.connection_timer.start(1000)  # 1Hz
        
        # 状态更新定时器
        self.status_timer = QTimer()
        self.status_timer.timeout.connect(self.update_status_display)
        self.status_timer.start(500)  # 2Hz
        
        # 编队控制定时器
        self.formation_timer = QTimer()
        self.formation_timer.timeout.connect(self.formation_control_loop)
        self.formation_timer.start(100)  # 10Hz - 持续发送编队控制信号
        
    def ros_spin(self):
        """处理ROS 2事件"""
        rclpy.spin_once(self.swarm_controller, timeout_sec=0.0)
        
    def formation_control_loop(self):
        """编队控制循环 - 持续发送offboard信号和位置设定点"""
        if self.formation_controller.current_formation is None:
            return
        
        formation_params = self.formation_controller.formation_parameters
        if 'positions' not in formation_params:
            return
        
        positions = formation_params['positions']
        
        for uav_id, (target_x, target_y, target_z) in positions.items():
            # 关键修复：检查无人机是否正在降落或返航，如果是则不发送offboard信号
            if uav_id in self.swarm_controller.landing_uavs:
                uav_data = self.swarm_controller.get_uav_data(uav_id)
                current_mode = uav_data.get('flight_mode', uav_data.get('flight_mode_str', ''))
                if current_mode == "Land":
                    print(f"UAV {uav_id}: 🛬 正在降落中，跳过编队控制")
                    continue
            
            if uav_id in self.swarm_controller.returning_uavs:
                uav_data = self.swarm_controller.get_uav_data(uav_id)
                current_mode = uav_data.get('flight_mode', uav_data.get('flight_mode_str', ''))
                if current_mode == "Return":
                    print(f"UAV {uav_id}: 🏠 正在返航中，跳过编队控制")
                    continue
            
            # 1. 发送offboard控制模式信号
            self.swarm_controller.publish_offboard_control_mode(uav_id)
            
            # 2. 发送位置设定点信号
            self.swarm_controller.publish_position_setpoint(uav_id, target_x, target_y, target_z)
            
            # 3. 确保无人机处于offboard模式
            uav_data = self.swarm_controller.get_uav_data(uav_id)
            current_mode = uav_data.get('flight_mode', uav_data.get('flight_mode_str', ''))
            if current_mode != "Offboard":
                print(f"UAV {uav_id}: 🔄 编队控制发送offboard模式切换指令")
                self.swarm_controller.publish_vehicle_command(uav_id, VehicleCommand.VEHICLE_CMD_DO_SET_MODE, param1=1.0, param2=6.0)
        
        # 每发送50次信号打印一次状态（减少频率）
        if hasattr(self, '_formation_signal_count'):
            self._formation_signal_count += 1
            if self._formation_signal_count % 50 == 0:
                print(f"编队控制运行中 - {len(positions)} 台无人机")
        else:
            self._formation_signal_count = 1
        
    def check_connections(self):
        """检查连接状态"""
        # 这里可以添加更复杂的连接状态检测逻辑
        pass
        
    def start_global_offboard_thread(self):
        """v2版本不需要全局offboard线程，定时器自动处理"""
        print("🚀 v2版本使用ROS2定时器自动处理offboard信号，无需全局线程")
        # v2版本废弃全局线程，由定时器自动处理
        pass
            
    def stop_global_offboard_thread(self):
        """v2版本不需要停止全局offboard线程"""
        print("🚀 v2版本无需停止全局offboard线程")
        # v2版本废弃全局线程，由定时器自动处理
        pass
                
    def global_offboard_worker(self):
        """v2版本废弃 - 由ROS2定时器自动处理offboard信号"""
        print("🌍 v2版本废弃全局offboard线程，使用ROS2定时器自动处理")
        # v2版本不需要此线程，定时器自动处理offboard信号发送
        pass
        
    def debug_thread_status(self):
        """调试函数：检查所有线程状态"""
        print("🔍 === 线程状态调试信息 ===")
        
        # 检查全局线程状态
        global_status = "运行中" if self.swarm_controller.global_offboard_running else "已停止"
        print(f"🌍 全局offboard线程: {global_status}")
        
        # 检查个体线程状态
        if self.swarm_controller.selected_uavs:
            print(f"🎯 选中的无人机: {sorted(self.swarm_controller.selected_uavs)}")
            for uav_id in self.swarm_controller.selected_uavs:
                thread_status = "运行中" if self.swarm_controller.selected_uavs_offboard_running.get(uav_id, False) else "已停止"
                uav_data = self.swarm_controller.get_uav_data(uav_id)
                current_mode = uav_data.get('flight_mode_str', '未知')
                is_armed = uav_data.get('is_armed', False)
                landing_status = "正在降落" if uav_id in self.swarm_controller.landing_uavs else "正常"
                print(f"  UAV {uav_id}: 线程={thread_status}, 模式={current_mode}, 解锁={is_armed}, 状态={landing_status}")
        else:
            print("🎯 没有选中的无人机")
            
        # 检查offboard定时器状态
        active_timers = len(self.swarm_controller.offboard_timers)
        print(f"⏰ 活跃的offboard定时器: {active_timers} 个")
        
        # 检查offboard活跃状态 (offboard_active 是集合，不是字典)
        active_offboard = len(self.swarm_controller.offboard_active)
        print(f"🚀 活跃的offboard控制: {active_offboard}/{len(self.swarm_controller.offboard_active)}")
        
        print("🔍 === 调试信息结束 ===\n")
        
    def closeEvent(self, event):
        """窗口关闭事件"""
        # 停止所有定时器
        self.ros_timer.stop()
        self.connection_timer.stop()
        self.status_timer.stop()
        self.debug_timer.stop()
        
        # 停止所有offboard控制定时器
        try:
            for timer in self.swarm_controller.offboard_timers.values():
                if hasattr(timer, 'stop'):
                    timer.stop()
        except Exception as e:
            print(f"停止定时器时出错: {e}")
        
        # v2版本不需要停止后台线程，定时器会自动清理
        print(f"  🔄 Offboard定时器自动清理")
        
        # 停止全局offboard线程
        self.stop_global_offboard_thread()
        
        # 关闭ROS 2节点
        try:
            # 只调用一次shutdown，避免重复调用
            if rclpy.ok():
                rclpy.shutdown()
        except Exception as e:
            print(f"关闭ROS节点时出错: {e}")
        
        event.accept()


def main():
    """主程序入口函数"""
    import sys
    from PyQt5.QtWidgets import QApplication
    
    # 解析命令行参数
    import argparse
    parser = argparse.ArgumentParser(description='Drone多无人机监控UI')
    parser.add_argument('--num-uavs', type=int, default=0, help='无人机数量 (0表示自动检测)')
    args = parser.parse_args()
    
    # 创建应用
    app = QApplication(sys.argv)
    
    # 创建主窗口
    window = UAVMonitorWindow(num_uavs=args.num_uavs)
    window.show()

    # 运行应用
    sys.exit(app.exec_())


if __name__ == "__main__":
    main()