'''
這個檔案是對 udp 進來的 mavlink 訊息做 "分流" "轉拋" 的地方 (並不會做 "分析")
概念上 把每個 udp 接口 視為一個獨立的 mavlink bus 針對 bus 來統籌管理

主要的重點部分:
1. stream_bridge_ring & return_packet_ring
2. mavlink_object & async_io_manager
3. mavlink_bridge


stream_bridge_ring & return_packet_ring:
    這兩個 ring buffer 是用來做 mavlink 訊息的分流
    stream_bridge_ring 這邊的資訊比較是給會固定更新的資訊 (例如 HEARTBEAT, ATTITUDE 之類的)
    return_packet_ring 比較是處理指令後的回應封包 (例如 PARAM_VALUE, MISSION_ITEM 等等)

mavlink_object:
    每個 mavlink bus 都會有一個 mavlink_object
    使用 asyncio 處理資料流 用 RingBuffer 來分配訊息
    內容中沒有獨立的執行緒 只有一個個 asyncio function 會被放到 async_io_manager 裡面執行

    關於分流與轉拋的具體實現是在 process_data 這個 asyncio function 裡面

async_io_manager:
    首先它紀律並管理所有 mavlink_object 實例
    有自己一個獨立的執行緒 執行 asyncio loop (mavlink_object 裡面的 asyncio function 都會被放到這個 loop 裡面執行)
    
mavlink_bridge:
    專門處理 stream_bridge_ring 裡面的訊息流
    會把訊息流解開後 存放到 mavlinkVehicleView.py 定義的載具結構視圖




'''

# 基礎功能的 import
import os 
import signal
import time
import threading
import asyncio
from enum import Enum, auto
from collections import deque
# from typing import Dict, List, Optional, Set, Any, Tuple

# mavlink 的 import
from pymavlink import mavutil

# 自定義的 import
from .mavlinkVehicleView import (
    vehicle_registry,
    VehicleView,
    VehicleComponent,
    ComponentType,
    ConnectionType
)
from .utils import RingBuffer, setup_logger

# ====================== 分割線 =====================

logger = setup_logger(os.path.basename(__file__))

stream_bridge_ring = RingBuffer(capacity=1024, buffer_id=255)
return_packet_ring = RingBuffer(capacity=256, buffer_id=254)

# ====================== 分割線 =====================

# 使用 vehicle_registry 來管理所有的載具視圖
# vehicle_registry 是從 mavlinkVehicleView 導入的全域實例

