AirTrapMine/src/GUI/mission_planner.py

#!/usr/bin/env python3
"""
飛行任務規劃模組
支援: M-Formation, Circle, Leader-Follower (arc-length virtual leader), Grid Sweep
"""
import math
from typing import List, Tuple, Optional, Dict, Any
from enum import Enum


class MissionType(Enum):
    """任務類型"""
    M_FORMATION = "m_formation"
    CIRCLE_FORMATION = "circle_formation"
    LEADER_FOLLOWER = "leader_follower"
    GRID_SWEEP = "grid_sweep"


class CoordinateConverter:
    """GPS 座標與 Local 座標的轉換器"""

    def __init__(self, origin_lat: float, origin_lon: float, origin_alt: float = 0):
        self.origin_lat = origin_lat
        self.origin_lon = origin_lon
        self.origin_alt = origin_alt
        self.R = 6371000.0

    def gps_to_local(self, lat: float, lon: float, alt: float) -> Tuple[float, float, float]:
        lat_rad = math.radians(lat)
        lon_rad = math.radians(lon)
        origin_lat_rad = math.radians(self.origin_lat)
        origin_lon_rad = math.radians(self.origin_lon)

        dlat = lat_rad - origin_lat_rad
        dlon = lon_rad - origin_lon_rad

        x = self.R * dlon * math.cos((lat_rad + origin_lat_rad) / 2)
        y = self.R * dlat
        z = alt - self.origin_alt

        return x, y, z

    def local_to_gps(self, x: float, y: float, z: float) -> Tuple[float, float, float]:
        origin_lat_rad = math.radians(self.origin_lat)
        origin_lon_rad = math.radians(self.origin_lon)

        lat_rad = origin_lat_rad + (y / self.R)
        lon_rad = origin_lon_rad + (x / (self.R * math.cos((lat_rad + origin_lat_rad) / 2)))

        lat = math.degrees(lat_rad)
        lon = math.degrees(lon_rad)
        alt = self.origin_alt + z

        return lat, lon, alt


