#!/usr/bin/env python3
# 遥控器设置标签页

import time
from PyQt5.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QGridLayout,
                            QLabel, QGroupBox, QProgressBar, QFrame, QPushButton,
                            QComboBox, QCheckBox, QSpinBox)
from PyQt5.QtCore import Qt, QTimer
from PyQt5.QtGui import QFont, QColor, QPalette


class JoystickTab(QWidget):
    """遥控器设置标签页 - 显示joy节点信号和飞行模式配置"""
    
    def __init__(self, swarm_controller):
        super().__init__()
        self.swarm_controller = swarm_controller
        self.flight_mode_config = {}  # 存储飞行模式配置
        self.uav_controller = None  # 将在主窗口初始化后设置
        self.active_mode_switches = {}  # 存储正在进行的模式切换
        self.setup_ui()
        self.setup_timers()
        self.setup_flight_mode_config()
        
    def setup_ui(self):
        """设置UI界面"""
        main_layout = QVBoxLayout(self)
        
        # 标题
        title_label = QLabel("遥控器设置")
        title_font = QFont()
        title_font.setBold(True)
        title_font.setPointSize(16)
        title_label.setFont(title_font)
        title_label.setAlignment(Qt.AlignCenter)
        main_layout.addWidget(title_label)
        
        # 连接状态组
        connection_group = QGroupBox("连接状态")
        connection_layout = QVBoxLayout(connection_group)
        
        self.connection_status = QLabel("🔴 未连接")
        self.connection_status.setFont(QFont("Arial", 12, QFont.Bold))
        connection_layout.addWidget(self.connection_status)
        
        self.last_update_label = QLabel("最后更新: 无")
        connection_layout.addWidget(self.last_update_label)
        
        main_layout.addWidget(connection_group)
        
        # 飞行模式配置组
        flight_mode_group = QGroupBox("飞行模式配置")
        flight_mode_layout = QGridLayout(flight_mode_group)
        
        # 飞行模式配置表格
        flight_mode_layout.addWidget(QLabel("按钮"), 0, 0)
        flight_mode_layout.addWidget(QLabel("飞行模式"), 0, 1)
        flight_mode_layout.addWidget(QLabel("启用"), 0, 2)
        
        self.flight_mode_combos = []
        self.enable_checks = []
        
        flight_modes = ["定点模式", "姿态模式", "定高模式", "Offboard模式"]
        button_names = ["A", "B", "X", "Y", "LB", "RB"]
        
        for i, button in enumerate(button_names):
            # 按钮名称
            flight_mode_layout.addWidget(QLabel(f"按钮 {button}"), i+1, 0)
            
            # 飞行模式选择
            mode_combo = QComboBox()
            mode_combo.addItems(flight_modes)
            mode_combo.setCurrentIndex(i if i < len(flight_modes) else 0)
            flight_mode_layout.addWidget(mode_combo, i+1, 1)
            self.flight_mode_combos.append(mode_combo)
            
            # 启用复选框
            enable_check = QCheckBox()
            enable_check.setChecked(True)
            flight_mode_layout.addWidget(enable_check, i+1, 2)
            self.enable_checks.append(enable_check)
        
        # 说明文本
        info_label = QLabel("注意：飞行模式将应用于所有被选中的无人机")
        info_label.setStyleSheet("color: #666; font-size: 10px;")
        flight_mode_layout.addWidget(info_label, len(button_names)+1, 0, 1, 3)
        
        main_layout.addWidget(flight_mode_group)
        
        # 摇杆轴显示组
        axes_group = QGroupBox("摇杆轴")
        axes_layout = QGridLayout(axes_group)
        
        self.axis_bars = []
        axis_names = [
            "左摇杆 X", "左摇杆 Y", "左摇杆 Z", "右摇杆 X",
            "右摇杆 Y", "右摇杆 Z", "方向舵", "油门"
        ]
        
        for i, name in enumerate(axis_names):
            # 轴名称标签
            name_label = QLabel(name)
            axes_layout.addWidget(name_label, i, 0)
            
            # 进度条显示轴值
            progress_bar = QProgressBar()
            progress_bar.setMinimum(-100)
            progress_bar.setMaximum(100)
            progress_bar.setValue(0)
            progress_bar.setFormat("%v")
            axes_layout.addWidget(progress_bar, i, 1)
            
            # 数值标签
            value_label = QLabel("0.00")
            value_label.setAlignment(Qt.AlignRight)
            value_label.setMinimumWidth(50)
            axes_layout.addWidget(value_label, i, 2)
            
            self.axis_bars.append((progress_bar, value_label))
        
        main_layout.addWidget(axes_group)
        
        # 按钮状态组
        buttons_group = QGroupBox("按钮状态")
        buttons_layout = QGridLayout(buttons_group)
        
        self.button_indicators = []
        button_names = [
            "A", "B", "X", "Y",
            "LB", "RB", "Back", "Start",
            "L3", "R3", "Xbox", "Power"
        ]
        
        for i, name in enumerate(button_names):
            # 按钮名称标签
            name_label = QLabel(name)
            buttons_layout.addWidget(name_label, i // 4, (i % 4) * 2)
            
            # 按钮状态指示器
            indicator = QLabel("○")
            indicator.setAlignment(Qt.AlignCenter)
            indicator.setStyleSheet("font-size: 16px;")
            buttons_layout.addWidget(indicator, i // 4, (i % 4) * 2 + 1)
            
            self.button_indicators.append(indicator)
        
        main_layout.addWidget(buttons_group)
        
        # 控制按钮组
        control_group = QGroupBox("控制")
        control_layout = QHBoxLayout(control_group)
        
        self.refresh_btn = QPushButton("🔄 刷新")
        self.refresh_btn.clicked.connect(self.refresh_data)
        control_layout.addWidget(self.refresh_btn)
        
        self.calibrate_btn = QPushButton("⚙️ 校准")
        self.calibrate_btn.clicked.connect(self.calibrate_joystick)
        control_layout.addWidget(self.calibrate_btn)
        
        self.save_config_btn = QPushButton("💾 保存配置")
        self.save_config_btn.clicked.connect(self.save_flight_mode_config)
        control_layout.addWidget(self.save_config_btn)
        
        control_layout.addStretch()
        main_layout.addWidget(control_group)
        
        main_layout.addStretch()
        
    def setup_flight_mode_config(self):
        """初始化飞行模式配置"""
        # 默认配置：按钮A到RB分别对应不同的飞行模式
        self.flight_mode_config = {
            'A': {'mode': '定点模式', 'enabled': True},
            'B': {'mode': '姿态模式', 'enabled': True},
            'X': {'mode': '定高模式', 'enabled': True},
            'Y': {'mode': 'Offboard模式', 'enabled': True},
            'LB': {'mode': '定点模式', 'enabled': True},
            'RB': {'mode': 'Offboard模式', 'enabled': True}
        }
        
        # 更新UI显示
        self.update_flight_mode_ui()
    
    def update_flight_mode_ui(self):
        """更新飞行模式配置UI"""
        button_names = ["A", "B", "X", "Y", "LB", "RB"]
        for i, button in enumerate(button_names):
            if button in self.flight_mode_config:
                config = self.flight_mode_config[button]
                # 设置飞行模式
                mode_index = self.flight_mode_combos[i].findText(config['mode'])
                if mode_index >= 0:
                    self.flight_mode_combos[i].setCurrentIndex(mode_index)
                # 设置启用状态
                self.enable_checks[i].setChecked(config['enabled'])
    
    def setup_timers(self):
        """设置定时器"""
        # 遥控器数据更新定时器
        self.joystick_timer = QTimer()
        self.joystick_timer.timeout.connect(self.update_joystick_data)
        self.joystick_timer.start(100)  # 10Hz更新频率
        
        # 飞行模式切换持续发送定时器
        self.mode_switch_timer = QTimer()
        self.mode_switch_timer.timeout.connect(self.continuous_mode_switch)
        self.mode_switch_timer.start(500)  # 2Hz持续发送
        
    def update_joystick_data(self):
        """更新遥控器数据显示"""
        joy_data = self.swarm_controller.get_joy_data()
        
        # 更新连接状态
        if joy_data['connected']:
            self.connection_status.setText("🟢 已连接")
            self.connection_status.setStyleSheet("color: green;")
            
            # 更新最后更新时间
            if joy_data['last_update']:
                import time
                time_diff = time.time() - joy_data['last_update']
                self.last_update_label.setText(f"最后更新: {time_diff:.1f}秒前")
            
            # 检查按钮按下事件
            self.check_button_events(joy_data['buttons'])
        else:
            self.connection_status.setText("🔴 未连接")
            self.connection_status.setStyleSheet("color: red;")
            self.last_update_label.setText("最后更新: 无")
        
        # 更新轴数据
        axes = joy_data['axes']
        for i, (progress_bar, value_label) in enumerate(self.axis_bars):
            if i < len(axes):
                value = axes[i]
                # 转换为百分比 (-1.0 到 1.0 转换为 -100 到 100)
                percentage = int(value * 100)
                progress_bar.setValue(percentage)
                value_label.setText(f"{value:.2f}")
                
                # 根据值设置颜色
                if abs(value) > 0.8:
                    progress_bar.setStyleSheet("QProgressBar::chunk { background-color: #ff4444; }")
                elif abs(value) > 0.3:
                    progress_bar.setStyleSheet("QProgressBar::chunk { background-color: #ffaa00; }")
                else:
                    progress_bar.setStyleSheet("QProgressBar::chunk { background-color: #44ff44; }")
        
        # 更新按钮数据
        buttons = joy_data['buttons']
        for i, indicator in enumerate(self.button_indicators):
            if i < len(buttons):
                if buttons[i] == 1:
                    indicator.setText("●")
                    indicator.setStyleSheet("color: red; font-size: 16px;")
                else:
                    indicator.setText("○")
                    indicator.setStyleSheet("color: gray; font-size: 16px;")
    
    def check_button_events(self, buttons):
        """检查按钮按下事件并执行相应的飞行模式切换"""
        button_names = ["A", "B", "X", "Y", "LB", "RB"]
        
        for i, button in enumerate(button_names):
            if i < len(buttons) and buttons[i] == 1:  # 按钮按下
                if button in self.flight_mode_config:
                    config = self.flight_mode_config[button]
                    if config['enabled']:
                        self.execute_flight_mode(config['mode'], button)
    
    def execute_flight_mode(self, mode, button_name):
        """执行飞行模式切换 - 应用于所有被选中的无人机"""
        if not self.uav_controller or not self.uav_controller.selected_uavs:
            print(f"按钮 {button_name} 按下: 没有选中的无人机")
            return
        
        selected_count = len(self.uav_controller.selected_uavs)
        print(f"按钮 {button_name} 按下: 设置 {selected_count} 台被选中的无人机为 {mode}")
        
        # 飞行模式对应的PX4命令
        mode_commands = {
            '定点模式': {'command': VehicleCommand.VEHICLE_CMD_DO_SET_MODE, 'param1': 1.0, 'param2': 3.0},  # Position
            '姿态模式': {'command': VehicleCommand.VEHICLE_CMD_DO_SET_MODE, 'param1': 1.0, 'param2': 2.0},  # Altitude
            '定高模式': {'command': VehicleCommand.VEHICLE_CMD_DO_SET_MODE, 'param1': 1.0, 'param2': 2.0},  # Altitude (same as attitude for simplicity)
            'Offboard模式': {'command': VehicleCommand.VEHICLE_CMD_DO_SET_MODE, 'param1': 1.0, 'param2': 6.0}  # Offboard
        }
        
        if mode in mode_commands:
            cmd = mode_commands[mode]
            
            # 为所有被选中的无人机启动持续模式切换
            for uav_id in self.uav_controller.selected_uavs:
                # 获取无人机当前状态
                uav_data = self.swarm_controller.get_uav_data(uav_id)
                current_mode = uav_data.get('flight_mode', '未知')
                is_armed = uav_data.get('is_armed', False)
                
                print(f"  UAV {uav_id}: 当前模式={current_mode}, 解锁状态={is_armed}")
                
                # 记录需要切换模式的无人机
                self.active_mode_switches[uav_id] = {
                    'target_mode': mode,
                    'command': cmd,
                    'start_time': time.time(),
                    'max_duration': 10.0,  # 最多持续10秒
                    'sent_count': 0
                }
                
                # 立即发送第一次命令
                self.swarm_controller.publish_vehicle_command(
                    uav_id,
                    cmd['command'],
                    param1=cmd['param1'],
                    param2=cmd['param2']
                )
                self.active_mode_switches[uav_id]['sent_count'] += 1
                print(f"  ✅ UAV {uav_id}: 开始持续发送 {mode} 命令")
            
            print(f"✅ 已启动持续模式切换: {selected_count} 台无人机 -> {mode}")
            print("💡 系统将持续发送模式切换命令直到切换成功或超时")
            
        else:
            print(f"❌ 未知飞行模式: {mode}")
    
    def continuous_mode_switch(self):
        """持续发送飞行模式切换命令"""
        if not self.active_mode_switches:
            return
        
        current_time = time.time()
        completed_uavs = []
        
        for uav_id, switch_info in self.active_mode_switches.items():
            # 检查是否超时
            if current_time - switch_info['start_time'] > switch_info['max_duration']:
                print(f"  ⏰ UAV {uav_id}: 模式切换超时，停止发送 {switch_info['target_mode']} 命令")
                completed_uavs.append(uav_id)
                continue
            
            # 检查是否已经切换到目标模式
            uav_data = self.swarm_controller.get_uav_data(uav_id)
            current_mode = uav_data.get('flight_mode', '未知')
            
            if current_mode == switch_info['target_mode']:
                print(f"  ✅ UAV {uav_id}: 已成功切换到 {switch_info['target_mode']} 模式")
                completed_uavs.append(uav_id)
                continue
            
            # 继续发送模式切换命令
            cmd = switch_info['command']
            self.swarm_controller.publish_vehicle_command(
                uav_id,
                cmd['command'],
                param1=cmd['param1'],
                param2=cmd['param2']
            )
            switch_info['sent_count'] += 1
            
            # 每发送5次打印一次状态
            if switch_info['sent_count'] % 5 == 0:
                print(f"  🔄 UAV {uav_id}: 第{switch_info['sent_count']}次发送 {switch_info['target_mode']} 命令")
        
        # 移除已完成或超时的无人机
        for uav_id in completed_uavs:
            del self.active_mode_switches[uav_id]
    
    def save_flight_mode_config(self):
        """保存飞行模式配置"""
        button_names = ["A", "B", "X", "Y", "LB", "RB"]
        
        for i, button in enumerate(button_names):
            mode = self.flight_mode_combos[i].currentText()
            enabled = self.enable_checks[i].isChecked()
            
            self.flight_mode_config[button] = {
                'mode': mode,
                'enabled': enabled
            }
        
        print("✅ 飞行模式配置已保存")
        print("当前配置:")
        for button, config in self.flight_mode_config.items():
            if config['enabled']:
                print(f"  按钮 {button}: {config['mode']} (应用于所有被选中的无人机)")
    
    def refresh_data(self):
        """手动刷新数据"""
        self.update_joystick_data()
        print("遥控器数据已刷新")
    
    def calibrate_joystick(self):
        """校准遥控器"""
        print("开始遥控器校准...")
        # 这里可以添加实际的校准逻辑
        # 目前只是显示消息
        self.connection_status.setText("🟡 校准中...")
        self.connection_status.setStyleSheet("color: orange;")
        
        # 模拟校准过程
        QTimer.singleShot(2000, self.finish_calibration)
    
    def finish_calibration(self):
        """完成校准"""
        self.connection_status.setText("🟢 已连接")
        self.connection_status.setStyleSheet("color: green;")
        print("遥控器校准完成")