class mavlink_bridge:
    '''
    這個 class 就是 固定串流橋接器
    是用來接收 mavlink 訊息 並進行橋接處理
    這個地方是針對 stream_bridge_ring 的資料做處理的
    記錄有 mavlink bus 上有那些 system id 和 component id
    為了每個 system id 都有一個對應的 device object
    
    此類別負責：
    1. 從 stream_bridge_ring 接收訊息
    2. 管理 mavlink_systems（device 和 component objects）
    3. 處理接收到的訊息並更新對應的 component 狀態
    4. 提供發送訊息的介面，將訊息路由到正確的 mavlink_object
    
    ps. 此 class 為 singleton，只能有一個 instance
    '''
    _instance = None 
    _lock = threading.Lock()  # 確保多線程安全

    def __new__(cls, *args, **kwargs):
        with cls._lock:  # 確保多線程環境下只有一個實例被創建
            if cls._instance is None:
                cls._instance = super(mavlink_bridge, cls).__new__(cls)
        return cls._instance

    def __init__(self):
        if not hasattr(self, "initialized"):  # 防止重複初始化
            self.initialized = True
            self.thread = None
            self.running = False

            # 初始化訊息處理器字典 (msg_id -> handler_function)
            self._init_message_handlers()
        else:
            logger.error('mavlink_bridge instance already exists. Do not create another one.')

    def _init_message_handlers(self):
        """初始化訊息處理器映射表，提高處理效率"""
        self.message_handlers = {
            0: self._handle_heartbeat,        # HEARTBEAT
            30: self._handle_attitude,        # ATTITUDE
            32: self._handle_local_position,  # LOCAL_POSITION_NED
            33: self._handle_global_position, # GLOBAL_POSITION_INT
            74: self._handle_vfr_hud,        # VFR_HUD
            147: self._handle_battery_status, # BATTERY_STATUS
        }

    def start(self):
        """啟動 mavlink_bridge 的運作"""
        if not self.running:
            self.running = True
            self.thread = threading.Thread(target=self._run_thread, name="MavlinkBridge")
            self.thread.start()
        else:
            logger.warning("mavlink_bridge is already running")

    def stop(self):
        """停止 mavlink_bridge 的運作"""
        self.running = False
        if self.thread and self.thread.is_alive():
            self.thread.join(timeout=5.0)

    # === Thread 區塊 ===
    def _run_thread(self):
        """主執行緒：從 stream_bridge_ring 接收並處理 mavlink 訊息"""
        logger.info('mavlink_bridge started <-')
        
        while self.running:
            # 檢查是否有訊息
            if stream_bridge_ring.is_empty():
                time.sleep(0.001)  # 避免忙碌等待
                continue
            
            # 取出訊息包：(socket_id, timestamp, mavlink_msg)
            msg_pack = stream_bridge_ring.get()
            socket_id, timestamp, msg = msg_pack[0], msg_pack[1], msg_pack[2]
            
            # 解析訊息基本資訊
            sysid = msg.get_srcSystem()
            compid = msg.get_srcComponent()
            msg_id = msg.get_msgId()
            
            # 確保 VehicleView 存在
            vehicle = vehicle_registry.get(sysid)
            if vehicle is None:
                vehicle = vehicle_registry.register(sysid)
                # 存儲 socket_id 到自定義 meta 中
                vehicle.custom_meta['socket_id'] = socket_id
            
            # 確保 VehicleComponent 存在
            component = vehicle.get_component(compid)
            if component is None:
                if msg_id == 0:  # 只有透過 HEARTBEAT 才能創建新的 component
                    # 根據 mav_type 判斷 component 類型
                    comp_type = self._determine_component_type(msg.type)
                    component = vehicle.add_component(compid, comp_type)
                    component.mav_type = msg.type
                    component.mav_autopilot = msg.autopilot
                else:
                    # component 不存在且非 HEARTBEAT，忽略此訊息
                    continue
            
            # 使用處理器字典分發訊息處理
            if msg_id in self.message_handlers:
                try:
                    self.message_handlers[msg_id](vehicle, component, msg, timestamp)
                except Exception as e:
                    logger.error(f'Error handling message type {msg_id}: {e}')
            else:
                logger.debug(f'Unhandled message type: {msg_id} / {msg.get_type()}')
        
        logger.info('mavlink_bridge END!')

    def _determine_component_type(self, mav_type: int) -> ComponentType:
        """根據 MAV_TYPE 判斷組件類型"""
        # MAV_TYPE 定義：
        # 0=通用, 1=固定翼, 2=四旋翼, 3=直升機, 4=天線追蹤器, 5=GCS, 6=飛船,
        # 26=雲台, 27=ADSB, 28=降落傘等
        if mav_type == 6:  # MAV_TYPE_GCS
            return ComponentType.GCS
        elif mav_type == 26:  # MAV_TYPE_GIMBAL
            return ComponentType.GIMBAL
        elif mav_type == 30:  # MAV_TYPE_CAMERA
            return ComponentType.CAMERA
        elif mav_type in [1, 2, 3, 4, 13, 14, 15, 19, 20, 21, 22]:  # 各種飛行器類型
            return ComponentType.AUTOPILOT
        else:
            return ComponentType.OTHER

    # === 訊息處理器 ===
    def _handle_heartbeat(self, vehicle, component, msg, timestamp):
        """處理 HEARTBEAT 訊息 (msg_id: 0)"""
        component.status.mode.base_mode = msg.base_mode
        component.status.mode.custom_mode = msg.custom_mode
        component.status.mode.mode_name = mavutil.mode_string_v10(msg)
        component.status.mode.timestamp = timestamp
        component.status.system_status = msg.system_status
        component.status.armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
        # print("get mode:", mavutil.mode_string_v10(msg))  # debug

    def _handle_attitude(self, vehicle, component, msg, timestamp):
        """處理 ATTITUDE 訊息 (msg_id: 30)"""
        component.status.attitude.roll = msg.roll
        component.status.attitude.pitch = msg.pitch
        component.status.attitude.yaw = msg.yaw
        component.status.attitude.rollspeed = msg.rollspeed
        component.status.attitude.pitchspeed = msg.pitchspeed
        component.status.attitude.yawspeed = msg.yawspeed
        component.status.attitude.timestamp = timestamp

    def _handle_local_position(self, vehicle, component, msg, timestamp):
        """處理 LOCAL_POSITION_NED 訊息 (msg_id: 32)"""
        # LOCAL_POSITION_NED 提供相對位置資訊
        component.status.position.relative_altitude = -msg.z  # NED 座標系，z 為負表示高度
        component.status.position.timestamp = timestamp
        # 也可以存儲到 custom_status 中保留原始資料
        component.status.custom_status['local_position'] = {
            'x': msg.x, 'y': msg.y, 'z': msg.z,
            'vx': msg.vx, 'vy': msg.vy, 'vz': msg.vz
        }

    def _handle_global_position(self, vehicle, component, msg, timestamp):
        """處理 GLOBAL_POSITION_INT 訊息 (msg_id: 33)"""
        component.status.position.latitude = msg.lat / 1e7  # 轉換為度
        component.status.position.longitude = msg.lon / 1e7  # 轉換為度
        component.status.position.altitude = msg.alt / 1000.0  # 轉換為公尺
        component.status.position.relative_altitude = msg.relative_alt / 1000.0  # 轉換為公尺
        component.status.position.timestamp = timestamp

    def _handle_vfr_hud(self, vehicle, component, msg, timestamp):
        """處理 VFR_HUD 訊息 (msg_id: 74)"""
        component.status.vfr.airspeed = msg.airspeed
        component.status.vfr.groundspeed = msg.groundspeed
        component.status.vfr.heading = msg.heading
        component.status.vfr.throttle = msg.throttle
        component.status.vfr.climb = msg.climb
        component.status.vfr.timestamp = timestamp

    def _handle_battery_status(self, vehicle, component, msg, timestamp):
        """處理 BATTERY_STATUS 訊息 (msg_id: 147)"""
        # 計算電池總電壓（mV 轉 V）
        if hasattr(msg, 'voltages') and msg.voltages[0] != 65535:
            # voltages 是一個陣列，包含各個電池單元的電壓
            total_voltage = sum(v for v in msg.voltages if v != 65535) / 1000.0
            component.status.battery.voltage = total_voltage
        
        component.status.battery.current = msg.current_battery / 100.0 if msg.current_battery != -1 else None  # cA 轉 A
        component.status.battery.remaining = msg.battery_remaining if msg.battery_remaining != -1 else None  # 百分比
        component.status.battery.temperature = msg.temperature / 100.0 if hasattr(msg, 'temperature') and msg.temperature != 32767 else None  # 轉換為攝氏
        component.status.battery.timestamp = timestamp

    # === 訊息發送功能 ===
    def send_message(self, message_bytes, target_sysid=None, target_socket_id=None, broadcast=False):
        """
        發送訊息到指定的 mavlink_object
        
        Args:
            message_bytes: 準備好的 mavlink 封包 (bytes)
            target_sysid: 目標系統 ID (可選，用於自動查找對應的 socket)
            target_socket_id: 目標 socket ID (可選，直接指定)
            
        Returns:
            bool: 是否成功發送
            
        使用方式：
            1. broadcast: 廣播到所有活動的 mavlink_object
            2. 指定 target_socket_id：直接發送到該 socket
            3. 指定 target_sysid：自動查找該系統對應的 socket 並發送
        """
        if not isinstance(message_bytes, (bytes, bytearray)):
            logger.error(f"Invalid message type: {type(message_bytes)}")
            return False

        # 情況 1: 廣播到所有活動的 mavlink_object
        if broadcast:
            success_count = 0
            for socket_id, mav_obj in mavlink_object.mavlinkObjects.items():
                if self._send_to_socket(message_bytes, socket_id):
                    success_count += 1
            
            return success_count > 0

        # 情況 2: 直接指定 socket_id
        if target_socket_id is not None:
            return self._send_to_socket(message_bytes, target_socket_id)
        
        # 情況 3: 透過 sysid 查找對應的 socket
        if target_sysid is not None:
            vehicle = vehicle_registry.get(target_sysid)
            if vehicle and 'socket_id' in vehicle.custom_meta:
                socket_id = vehicle.custom_meta['socket_id']
                return self._send_to_socket(message_bytes, socket_id)
            else:
                logger.warning(f"System ID {target_sysid} not found or no socket_id")
                return False

        logger.warning("No target specified for sending message. WTF ARE YOU DOING?")
        return False  # 若無指定任何目標，則返回失敗
       
    def _send_to_socket(self, message_bytes, socket_id):
        """
        將訊息發送到指定的 mavlink_object
        
        Args:
            message_bytes: mavlink 封包
            socket_id: 目標 socket ID
            
        Returns:
            bool: 是否成功
        """
        if socket_id not in mavlink_object.mavlinkObjects:
            logger.warning(f"mavlink_object {socket_id} not found")
            return False
        
        mav_obj = mavlink_object.mavlinkObjects[socket_id]
        return mav_obj.message_put_queue(message_bytes)

