#!/usr/bin/env python3
from PyQt6.QtWidgets import (QWidget, QVBoxLayout, QHBoxLayout, QLabel, QCheckBox)
from PyQt6.QtCore import pyqtSignal
from PyQt6.QtGui import QPainter, QPen, QColor, QFont, QPolygonF
from PyQt6.QtCore import QPointF, Qt
import math

class DronePanel(QWidget):
    """單個無人機面板類別"""
    
    # 定義信號
    mode_change_requested = pyqtSignal(str)  # drone_id
    arm_requested = pyqtSignal(str)  # drone_id
    takeoff_requested = pyqtSignal(str)  # drone_id
    setpoint_requested = pyqtSignal(str)  # drone_id
    selection_changed = pyqtSignal(str, int)  # drone_id, state
    
    def __init__(self, drone_id, parent=None):
        super().__init__(parent)
        self.drone_id = drone_id
        
        # 提取資訊 (格式: s{socket_seq}_{system_id}, 如 s0_11, s1_12)
        parts = drone_id.split('_')
        if len(parts) >= 2:
            self.socket_seq = parts[0][1:]  # socket 序號 (移除 's' 前綴)
            self.system_id = parts[1]       # system ID
            self.display_id = f"ID:{self.system_id}"  # 顯示為 ID:11, ID:12
        else:
            self.socket_seq = "?"
            self.system_id = "?"
            self.display_id = drone_id
        
        self.attitude_indicator = None
        self._init_ui()
    
    def _init_ui(self):
        """初始化UI"""
        self.setObjectName(f"panel_{self.drone_id}")
        self.setFixedHeight(140)
        self.setStyleSheet("""
            background-color: #2A2A2A;
            border-radius: 8px;
        """)
        
        # 主佈局
        main_layout = QHBoxLayout(self)
        main_layout.setContentsMargins(8, 8, 8, 8)
        main_layout.setSpacing(0)
        
        # 創建內容容器（包含 info 和 control）
        content_widget = QWidget()
        content_widget.setStyleSheet("background-color: #333; border-radius: 6px;")
        content_layout = QHBoxLayout(content_widget)
        content_layout.setContentsMargins(8, 8, 8, 8)
        content_layout.setSpacing(8)
        
        # 左側資訊區域
        info_widget = self._create_info_section()
        
        # 右側態度指示器
        attitude_widget = self._create_attitude_indicator()
        
        # 將 info 和 attitude 加入內容容器
        content_layout.addWidget(info_widget, 1)
        content_layout.addWidget(attitude_widget, 0)
        
        # 將內容容器加入主佈局
        main_layout.addWidget(content_widget)
    
    def _create_info_section(self):
        """創建資訊顯示區域"""
        info_widget = QWidget()
        info_layout = QVBoxLayout(info_widget)
        info_layout.setContentsMargins(0, 0, 0, 0)
        info_layout.setSpacing(4)
        
        # 頂部標題欄
        header = QWidget()
        header_layout = QHBoxLayout(header)
        header_layout.setContentsMargins(0, 0, 0, 0)
        
        # 勾選框
        self.checkbox = QCheckBox()
        self.checkbox.setObjectName(f"{self.drone_id}_checkbox")
        self.checkbox.setStyleSheet("""
            QCheckBox { 
                color: #DDD;
            }
            QCheckBox::indicator {
                width: 16px;
                height: 16px;
                border: 2px solid #888888;
                border-radius: 3px;
                background: transparent;
            }
            QCheckBox::indicator:checked {
                background-color: #7FFFD4;
                border: 2px solid #888888;
            }
        """)
        self.checkbox.stateChanged.connect(
            lambda state: self.selection_changed.emit(self.drone_id, state)
        )
        
        # ID 顯示
        id_label = QLabel(self.display_id)
        id_label.setStyleSheet("""
            font-weight: bold; 
            font-size: 14px; 
            color: #7FFFD4;
            min-width: 80px;
        """)
        
        header_layout.addWidget(self.checkbox)
        header_layout.addWidget(id_label)
        header_layout.addStretch()
        
        info_layout.addWidget(header)
        
        # 第一行：狀態 (模式 + ARM狀態)
        status_row = self._create_status_row()
        info_layout.addWidget(status_row)
        
        # 第二行：電池（拆成百分比與電壓兩欄）
        battery_row = self._create_battery_row()
        info_layout.addWidget(battery_row)
        
        # 第三行：高度
        altitude_row = self._create_altitude_row()
        info_layout.addWidget(altitude_row)
        
        # 第四行：航向 + 速度
        nav_row = self._create_nav_row()
        info_layout.addWidget(nav_row)
        
        return info_widget
    
    def _create_attitude_indicator(self):
        """創建態度指示器（ADI 人工地平儀）"""
        self.attitude_indicator = AttitudeIndicator(self.drone_id)
        self.attitude_indicator.setFixedSize(90, 100)
        return self.attitude_indicator
    
    def _create_status_row(self):
        """創建狀態行"""
        status_row = QWidget()
        status_layout = QHBoxLayout(status_row)
        status_layout.setContentsMargins(0, 0, 0, 0)
        
        status_title = QLabel("狀態：")
        status_title.setStyleSheet("color: #888; min-width: 50px;")
        
        self.mode_label = QLabel("--")
        self.mode_label.setObjectName(f"{self.drone_id}_mode")
        self.mode_label.setStyleSheet("color: #DDD;")
        
        self.armed_label = QLabel("--")
        self.armed_label.setObjectName(f"{self.drone_id}_armed")
        self.armed_label.setStyleSheet("color: #DDD;")
        
        status_layout.addWidget(status_title)
        status_layout.addWidget(self.mode_label)
        status_layout.addWidget(self.armed_label)
        status_layout.addStretch()
        
        return status_row
    
    def _create_connection_row(self):
        """創建連接資訊行 (Socket Seq + 連接方式)"""
        connection_row = QWidget()
        connection_layout = QHBoxLayout(connection_row)
        connection_layout.setContentsMargins(0, 0, 0, 0)
        
        connection_title = QLabel("Socket：")
        connection_title.setStyleSheet("color: #888; min-width: 50px;")
        
        # 根據解析的 drone_id 資訊設定初始值
        self.socket_seq_label = QLabel(self.socket_seq)
        self.socket_seq_label.setObjectName(f"{self.drone_id}_socket_seq")
        self.socket_seq_label.setStyleSheet("color: #DDD;")
        
        connection_sep = QLabel(" - ")
        connection_sep.setStyleSheet("color: #DDD;")
        
        # 設定連接方式顯示
        connection_type_map = {
            'r': 'ROS2',
            'u': 'UDP',
            's': 'Serial',
            'w': 'WS'
        }
        connection_type = connection_type_map.get(self.type_prefix, 'Unknown')
        
        self.connection_type_label = QLabel(connection_type)
        self.connection_type_label.setObjectName(f"{self.drone_id}_connection_type")
        self.connection_type_label.setStyleSheet("color: #DDD;")
        
        connection_layout.addWidget(connection_title)
        connection_layout.addWidget(self.socket_seq_label)
        connection_layout.addWidget(connection_sep)
        connection_layout.addWidget(self.connection_type_label)
        connection_layout.addStretch()
        
        return connection_row
    
    def _create_battery_row(self):
        """創建電池行"""
        battery_row = QWidget()
        battery_layout = QHBoxLayout(battery_row)
        battery_layout.setContentsMargins(0, 0, 0, 0)
        # 顯示百分比
        battery_title = QLabel("電池：")
        battery_title.setStyleSheet("color: #888; min-width: 50px;")

        self.battery_pct_label = QLabel("--")
        self.battery_pct_label.setObjectName(f"{self.drone_id}_battery_pct")
        self.battery_pct_label.setStyleSheet("color: #DDD;")

        # 分隔符
        separator1 = QLabel(" - ")
        separator1.setStyleSheet("color: #DDD;")

        # 顯示電壓
        self.battery_vol_label = QLabel("--")
        self.battery_vol_label.setObjectName(f"{self.drone_id}_battery_vol")
        self.battery_vol_label.setStyleSheet("color: #DDD;")

        # 分隔符
        separator2 = QLabel(" - ")
        separator2.setStyleSheet("color: #DDD;")

        # 顯示電池節數 (S count)
        self.battery_cells_label = QLabel("--")
        self.battery_cells_label.setObjectName(f"{self.drone_id}_battery_cells")
        self.battery_cells_label.setStyleSheet("color: #DDD;")

        battery_layout.addWidget(battery_title)
        battery_layout.addWidget(self.battery_pct_label)
        battery_layout.addWidget(separator1)
        battery_layout.addWidget(self.battery_vol_label)
        battery_layout.addWidget(separator2)
        battery_layout.addWidget(self.battery_cells_label)
        battery_layout.addStretch()
        
        return battery_row
    
    def _create_altitude_row(self):
        """創建高度和速度行"""
        altitude_row = QWidget()
        altitude_layout = QHBoxLayout(altitude_row)
        altitude_layout.setContentsMargins(0, 0, 0, 0)
        
        altitude_title = QLabel("高度：")
        altitude_title.setStyleSheet("color: #888; min-width: 50px;")
        
        self.altitude_label = QLabel("--")
        self.altitude_label.setObjectName(f"{self.drone_id}_altitude")
        self.altitude_label.setStyleSheet("color: #DDD;")
        
        speed_title = QLabel("速度：")
        speed_title.setStyleSheet("color: #888; margin-left: 10px;")
        
        self.speed_label = QLabel("--")
        self.speed_label.setObjectName(f"{self.drone_id}_speed")
        self.speed_label.setStyleSheet("color: #DDD;")
        
        altitude_layout.addWidget(altitude_title)
        altitude_layout.addWidget(self.altitude_label)
        altitude_layout.addWidget(speed_title)
        altitude_layout.addWidget(self.speed_label)
        altitude_layout.addStretch()
        
        return altitude_row
    
    def _create_position_row(self):
        """位置行已移除（位置座標不再顯示於面板）。"""
        return QWidget()
    
    def _create_nav_row(self):
        """創建導航行（已移除，不再顯示）"""
        return QWidget()
    
    def update_field(self, field, text, color=None):
        """更新指定欄位的值"""
        label = self.findChild(QLabel, f"{self.drone_id}_{field}")
        if label and label.text() != text:
            label.setText(text)
            if color:
                label.setStyleSheet(f"color: {color};")
    
    def set_connection_info(self, socket_seq, connection_type):
        """設定連接資訊（Socket Seq 和連接方式）
        connection_type: 'UDP' | 'Serial' | 'WS'
        """
        self.socket_seq_label.setText(str(socket_seq))
        
        # 顯示友善的連接方式
        type_display = {
            'UDP': 'UDP',
            'Serial': 'Serial',
            'WS': 'WS'
        }.get(connection_type, connection_type)
        
        self.connection_type_label.setText(type_display)

    
    def update_attitude(self, heading, roll, pitch):
        """更新態度指示器"""
        if self.attitude_indicator:
            self.attitude_indicator.update_attitude(heading, roll, pitch)
    
    def get_checkbox(self):
        """獲取勾選框"""
        return self.checkbox
    
    def set_checked(self, checked):
        """設置勾選狀態"""
        self.checkbox.setChecked(checked)
    
    def is_checked(self):
        """獲取勾選狀態"""
        return self.checkbox.isChecked()
    