class FormationPlanner:
    """隊形規劃器"""

    def __init__(self, spacing: float = 5.0,
                 base_altitude: float = 10.0,
                 altitude_diff: float = 2.0):
        self.spacing = spacing
        self.base_altitude = base_altitude
        self.altitude_diff = altitude_diff
        self.current_origin = None
        self.converter = None

    def plan_formation_mission(self,
                               drone_gps_positions: List[Tuple[float, float, float]],
                               target_gps: Tuple[float, float, float],
                               mission_type: MissionType = MissionType.M_FORMATION,
                               params: Optional[Dict[str, Any]] = None
                               ) -> Tuple[List[List[Tuple[float, float, float]]],
                                          Tuple[float, float, float],
                                          List[int]]:
        """
        回傳 (waypoints_gps, origin, rendezvous_indices)。
        rendezvous_indices：航點序列裡該群組需要等全員到齊才推進的 index 清單。
        """
        if len(drone_gps_positions) == 0:
            raise ValueError("無人機位置列表不能為空")

        center_lat = sum(pos[0] for pos in drone_gps_positions) / len(drone_gps_positions)
        center_lon = sum(pos[1] for pos in drone_gps_positions) / len(drone_gps_positions)

        # origin_alt = 0：送給飛控的高度透過 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
        # 直接就是「相對 home 高度」，不需要加 GPS 橢球高度偏移
        self.current_origin = (center_lat, center_lon, 0.0)
        self.converter = CoordinateConverter(center_lat, center_lon, 0.0)

        drone_local = [self.converter.gps_to_local(*pos) for pos in drone_gps_positions]
        target_local = self.converter.gps_to_local(*target_gps)

        if mission_type == MissionType.M_FORMATION:
            s1, s2 = self._calculate_m_formation(drone_local, target_local, params)
            waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
            # 起點集合後一起出發到 stage2
            rendezvous_indices = [0]

        elif mission_type == MissionType.CIRCLE_FORMATION:
            s1, s2 = self._calculate_circle_formation(drone_local, target_local, params)
            waypoints_local = [[s1[i], s2[i]] for i in range(len(drone_local))]
            # 半程集合後一起進最終圓環位置
            rendezvous_indices = [0]

        elif mission_type == MissionType.LEADER_FOLLOWER:
            params = params or {}
            route_wps_gps = params.get('route_waypoints')
            if route_wps_gps is None or len(route_wps_gps) < 2:
                raise ValueError("LEADER_FOLLOWER 需要至少 2 個路徑點")
            route_wps_local = [
                self.converter.gps_to_local(wp[0], wp[1], 0)[:2]
                for wp in route_wps_gps
            ]
            waypoints_local, rendezvous_indices = self._calculate_leader_follower(
                drone_local, route_wps_local, params
            )

        elif mission_type == MissionType.GRID_SWEEP:
            params = params or {}
            rect_corners_gps = params.get('rect_corners')
            if rect_corners_gps is None or len(rect_corners_gps) != 4:
                raise ValueError("GRID_SWEEP 需要 4 個 GPS 角點")
            rect_corners_local = [
                self.converter.gps_to_local(c[0], c[1], 0)[:2]
                for c in rect_corners_gps
            ]
            waypoints_local, rendezvous_indices = self._calculate_grid_sweep(
                drone_local, rect_corners_local, params
            )
        else:
            raise ValueError(f"不支援的任務類型: {mission_type}")

        waypoints_gps = []
        for drone_wps in waypoints_local:
            gps_wps = [self.converter.local_to_gps(*wp) for wp in drone_wps]
            waypoints_gps.append(gps_wps)

        return waypoints_gps, self.current_origin, rendezvous_indices

    # ------------------------------------------------------------------ M-Formation

    def _calculate_m_formation(self, drone_positions, target_point, params):
        params = params or {}
        N = len(drone_positions)
        spacing = params.get('spacing', self.spacing)
        base_altitude = params.get('base_altitude', self.base_altitude)
        altitude_diff = params.get('altitude_diff', self.altitude_diff)

        C_x = sum(pos[0] for pos in drone_positions) / N
        C_y = sum(pos[1] for pos in drone_positions) / N

        T_x, T_y, T_z = target_point
        V_x, V_y = T_x - C_x, T_y - C_y
        P_x, P_y = -V_y, V_x

        length = math.sqrt(P_x ** 2 + P_y ** 2)
        P_x_unit, P_y_unit = (P_x / length, P_y / length) if length > 0.01 else (1.0, 0.0)

        projections = [((pos[0] - C_x) * P_x_unit + (pos[1] - C_y) * P_y_unit, i)
                       for i, pos in enumerate(drone_positions)]
        projections.sort()

        stage1_positions = [None] * N
        stage2_positions = [None] * N

        for rank, (_, original_idx) in enumerate(projections):
            offset = (rank - (N - 1) / 2) * spacing
            altitude = base_altitude + (altitude_diff if rank % 2 == 0 else -altitude_diff)
            stage1_positions[original_idx] = (C_x + P_x_unit * offset, C_y + P_y_unit * offset, altitude)
            stage2_positions[original_idx] = (T_x + P_x_unit * offset, T_y + P_y_unit * offset, altitude)

        return stage1_positions, stage2_positions

    # ------------------------------------------------------------------ Circle

    def _calculate_circle_formation(self, drone_positions, target_point, params):
        params = params or {}
        N = len(drone_positions)
        radius = params.get('radius', 10.0)
        altitude = params.get('altitude', 10.0)
        start_angle = params.get('start_angle', 0.0)

        center_x, center_y, center_z = target_point
        stage1_positions = []
        stage2_positions = []
        angle_step = 360.0 / N

        for i in range(N):
            angle_rad = math.radians(start_angle + angle_step * i)
            final_x = center_x + radius * math.cos(angle_rad)
            final_y = center_y + radius * math.sin(angle_rad)
            final_z = altitude

            current_x, current_y, _current_z = drone_positions[i]
            # stage1 高度直接用 final_z：先爬升到目標高度再水平集合，
            # 避免用 current_z 做中點插值（current_z 在實飛時可能是 AMSL 而非相對 home，會導致衝高）
            stage1_positions.append((
                current_x + (final_x - current_x) * 0.5,
                current_y + (final_y - current_y) * 0.5,
                final_z,
            ))
            stage2_positions.append((final_x, final_y, final_z))

        return stage1_positions, stage2_positions

    # ------------------------------------------------------------------ 路徑跟隨 (虛擬領隊)

    def _calculate_leader_follower(self, drone_positions, route_wps_local, params):
        """
        路徑跟隨編隊 — 虛擬領隊 / 弧長參數化

        center_path = 使用者畫的路徑點（直線段連接，不插弧），
        每架 drone 沿弧長 s 參數化做隊形偏移：
            s_follower = s_leader - lon_k
            position = lookup(s_follower) + lat_k × perp(tangent)
        s 超出 path 範圍時外推（不 clip），確保每架 drone 起終點不重疊。

        Rank = 輸入順序（GUI 選取順序），drone_positions[0] = leader。
        隊形 (俯視):
            D0 (lat=-L, lon=0)   ← leader
                D1 (lat=+L, lon=5)
            D2 (lat=-L, lon=10)
                D3 (lat=+L, lon=15)
        """
        N = len(drone_positions)
        lateral_offset = params.get('lateral_offset', 3.0)
        longitudinal_spacing = params.get('longitudinal_spacing', 5.0)
        altitude = params.get('altitude', self.base_altitude)

        # Step 1: 建立中心路徑（直線段連接，每個中間航點設 barrier）
        center_path, barrier_indices = self._build_center_path(route_wps_local)

        s_list = [pt['s'] for pt in center_path]
        s_max = s_list[-1]
        n_pts = len(center_path)

        def lookup(s_target):
            """在 center path 上以弧長 s 回傳 (x, y, tangent_dir)。超出範圍時沿端點方向外推。"""
            if n_pts == 1:
                pt = center_path[0]
                return pt['x'], pt['y'], pt['dir']

            if s_target <= 0.0:
                a = center_path[0]
                b = center_path[1]
                sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
                slen = math.hypot(sdx, sdy)
                if slen > 1e-6:
                    dx, dy = sdx / slen, sdy / slen
                    return a['x'] + s_target * dx, a['y'] + s_target * dy, (dx, dy)
                return a['x'], a['y'], a['dir']

            if s_target >= s_max:
                a = center_path[-2]
                b = center_path[-1]
                sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
                slen = math.hypot(sdx, sdy)
                if slen > 1e-6:
                    dx, dy = sdx / slen, sdy / slen
                    overshoot = s_target - s_max
                    return b['x'] + overshoot * dx, b['y'] + overshoot * dy, (dx, dy)
                return b['x'], b['y'], b['dir']

            # Binary search：s_list[lo] <= s_target < s_list[hi]
            lo, hi = 0, n_pts - 1
            while lo + 1 < hi:
                mid = (lo + hi) // 2
                if s_list[mid] <= s_target:
                    lo = mid
                else:
                    hi = mid
            a = center_path[lo]
            b = center_path[hi]
            ds = s_list[hi] - s_list[lo]
            if ds < 1e-9:
                return a['x'], a['y'], a['dir']
            t = (s_target - s_list[lo]) / ds
            x = a['x'] + t * (b['x'] - a['x'])
            y = a['y'] + t * (b['y'] - a['y'])
            sdx, sdy = b['x'] - a['x'], b['y'] - a['y']
            slen = math.hypot(sdx, sdy)
            if slen > 1e-6:
                return x, y, (sdx / slen, sdy / slen)
            return x, y, a['dir']

        # Step 2: rank = 輸入順序（GUI 選取順序），不再做 projection sort
        # 避免浮點噪音在投影值相近時翻轉 leader，保證 run-to-run 穩定
        all_waypoints = [None] * N

        for rank in range(N):
            lat_sign = -1 if rank % 2 == 0 else 1
            lat = lat_sign * lateral_offset
            lon = rank * longitudinal_spacing

            waypoints = []
            for i in range(n_pts):
                s_follower = s_list[i] - lon
                x, y, (dx, dy) = lookup(s_follower)
                perp_x, perp_y = -dy, dx
                waypoints.append((
                    x + lat * perp_x,
                    y + lat * perp_y,
                    altitude,
                ))

            all_waypoints[rank] = waypoints

        return all_waypoints, barrier_indices

    def _build_center_path(self, waypoints):
        """
        建立中心路徑 — 直接用路徑點連接，不插弧。

        每個中間航點設為 barrier（轉彎集合點），讓隊伍到齊後再推進。
        飛控在 GUIDED 模式下自行處理直線飛行，不需要 GUI 預先平滑。

        Returns:
            (path, barrier_indices)
        """
        num_wps = len(waypoints)

        if num_wps < 2:
            return [{
                'seg': 'straight',
                'x': waypoints[0][0], 'y': waypoints[0][1],
                'dir': (0.0, 1.0),
                's': 0.0,
            }], []

        path = []
        barrier_indices = []

        for i in range(num_wps):
            if i < num_wps - 1:
                dx = waypoints[i + 1][0] - waypoints[i][0]
                dy = waypoints[i + 1][1] - waypoints[i][1]
            else:
                dx = waypoints[i][0] - waypoints[i - 1][0]
                dy = waypoints[i][1] - waypoints[i - 1][1]

            length = math.hypot(dx, dy)
            if length < 0.01:
                direction = (0.0, 1.0)
            else:
                direction = (dx / length, dy / length)

            path.append({
                'seg': 'straight',
                'x': waypoints[i][0], 'y': waypoints[i][1],
                'dir': direction,
            })

            if 0 < i < num_wps - 1:
                barrier_indices.append(len(path) - 1)

        path[0]['s'] = 0.0
        for i in range(1, len(path)):
            prev = path[i - 1]
            cur = path[i]
            cur['s'] = prev['s'] + math.hypot(
                cur['x'] - prev['x'], cur['y'] - prev['y']
            )

        return path, barrier_indices

    # ------------------------------------------------------------------ 柵狀掃描

    def _calculate_grid_sweep(self, drone_positions, rect_corners_local, params):
        """柵狀掃描：掃描方向沿矩形長邊，切割方向沿短邊"""
        N = len(drone_positions)
        line_spacing = params.get('line_spacing', 5.0)
        altitude = params.get('altitude', self.base_altitude)

        c0, c1, c2, c3 = rect_corners_local

        edge_a = (c1[0] - c0[0], c1[1] - c0[1])
        len_a = math.sqrt(edge_a[0] ** 2 + edge_a[1] ** 2)
        edge_b = (c3[0] - c0[0], c3[1] - c0[1])
        len_b = math.sqrt(edge_b[0] ** 2 + edge_b[1] ** 2)

        if len_a >= len_b:
            sweep_vec = edge_a
            sweep_len = len_a
            cut_vec = edge_b
            cut_len = len_b
            sweep_start_mid = ((c0[0] + c3[0]) / 2, (c0[1] + c3[1]) / 2)
            sweep_end_mid = ((c1[0] + c2[0]) / 2, (c1[1] + c2[1]) / 2)
            cut_start_corner = c0
        else:
            sweep_vec = edge_b
            sweep_len = len_b
            cut_vec = edge_a
            cut_len = len_a
            sweep_start_mid = ((c0[0] + c1[0]) / 2, (c0[1] + c1[1]) / 2)
            sweep_end_mid = ((c3[0] + c2[0]) / 2, (c3[1] + c2[1]) / 2)
            cut_start_corner = c0

        sweep_dir = (sweep_vec[0] / sweep_len, sweep_vec[1] / sweep_len)
        cut_dir = (cut_vec[0] / cut_len, cut_vec[1] / cut_len)

        C_x = sum(p[0] for p in drone_positions) / N
        C_y = sum(p[1] for p in drone_positions) / N

        dist_to_start = math.sqrt(
            (C_x - sweep_start_mid[0]) ** 2 + (C_y - sweep_start_mid[1]) ** 2
        )
        dist_to_end = math.sqrt(
            (C_x - sweep_end_mid[0]) ** 2 + (C_y - sweep_end_mid[1]) ** 2
        )

        if dist_to_start <= dist_to_end:
            near_corner_a = cut_start_corner
        else:
            sweep_dir = (-sweep_dir[0], -sweep_dir[1])
            near_corner_a = (cut_start_corner[0] + sweep_vec[0],
                            cut_start_corner[1] + sweep_vec[1])

        projections = [
            ((pos[0] - C_x) * cut_dir[0] + (pos[1] - C_y) * cut_dir[1], i)
            for i, pos in enumerate(drone_positions)
        ]
        projections.sort()

        strip_width = cut_len / N
        drone_sweep_proj = C_x * sweep_dir[0] + C_y * sweep_dir[1]
        near_sweep_proj = near_corner_a[0] * sweep_dir[0] + near_corner_a[1] * sweep_dir[1]
        gather_sweep_proj = (drone_sweep_proj + near_sweep_proj) / 2

        all_waypoints = [None] * N

        for rank, (_, original_idx) in enumerate(projections):
            strip_center_offset = (rank + 0.5) * strip_width
            base_x = near_corner_a[0] + cut_dir[0] * strip_center_offset
            base_y = near_corner_a[1] + cut_dir[1] * strip_center_offset

            waypoints_list = []

            gather_offset = gather_sweep_proj - near_sweep_proj
            gx = base_x + sweep_dir[0] * gather_offset
            gy = base_y + sweep_dir[1] * gather_offset
            waypoints_list.append((gx, gy, altitude))

            num_lines = max(1, int(strip_width / line_spacing))
            actual_spacing = strip_width / num_lines
            first_line_offset = (rank * strip_width) + actual_spacing / 2

            entry_x = near_corner_a[0] + cut_dir[0] * first_line_offset
            entry_y = near_corner_a[1] + cut_dir[1] * first_line_offset
            waypoints_list.append((entry_x, entry_y, altitude))

            current_cut_offset = first_line_offset

            for line_idx in range(num_lines):
                line_near_x = near_corner_a[0] + cut_dir[0] * current_cut_offset
                line_near_y = near_corner_a[1] + cut_dir[1] * current_cut_offset
                line_far_x = line_near_x + sweep_dir[0] * sweep_len
                line_far_y = line_near_y + sweep_dir[1] * sweep_len

                if line_idx % 2 == 0:
                    waypoints_list.append((line_far_x, line_far_y, altitude))
                else:
                    waypoints_list.append((line_near_x, line_near_y, altitude))

                if line_idx < num_lines - 1:
                    next_cut_offset = current_cut_offset + actual_spacing
                    next_near_x = near_corner_a[0] + cut_dir[0] * next_cut_offset
                    next_near_y = near_corner_a[1] + cut_dir[1] * next_cut_offset
                    next_far_x = next_near_x + sweep_dir[0] * sweep_len
                    next_far_y = next_near_y + sweep_dir[1] * sweep_len

                    if line_idx % 2 == 0:
                        waypoints_list.append((next_far_x, next_far_y, altitude))
                    else:
                        waypoints_list.append((next_near_x, next_near_y, altitude))
                    current_cut_offset = next_cut_offset

            all_waypoints[original_idx] = waypoints_list

        # 每一條掃描線的「起點」都做一次同步：
        #   wp 0 = gather, wp 1 = 第一條線起點, wp 2 = 第一條線終點,
        #   wp 3 = 第二條線起點, wp 4 = 第二條線終點, ...
        # 每條線的「起點 index」= 1, 3, 5, ... = 2*i + 1（i 從 0 開始）
        # 所有 drone 的 waypoint 數量相同（num_lines 對所有 strip 都一致），
        # 所以用同一份 rendezvous_indices
        rendezvous_indices = [2 * i + 1 for i in range(num_lines)]

        return all_waypoints, rendezvous_indices