# 定義 mavlink_object 的狀態
class MavlinkObjectState(Enum):
    INIT = auto()         # 初始化狀態
    RUNNING = auto()      # 運行中狀態
    SHUTTINGDOWN = auto() # 關閉中狀態
    ERROR = auto()        # 錯誤狀態
    CLOSED = auto()       # 已關閉狀態

class mavlink_object:
    '''
    每個 mavlink bus 都會有一個 mavlink_object
    使用 asyncio 處理資料流 用 RingBuffer 來分配訊息
    直接透過 socket 寫出
    '''
    mavlinkObjects = {}  # 用來記錄所有的 mavlink_object instance 資料格式 { socket_id(序號) : mavlink_object(物件實例) }
    socket_num = 0  # 用來記錄目前的 socket 數量

    def __new__(cls, *args, **kwargs):
        # 每創建一個實例 就將其添加到 mavlinkObjects 列表中
        # 創建時 會檢查 mavlinkObjects 列表中空缺的 socket_id 序號 
        # 若序號無中斷 則 socket_id 往後加一 若序號有中斷 則填補最小的序號
        # socket_id 從 0 開始
        instance = super().__new__(cls)
        socket_id = 0
        for k in cls.mavlinkObjects.keys():
            if k == socket_id:
                socket_id += 1
            else:
                break
        instance.socket_id = socket_id
        cls.socket_num += 1
        cls.mavlinkObjects[socket_id] = instance
        return instance

    def __init__(self, socket):
        # 登入所需的 socket
        self.mavlink_socket = socket

        # 用於主線程發送消息的緩衝區
        self.outgoing_msgs = deque()
        
        # 記錄訊息過濾類型 (可選)
        self.bridge_msg_types = set([0, 30, 33, 74, 147])  # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS
        self.return_msg_types = set()

        # 轉發到別的 mavlink object 作為目標端口 的列表
        self.target_sockets = set()
        
        # 物件變數
        self.task = None # Task reference
        self.dirtyDataCount = 0  # 髒資料計數器
        self.dirtyDataMax = 10  # 髒資料容許閾值
        
        self.state = MavlinkObjectState.INIT
        # logger.info(f'mavlink_object instance {self.socket_id} created') # 先註解掉避免太多 log 但是為了 debug 保留

        # 附加參數 (並非 mavlink_object 運行本體必要 但是要給上層結構運用的)
        # 若這個 socket 是 另一個"主要 socket"的備用連接 則設定為"主要 socket id"
        self.primary_socket_id = None  # None 表示不是備用連接
        # socket type
        self.socket_type = 'UNDEFINED'  # 可選 'UDP_INBOUND', 'SERIAL_XBEE'...etc

    def __del__(self):
        # # 先移除其他 socket 指向這個 socket 的目標 # 還是不要在這邊做了 畢竟本來就有判斷 object 不活躍就不轉拋
        # for mavlink_obj in self.mavlinkObjects.values():
        #     if self.socket_id in mavlink_obj.target_sockets:
        #         mavlink_obj.remove_target_socket(self.socket_id)

        # 關閉 socket
        if hasattr(self, 'mavlink_socket') and self.mavlink_socket:
            try:
                self.mavlink_socket.close()
            except:
                pass
        
        # 處理 class 的 instance 記錄
        if hasattr(self.__class__, 'socket_num'):
            self.__class__.socket_num -= 1
        
        if hasattr(self.__class__, 'mavlinkObjects') and hasattr(self, 'socket_id'):
            if self.socket_id in self.__class__.mavlinkObjects:
                self.__class__.mavlinkObjects.pop(self.socket_id)
        
        # 這段不知道怎麼了 反正會一直讓 logger ERROR 我先關掉
        # try:
        #     logger.info(f'mavlink_object instance {self.socket_id} deleted')
        # except Exception as e:
        #     print(f"Error logging in __del__: {e}")

    async def process_data(self):
        """處理 mavlink 數據的主要 asyncio 協程"""
        # logger.info(f'Start of mavlink_object id: {self.socket_id}') # 先註解掉避免太多 log 但是為了 debug 保留
        
        while self.state == MavlinkObjectState.RUNNING:
            timestamp = time.time()
            
            # 處理接收到的封包
            mavlinkMsg = self.mavlink_socket.recv_msg()

            if mavlinkMsg:
                # 統計收到的訊息 & 透過 mavlink 封包的序列號來檢查是否有遺失的封包 & 記錄最後收到的封包時間
                sysid = mavlinkMsg.get_srcSystem()
                compid = mavlinkMsg.get_srcComponent()
                
                # 更新封包統計資訊
                vehicle = vehicle_registry.get(sysid)
                if vehicle:  # 只有當這個 system id 已經被註冊過才會記錄統計
                    component = vehicle.get_component(compid)
                    if component:
                        component.update_packet_stats(
                            mavlinkMsg.get_seq(), 
                            mavlinkMsg.get_msgId(), 
                            timestamp
                        )
                
                # 分發訊息到 RingBuffer
                msg_id = mavlinkMsg.get_msgId()
                
                if (msg_id in self.bridge_msg_types or -1 in self.bridge_msg_types):
                    stream_bridge_ring.put((self.socket_id, timestamp, mavlinkMsg))
                    
                if (msg_id in self.return_msg_types or -1 in self.return_msg_types):
                    return_packet_ring.put((self.socket_id, timestamp, mavlinkMsg))
                
                # 將全部接收到的訊息轉發給目標列表中的 mavlink_object
                for target_socket in self.target_sockets:
                    if target_socket in self.mavlinkObjects:
                        target_obj = self.mavlinkObjects[target_socket]
                        if target_obj.state == MavlinkObjectState.RUNNING:
                            target_obj.mavlink_socket.write(mavlinkMsg.get_msgbuf())

            if self.outgoing_msgs:
                send_msg = self.outgoing_msgs.popleft()
                self.mavlink_socket.write(send_msg)
                

            # 這邊的重點不是延遲 而是透過 await 讓出 event loop 的控制權
            await asyncio.sleep(0.001)

        logger.info(f'End of mavlink_object id: {self.socket_id}')
        self.state = MavlinkObjectState.CLOSED

    def add_target_socket(self, target_socket_id):
        """添加一個目標端口用於轉發"""
        if (target_socket_id != self.socket_id) and (target_socket_id != self.primary_socket_id):
            if target_socket_id not in self.target_sockets:
                self.target_sockets.add(target_socket_id)
                logger.info(f"mavlink_object Added target port {target_socket_id} to mavlink_object {self.socket_id}")
                return True
            return False  # 已存在
        return False  # 不能添加自己 也不能添加主要 socket
    
    def remove_target_socket(self, target_socket_id):
        """移除目標端口"""
        if target_socket_id in self.target_sockets:
            self.target_sockets.remove(target_socket_id)
            logger.info(f"mavlink_object Removed target port {target_socket_id} from mavlink_object {self.socket_id}")
            return True
        return False  # 不存在
    
    def set_bridge_message_types(self, msg_types):
        """設置需要分流到橋接器的訊息類型"""
        if isinstance(msg_types, list) and all(isinstance(t, int) for t in msg_types):
            self.bridge_msg_types = set(msg_types)
            return True
        logger.error(f"Invalid bridge message types: {msg_types}")
        return False
    
    def set_return_message_types(self, msg_types):
        """設置需要分流到回傳處理器的訊息類型"""
        if isinstance(msg_types, list) and all(isinstance(t, int) for t in msg_types):
            self.return_msg_types = set(msg_types)
            return True
        logger.error(f"Invalid return message types: {msg_types}")
        return False

    def message_put_queue(self, message_bytes):
        """
        從主線程向此 mavlink_object 的 socket 發送數據
        將數據添加到簡單的列表中，由 asyncio 任務處理
        
        Args:
            message_bytes: 要發送的 mavlink 消息的字節數據
            
        Returns:
            bool: 是否成功添加消息到列表
        """
        # 狀態檢查
        if self.state != MavlinkObjectState.RUNNING:
            logger.warning(f"Cannot send message: mavlink_object {self.socket_id} is not running")
            return False
        
        # 基本數據類型檢查（輕量級）
        if not isinstance(message_bytes, (bytes, bytearray)):
            logger.error(f"Invalid message type: expected bytes/bytearray, got {type(message_bytes)}")
            return False
        
        # 基本長度檢查（MAVLink v1.0 最小8字節，v2.0最小12字節）
        if len(message_bytes) < 8:
            logger.error(f"Message too short: {len(message_bytes)} bytes (minimum 8)")
            return False
        
        # MAVLink 起始標記檢查（輕量級）
        if len(message_bytes) > 0 and message_bytes[0] not in (0xFE, 0xFD):  # v1.0: 0xFE, v2.0: 0xFD
            logger.error(f"Invalid MAVLink start marker: 0x{message_bytes[0]:02X}")
            return False
        
        # 緩衝區大小保護（防止記憶體耗盡）
        if len(self.outgoing_msgs) > 1000:  # 可調整的閾值
            logger.warning(f"Outgoing message buffer full for mavlink_object {self.socket_id}")
            return False

        self.outgoing_msgs.append(message_bytes)
        return True 

