
'''

主要調配流程的程式

這個檔案包含 Terminal Utility Layer (TUL) 作為人機互動介面，並調用 mavlinkDevice 和 mavlinkObject 來處理 MAVLink 通訊和物件管理。

'''

import os
import time
import sys

import curses
import threading
import queue
import signal

from pymavlink import mavutil

# 自定義的 import
from . import mavlinkObject as mo
from . import serialManager as sm
from . import mavlinkVehicleView as mvv
from . import mavlinkROS2Nodes as mros

from .utils import RingBuffer, setup_logger
from .utils import acquireSerial, acquirePort
from .utils.acquirePort import find_available_port

logger = setup_logger(os.path.basename(__file__))
VERSION_NO = "v0.59"

class PanelState:
    def __init__(self):
        self.panel_status = "Idle"
        termination_start_time = None
        self.mavlink_bridge_state = "Stopped"
        self.object_manager_state = "Stopped"
        self.serial_manager_state = "Stopped"
        self.ros2_manager_state = "Stopped"
        self.socket_object_list = [] # 已有的 mavlink object 
        self.linked_serial_dict = {} # 已連線的 serial 端口 serial id num : serial_port string
        self.panel_info_msg_list = [] # 顯示在面板上的資訊訊息 

        # 關於創建通道時的暫存資訊
        self.udp_info_temp = {"IP": "127.0.0.1", "Port": "", "Direction": ""} # 暫存 UDP 設定資訊
        self.serial_info_temp = {"Port": "", "Baud": 115200, "CommunicationType": "", "Go2Middleware": False} # 暫存 Serial 設定資訊

        # 關於顯示通道資訊
        self.socket_info_single = {
            "socket_type": "", "socket_state": "", "bridge_msg_types": "", "return_msg_types": "",
            "target_sockets": "", "primary_socket_id": "", "socket_connection_string": "",
            "InfoReady": False} # 暫存單一 socket 的資訊
        self.serial_info_single = {
            "serial_port": "", "baudrate": "", "receiver_type": "", "target_port": "",
            "InfoReady": False} # 暫存單一 serial 連結的資訊

        # 關於顯示載具資訊
        self.connected_vehicles_dict = {}  # {(sysid, compid): {...基本資訊...}}
        self.vehicle_info_single = {
            "sysid": 0,
            "compid": 0,
            "vehicle_type": "",
            "component_type": "",
            "mav_autopilot": "",
            "socket_id": None,
            "connection_type": "",
            "packet_stats": {},
            "msg_type_counts": {},
            "prev_stats": {},  # 用於計算變化率
            "InfoReady": False
        }

    def intoSTART(self):
        self.panel_status = "Running"

    def intoTERMINATION(self):
        self.termination_start_time = time.time()
        self.panel_status = "Terminating"

    def intoENGINEER(self):
        self.panel_status = "Engineer"

    def intoSTOPPED(self):
        self.panel_status = "Stopped"

    # def set_user_input(self, text):
    #     self.user_input = text

class MenuNode:
    def __init__(self, name, desc="", action=None, children=None):
        self.name = name
        self.desc = desc
        self.action = action  # 可以是函式或特殊字串
        self.children = children or []  # 子選單列表

