"""
MavlinkCommandService 使用範例

展示如何從其他 ROS2 節點調用 MAVLink 指令服務
"""

import rclpy
from rclpy.node import Node
from std_srvs.srv import Trigger
import time


class MavlinkClientExample(Node):
    """
    範例：MAVLink Service Client
    
    這個節點展示如何調用 MavlinkCommandService 提供的服務
    """
    
    def __init__(self):
        super().__init__('mavlink_client_example')
        
        # 創建 service client
        self.client = self.create_client(
            Trigger,
            '/mavlink/send_command_long'
        )
        
        # 等待服務可用
        self.get_logger().info('等待 service 可用...')
        self.client.wait_for_service()
        self.get_logger().info('Service 已連接！')
    
    def send_arm_command(self):
        """
        範例：發送 ARM 指令
        
        實際使用時應該發送：
        - message_id = 76 (COMMAND_LONG)
        - command = 400 (MAV_CMD_COMPONENT_ARM_DISARM)
        - param1 = 1 (ARM)
        """
        self.get_logger().info('發送 ARM 指令...')
        
        request = Trigger.Request()
        # TODO: 實際使用時應該是自定義的 SendCommandLong.Request
        # request.target_sysid = 1
        # request.target_compid = 1
        # request.command = 400  # MAV_CMD_COMPONENT_ARM_DISARM
        # request.param1 = 1.0   # 1 = ARM, 0 = DISARM
        # request.param2 = 0.0
        # ... param3-7
        # request.timeout = 3.0
        
        # 異步調用
        future = self.client.call_async(request)
        
        # 等待結果
        rclpy.spin_until_future_complete(self, future)
        
        if future.done():
            response = future.result()
            if response.success:
                self.get_logger().info('✓ ARM 指令發送成功')
            else:
                self.get_logger().error(f'✗ ARM 指令失敗: {response.message}')
        else:
            self.get_logger().error('✗ Service 調用超時')


def main():
    """主函數"""
    rclpy.init()
    
    # 創建客戶端節點
    client = MavlinkClientExample()
    
    # 發送指令
    client.send_arm_command()
    
    # 清理
    client.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()


"""
═══════════════════════════════════════════════════════════════════════════
進階使用範例：高層應用程式

假設我們要創建一個 "任務控制器"，通過 MavlinkCommandService 來控制載具：

```python
class MissionController(Node):
    def __init__(self):
        super().__init__('mission_controller')
        
        # 創建各種 service clients
        self.client_arm = self.create_client(
            SendCommandLong, '/mavlink/send_command_long')
        self.client_mode = self.create_client(
            SendCommandLong, '/mavlink/send_command_long')
        self.client_takeoff = self.create_client(
            SendCommandLong, '/mavlink/send_command_long')
        self.client_goto = self.create_client(
            SendCommandInt, '/mavlink/send_command_int')
    
    def arm_vehicle(self, sysid=1):
        \"\"\"解鎖載具\"\"\"
        request = SendCommandLong.Request()
        request.target_sysid = sysid
        request.target_compid = 1
        request.command = 400  # MAV_CMD_COMPONENT_ARM_DISARM
        request.param1 = 1.0   # ARM
        
        future = self.client_arm.call_async(request)
        # 處理回應...
    
    def set_mode(self, sysid=1, mode='GUIDED'):
        \"\"\"設置飛行模式\"\"\"
        # 實現模式切換邏輯...
        pass
    
    def takeoff(self, sysid=1, altitude=10.0):
        \"\"\"起飛\"\"\"
        request = SendCommandLong.Request()
        request.target_sysid = sysid
        request.target_compid = 1
        request.command = 22  # MAV_CMD_NAV_TAKEOFF
        request.param7 = altitude
        
        future = self.client_takeoff.call_async(request)
        # 處理回應...
    
    def goto_position(self, sysid=1, lat=0, lon=0, alt=10):
        \"\"\"前往指定位置\"\"\"
        request = SendCommandInt.Request()
        request.target_sysid = sysid
        request.target_compid = 1
        request.command = 192  # MAV_CMD_DO_REPOSITION
        request.x = int(lat * 1e7)
        request.y = int(lon * 1e7)
        request.z = alt
        
        future = self.client_goto.call_async(request)
        # 處理回應...
    
    def execute_mission(self):
        \"\"\"執行完整任務\"\"\"
        # 1. 解鎖
        self.arm_vehicle()
        time.sleep(1)
        
        # 2. 切換到 GUIDED 模式
        self.set_mode(mode='GUIDED')
        time.sleep(1)
        
        # 3. 起飛到 10 公尺
        self.takeoff(altitude=10.0)
        time.sleep(10)
        
        # 4. 前往目標點
        self.goto_position(lat=25.033, lon=121.565, alt=10)
        time.sleep(30)
        
        # 5. 返回並降落
        # ...
```

這樣的設計讓高層應用可以：
1. 完全不需要知道 MAVLink 協議細節
2. 通過 ROS2 service 與載具互動
3. 模組化開發不同功能
4. 易於測試和維護

═══════════════════════════════════════════════════════════════════════════
"""