# ================================================================================
# 測試
# ================================================================================
if __name__ == "__main__":
    planner = FormationPlanner()

    drones = [
        (24.123450, 120.567800, 0.0),
        (24.123470, 120.567820, 0.0),
        (24.123440, 120.567810, 0.0),
        (24.123460, 120.567830, 0.0),
    ]
    target = (24.12400, 120.56795, 10.0)

    # M-Formation
    wps, origin, rv = planner.plan_formation_mission(drones, target, MissionType.M_FORMATION)
    print("M-Formation:")
    for i, wp_list in enumerate(wps):
        print(f"  Drone {i}: {len(wp_list)} waypoints")

    # Leader-Follower (虛擬領隊 / 弧長參數化)
    route = [
        (24.12345, 120.56780),
        (24.12370, 120.56800),
        (24.12390, 120.56820),
        (24.12400, 120.56800),
        (24.12420, 120.56790),
    ]
    wps_lf, origin_lf, rv_lf = planner.plan_formation_mission(
        drones, target, MissionType.LEADER_FOLLOWER,
        params={
            'route_waypoints': route,
            'lateral_offset': 3.0,
            'longitudinal_spacing': 5.0,
            'min_inner_radius': 3.0,
            'arc_resolution': 8,
            'altitude': 10.0
        }
    )
    print(f"\nLeader-Follower (arc-length virtual leader):")
    for i, wp_list in enumerate(wps_lf):
        print(f"  Drone {i}: {len(wp_list)} waypoints")
        for j, wp in enumerate(wp_list):
            print(f"    WP{j}: ({wp[0]:.6f}, {wp[1]:.6f}, {wp[2]:.1f})")

    # Grid Sweep
    rect = [
        (24.1237, 120.5679),
        (24.1237, 120.5683),
        (24.1240, 120.5683),
        (24.1240, 120.5679)
    ]
    wps2, origin2, rv2 = planner.plan_formation_mission(
        drones, target, MissionType.GRID_SWEEP,
        params={'rect_corners': rect, 'line_spacing': 5.0, 'altitude': 10.0}
    )
    print(f"\nGrid Sweep:")
    for i, wp_list in enumerate(wps2):
        print(f"  Drone {i}: {len(wp_list)} waypoints")