class async_io_manager:
    """
    管理所有 mavlink_object 實例的 asyncio 任務
    提供單一線程來處理所有 mavlink 通道的數據

    首先 async_io_manager 是 singleton 的 所以只能有一個實例

    這個 async_io_manager 是藉由 start 方法來啟動的

    start 方法 會先做一個新的執行緒 然後讓新的執行緒 透過 _run_event_loop 方法來建立一個空的事件循環 self.loop
    然後在 _run_event_loop 方法中 會建立一個異步任務 _main_task 來監控和管理所有的 mavlink_object 任務
    """

    _instance = None
    _lock = threading.Lock()
    
    def __new__(cls, *args, **kwargs):
        with cls._lock:
            if cls._instance is None:
                cls._instance = super(async_io_manager, cls).__new__(cls)
        return cls._instance
    
    def __init__(self):
        if not hasattr(self, 'initialized'):
            self.initialized = True
            self.loop = None
            self.running = False
            # self.main_task = None
            self.thread = None

    def __del__(self):
        self.loop = None
        self.thread = None

    def start(self):
        """
        啟動 async_io_manager 
        
        """
        if self.running:
            logger.warning("async_io_manager already running")
            return
        
        self.running = True

        # 啟動獨立線程 命名為 AsyncIOManager
        self.thread = threading.Thread(
            target=self._run_event_loop, 
            name="AsyncIOManager"
        )
        self.thread.daemon = False  # 不設為 daemon，確保正確關閉
        self.thread.start()

        # 等待 _run_event_loop 建立事件循環的物件 self.loop
        start_timeout = 2.0
        start_time = time.time()
        while not self.loop and time.time() - start_time < start_timeout:
            time.sleep(0.1)

        # 檢查另一個執行緒有沒有成功建立事件循環物件 self.loop
        if self.loop:
            logger.info("async_io_manager thread started <-")
            return True
        else:
            logger.error("async_io_manager failed to start")
            return False
    
    def shutdown(self):
        """停止 async_io_manager 和其管理的所有 mavlink_object"""

        # 自己在 running 狀態下才執行停止程序
        if not self.running:
            return

        # 停止所有被管理的 mavlink_object 所屬的 task
        for socket_id in list(mavlink_object.mavlinkObjects.keys()):
            self.remove_mavlink_object(socket_id)

        # 停止自己的 task
        self.running = False

        # 解開事件循環的阻塞
        self.loop.call_soon_threadsafe(self.loop.stop)

        # print("mark A", len(asyncio.all_tasks(self.loop)))  # debug
        
        # 等待線程結束
        if self.thread and self.thread.is_alive():
            self.thread.join(timeout=5.0)
            if self.thread.is_alive():
                logger.warning("async_io_manager thread did not stop gracefully")
                os.kill(os.getpid(), signal.SIGTERM) # 強制終止程序
            
        logger.info("async_io_manager thread END!")
    
    def _run_event_loop(self):
        """在獨立線程中運行事件循環"""
        try:
            self.loop = asyncio.new_event_loop()
            asyncio.set_event_loop(self.loop)

            logger.info("async_io_manager event loop started <-")
            
            # 創建主任務
            # main_task = self.loop.create_task(self._main_task())
            
            # 運行事件循環
            self.loop.run_forever()
            
        except Exception as e:
            logger.error(f"Error in async_io_manager event loop: {e}")
        finally:
            # 清理
            if self.loop:
                self.loop.close()
            self.loop = None
            self.running = False
            logger.info("async_io_manager event loop END!")
   
    def add_mavlink_object(self, mavlink_obj: mavlink_object):
        """添加 mavlink_object"""
        # 一個防呆 確保有 event loop 與 _main_task 正在運作
        if not self.running or not self.loop:
            logger.error("Cannot add mavlink_object: async_io_manager is not running")
            return False
        
        socket_id = mavlink_obj.socket_id
        
        # 檢查該對象是否已經在運行中
        if socket_id in mavlink_object.mavlinkObjects:
            existing_obj = mavlink_object.mavlinkObjects[socket_id]
            if existing_obj.state == MavlinkObjectState.RUNNING:
                logger.warning(f"mavlink_object {socket_id} already managed")
                return False

        # 使用 run_coroutine_threadsafe 執行協程並獲取結果
        future = asyncio.run_coroutine_threadsafe(
            self._async_add_mavlink_object(mavlink_obj),
            self.loop
        )

        try:
            # 等待結果，設定合理的超時時間
            result = future.result(timeout=3.0)
            return result
        except asyncio.TimeoutError:
            logger.error(f"Timeout adding mavlink_object {socket_id}")
            return False
        except Exception as e:
            logger.error(f"Failed to add mavlink_object {socket_id}: {e}")
            return False
    
    async def _async_add_mavlink_object(self, mavlink_obj):
        """在事件循環線程中同步執行"""
        socket_id = mavlink_obj.socket_id

        try:
            task = asyncio.create_task(mavlink_obj.process_data())
            mavlink_obj.task = task
            mavlink_obj.state = MavlinkObjectState.RUNNING
            mavlink_obj.outgoing_msgs.clear()
            logger.info(f"Added mavlink_object {socket_id} into manager.")
            return True
        except Exception as e:
            logger.error(f"Failed to create task for mavlink_object {socket_id}: {e}")
            return False
    
    def remove_mavlink_object(self, socket_id: int):
        """移除 mavlink_object"""

        # 一個防呆 確保有 event loop 正在運作
        if not self.loop:
            return False
        
        # 同樣使用 run_coroutine_threadsafe
        future = asyncio.run_coroutine_threadsafe(
            self._async_remove_mavlink_object(socket_id),
            self.loop
        )
        
        try:
            result = future.result(timeout=3.0)
            return result
        except asyncio.TimeoutError:
            logger.error(f"Timeout removing mavlink_object {socket_id}")
            return False
        except Exception as e:
            logger.error(f"Failed to remove mavlink_object {socket_id}: {e}")
            return False
    
    async def _async_remove_mavlink_object(self, socket_id):
        """在事件循環線程中同步執行"""
        if socket_id not in mavlink_object.mavlinkObjects:
            logger.warning(f"mavlink_object {socket_id} not found")
            return False
        
        mavlink_obj = mavlink_object.mavlinkObjects[socket_id]
        mavlink_obj.state = MavlinkObjectState.SHUTTINGDOWN

        if not mavlink_obj.task.done():
            mavlink_obj.task.cancel()
        
        # 等待一秒或者 task完全結束 
        timeout = 1.0
        start_time = asyncio.get_event_loop().time()
        while not mavlink_obj.task.done():
            if asyncio.get_event_loop().time() - start_time > timeout:
                break
            await asyncio.sleep(0.1)

        # 如果正常結束 則設置為關閉狀態（物件的清理由 __del__ 處理）
        if mavlink_obj.task.done():
            mavlink_obj.state = MavlinkObjectState.CLOSED
            logger.info(f"Removed mavlink_object {socket_id} from manager.")
            return True
        else:
            mavlink_obj.state = MavlinkObjectState.ERROR
            logger.warning(f"mavlink_object {socket_id} task did not terminate in time")
            return False
         
    def get_managed_objects(self):
        """獲取所有被管理的對象列表（狀態為 RUNNING 的對象）"""
        return [socket_id for socket_id, obj in mavlink_object.mavlinkObjects.items() 
                if obj.state == MavlinkObjectState.RUNNING]