class SocketGroupPanel(QWidget):
    # 定義信號
    group_selection_changed = pyqtSignal(str, int)  # socket_id, state
    
    def __init__(self, socket_id, color='#AAAAAA', socket_type=None, parent=None):
        super().__init__(parent)
        self.socket_id = socket_id
        self.color = color
        self.socket_type = socket_type
        self._init_ui()
    
    def _init_ui(self):
        """初始化UI"""
        self.setObjectName(f"socket_group_{self.socket_id}")
        self.setStyleSheet("""
            background-color: #1E1E1E;
            border-radius: 12px;
        """)
        
        layout = QVBoxLayout(self)
        layout.setContentsMargins(10, 10, 10, 10)
        layout.setSpacing(6)
        
        # Socket 分組標題行 - 包含勾選框
        title_row = QWidget()
        title_layout = QHBoxLayout(title_row)
        title_layout.setContentsMargins(0, 0, 0, 0)
        
        # 分組勾選框
        self.group_checkbox = QCheckBox()
        self.group_checkbox.setObjectName(f"socket_{self.socket_id}_checkbox")
        self.group_checkbox.setStyleSheet(f"""
            QCheckBox {{ color: #DDD; }}
            QCheckBox::indicator {{
                width: 14px;
                height: 14px;
                border: 2px solid #888888;
                border-radius: 3px;
                background: transparent;
            }}
            QCheckBox::indicator:checked {{
                background-color: {self.color};
                border: 2px solid #888888;
            }}
            QCheckBox::indicator:indeterminate {{
                background-color: #666;
                border: 2px solid #888888;
            }}
        """)
        self.group_checkbox.stateChanged.connect(
            lambda state: self.group_selection_changed.emit(self.socket_id, state)
        )
        
        # Socket 分組標題
        if self.socket_type:
            title_text = f"{self.socket_type} {self.socket_id}"
        else:
            title_text = f"Socket {self.socket_id}"
        self.title_label = QLabel(title_text)
        self.title_label.setStyleSheet(f"""
            font-weight: bold; 
            font-size: 16px; 
            color: {self.color};
            margin-bottom: 8px;
            padding: 4px 8px;
            border-radius: 6px;
        """)
        
        title_layout.addWidget(self.group_checkbox)
        title_layout.addWidget(self.title_label)
        title_layout.addStretch()
        
        layout.addWidget(title_row)
        
        # 創建子容器用於放置該 socket 下的所有無人機面板
        self.drones_container = QWidget()
        self.drones_layout = QVBoxLayout(self.drones_container)
        self.drones_layout.setContentsMargins(0, 0, 0, 0)
        self.drones_layout.setSpacing(4)
        
        layout.addWidget(self.drones_container)
    
    def add_drone_panel(self, panel):
        """添加無人機面板到分組"""
        self.drones_layout.addWidget(panel)
    
    def clear_drones(self):
        """清空所有無人機面板"""
        while self.drones_layout.count():
            item = self.drones_layout.takeAt(0)
            if item.widget():
                item.widget().setParent(None)
    
    def get_checkbox(self):
        """獲取分組勾選框"""
        return self.group_checkbox
    
    def set_checked(self, checked):
        """設置分組勾選狀態"""
        self.group_checkbox.setChecked(checked)
    
    def set_socket_type(self, conn_type):
        """設置 socket 類型並更新標題"""
        self.title_label.setText(f"{conn_type} {self.socket_id}")
    
    def set_check_state(self, state):
        """設置分組勾選狀態（支持半選）"""
        self.group_checkbox.setCheckState(state)


