"""
MAVLink ROS2 Nodes
包含兩個獨立的 ROS2 Node：
1. VehicleStatusPublisher - 發布載具狀態到 ROS2 topics
2. MavlinkCommandService - 提供 MAVLink 指令 service 介面

從 vehicle_registry 讀取狀態數據，頻率控制，模組化設計
"""

import os
import time
import math
import threading
from typing import Dict, Optional

# ROS2 imports
import rclpy
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor

# ROS2 Message imports
import std_msgs.msg
import sensor_msgs.msg
import geometry_msgs.msg
import mavros_msgs.msg

# 自定義 imports
from . import mavlinkVehicleView as mvv
from .utils import setup_logger

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


# ============================================================================
#                          頻率控制器
# ============================================================================

class PublishRateController:
    """發布頻率控制器 - 按時間間隔控制發布頻率"""
    
    def __init__(self):
        # ═══════════════════════════════════════════════════════════════
        # 【新增 Topic 位置 1/4】
        # 若要新增 topic 種類，請在此字典中加入新的 topic 名稱和發布間隔
        # 例如：'ekf_status': 1.0,  # EKF 狀態
        # ═══════════════════════════════════════════════════════════════
        # 各 topic 的發布間隔（秒）
        self.topic_intervals = {
            'position': 0.5,       # GPS位置
            'attitude': 0.5,       # 姿態
            'velocity': 0.5,       # 速度
            'battery': 1.0,        # 電池
            'vfr_hud': 0.5,        # VFR HUD
            'mode': 1.0,           # 飛行模式
            'summary': 1.0,        # 載具摘要
            # 在這裡新增更多 topics...
        }
        # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp}
        self.last_publish_time: Dict[tuple, float] = {}
    
    def should_publish(self, sysid: int, topic: str) -> bool:
        """
        檢查是否應該發布此 topic
        
        Args:
            sysid: 系統ID
            topic: topic 名稱
            
        Returns:
            bool: True 表示應該發布
        """
        key = (sysid, topic)
        now = time.time()
        
        # 當間隔設定為0或負數時 關閉該 topic 的發布
        interval = self.topic_intervals.get(topic, 0)
        if interval <= 0:
            return False 

        # 首次發布
        if key not in self.last_publish_time:
            self.last_publish_time[key] = now
            return True
        
        # 檢查時間間隔
        if now - self.last_publish_time[key] >= interval:
            self.last_publish_time[key] = now
            return True
        
        return False
    
    def reset(self):
        """重置所有計時器"""
        self.last_publish_time.clear()


# ============================================================================
#                       VehicleStatusPublisher Node
# ============================================================================