class ControlPanel:
    def __init__(self):
        pass

    def input_dialog(stdscr, prompt="請輸入文字: "):
        """顯示輸入對話框"""
        height, width = stdscr.getmaxyx()
        
        # 建立輸入視窗
        dialog_height = 5
        dialog_width = min(60, width - 4)
        start_y = (height - dialog_height) // 2
        start_x = (width - dialog_width) // 2
        
        # 建立視窗邊框
        dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
        dialog_win.border()
        dialog_win.addstr(1, 2, prompt)
        dialog_win.addstr(3, 2, "按 Enter 確認, ESC 取消")
        dialog_win.refresh()
        
        # 輸入區域
        input_win = curses.newwin(1, dialog_width - 6, start_y + 2, start_x + 2)
        input_win.keypad(True)
        
        curses.echo()
        curses.curs_set(1)
        
        user_input = ""
        
        while True:
            input_win.clear()
            input_win.addstr(0, 0, user_input[-dialog_width+8:])  # 顯示輸入內容（滾動）
            input_win.refresh()
            
            ch = input_win.getch()
            
            if ch == 27:  # ESC
                user_input = None
                break
            elif ch in (curses.KEY_ENTER, 10, 13):  # Enter
                break
            elif ch in (curses.KEY_BACKSPACE, 127, 8):  # Backspace
                user_input = user_input[:-1]
            elif 32 <= ch <= 126:  # 可打印字符
                user_input += chr(ch)
        
        curses.noecho()
        curses.curs_set(0)
        
        # 清理視窗
        del input_win
        del dialog_win
        stdscr.clear()
        stdscr.refresh()
        
        return user_input

    # ================ 關於 主要選單 的部份 ===================

    def menu_tree(self):
        """建立多層選單結構"""
        return MenuNode("Main Menu", children=[
                MenuNode("MavLink Object", "UDP MavLink 通道選項", children=[
                    MenuNode("New+", children=[
                        MenuNode("UDP InBound", children=[
                            MenuNode("IP(Listen)", "設定監聽的 IP 位址", "TEXT_UDP_IP"),
                            MenuNode("Port(Listen)", "設定監聽的 Port", "TEXT_UDP_PORT"),
                            MenuNode("Create", "建立 UDP InBound 連結口", "CREATE_UDP_INBOUND"),
                        ]),
                        MenuNode("UDP OutBound", children=[
                            MenuNode("IP(Target)", "設定目標的 IP 位址", "TEXT_UDP_IP"),
                            MenuNode("Port(Target)", "設定目標的 Port", "TEXT_UDP_PORT"),
                            MenuNode("Create", "建立 UDP OutBound 連結口", "CREATE_UDP_OUTBOUND"),
                        ]),
                    ]),
                    MenuNode("ListAll", "顯示並管理所有連結口", "LIST_MAV_OBJECT"),
                ]),
                MenuNode("Serial Manager", "Serial 連接埠選項", children=[
                    MenuNode("New+", "新增 Serial 連接埠", action = "LIST_SERIAL_RES"),
                    MenuNode("ListAll", "顯示並管理已連線的 Serial", action = "LIST_SERIAL_LINKS"),
                ]),
                MenuNode("Vehicles Insp.", "檢視已連線的遠端載具", action = "INSPECT_VEHICLES"),
                MenuNode("Engineer Mode", "工程模式", children=[
                    MenuNode("Stop Manager", "停止 Mavlink 物件管理", "STOP_MANAGER"), 
                    MenuNode("Stop Bridge", "停止 Mavlink-ROS 橋接", "STOP_BRIDGE"), 
                    MenuNode("Stop Serial M.", "停止 Serial 端口轉接", "STOP_SERIAL_MANAGER"), 
                ]),
                MenuNode("Shutdown", "關閉整個系統", children=[
                    MenuNode("Return", "繼續運行", "BACK"),
                    MenuNode("Confirm", "關閉系統", "QUIT"),
                ]),
        ])

    def panel_thread(self, cmd_q: queue.Queue, state: PanelState, stop_evt: threading.Event):
        stdscr = None
    
        def cleanup():
            """清理 curses 狀態"""
            if stdscr:
                stdscr.keypad(False)
                curses.nocbreak()
                curses.echo()
                curses.endwin()

        def pre_panel_shutdown():
            # 先關閉所有模組 再關閉面板
            cmd_q.put("SHUTDOWN_BRIDGE")
            cmd_q.put("SHUTDOWN_MANAGER")
            cmd_q.put("SHUTDOWN_SERIAL_MANAGER")

        def draw_menu(screen):
            nonlocal stdscr
            stdscr = screen
            
            curses.curs_set(0)
            stdscr.nodelay(False)  # 阻塞讀鍵
            stdscr.keypad(True)

            # 選單導航狀態
            menu_stack = [self.menu_tree()]  # 選單堆疊
            idx_stack = [0]  # 索引堆疊

            state.intoSTART() # 設定狀態為運行中
            
            while not stop_evt.is_set():

                current_menu = menu_stack[-1]
                current_idx = idx_stack[-1]
                
                # 獲取終端機尺寸
                height, width = stdscr.getmaxyx()
                # 簡單暴力的限制視窗的大小
                MIN_HEIGHT = (
                    2 + # 邊界
                    6 + # 狀態列 操作說明列 一個空白
                    11+ # 最大選單 與 空白區
                    5   # 訊息區域
                    )
                if height < MIN_HEIGHT or width < 60:
                    logger.error("Terminal size too small for Control Panel.")
                    break
                
                stdscr.clear()
                stdscr.border()

                # 更新模組狀態顯示
                stdscr.addstr(0, 10,  " MavLink MiddleWare ", curses.A_BOLD)
                stdscr.addstr(1,  2, f"   Panel  Status     : {state.panel_status}")
                stdscr.addstr(2,  2, f"Object Manager State : {state.object_manager_state}")
                stdscr.addstr(3,  2, f"Mavlink Bridge State : {state.mavlink_bridge_state}")
                stdscr.addstr(4,  2, f"Socket Object number : {len(state.socket_object_list)}")
                stdscr.addstr(2, 36, f"Serial Manager State : {state.serial_manager_state}")
                stdscr.addstr(3, 36, f"ROS2 Manager State   : {state.ros2_manager_state}")

                # 顯示當前選單項目
                start_line = 6
                for i, child in enumerate(current_menu.children):
                    marker = "➤ " if i == current_idx else "  "
                    # 動態顯示已輸入的值
                    desc = child.desc
                    if child.action == "TEXT_UDP_IP" and state.udp_info_temp["IP"]:
                        desc = f"{child.desc} [{state.udp_info_temp['IP']}]"
                    elif child.action == "TEXT_UDP_PORT" and state.udp_info_temp["Port"]:
                        desc = f"{child.desc} [{state.udp_info_temp['Port']}]"
                    elif child.action == "SET_SERIAL_COMM" and state.serial_info_temp["CommunicationType"]:
                        desc = f"{child.desc} [{state.serial_info_temp['CommunicationType']}]"
                    elif child.action == "TEXT_BAUD_SERIAL" and state.serial_info_temp["Baud"]:
                        desc = f"{child.desc} [{state.serial_info_temp['Baud']}]"
                    elif child.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP":
                        link_status = "Yes" if state.serial_info_temp["Go2Middleware"] else "No"
                        desc = f"{child.desc} [{link_status}]"
                    
                    line = f"{marker}{child.name:15s} – {desc}"
                    attr = curses.A_REVERSE if i == current_idx else curses.A_NORMAL
                    stdscr.addstr(start_line + i, 4, line, attr)

                # 顯示訊息區域 
                # info_start_line = start_line + len(current_menu.children) + 1
                info_start_line = height - 8
                max_msg_lines = 5  # 最多顯示 5 行訊息
                current_time = time.time()
                
                    # 清理過時的訊息
                state.panel_info_msg_list = [
                    (msg, timestamp)  for msg, timestamp in state.panel_info_msg_list
                    if current_time - timestamp < 2.0 #秒數
                ]
                
                    # 只顯示最新的 max_msg_lines 條訊息
                display_msgs = state.panel_info_msg_list[-max_msg_lines:]
                
                for i, msg_data in enumerate(display_msgs):
                    if info_start_line + i >= help_line - 1:  # 避免超出邊界
                        break
                    msg = msg_data[0] if isinstance(msg_data, tuple) else msg_data
                    # 截斷過長的訊息
                    max_msg_width = width - 6
                    if len(msg) > max_msg_width:
                        msg = msg[:max_msg_width-3] + "..."

                    stdscr.addstr(info_start_line + i, 2, f"💬 {msg}", curses.A_BOLD)

                
                
                # 操作說明
                # help_line = start_line + len(current_menu.children) + 2
                help_line = height - 2
                stdscr.addstr(help_line, 2, "操作: ↑↓選擇 Enter確認 ←返回上層 →進入下層", curses.A_DIM)
                stdscr.addstr(height-1 , width-12, f" {VERSION_NO} ", curses.A_DIM)

                stdscr.refresh()

                # 若進入 TERMINATION 狀態，畫面可以刷新 但是不能操作
                # 驗證 其他附屬模組的狀態都停止後 就進入 STOPPED 狀態並跳出迴圈
                # 超過幾秒沒有反應就強制關閉
                if state.panel_status == "Terminating":
                    if time.time() - state.termination_start_time > 7: # 其他組件設定5秒 這邊給多一點
                        logger.warning("Control Panel forced shutdown after timeout.")
                        state.intoSTOPPED()
                        # stop_evt.set()
                        # continue
                        break
                    time.sleep(0.1)
                    if (state.mavlink_bridge_state == "Stopped" and 
                        state.object_manager_state == "Stopped" and 
                        state.serial_manager_state == "Stopped"):
                        state.intoSTOPPED()
                        # stop_evt.set()
                        break
                    continue

                # 設定短暫的 timeout，讓執行緒能夠響應 stop_evt
                stdscr.timeout(100)
                ch = stdscr.getch()
                
                if ch == -1: # 沒有操作
                    continue
                    
                # 處理按鍵
                if ch in (curses.KEY_UP, ord('k')):
                    idx_stack[-1] = (current_idx - 1) % len(current_menu.children)
                    
                elif ch in (curses.KEY_DOWN, ord('j')):
                    idx_stack[-1] = (current_idx + 1) % len(current_menu.children)

                elif ch == (ord('O')):
                    # 進入工程模式
                    state.intoENGINEER()

                elif ch == (ord('o')):
                    # 離開工程模式
                    state.intoSTART()                
                    
                elif ch == curses.KEY_LEFT:
                    # 返回上層
                    if len(menu_stack) > 1:
                        menu_stack.pop()
                        idx_stack.pop()
                
                elif ch == curses.KEY_RIGHT:
                    # 進入下層 (但不執行動作)
                    selected = current_menu.children[current_idx]
                    if selected.children:  # 有子選單
                        menu_stack.append(selected)
                        idx_stack.append(0)

                elif ch in (ord('q'), 27):
                    if state.panel_status == "Engineer":
                        state.intoTERMINATION()
                        pre_panel_shutdown()

                elif ch in (curses.KEY_ENTER, 10, 13):
                    selected = current_menu.children[current_idx]
                    
                    # 處理不同類型的動作
                    if selected.children:  # 有子選單
                        menu_stack.append(selected)
                        idx_stack.append(0)
                        
                    elif selected.action == "BACK":
                        if len(menu_stack) > 1:
                            menu_stack.pop()
                            idx_stack.pop()
                            
                    elif selected.action == "QUIT":
                        state.intoTERMINATION()
                        pre_panel_shutdown()
                    
                    elif selected.action == "TEXT_UDP_IP":
                        result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 IP 位址: ")
                        if result is not None:
                            state.udp_info_temp["IP"] = result
                    
                    elif selected.action == "TEXT_UDP_PORT":
                        result = ControlPanel.input_dialog(stdscr, "請輸入監聽的 Port: ")
                        if result is not None:
                            state.udp_info_temp["Port"] = result

                    elif selected.action == "CREATE_UDP_INBOUND":
                        cmd_q.put("CREATE_UDP_INBOUND")
                        # 確認後回到上兩層
                        if len(menu_stack) > 1:
                            menu_stack.pop()
                            idx_stack.pop()
                            # menu_stack.pop()
                            # idx_stack.pop()
                    
                    elif selected.action == "CREATE_UDP_OUTBOUND":
                        cmd_q.put("CREATE_UDP_OUTBOUND")
                        # 確認後回到上兩層
                        if len(menu_stack) > 1:
                            menu_stack.pop()
                            idx_stack.pop()
                            # menu_stack.pop()
                            # idx_stack.pop()

                    elif selected.action == "TEXT_BAUD_SERIAL":
                        result = ControlPanel.input_dialog(stdscr, "請輸入 Baud Rate (e.g., 9600, 115200): ")
                        if result is not None:
                            try:
                                baud_rate = int(result)
                            except ValueError:
                                state.panel_info_msg_list.append(("Invalid Baud Rate input.", time.time()))
                            state.serial_info_temp["Baud"] = baud_rate

                    elif selected.action == "SET_SERIAL_COMM_XBEE":
                        state.serial_info_temp["CommunicationType"] = "XBee(API-AT)"
                        menu_stack.pop()
                        idx_stack.pop()
                    elif selected.action == "SET_SERIAL_COMM_TELEMETRY":
                        state.serial_info_temp["CommunicationType"] = "XBee(AT-AT)"
                        menu_stack.pop()
                        idx_stack.pop()

                    elif selected.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP_YES":
                        state.serial_info_temp["Go2Middleware"] = True
                        menu_stack.pop()
                        idx_stack.pop()
                    elif selected.action == "LINK_SERIAL_TO_MIDDLEWARE_UDP_NO":
                        state.serial_info_temp["Go2Middleware"] = False
                        menu_stack.pop()
                        idx_stack.pop()

                    elif selected.action == "CREATE_SERIAL_PORT":
                        state.serial_info_temp["Port"] = menu_stack[-1].name  # 從選單取得 Port 名稱
                        cmd_q.put("CREATE_SERIAL_PORT")
                        # 確認後回到上兩層
                        if len(menu_stack) > 1:
                            menu_stack.pop()
                            idx_stack.pop()
                            menu_stack.pop()
                            idx_stack.pop()

                    elif selected.action == "LIST_SERIAL_RES":
                        created_list_menu = self.create_serial_port_menu(state, page=0)
                        menu_stack.append(created_list_menu)
                        idx_stack.append(0)

                    elif selected.action == "LIST_SERIAL_LINKS":
                        created_list_menu = self.create_linked_serial_menu(state, page=0)
                        menu_stack.append(created_list_menu)
                        idx_stack.append(0)                        
                    
                    elif selected.action == "INSPECT_LINKED_SERIAL":
                        # 顯示 Serial 連結詳細資訊
                        if hasattr(selected, 'serial_id'):
                            cmd_q.put(("INSPECT_LINKED_SERIAL", selected.serial_id))
                            self.show_linked_serial_info(stdscr, selected.serial_id, state)

                    elif selected.action == "REMOVE_LINKED_SERIAL":
                        # 移除 Serial 連結
                        if hasattr(selected, 'serial_id'):
                            cmd_q.put(("REMOVE_LINKED_SERIAL", selected.serial_id))
                            # 返回上層（回到列表）
                            if len(menu_stack) > 1:
                                menu_stack.pop()
                                idx_stack.pop()
                            # 一樣退兩層
                                menu_stack.pop()
                                idx_stack.pop()

                    elif selected.action == "LIST_MAV_OBJECT":
                        # 動態生成 mavlink_object 列表選單
                        created_list_menu = self.create_object_list_menu(state, page=0)
                        menu_stack.append(created_list_menu)
                        idx_stack.append(0)

                    elif selected.action in ("PREV_PAGE", "NEXT_PAGE"):
                        if hasattr(selected, 'page'):
                            current_list_menu = menu_stack[-1]
                            menu_stack.pop()
                            idx_stack.pop()
                            
                            # 依據選單種類 重新建立分頁
                            if current_list_menu.name == "Serial Port List":
                                created_list_menu = self.create_serial_port_menu(state, page=selected.page)
                            elif current_list_menu.name == "Object List":
                                created_list_menu = self.create_object_list_menu(state, page=selected.page)
                            elif current_list_menu.name == "Linked Serial List":
                                created_list_menu = self.create_linked_serial_menu(state, page=selected.page)
                            elif current_list_menu.name == "Connected Vehicles":
                                created_list_menu = self.create_vehicles_list_menu(state, page=selected.page)
                            else:
                                # 不支援的選單類型，回到原本的選單
                                menu_stack.append(current_list_menu)
                                idx_stack.append(0)
                                continue
                            
                            menu_stack.append(created_list_menu)
                            idx_stack.append(0)
                    
                    elif selected.action == "INSPECT_MAV_OBJECT":
                        # 顯示物件詳細資訊
                        if hasattr(selected, 'socket_id'):
                            cmd_q.put(("INSPECT_MAV_OBJECT", selected.socket_id))
                            self.show_object_info(stdscr, selected.socket_id, state)
                    
                    elif selected.action == "REMOVE_MAV_OBJECT":
                        # 移除物件
                        if hasattr(selected, 'socket_id'):
                            cmd_q.put(("REMOVE_OBJECT", selected.socket_id))
                            # 返回上層（回到列表）
                            if len(menu_stack) > 1:
                                menu_stack.pop()
                                idx_stack.pop()
                            # 反正刷新列表會出錯 乾脆再退一層 在下一次進入列表時刷新就好
                                menu_stack.pop()
                                idx_stack.pop()

                    elif selected.action == "MAVOBJ_MAKE_LINK":
                        # 建立轉發連結
                        if hasattr(selected, 'socket_id'):
                            target_id = self.select_target_socket(stdscr, selected.socket_id, state)
                            if target_id is not None:
                                cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id))
                                cmd_q.put(("MAVOBJ_ADD_TARGET", target_id, selected.socket_id))  # 雙向連結

                    elif selected.action == "MAVOBJ_CANCEL_LINK":
                        # 取消轉發連結
                        if hasattr(selected, 'socket_id'):
                            target_id = self.select_target_socket(stdscr, selected.socket_id, state, remove_mode=True)
                            if target_id is not None:
                                cmd_q.put(("MAVOBJ_REMOVE_TARGET", selected.socket_id, target_id))
                                cmd_q.put(("MAVOBJ_REMOVE_TARGET", target_id, selected.socket_id))  # 雙向取消連結

                    elif selected.action == "MAVOBJ_ADD_TARGET":
                        # 添加目標端口
                        if state.panel_status != "Engineer":
                            state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
                            continue  # 只有在工程模式下才能操作
                        if hasattr(selected, 'socket_id'):
                            target_id = self.select_target_socket(stdscr, selected.socket_id, state)
                            if target_id is not None:
                                cmd_q.put(("MAVOBJ_ADD_TARGET", selected.socket_id, target_id))

                    elif selected.action == "STOP_MANAGER":
                        if state.panel_status != "Engineer":
                            state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
                            continue  # 只有在工程模式下才能操作
                        cmd_q.put("SHUTDOWN_MANAGER")

                    elif selected.action == "STOP_BRIDGE":
                        if state.panel_status != "Engineer":
                            state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
                            continue  # 只有在工程模式下才能操作
                        cmd_q.put("SHUTDOWN_BRIDGE")

                    elif selected.action == "STOP_SERIAL_MANAGER":
                        if state.panel_status != "Engineer":
                            state.panel_info_msg_list.append(("Not in Engineer Mode.", time.time()))
                            continue  # 只有在工程模式下才能操作
                        cmd_q.put("SHUTDOWN_SERIAL_MANAGER")

                    elif selected.action == "INSPECT_VEHICLES":
                        # 進入載具檢視選單
                        cmd_q.put("UPDATE_VEHICLES_LIST")
                        created_list_menu = self.create_vehicles_list_menu(state, page=0)
                        menu_stack.append(created_list_menu)
                        idx_stack.append(0)
                    
                    elif selected.action == "INSPECT_VEHICLE":
                        # 顯示載具詳細資訊
                        if hasattr(selected, 'sysid') and hasattr(selected, 'compid'):
                            cmd_q.put(("INSPECT_VEHICLE", selected.sysid, selected.compid))
                            self.show_vehicle_info(stdscr, selected.sysid, selected.compid, cmd_q, state)

                    elif callable(selected.action):
                        # 執行函式
                        cmd_q.put(selected.action)
                        
        try:
            curses.wrapper(draw_menu)
        except KeyboardInterrupt:
            pass
        finally:
            cleanup()

    # ================ 關於 mavlink object 的部份 =================== 

    def create_object_list_menu(self, state: PanelState, page=0, items_per_page=8):
        """動態創建 mavlink_object 列表選單（支持分頁）"""
        children = []
        
        if not state.socket_object_list:
            children.append(MenuNode("(Empty)", "目前沒有連結口", None))
        else:
            total_items = len(state.socket_object_list)
            total_pages = (total_items + items_per_page - 1) // items_per_page
            start_idx = page * items_per_page
            end_idx = min(start_idx + items_per_page, total_items)
            
            # 顯示當前頁的物件
            for socket_id in state.socket_object_list[start_idx:end_idx]:
                # 為每個 socket 創建子選單
                obj_menu = MenuNode(f"Socket #{socket_id}", f"連結口 {socket_id}", None, children=[
                    MenuNode("Info", "查看詳細資訊", "INSPECT_MAV_OBJECT"),
                    MenuNode("Make Link", "建立轉發連結", "MAVOBJ_MAKE_LINK"),
                    MenuNode("Cancel Link", "取消轉發連結", "MAVOBJ_CANCEL_LINK"),
                    MenuNode("Add Target", "添加轉發目標(工程)", "MAVOBJ_ADD_TARGET"),
                    MenuNode("Remove", "移除此連結口", "REMOVE_MAV_OBJECT"),
                    MenuNode("GoUp", "回到列表", "BACK"),
                ])
                # 將 socket_id 附加到每個子選單項目上
                for child in obj_menu.children:
                    child.socket_id = socket_id
                children.append(obj_menu)

            # 添加分頁控制
            if total_pages > 1:
                children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None))
                if page > 0:
                    prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE")
                    prev_node.page = page - 1
                    children.append(prev_node)
                if page < total_pages - 1:
                    next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE")
                    next_node.page = page + 1
                    children.append(next_node)

        children.append(MenuNode("GoUp", "回到上層選單", "BACK"))
        menu = MenuNode("Object List", f"連結口列表 (第 {page + 1} 頁)", children=children)
        menu.current_page = page
        return menu

    def show_object_info(self, stdscr, socket_id, state: PanelState):
        """顯示物件詳細資訊的對話框"""

        start = time.time()
        while not state.socket_info_single.get('InfoReady', False):
            # 太久沒有回應
            if time.time() - start > 2:
                state.panel_info_msg_list.append(("Fail! Socket Info NOT Aquire!", time.time()))
                return
            time.sleep(0.05)  # 等待資訊準備好

        height, width = stdscr.getmaxyx()
        dialog_height = 15
        dialog_width = min(70, width - 4)
        start_y = (height - dialog_height) // 2
        start_x = (width - dialog_width) // 2
        
        dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
        dialog_win.border()
        dialog_win.addstr(0, 2, f" Socket #{socket_id} 詳細資訊 ", curses.A_BOLD)
        
        # 這裡顯示基本資訊
        dialog_win.addstr(2, 2, f"Socket ID     : {socket_id}")
        dialog_win.addstr(3, 2, f"Socket status : {state.socket_info_single.get('socket_state', 'N/A')}")
        # show_str = ", ".join(map(str, state.socket_info_single.get('socket_type', '')))
        dialog_win.addstr(4, 2, f"Socket Type : {state.socket_info_single.get('socket_type', '')}")
        dialog_win.addstr(4, 30, f"{state.socket_info_single.get('socket_connection_string', '')}")
        show_str = ",".join(map(str, state.socket_info_single.get('bridge_msg_types', '')))
        dialog_win.addstr(5, 2, f"Bridge Pack : {show_str if show_str else 'N/A'}")
        show_str = ",".join(map(str, state.socket_info_single.get('return_msg_types', '')))
        dialog_win.addstr(6, 2, f"Return Pack : {show_str if show_str else 'N/A'}")
        dialog_win.addstr(7, 2, f"Primary Socket ID: {state.socket_info_single.get('primary_socket_id', 'It Self')}")
        show_str = ",".join(map(str, state.socket_info_single.get('target_sockets', '')))
        dialog_win.addstr(8, 2, f"Switching Targets: {show_str if show_str else 'N/A'}")

        state.socket_info_single['InfoReady'] = False  # 重置狀態以便下次使用
        
        dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...")
        dialog_win.refresh()
        
        dialog_win.getch()
        del dialog_win
        stdscr.clear()
        stdscr.refresh()
    
    def select_target_socket(self, stdscr, source_socket_id, state: PanelState, remove_mode=False):
        """選擇目標 socket 的對話框"""
        height, width = stdscr.getmaxyx()
        dialog_height = min(15, len(state.socket_object_list) + 5)
        dialog_width = min(50, width - 4)
        start_y = (height - dialog_height) // 2
        start_x = (width - dialog_width) // 2
        
        dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
        dialog_win.keypad(True)
        
        title = "選擇要移除的目標" if remove_mode else "選擇轉發目標"
        available_sockets = [sid for sid in state.socket_object_list if sid != source_socket_id]
        
        if not available_sockets:
            dialog_win.border()
            dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD)
            dialog_win.addstr(2, 2, "沒有可用的目標")
            dialog_win.addstr(4, 2, "按任意鍵返回...")
            dialog_win.refresh()
            dialog_win.getch()
            del dialog_win
            stdscr.clear()
            stdscr.refresh()
            return None
        
        selected_idx = 0
        
        while True:
            dialog_win.clear()
            dialog_win.border()
            dialog_win.addstr(0, 2, f" {title} ", curses.A_BOLD)
            
            for i, socket_id in enumerate(available_sockets):
                marker = "➤" if i == selected_idx else " "
                attr = curses.A_REVERSE if i == selected_idx else curses.A_NORMAL
                dialog_win.addstr(2 + i, 2, f"{marker} Socket #{socket_id}", attr)
            
            dialog_win.addstr(dialog_height - 2, 2, "Enter確認 ESC取消")
            dialog_win.refresh()
            
            ch = dialog_win.getch()
            
            if ch in (curses.KEY_UP, ord('k')):
                selected_idx = (selected_idx - 1) % len(available_sockets)
            elif ch in (curses.KEY_DOWN, ord('j')):
                selected_idx = (selected_idx + 1) % len(available_sockets)
            elif ch in (curses.KEY_ENTER, 10, 13):
                result = available_sockets[selected_idx]
                del dialog_win
                stdscr.clear()
                stdscr.refresh()
                return result
            elif ch == 27:  # ESC
                del dialog_win
                stdscr.clear()
                stdscr.refresh()
                return None

    # ================ 關於 serial link 的部份 ===================

    def create_serial_port_menu(self, state: PanelState, page=0, items_per_page=8):
        """動態創建 serial port 列表選單（支持分頁）"""
        children = []
        
        # 獲取可用的 Serial 連接埠列表
        # serial_ports = acquireSerial.get_serial_ports() # debug 全部抓一抓
        serial_ports = acquireSerial.get_serial_ports_with_filter(['/dev/ttyUSB*', '/dev/ttyACM*'])
        
        if not serial_ports:
            children.append(MenuNode("(Empty)", "目前沒有串口設備", None))
        else:
            total_items = len(serial_ports)
            total_pages = (total_items + items_per_page - 1) // items_per_page
            start_idx = page * items_per_page
            end_idx = min(start_idx + items_per_page, total_items)
            
            # 顯示當前頁的串口
            for port in serial_ports[start_idx:end_idx]:
                port_menu = MenuNode(f"{port}", children=[
                    MenuNode("Set Comm Type", "設定通訊形態", "SET_SERIAL_COMM", children=[
                        MenuNode("XBee(API-AT)", "XBee 模式", "SET_SERIAL_COMM_XBEE"),
                        # MenuNode("Telemetry", "數傳模式", "SET_SERIAL_COMM_TELEMETRY"),
                    ]),
                    MenuNode("Set Baud", "設定 Baud", "TEXT_BAUD_SERIAL"),
                    MenuNode("Link to MW", "直接建立 Middleware UDP", "LINK_SERIAL_TO_MIDDLEWARE_UDP", children=[
                        MenuNode("Yes", action = "LINK_SERIAL_TO_MIDDLEWARE_UDP_YES"),
                        MenuNode("No",  action = "LINK_SERIAL_TO_MIDDLEWARE_UDP_NO"),
                    ]),
                    MenuNode("Create", "建立此串口", "CREATE_SERIAL_PORT"),
                    MenuNode("GoUp", "回到列表", "BACK"),
                ])
                # 將 port 附加到每個子選單項目上
                for child in port_menu.children:
                    child.port = port
                children.append(port_menu)
            
            # 添加分頁控制
            if total_pages > 1:
                children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None))
                if page > 0:
                    prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE")
                    prev_node.page = page - 1
                    children.append(prev_node)
                if page < total_pages - 1:
                    next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE")
                    next_node.page = page + 1
                    children.append(next_node)
        
        children.append(MenuNode("GoUp", "回到上層選單", "BACK"))
        menu = MenuNode("Serial Port List", f"串口列表 (第 {page + 1} 頁)", children=children)
        menu.current_page = page
        return menu

    def create_linked_serial_menu(self, state: PanelState, page=0, items_per_page=8):
        """動態創建 已連線的 serial port 列表選單（支持分頁）並包含後續管理功能"""
        children = []
        
        if not state.linked_serial_dict:
            children.append(MenuNode("(Empty)", "目前沒有連結口", None))
        else:
            total_items = len(state.linked_serial_dict)
            total_pages = (total_items + items_per_page - 1) // items_per_page
            start_idx = page * items_per_page
            end_idx = min(start_idx + items_per_page, total_items)
            
            # 顯示當前頁的物件
            linked_serial_id_list = list(state.linked_serial_dict.keys())
            for serial_id in linked_serial_id_list[start_idx:end_idx]:
                # 為每個 socket 創建子選單
                obj_menu = MenuNode(f"Serial #{serial_id}", f"連結口 {serial_id}", None, children=[
                    MenuNode("Info", "查看詳細資訊", "INSPECT_LINKED_SERIAL"),
                    MenuNode("Remove", "移除此連結口", "REMOVE_LINKED_SERIAL"),
                    # MenuNode("Change UDP Target", "變更目標 UDP (工程)", "CHANGE_LINKED_SERIAL_TARGET"),
                    MenuNode("GoUp", "回到列表", "BACK"),
                ])
                # 將 serial_id 附加到每個子選單項目上
                for child in obj_menu.children:
                    child.serial_id = serial_id
                children.append(obj_menu)
            
            # 添加分頁控制
            if total_pages > 1:
                children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None))
                if page > 0:
                    prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE")
                    prev_node.page = page - 1
                    children.append(prev_node)
                if page < total_pages - 1:
                    next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE")
                    next_node.page = page + 1
                    children.append(next_node)
        
        children.append(MenuNode("GoUp", "回到上層選單", "BACK"))
        menu = MenuNode("Linked Serial List", f"連結口列表 (第 {page + 1} 頁)", children=children)
        menu.current_page = page
        return menu

    def show_linked_serial_info(self, stdscr, serial_id, state: PanelState):
        """顯示 Serial 連結詳細資訊的對話框"""

        start = time.time()
        while not state.serial_info_single.get('InfoReady', False):
            # 太久沒有回應
            if time.time() - start > 2:
                state.panel_info_msg_list.append(("Fail! Serial Info NOT Aquire!", time.time()))
                return
            time.sleep(0.05)  # 等待資訊準備好

        height, width = stdscr.getmaxyx()
        dialog_height = 15
        dialog_width = min(70, width - 4)
        start_y = (height - dialog_height) // 2
        start_x = (width - dialog_width) // 2
        
        dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
        dialog_win.border()
        dialog_win.addstr(0, 2, f" Serial Link #{serial_id} 詳細資訊 ", curses.A_BOLD)

        # 從 linked_serial_dict 獲取資訊
        serial_info = state.linked_serial_dict.get(serial_id, {})
        
        if not serial_info:
            dialog_win.addstr(2, 2, f"無法取得 Serial #{serial_id} 的資訊")
        else:
            # 顯示基本資訊
            dialog_win.addstr(2, 2, f"Serial ID       : {serial_id}")
            dialog_win.addstr(3, 2, f"Serial Port     : {state.serial_info_single.get('serial_port', 'N/A')}")
            dialog_win.addstr(4, 2, f"Baudrate        : {state.serial_info_single.get('baudrate', 'N/A')}")
            dialog_win.addstr(5, 2, f"Communication   : {state.serial_info_single.get('receiver_type', 'N/A')}")
            dialog_win.addstr(6, 2, f"Target UDP Port : {state.serial_info_single.get('target_port', 'N/A')}")
            dialog_win.addstr(7, 2, f"Status          : {state.serial_info_single.get('status', 'Running')}")
            
            # 如果有額外資訊可以繼續添加
            if 'thread_alive' in serial_info:
                thread_status = "Alive" if serial_info['thread_alive'] else "Stopped"
                dialog_win.addstr(8, 2, f"Thread Status   : {thread_status}")
        
        state.serial_info_single['InfoReady'] = False  # 重置狀態以便下次使用

        dialog_win.addstr(dialog_height - 2, 2, "按任意鍵返回...")
        dialog_win.refresh()
        
        dialog_win.getch()
        del dialog_win
        stdscr.clear()
        stdscr.refresh()

    # ================ 關於載具檢視的部份 ===================

    def create_vehicles_list_menu(self, state: PanelState, page=0, items_per_page=8):
        """動態創建 已連線載具 列表選單（支持分頁）
        每個 vehicle-component 組合都是獨立的選單項目
        """
        children = []
        
        if not state.connected_vehicles_dict:
            children.append(MenuNode("(Empty)", "目前沒有已連線的載具", None))
        else:
            total_items = len(state.connected_vehicles_dict)
            total_pages = (total_items + items_per_page - 1) // items_per_page
            start_idx = page * items_per_page
            end_idx = min(start_idx + items_per_page, total_items)
            
            # vehicle_id_list 現在是 (sysid, compid) 的元組列表
            vehicle_comp_list = list(state.connected_vehicles_dict.keys())
            
            # 顯示當前頁的物件
            for sysid, compid in vehicle_comp_list[start_idx:end_idx]:
                # 建立顯示名稱
                display_name = f"Vehicle #{sysid} - Comp #{compid}"
                desc = f"載具 {sysid} 組件 {compid}"
                
                vehicle_menu = MenuNode(display_name, desc, "INSPECT_VEHICLE")
                # 將 sysid 和 compid 附加到選單項目上
                vehicle_menu.sysid = sysid
                vehicle_menu.compid = compid
                children.append(vehicle_menu)
            
            # 添加分頁控制
            if total_pages > 1:
                children.append(MenuNode("---", f"第 {page+1}/{total_pages} 頁", None))
                if page > 0:
                    prev_node = MenuNode("◀ Prev", "上頁", "PREV_PAGE")
                    prev_node.page = page - 1
                    children.append(prev_node)
                if page < total_pages - 1:
                    next_node = MenuNode("Next ▶", "下頁", "NEXT_PAGE")
                    next_node.page = page + 1
                    children.append(next_node)
        
        children.append(MenuNode("GoUp", "回到上層選單", "BACK"))
        menu = MenuNode("Connected Vehicles", f"已連線載具列表 (第 {page + 1} 頁)", children=children)
        menu.current_page = page
        return menu

    def show_vehicle_info(self, stdscr, sysid, compid, cmd_q: queue.Queue, state: PanelState):
        """顯示載具組件詳細資訊（動態更新，顯示變化率）"""
        
        # 等待資訊準備
        start = time.time()
        while not state.vehicle_info_single.get('InfoReady', False):
            if time.time() - start > 2:
                state.panel_info_msg_list.append(("Fail! Vehicle Info NOT Acquired!", time.time()))
                return
            time.sleep(0.05)
        
        info = state.vehicle_info_single
        height, width = stdscr.getmaxyx()
        dialog_height = min(22, height - 4)
        dialog_width = min(70, width - 4)
        start_y = (height - dialog_height) // 2
        start_x = (width - dialog_width) // 2
        
        dialog_win = curses.newwin(dialog_height, dialog_width, start_y, start_x)
        dialog_win.nodelay(True)  # 非阻塞模式，允許動態更新
        dialog_win.keypad(True)
        
        # MAV_TYPE 名稱對應
        MAV_TYPE_NAMES = {
            0: "Generic", 1: "Fixed Wing", 2: "Quadrotor", 3: "Helicopter",
            4: "Antenna Tracker", 5: "GCS", 6: "Airship", 10: "Ground Rover",
            12: "Boat", 13: "Submarine", 26: "Gimbal", 30: "Camera"
        }
        
        # 動態更新迴圈
        last_update = time.time()
        while True:
            current_time = time.time()
            
            # 每 1 秒重新請求資料
            if current_time - last_update >= 1.0:
                # 觸發資料更新（透過 Orchestrator）
                cmd_q.put(("INSPECT_VEHICLE", sysid, compid))
                # 等待新資料準備好
                wait_start = time.time()
                state.vehicle_info_single['InfoReady'] = False
                while not state.vehicle_info_single.get('InfoReady', False):
                    if time.time() - wait_start > 0.5:  # 最多等 0.5 秒
                        break
                    time.sleep(0.01)
                # 更新 info 參照
                info = state.vehicle_info_single
                last_update = current_time
            
            dialog_win.clear()
            dialog_win.border()
            dialog_win.addstr(0, 2, f" Vehicle #{info['sysid']} - Component #{info['compid']} ", curses.A_BOLD)
            
            # === 基礎資訊 ===
            dialog_win.addstr(2, 2, "[Identity]", curses.A_UNDERLINE)
            dialog_win.addstr(2, 32, "[Connection]", curses.A_UNDERLINE)
            
            # # MAV Type # 這個用不到
            # mav_type = info.get('vehicle_type', 'N/A')
            # mav_type_str = f"{mav_type} ({MAV_TYPE_NAMES.get(mav_type, 'Unknown')})" if isinstance(mav_type, int) else str(mav_type)
            # dialog_win.addstr(3, 2, f"MAV Type       : {mav_type_str}")
            
            # Component Type
            dialog_win.addstr(3, 2, f"Component Type : {info.get('component_type', 'N/A')}")
            
            # Autopilot Type
            if info.get('mav_autopilot') is not None:
                dialog_win.addstr(4, 2, f"Autopilot      : {info.get('mav_autopilot', 'N/A')}")
            
            # Connection Info
            dialog_win.addstr(3, 32, f"Connection : {info.get('connection_type', 'N/A')}")
            dialog_win.addstr(4, 32, f"Socket ID  : #{info.get('socket_id', 'N/A')}")
            
            # === 封包統計 ===
            stats = info.get('packet_stats', {})
            dialog_win.addstr(7, 2, "[Packet Statistics]", curses.A_UNDERLINE)
            
            received = stats.get('received', 0)
            lost = stats.get('lost', 0)
            loss_rate = stats.get('loss_rate', 0.0)
            last_seq = stats.get('last_seq', 'N/A')
            
            # 計算變化
            received_delta = stats.get('received_delta', 0)
            lost_delta = stats.get('lost_delta', 0)
            
            # 顯示變化率
            recv_str = f"{received:6d}"
            if received_delta > 0:
                recv_str += f" (+{received_delta}↑)"
            
            lost_str = f"{lost:4d}"
            if lost_delta > 0:
                lost_str += f" (+{lost_delta}↑)"
            
            dialog_win.addstr(8, 2, f"Received  : {recv_str}")
            dialog_win.addstr(8, 32, f"Lost       : {lost_str}")
            dialog_win.addstr(9, 2, f"Loss Rate : {loss_rate:.2f}%")
            dialog_win.addstr(9, 32, f"Last Seq   : {last_seq}")
            
            # 最後接收時間
            last_msg_time = stats.get('last_msg_time')
            if last_msg_time:
                time_str = time.strftime("%H:%M:%S", time.localtime(last_msg_time))
                elapsed = current_time - last_msg_time
                dialog_win.addstr(10, 2, f"Last Time : {time_str}")
                dialog_win.addstr(10, 32, f"Elapsed    : {elapsed:.1f}s")
            else:
                dialog_win.addstr(10, 2, "Last Time : N/A")
            
            # === 訊息類型分佈 ===
            dialog_win.addstr(12, 2, "[Message Types] (Top 12)", curses.A_UNDERLINE)
            
            msg_counts = info.get('msg_type_counts', {})
            
            # MAVLink 訊息名稱對應（縮寫版本）
            MSG_NAMES = {
                0: "HB", 1: "SYS_ST", 24: "GPS_RAW", 27: "RAW_IMU",
                30: "ATT", 32: "LOC_POS", 33: "GLB_POS", 62: "NAV_CTL",
                74: "VFR_HUD", 147: "BATT_ST"
            }
            
            # 顯示前 12 個最常見的訊息類型（兩列各 6 個）
            msg_items = list(msg_counts.items())[:12]
            line = 13
            for i, (msg_id, count) in enumerate(msg_items):
                msg_name = MSG_NAMES.get(msg_id, "???")
                delta = stats.get(f'msg_delta_{msg_id}', 0)
                
                # 格式化數據
                if delta > 0:
                    data_str = f"{count}(+{delta}↑)"
                else:
                    data_str = f"{count}"
                
                # 格式化顯示：[ID]NAME  DATA (ID固定3字符寬度，右對齊)
                display_str = f"[{msg_id:3d}]{msg_name:8s} {data_str}"
                
                # 左列(偶數索引)或右列(奇數索引)
                col = 2 if i % 2 == 0 else 36
                row = line + (i // 2)
                
                if row >= dialog_height - 3:  # 避免超出邊界
                    break
                    
                dialog_win.addstr(row, col, display_str)
            
            # 確保跳過顯示區域
            line = line + 6
            
            dialog_win.addstr(dialog_height - 2, 2, "Press R: Reset Stats, C: Unregister, other key to return...")
            dialog_win.refresh()
            
            # 檢查是否有按鍵（非阻塞）
            ch = dialog_win.getch()
            if ch in (ord('R'), ord('r')):
                cmd_q.put(("RESET_VEHICLE_STATISTICS", sysid, compid))
            elif ch in (ord('C'), ord('c')):
                cmd_q.put(("UNREGISTER_VEHICLE", sysid))
                break
            elif ch != -1:  # 有按鍵則退出
                break
            
            # 短暫延遲
            time.sleep(0.1)
        
        state.vehicle_info_single['InfoReady'] = False
        del dialog_win
        stdscr.clear()
        stdscr.refresh()



class Orchestrator:
    def __init__(self, stop_sig):
        self.stop_evt = stop_sig # 外部操作去中斷 "面板" 執行緒的訊號 (內部自己停止的話不需要用這個)
        self.occupied_ip_ports = {} # 紀錄已被佔用的 ip:port 組合 "ip str" : [port int, port int, ...]

        self.vehicle_registry = mvv.vehicle_registry

        # === 1) 面板部分的準備 ===
        self.cmd_q = queue.Queue()
        self.panelState = PanelState() # 面板的狀態儲存
        self.cPanel = ControlPanel() # 面板的功能
        self.panel_thread = None

        # === 2) async_io_manager & mavlink_bridge 部分的準備 ===
        mo.stream_bridge_ring.clear()
        mo.return_packet_ring.clear()
        self.manager = mo.async_io_manager()
        self.bridge = mo.mavlink_bridge()

        # === 3) serial_manager 部分的準備 ===
        self.plumber = sm.serial_manager()

        # === 4) ros 部分的準備 ===
        self.fc_ros_manager = mros.ros2_manager
        if not self.fc_ros_manager.initialized:
            self.fc_ros_manager.initialize()
        self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
            'position': 1.0,   
            'attitude': 0.0,    
            'velocity': 0.0,     
            'battery': 1.0,   
            'vfr_hud': 1.0,    
            'mode': 0.0,       
            'summary': 1.0,    
        }

    def engageWholeSystem(self):
        """啟動整個系統"""
        # === 1) 面板部分的啟動 ===
        self.panel_thread = threading.Thread(target=self.cPanel.panel_thread, args=(self.cmd_q, self.panelState, self.stop_evt)) 
        self.panel_thread.start()

        # === 2) async_io_manager & mavlink_bridge 部分的啟動 ===
        self.manager.start()
        self.bridge.start()

        # === 3) serial_manager 部分的啟動 ===
        self.plumber.start()

        # === 4) ros 部分的啟動 ===
        self.fc_ros_manager.start()

    def mainLoop(self):
        logger.info("Main orchestrator started <-")
        try:
            # while not self.stop_evt.is_set():
            while self.panel_thread.is_alive():

                # A. 更新模組狀態
                if self.manager.running:
                    self.panelState.object_manager_state = 'Running'
                else:
                    self.panelState.object_manager_state = 'Stopped'

                socketid_list = self.manager.get_managed_objects()
                self.panelState.socket_object_list = socketid_list

                if self.bridge.running:
                    self.panelState.mavlink_bridge_state = 'Running'
                else:
                    self.panelState.mavlink_bridge_state = 'Stopped'

                if self.plumber.running:
                    self.panelState.serial_manager_state = 'Running'
                else:
                    self.panelState.serial_manager_state = 'Stopped'

                linked_serial_dict = self.plumber.get_serial_link()
                self.panelState.linked_serial_dict = linked_serial_dict

                if self.fc_ros_manager.running:
                    self.panelState.ros2_manager_state = 'Running'
                else:
                    self.panelState.ros2_manager_state = 'Stopped'

                # B. 更新載具列表（從 vehicle_registry 獲取）
                self._update_vehicles_list()

                # 取出面板丟過來的「動作」
                try:
                    cmd = self.cmd_q.get_nowait()
                    if callable(cmd):
                        cmd()  # 執行對應動作
                    elif isinstance(cmd, tuple):
                        # 處理多參數命令
                        action = cmd[0]
                        if action == "REMOVE_OBJECT":
                            socket_id = cmd[1]
                            # 先移除所有跟他關聯的 target sockets
                            for s_id in mo.mavlink_object.mavlinkObjects:
                                s_obj = mo.mavlink_object.mavlinkObjects[s_id]
                                if socket_id in s_obj.target_sockets:
                                    self.remove_target_from_object(s_id, socket_id)   
                            # 再移除該物件
                            self.delete_udp_object(socket_id)
                        elif action == "MAVOBJ_ADD_TARGET":
                            source_id, target_id = cmd[1], cmd[2]
                            self.add_target_to_object(source_id, target_id)
                        elif action == "MAVOBJ_REMOVE_TARGET":
                            source_id, target_id = cmd[1], cmd[2]
                            self.remove_target_from_object(source_id, target_id)
                        elif action == "INSPECT_MAV_OBJECT":
                            socket_id = cmd[1]
                            mav_obj = mo.mavlink_object.mavlinkObjects.get(socket_id, None)
                            if mav_obj:
                                self.panelState.socket_info_single["socket_type"] = mav_obj.socket_type
                                self.panelState.socket_info_single["socket_state"] = mav_obj.state.name
                                self.panelState.socket_info_single["bridge_msg_types"] = mav_obj.bridge_msg_types
                                self.panelState.socket_info_single["return_msg_types"] = mav_obj.return_msg_types
                                self.panelState.socket_info_single["primary_socket_id"] = mav_obj.primary_socket_id
                                self.panelState.socket_info_single["target_sockets"] = mav_obj.target_sockets
                                self.panelState.socket_info_single["socket_connection_string"] = mav_obj.mavlink_socket.address
                                self.panelState.socket_info_single["InfoReady"] = True # 標記資訊已準備好
                        elif action == "INSPECT_LINKED_SERIAL":
                            serial_id = cmd[1]
                            serial_obj = self.plumber.serial_objects.get(serial_id, None)
                            if serial_obj:
                                self.panelState.serial_info_single["serial_port"] = serial_obj.serial_port
                                self.panelState.serial_info_single["baudrate"] = serial_obj.baudrate
                                self.panelState.serial_info_single["receiver_type"] = serial_obj.receiver_type.name
                                self.panelState.serial_info_single["target_port"] = serial_obj.target_port
                                self.panelState.serial_info_single["InfoReady"] = True # 標記資訊已準備好
                        elif action == "REMOVE_LINKED_SERIAL":
                            serial_id = cmd[1]
                            self.plumber.remove_serial_link(serial_id)

                        elif action == "INSPECT_VEHICLE":
                            sysid, compid = cmd[1], cmd[2]
                            self._prepare_vehicle_info(sysid, compid)
                        # elif action == "UPDATE_VEHICLES_LIST": # TODO 這個擺這邊 不知道為何可以有作用 先不動 也許後面有bug?
                        #     logger.debug("Orchestrator: Updating vehicles list upon request")
                        #     self._update_vehicles_list()
                        elif action == "RESET_VEHICLE_STATISTICS":
                            sysid, compid = cmd[1], cmd[2]
                            vehicle_sys = self.vehicle_registry.get(sysid)
                            vehicle_sys.reset_component_stats(compid)
                        elif action == "UNREGISTER_VEHICLE":
                            sysid = cmd[1]
                            self.vehicle_registry.unregister(sysid)
                            
                    elif cmd == "CREATE_UDP_INBOUND":
                        self.panelState.udp_info_temp["direction"] = "inbound"
                        self.create_udp_object()
                    elif cmd == "CREATE_UDP_OUTBOUND":
                        self.panelState.udp_info_temp["direction"] = "outbound"
                        self.create_udp_object()
                    elif cmd == "CREATE_SERIAL_PORT":
                        self.create_serial_port_object()
                    elif cmd == "SHUTDOWN_BRIDGE":
                        self.bridge.stop()
                    elif cmd == "SHUTDOWN_MANAGER":
                        self.manager.shutdown()
                    elif cmd == "SHUTDOWN_SERIAL_MANAGER":
                        self.plumber.shutdown()

                except queue.Empty:
                    pass
                except Exception as e:
                    logger.error(f"Error processing command: {e}")

                time.sleep(0.1)

        except KeyboardInterrupt:
            pass
        except Exception as e:
            logger.error(f"Unexpected error in main loop: {e}")
        finally:

            # 驗證並確保所有模組都被下達關閉訊號 
            # 若是由面板操作結束系統 這些關閉行為將於 ControlPanel.pre_panel_shutdown() 觸發 
            if self.bridge.thread.is_alive():
                if self.bridge.running:
                    self.bridge.stop()
                self.bridge.thread.join(timeout=2)

            if self.manager.thread.is_alive():
                if self.manager.running:
                    self.manager.shutdown()
                self.manager.thread.join(timeout=2)

            if self.plumber.thread.is_alive():
                if self.plumber.running:
                    self.plumber.shutdown()
                self.plumber.thread.join(timeout=2)

            if self.fc_ros_manager.spin_thread.is_alive():
                if self.fc_ros_manager.running:
                    self.fc_ros_manager.stop()
                self.fc_ros_manager.spin_thread.join(timeout=2)

            # 關閉面板執行緒
            if self.panel_thread.is_alive():
                self.panel_thread.join(timeout=2)

            time.sleep(0.5) # 等待各模組穩定關閉
                
            logger.info("Main orchestrator END!")

    # =============== 面板動作 - Mavlink Object ===============

    def create_udp_object(self, socket_type:str = ""):
        # 監聽部分
        if self.panelState.udp_info_temp["direction"] == "inbound":
            connection_string = f"udp:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}"
            # 監聽的 port 要先檢查是否被佔用
            ip = self.panelState.udp_info_temp['IP']
            port = int(self.panelState.udp_info_temp['Port'])
            port_check_list = self.occupied_ip_ports.get(ip, []) + self.occupied_ip_ports.get("0.0.0.0", [])
            if port in port_check_list:
                self.panelState.panel_info_msg_list.append((f"Failed! Port {port} on IP {ip} occupied.", time.time()-1))
                return
            # 再記錄被佔用的 port
            if ip in self.occupied_ip_ports:
                self.occupied_ip_ports[ip].append(port)
            else:
                self.occupied_ip_ports[ip] = [port]
        # 外放資訊部分
        elif self.panelState.udp_info_temp["direction"] == "outbound":
            connection_string = f"udpout:{self.panelState.udp_info_temp['IP']}:{self.panelState.udp_info_temp['Port']}"
        
        try:
            # 檢測這個 connection_string 是否能成功建立 mavlink 連結
            mavlink_socket = mavutil.mavlink_connection(connection_string)
        except Exception as e:
            self.panelState.panel_info_msg_list.append((f"Failed to create UDP {self.panelState.udp_info_temp['direction']} object: {e}", time.time()-1))
            return

        # mavlink 連結建立成功 把他丟到 mavlink_object # 重點句
        mavlink_object = mo.mavlink_object(mavlink_socket)
        # 再把 mavlink_object 丟到 manager 的 event loop 裡面去管理與執行 # 重點句
        self.manager.add_mavlink_object(mavlink_object)

        # 設定一下 mavlink_object 的類型描述
        if socket_type == "":
            mavlink_object.socket_type = "UDP " + self.panelState.udp_info_temp['direction'].capitalize()
        else:
            mavlink_object.socket_type = socket_type

        self.panelState.panel_info_msg_list.append((f"Created UDP {self.panelState.udp_info_temp['direction']} object: {connection_string}", time.time()))
    
    def delete_udp_object(self, socket_id):
        """移除指定的 mavlink_object"""

        mavlink_obj = mo.mavlink_object.mavlinkObjects[socket_id]
        connection_string = mavlink_obj.mavlink_socket.address
        strings = connection_string.split(':')
        ip = strings[0]
        port = int(strings[1])
        self.occupied_ip_ports[ip].remove(port)

        self.manager.remove_mavlink_object(socket_id)

    def add_target_to_object(self, source_id, target_id):
        """為 mavlink_object 添加轉發目標"""
        if source_id in mo.mavlink_object.mavlinkObjects:
            source_obj = mo.mavlink_object.mavlinkObjects[source_id]
        else:
            self.panelState.panel_info_msg_list.append((f"Source object {source_id} not found", time.time()))
            return

        if source_obj.add_target_socket(target_id):
            self.panelState.panel_info_msg_list.append((f"Added target {target_id} to socket {source_id}", time.time()))
        else:
            self.panelState.panel_info_msg_list.append((f"Fail Adding target {target_id} to socket {source_id}", time.time()))

    def remove_target_from_object(self, source_id, target_id):
        """從 mavlink_object 移除轉發目標"""
        if source_id in mo.mavlink_object.mavlinkObjects:
            source_obj = mo.mavlink_object.mavlinkObjects[source_id]
        else:
            self.panelState.panel_info_msg_list.append((f"Source object {source_id} not found", time.time()))
            return

        if source_obj.remove_target_socket(target_id):
            self.panelState.panel_info_msg_list.append((f"Removed target {target_id} from socket {source_id}", time.time()))
        else:
            self.panelState.panel_info_msg_list.append((f"Fail Removing target {target_id} from socket {source_id}", time.time()))

    # =============== 面板動作 - Vehicle Inspector ===============

    def _update_vehicles_list(self):
        """更新已連線載具列表（從 vehicle_registry 獲取）"""
        vehicles_dict = {}
        
        # 從 vehicle_registry 獲取所有載具
        all_vehicles = self.vehicle_registry.get_all()
        
        for sysid, vehicle in all_vehicles.items():
            # 遍歷每個載具的所有組件
            for compid, component in vehicle.components.items():
                # 使用 (sysid, compid) 作為 key
                vehicles_dict[(sysid, compid)] = {
                    'sysid': sysid,
                    'compid': compid,
                    'vehicle_type': vehicle.vehicle_type,
                    'component_type': component.type.value,
                    'connection_via': vehicle.connected_via.value,
                    'socket_id': vehicle.custom_meta.get('socket_id', 'N/A')
                }
        
        self.panelState.connected_vehicles_dict = vehicles_dict
    
    def _prepare_vehicle_info(self, sysid, compid):
        """準備載具組件的詳細資訊（包含變化率計算）"""
        vehicle = self.vehicle_registry.get(sysid)
        if not vehicle:
            logger.warning(f"Vehicle {sysid} not found")
            return

        socket_id = vehicle.custom_meta.get('socket_id', 'N/A')
        
        component = vehicle.get_component(compid)
        if not component:
            logger.warning(f"Component {compid} not found in vehicle {sysid}")
            return
        
        stats = component.packet_stats
        
        # 取得之前的統計資料（用於計算變化）
        prev_stats = self.panelState.vehicle_info_single.get('prev_stats', {})
        prev_received = prev_stats.get('received', 0)
        prev_lost = prev_stats.get('lost', 0)
        prev_msg_counts = prev_stats.get('msg_counts', {})
        
        # 計算變化率
        received_delta = stats.received_count - prev_received
        lost_delta = stats.lost_count - prev_lost
        
        # 準備訊息類型計數（排序後取前幾個）
        sorted_msg_counts = dict(sorted(
            stats.msg_type_count.items(),
            key=lambda x: x[1],
            reverse=True
        )[:12])  # 取前 12 個最常見的
        
        # 計算每種訊息類型的變化
        msg_deltas = {}
        for msg_id, count in sorted_msg_counts.items():
            prev_count = prev_msg_counts.get(msg_id, 0)
            msg_deltas[f'msg_delta_{msg_id}'] = count - prev_count
        
        # 更新 vehicle_info_single
        socket_type = "N/A"
        socket_obj =mo.mavlink_object.mavlinkObjects.get(socket_id, None)
        if socket_obj:
            socket_type = socket_obj.socket_type

        self.panelState.vehicle_info_single = {
            "sysid": sysid,
            "compid": compid,
            # "vehicle_type": vehicle.vehicle_type, # 這個用不到
            "component_type": component.type.value,
            "mav_autopilot": component.mav_autopilot,
            "socket_id": socket_id,
            "connection_type": socket_type,
            "packet_stats": {
                "received": stats.received_count,
                "lost": stats.lost_count,
                "loss_rate": (stats.lost_count / stats.received_count * 100 
                             if stats.received_count > 0 else 0),
                "last_seq": stats.last_seq,
                "last_msg_time": stats.last_msg_time,
                "received_delta": received_delta,
                "lost_delta": lost_delta,
                **msg_deltas  # 展開訊息類型的變化
            },
            "msg_type_counts": sorted_msg_counts,
            "prev_stats": {  # 保存當前數據用於下次計算變化
                "received": stats.received_count,
                "lost": stats.lost_count,
                "msg_counts": dict(stats.msg_type_count)
            },
            "InfoReady": True
        }

    # =============== 面板動作 - Serial Manager ===============

    def create_serial_port_object(self):
        # 先檢查是否已有相同的 Serial Port 被建立
        serial_port_strs = self.panelState.linked_serial_dict.values() # linked_serial_dict 會在上面的 mainLoop 被不斷更新
        if self.panelState.serial_info_temp['Port'] in serial_port_strs:
            self.panelState.panel_info_msg_list.append(
                (f"Fail! Serial Port {self.panelState.serial_info_temp['Port']} already linked.", time.time())
            )
            return
        
        # 獲取可用的 udp port 
        udp_port_tmp = find_available_port(19000, 20000)

        # 定義通訊類型映射表
        COMM_TYPE_MAP = {
            "XBee(API-AT)": sm.SerialReceiverType.XBEEAPI2AT,
            # "XBee(AT-AT)": sm.SerialReceiverType.TELEMETRY,  # TODO: 之後再弄
            # 新增區
        }

        # 驗證輸入
        comm_type = self.panelState.serial_info_temp['CommunicationType']
        if not comm_type:
            self.panelState.panel_info_msg_list.append(
                ("Please select Communication Type first.", time.time())
            )
            return
        
        # 查找對應的通訊類型
        comm_type_tmp = COMM_TYPE_MAP.get(comm_type)
        if comm_type_tmp is None:
            self.panelState.panel_info_msg_list.append(
                (f"Communication type '{comm_type}' not supported yet.", time.time())
            )
            return

        ret = self.plumber.create_serial_link(
            serial_port=self.panelState.serial_info_temp['Port'],
            baudrate=self.panelState.serial_info_temp['Baud'],
            target_port=udp_port_tmp,
            receiver_type=comm_type_tmp,
        )

        if not ret:
            self.panelState.panel_info_msg_list.append((f"Failed to create Serial Port object at {self.panelState.serial_info_temp['Port']}.", time.time()))
            return

        self.panelState.panel_info_msg_list.append((f"Created Serial Port object at {self.panelState.serial_info_temp['Port']}.", time.time()))

        # 自動建立對應的 UDP 監聽端口
        if self.panelState.serial_info_temp['Go2Middleware']:
            self.panelState.udp_info_temp['IP'] = "127.0.0.1"
            self.panelState.udp_info_temp['Port'] = str(udp_port_tmp)
            self.panelState.udp_info_temp['direction'] = "inbound"
            self.create_udp_object("SERIAL_XBEE")
        