class AttitudeIndicator(QWidget):
    """
    人工地平儀 (ADI) — 仿 Mission Planner 風格
    上半部顯示 roll/pitch 人工地平儀，下方細條顯示航向
    """

    def __init__(self, drone_id, parent=None):
        super().__init__(parent)
        self.drone_id = drone_id
        self.heading = 0.0   # 航向 yaw (0–360)
        self.roll    = 0.0   # 滾轉 (deg, left- negative)
        self.pitch   = 0.0   # 俯仰 (deg, nose-up positive)
        self.setStyleSheet("background-color: transparent;")

    def update_attitude(self, heading, roll, pitch):
        self.heading = heading % 360
        self.roll    = roll
        self.pitch   = pitch
        self.update()

    # ------------------------------------------------------------------ helpers
    def _adi_rect(self):
        """Returns the square rect used for the ADI ball."""
        w, h = self.width(), self.height()
        side  = min(w, h - 14)   # leave 14 px at bottom for heading strip
        x = (w - side) / 2
        y = 0
        return x, y, side, side

    # ------------------------------------------------------------------ paint
    def paintEvent(self, event):
        p = QPainter(self)
        p.setRenderHint(QPainter.RenderHint.Antialiasing)
        self._draw_adi(p)
        self._draw_heading_strip(p)

    # ---- artificial horizon ------------------------------------------------
    def _draw_adi(self, p):
        from PyQt6.QtGui import QPainterPath
        x0, y0, side, _ = self._adi_rect()
        cx = x0 + side / 2
        cy = y0 + side / 2
        r  = side / 2 - 1

        # clip to circle
        clip_path = QPainterPath()
        clip_path.addEllipse(QPointF(cx, cy), r, r)
        p.setClipPath(clip_path)

        # pixels-per-degree for pitch (10 deg ≈ side/5)
        ppd = side / 50.0

        # ---- rotate + translate canvas for roll & pitch
        p.save()
        p.translate(cx, cy)
        p.rotate(self.roll)
        pitch_offset = self.pitch * ppd

        # sky (above horizon)
        sky_color = QColor(30, 100, 180)
        p.fillRect(int(-r*2), int(-r*2 + pitch_offset), int(r*4), int(r*4), sky_color)

        # ground (below horizon)
        ground_color = QColor(140, 90, 40)
        p.fillRect(int(-r*2), int(pitch_offset), int(r*4), int(r*4), ground_color)

        # horizon line
        p.setPen(QPen(QColor("#FFFFFF"), 2))
        p.drawLine(int(-r), int(pitch_offset), int(r), int(pitch_offset))

        # pitch ladder (every 10°, ±30°)
        p.setPen(QPen(QColor(255, 255, 255, 180), 1))
        p.setFont(QFont("Arial", 6))
        for deg in range(-30, 31, 10):
            if deg == 0:
                continue
            yy = int(pitch_offset - deg * ppd)
            half = int(r * (0.35 if deg % 20 == 0 else 0.22))
            p.drawLine(-half, yy, half, yy)

        p.restore()
        p.setClipping(False)

        # ---- roll arc & tick marks (outside clip, fixed frame) ----
        p.save()
        p.translate(cx, cy)

        arc_r = r - 2
        p.setPen(QPen(QColor("#FFFFFF"), 1))
        # draw arc from -60° to +60°  (Qt arc: 0=3o'clock, CCW, 16ths of deg)
        p.drawArc(int(-arc_r), int(-arc_r), int(2*arc_r), int(2*arc_r),
                  (90 - 60) * 16, 120 * 16)

        # tick marks at 0, ±10, ±20, ±30, ±45, ±60
        for deg in [0, 10, 20, 30, 45, 60, -10, -20, -30, -45, -60]:
            rad = math.radians(deg - 90)
            tick = 6 if deg % 30 == 0 else 4
            x1 = arc_r * math.cos(rad)
            y1 = arc_r * math.sin(rad)
            x2 = (arc_r - tick) * math.cos(rad)
            y2 = (arc_r - tick) * math.sin(rad)
            p.drawLine(QPointF(x1, y1), QPointF(x2, y2))

        # roll pointer triangle (rotates with roll)
        p.rotate(self.roll)
        ptr_r = arc_r - 1
        tri = QPolygonF([
            QPointF(0, -ptr_r),
            QPointF(-5, -ptr_r + 9),
            QPointF(5,  -ptr_r + 9),
        ])
        p.setBrush(QColor("#FFFFFF"))
        p.setPen(Qt.PenStyle.NoPen)
        p.drawPolygon(tri)
        p.restore()

        # ---- fixed aircraft symbol ----
        p.save()
        p.translate(cx, cy)
        p.setPen(QPen(QColor("#FFD700"), 2))
        # left wing
        p.drawLine(int(-r*0.5), 0, int(-r*0.15), 0)
        p.drawLine(int(-r*0.15), 0, int(-r*0.15), int(r*0.12))
        # right wing
        p.drawLine(int(r*0.15), 0, int(r*0.5), 0)
        p.drawLine(int(r*0.15), 0, int(r*0.15), int(r*0.12))
        # centre dot
        p.setBrush(QColor("#FFD700"))
        p.drawEllipse(QPointF(0, 0), 2.5, 2.5)
        p.restore()

        # ---- outer ring ----
        p.setPen(QPen(QColor("#888888"), 1))
        p.setBrush(Qt.BrushStyle.NoBrush)
        p.drawEllipse(QPointF(cx, cy), r, r)

    # ---- heading strip at bottom ------------------------------------------
    def _draw_heading_strip(self, p):
        w         = self.width()
        x0, y0, side, _ = self._adi_rect()
        strip_y   = y0 + side
        strip_h   = self.height() - strip_y
        if strip_h < 4:
            return

        # background
        p.fillRect(0, int(strip_y), w, strip_h, QColor(30, 30, 30))

        # heading text centred (bigger)
        p.setPen(QPen(QColor("#FFFFFF")))
        p.setFont(QFont("Arial", 10, QFont.Weight.Bold))
        hdg_str = f"{int(self.heading)}°"
        p.drawText(0, int(strip_y), w, strip_h, Qt.AlignmentFlag.AlignCenter, hdg_str)