class VehicleStatusPublisher(Node):
    """
    載具狀態發布者 - 從 vehicle_registry 讀取數據並發布到 ROS2 topics
    
    職責:
    - 定期從 vehicle_registry 讀取載具狀態
    - 頻率控制（位置/姿態 2Hz，電池/摘要 1Hz）
    - 發布標準 ROS2 消息類型
    - 檢測訂閱者，按需發布
    """
    topicString_prefix = f'/fc_network/vehicle'
    
    def __init__(self):
        super().__init__('vehicle_status_publisher')
        
        # 頻率控制器
        self.rate_controller = PublishRateController()
        
        # fc_publishers 字典 {(sysid, topic_name): publisher}
        self.fc_publishers: Dict[tuple, any] = {}
        
        # 定時器：以較高頻率檢查 vehicle_registry 並發布
        # 10Hz 檢查頻率，但通過 rate_controller 控制實際發布頻率
        self.timer_period = 0.1  # 100ms
        self.timer = self.create_timer(self.timer_period, self.timer_callback)
        
        # 狀態標誌
        self.running = True
        
        # logger.info("VehicleStatusPublisher initialized")
    
    def timer_callback(self):
        """定時器回調 - 檢查所有載具並發布狀態"""
        if not self.running:
            return

        # 從 vehicle_registry 獲取所有載具
        all_vehicles = mvv.vehicle_registry.get_all()
        
        for sysid, vehicle in all_vehicles.items():
            self._publish_vehicle_status(vehicle)
    
    def _publish_vehicle_status(self, vehicle: mvv.VehicleView):
        """
        發布單個載具的所有狀態
        
        Args:
            vehicle: VehicleView 實例
        """
        sysid = vehicle.sysid
        
        # 假設只有一個 autopilot component (component_id=1)
        component = vehicle.get_component(1)
        if not component:
            return
        
        status = component.status
        
        # ═══════════════════════════════════════════════════════════════
        # 【新增 Topic 位置 2/4】
        # 若要新增 topic，請在此處調用對應的發布方法
        # 例如：self._publish_ekf_status(sysid, status)
        # ═══════════════════════════════════════════════════════════════
        # 發布各種狀態（通過頻率控制器判斷是否發布）
        self._publish_position(sysid, status)
        self._publish_attitude(sysid, status)
        self._publish_velocity(sysid, status)
        self._publish_battery(sysid, status)
        self._publish_vfr_hud(sysid, status)
        self._publish_mode(sysid, status)
        self._publish_summary(vehicle)
        # 在這裡新增更多 publish 方法調用...
    
    def _get_or_create_publisher(self, sysid: int, topic: str, msg_type, qos: int = 1):
        """
        獲取或創建 publisher
        
        Args:
            sysid: 系統ID
            topic: topic 名稱
            msg_type: ROS2 消息類型
            qos: QoS 設置
            
        Returns:
            publisher 對象
        """
        key = (sysid, topic)
        if key not in self.fc_publishers:
            topic_name = f'{self.topicString_prefix}/sys{sysid}/{topic}'
            publisher = self.create_publisher(msg_type, topic_name, qos)
            self.fc_publishers[key] = publisher
            logger.info(f"Created publisher: {topic_name}")
        return self.fc_publishers[key]
    
    def _publish_position(self, sysid: int, status: mvv.ComponentStatus):
        """發布 GPS 位置"""
        if not self.rate_controller.should_publish(sysid, 'position'):
            return
        
        pos = status.position
        if pos.latitude is None or pos.longitude is None:
            return
        
        publisher = self._get_or_create_publisher(sysid, 'position', sensor_msgs.msg.NavSatFix)
        
        # 檢查是否有訂閱者
        if publisher.get_subscription_count() == 0:
            return
        
        msg = sensor_msgs.msg.NavSatFix()
        msg.latitude = pos.latitude
        msg.longitude = pos.longitude
        msg.altitude = pos.altitude if pos.altitude is not None else 0.0
        
        # GPS 狀態資訊
        gps = status.gps
        if gps.fix_type is not None:
            msg.status.status = gps.fix_type - 1  # MAVLink fix_type 轉 NavSatStatus
        
        publisher.publish(msg)
    
    def _publish_attitude(self, sysid: int, status: mvv.ComponentStatus):
        """發布姿態（IMU）"""
        if not self.rate_controller.should_publish(sysid, 'attitude'):
            return
        
        att = status.attitude
        if att.roll is None:
            return
        
        publisher = self._get_or_create_publisher(sysid, 'attitude', sensor_msgs.msg.Imu)
        
        if publisher.get_subscription_count() == 0:
            return
        
        msg = sensor_msgs.msg.Imu()
        
        # 歐拉角轉四元數
        qx, qy, qz, qw = self._euler_to_quaternion(
            att.roll, att.pitch, att.yaw
        )
        msg.orientation.x = qx
        msg.orientation.y = qy
        msg.orientation.z = qz
        msg.orientation.w = qw
        
        # 角速度
        if att.rollspeed is not None:
            msg.angular_velocity.x = att.rollspeed
            msg.angular_velocity.y = att.pitchspeed
            msg.angular_velocity.z = att.yawspeed
        
        publisher.publish(msg)
    
    def _publish_velocity(self, sysid: int, status: mvv.ComponentStatus):
        """發布速度"""
        if not self.rate_controller.should_publish(sysid, 'velocity'):
            return
        
        vfr = status.vfr
        if vfr.groundspeed is None:
            return
        
        publisher = self._get_or_create_publisher(sysid, 'velocity', geometry_msgs.msg.TwistStamped)
        
        if publisher.get_subscription_count() == 0:
            return
        
        msg = geometry_msgs.msg.TwistStamped()
        msg.header.stamp = self.get_clock().now().to_msg()
        
        # 使用 VFR HUD 的地速和航向計算速度分量
        if vfr.heading is not None:
            heading_rad = math.radians(vfr.heading)
            msg.twist.linear.x = vfr.groundspeed * math.cos(heading_rad)
            msg.twist.linear.y = vfr.groundspeed * math.sin(heading_rad)
        
        # 爬升率作為 z 軸速度
        if vfr.climb is not None:
            msg.twist.linear.z = vfr.climb
        
        publisher.publish(msg)
    
    def _publish_battery(self, sysid: int, status: mvv.ComponentStatus):
        """發布電池狀態"""
        if not self.rate_controller.should_publish(sysid, 'battery'):
            return
        
        bat = status.battery
        if bat.voltage is None:
            return
        
        publisher = self._get_or_create_publisher(sysid, 'battery', sensor_msgs.msg.BatteryState)
        
        if publisher.get_subscription_count() == 0:
            return
        
        msg = sensor_msgs.msg.BatteryState()
        msg.voltage = bat.voltage
        
        if bat.current is not None:
            msg.current = bat.current
        
        if bat.remaining is not None:
            msg.percentage = bat.remaining / 100.0
        
        if bat.temperature is not None:
            msg.temperature = bat.temperature
        
        publisher.publish(msg)
    
    def _publish_vfr_hud(self, sysid: int, status: mvv.ComponentStatus):
        """發布 VFR HUD"""
        if not self.rate_controller.should_publish(sysid, 'vfr_hud'):
            return
        
        vfr = status.vfr
        if vfr.airspeed is None:
            return
        
        publisher = self._get_or_create_publisher(sysid, 'vfr_hud', mavros_msgs.msg.VfrHud)
        
        if publisher.get_subscription_count() == 0:
            return
        
        msg = mavros_msgs.msg.VfrHud()
        msg.airspeed = vfr.airspeed if vfr.airspeed is not None else 0.0
        msg.groundspeed = vfr.groundspeed if vfr.groundspeed is not None else 0.0
        msg.heading = vfr.heading if vfr.heading is not None else 0
        msg.throttle = float(vfr.throttle) if vfr.throttle is not None else 0.0
        msg.climb = vfr.climb if vfr.climb is not None else 0.0
        
        # altitude 需要從 position 獲取
        if status.position.altitude is not None:
            msg.altitude = status.position.altitude
        
        publisher.publish(msg)
    
    def _publish_mode(self, sysid: int, status: mvv.ComponentStatus):
        """發布飛行模式"""
        if not self.rate_controller.should_publish(sysid, 'mode'):
            return
        
        mode = status.mode
        if mode.mode_name is None:
            return
        
        publisher = self._get_or_create_publisher(sysid, 'mode', std_msgs.msg.String)
        
        if publisher.get_subscription_count() == 0:
            return
        
        msg = std_msgs.msg.String()
        msg.data = mode.mode_name
        publisher.publish(msg)
    
    def _publish_summary(self, vehicle: mvv.VehicleView):
        """
        發布載具摘要資訊（自定義格式，使用 String 暫時代替）
        TODO: 未來可以定義專門的 VehicleSummary.msg
        """
        sysid = vehicle.sysid
        
        if not self.rate_controller.should_publish(sysid, 'summary'):
            return
        
        publisher = self._get_or_create_publisher(sysid, 'summary', std_msgs.msg.String)
        
        if publisher.get_subscription_count() == 0:
            return
        
        # 獲取 autopilot component
        component = vehicle.get_component(1)
        if not component:
            return
        
        status = component.status
        
        # 構建摘要資訊（JSON 格式字串）
        import json
        summary = {
            'sysid': sysid,
            'vehicle_type': vehicle.vehicle_type if vehicle.vehicle_type else 0,
            'autopilot': component.mav_autopilot if component.mav_autopilot else 0,
            'socket_id': vehicle.custom_meta.get('socket_id', -1),  # 重要！
            'armed': status.armed if status.armed is not None else False,
            # 'mode_custom': status.mode.custom_mode if status.mode.custom_mode else 0,
            'mode_name': status.mode.mode_name if status.mode.mode_name else "UNKNOWN",
            # 'latitude': status.position.latitude if status.position.latitude else 0.0,
            # 'longitude': status.position.longitude if status.position.longitude else 0.0,
            # 'altitude': status.position.altitude if status.position.altitude else 0.0,
            # 'battery_percent': status.battery.remaining if status.battery.remaining else 0,
            'gps_fix': status.gps.fix_type if status.gps.fix_type else 0,
            'connection_type': vehicle.connected_via.value,
            'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0,
        }
        
        msg = std_msgs.msg.String()
        msg.data = json.dumps(summary)
        publisher.publish(msg)
    
    # ═══════════════════════════════════════════════════════════════
    # 【新增 Topic 位置 3/4】
    # 若要新增 topic，請在此處實作對應的發布方法
    # 方法命名規則：def _publish_<topic_name>(self, sysid: int, status: mvv.ComponentStatus):
    # 例如：
    # def _publish_ekf_status(self, sysid: int, status: mvv.ComponentStatus):
    #     """發布 EKF 狀態"""
    #     if not self.rate_controller.should_publish(sysid, 'ekf_status'):
    #         return
    #     
    #     ekf = status.ekf
    #     if ekf.flags is None:
    #         return
    #     
    #     publisher = self._get_or_create_publisher(sysid, 'ekf_status', ...
    #     # ... 實作發布邏輯
    # ═══════════════════════════════════════════════════════════════
    
    @staticmethod
    def _euler_to_quaternion(roll, pitch, yaw):
        """
        歐拉角轉四元數
        
        Args:
            roll: 橫滾角 (弧度)
            pitch: 俯仰角 (弧度)
            yaw: 偏航角 (弧度)
            
        Returns:
            tuple: (qx, qy, qz, qw)
        """
        qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - \
             math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
        qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + \
             math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2)
        qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - \
             math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2)
        qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + \
             math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2)
        return (qx, qy, qz, qw)
    
    def stop(self):
        """停止發布"""
        self.running = False
        # logger.info("VehicleStatusPublisher stopped")


