#!/usr/bin/env python3
"""
指令發送模組
負責將目標位置轉成具體的通訊指令發送到飛控

現階段: MavlinkSender (直接 pymavlink 發送)
未來:   替換為 ROS2Sender (發到 ROS2 topic，由 fc_network_adapter 轉發)
"""
from abc import ABC, abstractmethod
from pymavlink import mavutil


class CommandSender(ABC):
    """指令發送抽象介面"""

    @abstractmethod
    def send_position_global(self, sysid, lat, lon, alt):
        """
        發送全球座標位置指令

        Args:
            sysid: 目標無人機的 MAVLink system ID
            lat: 緯度 (度)
            lon: 經度 (度)
            alt: 高度 (公尺)
        """
        pass

    @abstractmethod
    def close(self):
        """關閉連線"""
        pass


class MavlinkSender(CommandSender):
    """
    MAVLink 直接發送 (驗證階段用)
    透過 SET_POSITION_TARGET_GLOBAL_INT 發送目標位置

    使用方式:
        sender = MavlinkSender("udpout:127.0.0.1:14550")
        sender.send_position_global(sysid=1, lat=24.123, lon=120.567, alt=10.0)
    """

    # type_mask: 只使用位置 (lat, lon, alt)
    # 忽略速度 (bit 3,4,5)、加速度 (bit 6,7,8)、yaw (bit 10)、yaw_rate (bit 11)
    TYPE_MASK_POSITION_ONLY = (
        0b0000_1101_1111_1000  # = 0x0DF8
    )

    def __init__(self, connection_string="udpout:127.0.0.1:14550"):
        """
        Args:
            connection_string: MAVLink 連線字串
                SITL 範例: "udpout:127.0.0.1:14550"
        """
        self.connection_string = connection_string
        self.mav = mavutil.mavlink_connection(connection_string)
        print(f"MavlinkSender 已建立連線: {connection_string}")

    def send_position_global(self, sysid, lat, lon, alt):
        """
        發送 SET_POSITION_TARGET_GLOBAL_INT

        注意:
            - coordinate_frame 使用 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
              高度是相對起飛點的高度 (公尺)
            - 如果 FormationPlanner 產出的 alt 是 AMSL 絕對高度，
              需要在外部先減去起飛點高度再傳入
        """
        self.mav.mav.set_position_target_global_int_send(
            0,                          # time_boot_ms (不使用)
            sysid,                      # target_system
            1,                          # target_component (autopilot)
            mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
            self.TYPE_MASK_POSITION_ONLY,
            int(lat * 1e7),             # lat_int
            int(lon * 1e7),             # lon_int
            float(alt),                 # alt
            0, 0, 0,                    # vx, vy, vz (忽略)
            0, 0, 0,                    # afx, afy, afz (忽略)
            0, 0                        # yaw, yaw_rate (忽略)
        )

    def close(self):
        """關閉 MAVLink 連線"""
        if self.mav:
            self.mav.close()
            self.mav = None
            print("MavlinkSender 已關閉")