# ====================== 分割線 =====================

if __name__ == '__main__':
    pass


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

2025年 6月 20日
1. mavlink_object 中 由於 Queue 的效能太差 會完全移除
    其中 multiplexingToAnalysis multiplexingToReturn 的功能會改用 ring_buffer 來實現
    而 multiplexingToSwap 會完全被移除代替方式下一條描述
2. mavlink_object 會捨棄每個通道單獨 thread 的實現 轉而採用 asyncio 的方式 將需要資料轉換的通道 以群組方式處理其數據流
    (註解: 因為本專案的規模還不大 目前不做動態分配 asyncio thread 而是簡單的採用單一 thread 處理所有的 mavlink_object)
    因此 資料轉換直接使用通道的 socket 寫出 進而節省任何資料的複製與搬移
    並且 完全捨棄 multiplexingToSwap 不會再對需要轉傳的資料進行過濾 而是將全部的 mavlink_msg 直接 socket 透過寫出
3. mavlink_object 需要加上 state 去管理其狀態
4. mavlink_object 需要加上 target port 去管理寫出的目標
5. mavlink_object 要容忍髒資料流入 而不是直接關閉通道
6. 基於第1,2項 updateMultiplexingList 會被完全移除
7. 基於第2項 需要新建一個 async_io_manager 類別去管理所有的 mavlink_object
8. 基於第1項全域變數 fixed_stream_bridge_queue  return_packet_processor_queue 會用 stream_bridge_ring 與 return_packet_ring 來取代 另外 swap_queues 會被完全移除


2025年 11月 15日
1. mavlink_bridge 類別新增為 singleton 模式 確保全系統只有一個實例在運行
2. mavlink_bridge 處理封包改為映射表 (需要在 _init_message_handlers 中新增處理器函式)
3. mavlink_bridge 的主要迴圈 增加 send_message 功能 可指定目標 sysid 或 socket_id 發送 mavlink 封包
4. async_io_manager 循環邏輯大改動 優化 mavlink_object 加入與移除的邏輯 並使得 task 與 evenlt loop 分層更清楚
5. mavlink_object 移除不必要的 start 與 stop 方法 由 async_io_manager 統一管理其生命週期
6. mavlink_object 優化 message_put_queue 方法 避免無效判斷 與 增加一些防呆檢驗 並與 mavlink_bridge 連動工作
7. 移除迴圈內的 try except 堆疊 增加效能
8. 移除對於 mavlinkDevice 的依賴 改用 vehicle_registry 來管理所有的載具

2026年 01月 15日
1. async_io_manager.managed_objects 與 mavlink_object.mavlinkObjects 功能重複整合 保留 mavlink_object.mavlinkObjects
2. async_io_manager 的 _stop_event 無效變數移除

'''