# ============================================================================
#                       MavlinkCommandService Node
# ============================================================================

class MavlinkCommandService(Node):
    """
    MAVLink 指令服務節點 - 提供 ROS2 service 介面來發送 MAVLink 指令
    
    職責:
    - 作為 service server，等待 client 請求
    - 接收請求，組裝 MAVLink 封包
    - 調用 mavlinkObject 發送封包
    - 處理 ACK 等待和超時（未來實現）
    
    設計理念：回歸 MAVLink 純粹結構
    - 只負責將 ROS2 請求轉換為 MAVLink 封包
    - 不預設功能（如 ARM/DISARM），保持模組化
    - 高層應用可透過此 service 實現各種功能
    """
    
    def __init__(self):
        super().__init__('mavlink_command_service')
        
        # ═══════════════════════════════════════════════════════════════════
        # ROS2 Service 架構說明：
        # 
        # 1. Service 定義：由 .srv 檔案定義（Request + Response）
        #    - Request: client 發送的請求內容
        #    - Response: server 回傳的結果
        # 
        # 2. Service Server 創建：
        #    self.create_service(srv_type, service_name, callback_function)
        #    - srv_type: service 的訊息類型（需要自定義或使用標準）
        #    - service_name: service 的名稱（client 用此名稱呼叫）
        #    - callback_function: 處理請求的回調函數
        # 
        # 3. Callback 函數：
        #    def callback(self, request, response):
        #        # request: 包含 client 發送的數據
        #        # response: 需要填充並返回給 client
        #        return response
        # 
        # 4. Service Client 使用方式（在其他程式中）：
        #    client = node.create_client(srv_type, service_name)
        #    request = srv_type.Request()
        #    future = client.call_async(request)  # 異步調用
        #    # 或 response = client.call(request)  # 同步調用
        # ═══════════════════════════════════════════════════════════════════
        
        # 由於 ROS2 自定義 service 需要 .srv 檔案編譯
        # 這裡先使用標準 String service 作為簡化實現
        # TODO: 未來可創建專門的 .srv 檔案
        from std_srvs.srv import Trigger
        from example_interfaces.srv import SetBool
        
        # ═══════════════════════════════════════════════════════════════════
        # Service 1: 發送 MAVLink Message（通用介面）
        # 使用 Trigger 作為臨時實現，未來應使用自定義 service
        # ═══════════════════════════════════════════════════════════════════
        # TODO: 創建 SendMavlinkMessage.srv
        # Request:
        #   uint8 target_sysid
        #   uint8 target_compid
        #   uint16 message_id
        #   string fields_json  # JSON 格式的字段數據
        #   bool wait_response
        #   uint16 response_msgid
        #   float32 timeout
        # Response:
        #   bool success
        #   string response_json
        #   string error_message
        
        # 暫時使用簡化版本（僅示範架構）
        self.srv_send_message = self.create_service(
            Trigger,
            '/mavlink/send_message',
            self.handle_send_message
        )
        
        # ═══════════════════════════════════════════════════════════════════
        # Service 2: 發送 COMMAND_LONG
        # ═══════════════════════════════════════════════════════════════════
        self.srv_command_long = self.create_service(
            Trigger,
            '/mavlink/send_command_long',
            self.handle_command_long
        )
        
        # ═══════════════════════════════════════════════════════════════════
        # Service 3: 參數請求
        # ═══════════════════════════════════════════════════════════════════
        self.srv_param_request = self.create_service(
            Trigger,
            '/mavlink/param_request',
            self.handle_param_request
        )
        
        # 狀態標記
        self.running = True
        
        # mavlinkObject 的引用（將由外部設置）
        self.mavlink_analyzer = None
        
        logger.info("MavlinkCommandService initialized")
    
    def set_mavlink_analyzer(self, mavlink_analyzer):
        """
        設置 mavlink_analyzer 引用
        
        Args:
            mavlink_analyzer: mavlinkObject.mavlink_analyzer 實例
        """
        self.mavlink_analyzer = mavlink_analyzer
        logger.info("MavlinkCommandService: mavlink_analyzer set")
    
    # ═══════════════════════════════════════════════════════════════════════
    # Service Handler: 發送 MAVLink Message
    # ═══════════════════════════════════════════════════════════════════════
    def handle_send_message(self, request, response):
        """
        處理發送 MAVLink 訊息的請求
        
        ROS2 Service Callback 說明：
        - 此函數會在 client 調用 service 時被執行
        - request: 包含 client 傳入的參數
        - response: 需要填充結果並返回給 client
        - 必須 return response
        
        Args:
            request: Trigger.Request (暫時使用，未來改為自定義)
            response: Trigger.Response
            
        Returns:
            response: 填充後的回應
        """
        logger.info("Received send_message request")
        
        # 檢查 mavlink_analyzer 是否已設置
        if self.mavlink_analyzer is None:
            response.success = False
            response.message = "Error: mavlink_analyzer not set"
            logger.error(response.message)
            return response
        
        # TODO: 實際實現
        # 1. 從 request 解析參數（target_sysid, message_id, fields 等）
        # 2. 使用 pymavlink 組裝 MAVLink 封包
        # 3. 調用 mavlink_analyzer.send_message() 發送
        # 4. 如果 wait_response=True，則等待 return_packet_ring 中的回應
        
        # 暫時返回成功（示範用）
        response.success = True
        response.message = "Message sent (placeholder implementation)"
        return response
    
    # ═══════════════════════════════════════════════════════════════════════
    # Service Handler: 發送 COMMAND_LONG
    # ═══════════════════════════════════════════════════════════════════════
    def handle_command_long(self, request, response):
        """
        處理發送 COMMAND_LONG 的請求
        
        COMMAND_LONG (MAVLink message ID=76):
        - 用於發送簡單命令給載具
        - 常用於 ARM/DISARM, 模式切換, TAKEOFF, LAND 等
        
        Args:
            request: Trigger.Request
            response: Trigger.Response
            
        Returns:
            response: 填充後的回應
        """
        logger.info("Received command_long request")
        
        if self.mavlink_analyzer is None:
            response.success = False
            response.message = "Error: mavlink_analyzer not set"
            return response
        
        # TODO: 實際實現
        # 1. 從 request 解析 COMMAND_LONG 參數
        #    - target_sysid, target_compid
        #    - command (MAV_CMD_xxx)
        #    - param1~param7
        # 2. 組裝 COMMAND_LONG 封包
        # 3. 發送並等待 COMMAND_ACK (message ID=77)
        # 4. 解析 ACK 結果（ACCEPTED/FAILED 等）
        
        response.success = True
        response.message = "Command sent (placeholder implementation)"
        return response
    
    # ═══════════════════════════════════════════════════════════════════════
    # Service Handler: 參數請求
    # ═══════════════════════════════════════════════════════════════════════
    def handle_param_request(self, request, response):
        """
        處理參數讀取請求
        
        MAVLink 參數協議：
        - PARAM_REQUEST_READ (ID=20): 請求讀取參數
        - PARAM_VALUE (ID=22): 參數值回應
        - PARAM_SET (ID=23): 設置參數值
        
        ═══════════════════════════════════════════════════════════════════
        【使用 mavlinkObject 回應機制的步驟】
        
        1. 設置回應訊息類型：
           self.mavlink_analyzer.set_return_message_types([22])  # PARAM_VALUE
           
        2. 發送請求封包：
           message_bytes = ... # 組裝 PARAM_REQUEST_READ
           self.mavlink_analyzer.send_message(
               message_bytes,
               target_sysid=1
           )
           
        3. 監聽回應（在獨立線程或定時器中）：
           from ..fc_network_adapter import mavlinkObject as mo
           
           # 等待回應（帶超時）
           timeout = 3.0
           start_time = time.time()
           while time.time() - start_time < timeout:
               items = mo.return_packet_ring.get_all()
               for socket_id, timestamp, msg in items:
                   if msg.get_type() == 'PARAM_VALUE':
                       # 找到回應！
                       param_id = msg.param_id
                       param_value = msg.param_value
                       # 處理回應...
                       return
               time.sleep(0.01)  # 短暫等待
           
           # 超時處理
           
        4. 清理（可選）：
           self.mavlink_analyzer.set_return_message_types([])  # 清空
           mo.return_packet_ring.clear()  # 清空緩衝區
        
        注意事項：
        - return_packet_ring 是全域的，所有 mavlink_object 共用
        - 需要通過 socket_id 或 sysid 來識別回應來源
        - 實際使用時建議實現專門的回應管理器
        ═══════════════════════════════════════════════════════════════════
        
        Args:
            request: Trigger.Request
            response: Trigger.Response
            
        Returns:
            response: 填充後的回應
        """
        logger.info("Received param_request")
        
        if self.mavlink_analyzer is None:
            response.success = False
            response.message = "Error: mavlink_analyzer not set"
            return response
        
        # TODO: 實際實現
        # 1. 從 request 解析參數名稱或索引
        # 2. 設置 mavlink_analyzer.set_return_message_types([22])  # PARAM_VALUE
        # 3. 發送 PARAM_REQUEST_READ
        # 4. 監聽 return_packet_ring，等待 PARAM_VALUE
        # 5. 解析回應並填充到 response
        
        response.success = True
        response.message = "Param request sent (placeholder implementation)"
        return response
    
    # ═══════════════════════════════════════════════════════════════════════
    # 【新增 Service 位置 4/4】
    # 若要新增 service，請在此處添加新的 handler 方法
    # 並在 __init__ 中創建對應的 service server
    # ═══════════════════════════════════════════════════════════════════════
    
    def stop(self):
        """停止服務"""
        self.running = False
        logger.info("MavlinkCommandService stopped")