def main():

    stop_evt = threading.Event()

    def signal_handler(signum, frame):
        """處理 Ctrl+C 信號 藉此訊號 會結束下面的 while 迴圈 並逐步關閉各模組"""
        stop_evt.set()

    # 註冊信號處理器
    signal.signal(signal.SIGINT, signal_handler)
    signal.signal(signal.SIGTERM, signal_handler)

    orchestrator = Orchestrator(stop_evt)
    orchestrator.engageWholeSystem()
    orchestrator.mainLoop() # 程式會 block 在這邊 直到收到中斷信號

if __name__ == "__main__":
    main()


'''
================= 改版記錄 ============================

2025.12.23
1. 新增 serial 通道的資訊顯示完整化
2. 新增 serial 通道刪除功能
3. 新增 serial 直接順便開 ip object
4. 修改 避免 serial 與 ip port 重複建立相同的通道
5. 修改 show_object_info 與 show_linked_serial_info 改變檢核 Ready 方式 避免卡死

2025.01.16
1. 新增 "移除載具" 與 "重置載具統計" 功能
2. 修正 udp port 在移除後仍被記錄為佔用的問題
3. 因應 mvalinkObject.py 中 mavlinkObjects 修正變數存取方式
4. 註解掉無效代碼 action == "UPDATE_VEHICLES_LIST" 區塊
5. 系統納入 mavlink ROS2 Manager 模組

'''