# ============================================================================
#                         ROS2 節點管理器
# ============================================================================

class fc_ros_manager:
    """
    MAVLink ROS2 節點管理器
    
    管理兩個獨立的 ROS2 Node：
    - VehicleStatusPublisher
    - MavlinkCommandService
    
    提供統一的啟動/停止介面給 mainOrchestrator
    """
    
    def __init__(self):
        self.initialized = False
        self.running = False
        
        # 两个 node 实例
        self.status_publisher: Optional[VehicleStatusPublisher] = None
        self.command_service: Optional[MavlinkCommandService] = None
        
        # Executor & Thread
        self.spin_thread: Optional[threading.Thread] = None
        self.executor: Optional[MultiThreadedExecutor] = None
    
    def initialize(self):
        """初始化 ROS2 环境和 nodes"""
        if self.initialized:
            logger.warning("fc_ros_manager already initialized")
            return False
        
        try:
            # 初始化 ROS2
            rclpy.init()
            
            # 創建節點 node
            self.status_publisher = VehicleStatusPublisher()
            self.command_service = MavlinkCommandService()
    
            # 創建執行者 MultiThreadedExecutor
            self.executor = MultiThreadedExecutor()
            self.executor.add_node(self.status_publisher)
            self.executor.add_node(self.command_service)
            
            self.initialized = True
            # logger.info("fc_ros_manager initialized")
            return True
            
        except Exception as e:
            logger.error(f"Failed to initialize fc_ros_manager: {e}")
            return False
    
    def start(self):
        """啟動 ROS2 nodes (在獨立執行緒中運行 executor) """
        if not self.initialized:
            logger.error("fc_ros_manager initialize failed or not called")
            return False
        
        if self.running:
            logger.warning("fc_ros_manager already running")
            return False
        
        try:
            self.running = True
            
            self.spin_thread = threading.Thread(
                target=self._spin_executor,
                daemon=True,
                name="ROS2ExecutorThread"
            )
            self.spin_thread.start()
            
            logger.info("fc_ros_manager started <-")
            return True
            
        except Exception as e:
            logger.error(f"Failed to start fc_ros_manager: {e}")
            self.running = False
            return False
    
    def _spin_executor(self):
        """在 thread 中運行的 executor"""
        try:
            # logger.info("ROS2 executor spinning...")
            while self.running:
                self.executor.spin_once(timeout_sec=0.1)
        except Exception as e:
            logger.error(f"fc_ros_manager error in spinning executor: {e}")
    
    def stop(self):
        """停止 ROS2 nodes"""
        if not self.running:
            logger.warning("fc_ros_manager not running")
            return False
        
        try:            
            # 標記停止
            self.running = False
            
            # 停止各個 node
            if self.status_publisher:
                self.status_publisher.stop()
            if self.command_service:
                self.command_service.stop()
            
            # 等待 spin 執行緒結束
            if self.spin_thread and self.spin_thread.is_alive():
                self.spin_thread.join(timeout=2.0)
            
            logger.info("fc_ros_manager thread END!")
            return True
            
        except Exception as e:
            logger.error(f"Error stopping fc_ros_manager: {e}")
            return False
    
    def shutdown(self):
        """完全關閉並清理資源"""
        if self.running:
            self.stop()
        
        if self.initialized:
            try:
                # 銷毀 nodes
                if self.status_publisher:
                    self.status_publisher.destroy_node()
                if self.command_service:
                    self.command_service.destroy_node()
                
                # 關閉 ROS2
                if rclpy.ok():
                    rclpy.shutdown()
                
                self.initialized = False
                logger.info("fc_ros_manager Node END!")
                
            except Exception as e:
                logger.error(f"Error during shutdown: {e}")
    
    def get_status(self) -> dict:
        return {
            'initialized': self.initialized,
            'running': self.running,
            'status_publisher_active': self.status_publisher is not None and self.status_publisher.running,
            'command_service_active': self.command_service is not None,
        }


# ============================================================================
#                              全域實例
# ============================================================================

# 全域管理器實例（供 mainOrchestrator 使用）
ros2_manager = fc_ros_manager()


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

2026.01.20
1. 重構自 mavlinkPublish.py (該檔案將被棄用)
2. 提供 fc_ros_manager 統一管理介面
3. 實現 VehicleStatusPublisher - 從 vehicle_registry 讀取並發布狀態
4. 添加頻率控制器 控制各 topic 發布頻率 以及是否發布
5. 預留 MavlinkCommandService 結構（稍後實現）

'''
