
#!/usr/bin/env python3
# 集群编队控制模块

import math
from typing import Dict, List, Tuple, Set
import numpy as np


class FormationController:
    """集群编队控制器，负责各种编队模式的计算和控制
    
    注意：PX4使用NED坐标系 (North-East-Down)
    - X轴指向北
    - Y轴指向东
    - Z轴指向下（高度为负值）
    """
    
    def __init__(self, uav_controller):
        self.uav_controller = uav_controller
        self.current_formation = None
        self.formation_parameters = {}
        self.formation_timer = None
        self.formation_active = False
        self.collision_avoidance_enabled = True
        self.min_safe_distance = 2.5  # 增加最小安全距离（米）
        
    def _check_collisions(self, positions: Dict[int, Tuple[float, float, float]]) -> bool:
        """检查位置设定是否存在碰撞风险
        
        Args:
            positions: 无人机ID到目标位置的映射
            
        Returns:
            bool: 是否存在碰撞风险
        """
        if not self.collision_avoidance_enabled:
            return False
            
        position_list = list(positions.values())
        uav_ids = list(positions.keys())
        
        for i in range(len(position_list)):
            for j in range(i + 1, len(position_list)):
                pos1 = position_list[i]
                pos2 = position_list[j]
                
                # 计算距离
                distance = math.sqrt(
                    (pos1[0] - pos2[0])**2 + 
                    (pos1[1] - pos2[1])**2 + 
                    (pos1[2] - pos2[2])**2
                )
                
                if distance < self.min_safe_distance:
                    print(f"⚠️ 碰撞警告: UAV {uav_ids[i]} 和 UAV {uav_ids[j]} 距离 {distance:.2f}m < {self.min_safe_distance}m")
                    return True
                    
        return False
        
    def _adjust_positions_for_collision_avoidance(self, positions: Dict[int, Tuple[float, float, float]]) -> Dict[int, Tuple[float, float, float]]:
        """调整位置以避免碰撞
        
        Args:
            positions: 原始位置设定
            
        Returns:
            调整后的位置设定
        """
        if not self.collision_avoidance_enabled:
            return positions
            
        adjusted_positions = positions.copy()
        position_list = list(adjusted_positions.values())
        uav_ids = list(adjusted_positions.keys())
        
        max_iterations = 10
        for iteration in range(max_iterations):
            collision_found = False
            
            for i in range(len(position_list)):
                for j in range(i + 1, len(position_list)):
                    pos1 = np.array(position_list[i])
                    pos2 = np.array(position_list[j])
                    
                    distance = np.linalg.norm(pos1 - pos2)
                    
                    if distance < self.min_safe_distance:
                        collision_found = True
                        
                        # 计算调整向量
                        direction = pos1 - pos2
                        if np.linalg.norm(direction) < 0.1:  # 避免除零
                            direction = np.array([1.0, 0.0, 0.0])
                        else:
                            direction = direction / np.linalg.norm(direction)
                            
                        # 计算需要移动的距离
                        move_distance = (self.min_safe_distance - distance) / 2
                        
                        # 调整两个无人机的位置
                        adjustment = direction * move_distance
                        new_pos1 = pos1 + adjustment
                        new_pos2 = pos2 - adjustment
                        
                        position_list[i] = tuple(new_pos1)
                        position_list[j] = tuple(new_pos2)
                        
                        print(f"🔄 调整 UAV {uav_ids[i]} 和 UAV {uav_ids[j]} 的位置以避免碰撞")
            
            if not collision_found:
                break
                
        # 更新调整后的位置
        for i, uav_id in enumerate(uav_ids):
            adjusted_positions[uav_id] = position_list[i]
            
        return adjusted_positions
        
    def set_formation_line(self, uav_ids: Set[int], spacing: float = 4.0,
                          direction: str = 'x', center_position: Tuple[float, float, float] = None):
        """设置一字形编队 - 重构版：确保正确的线性排列
        
        Args:
            uav_ids: 参与编队的无人机ID集合
            spacing: 无人机间距（米），增加间距避免碰撞
            direction: 编队方向 ('x' 或 'y') - 注意：在NED坐标系中，x=北，y=东
            center_position: 编队中心位置 (x, y, z)，如果为None则使用第一个无人机的位置
        """
        print(f"🎯 设置一字形编队: {sorted(uav_ids)} 台无人机")
        
        if len(uav_ids) < 2:
            print("⚠️ 一字形编队至少需要2台无人机")
            return
        
        # 获取无人机位置数据
        uav_positions = {}
        for uav_id in uav_ids:
            uav_data = self.uav_controller.get_uav_data(uav_id)
            if uav_data and uav_data.get('connected', False):
                uav_positions[uav_id] = (
                    uav_data.get('local_x', 0),
                    uav_data.get('local_y', 0),
                    uav_data.get('local_z', 0)
                )
        
        if not uav_positions:
            print("❌ 错误：没有可用的无人机位置数据")
            return
        
        # 确定编队中心
        if center_position is None:
            # 使用所有无人机的平均位置作为中心
            all_x = [pos[0] for pos in uav_positions.values()]
            all_y = [pos[1] for pos in uav_positions.values()]
            all_z = [pos[2] for pos in uav_positions.values()]
            center_x = sum(all_x) / len(all_x)
            center_y = sum(all_y) / len(all_y)
            center_z = sum(all_z) / len(all_z)
        else:
            center_x, center_y, center_z = center_position
        
        # 计算编队位置 - 确保正确的线性排列
        sorted_uav_ids = sorted(uav_ids)
        formation_positions = {}
        
        # 计算每个无人机的位置
        num_uavs = len(sorted_uav_ids)
        for i, uav_id in enumerate(sorted_uav_ids):
            # 计算相对于中心的位置偏移
            if direction == 'x':
                # x方向：北向排列 - 形成明显的水平线
                offset_x = (i - (num_uavs - 1) / 2) * spacing
                offset_y = 0
            else:  # direction == 'y'
                # y方向：东向排列 - 形成明显的垂直线
                offset_x = 0
                offset_y = (i - (num_uavs - 1) / 2) * spacing
            
            target_x = center_x + offset_x
            target_y = center_y + offset_y
            target_z = center_z
            
            formation_positions[uav_id] = (target_x, target_y, target_z)
            print(f"  🚁 UAV {uav_id}: 目标位置 [{target_x:.2f}, {target_y:.2f}, {target_z:.2f}]")
        
        # 碰撞避免检查
        if self._check_collisions(formation_positions):
            print("⚠️ 检测到碰撞风险，正在调整位置...")
            formation_positions = self._adjust_positions_for_collision_avoidance(formation_positions)
        
        self.current_formation = 'line'
        self.formation_parameters = {
            'type': 'line',
            'positions': formation_positions,
            'spacing': spacing,
            'direction': direction,
            'center': (center_x, center_y, center_z)
        }
        
        print(f"✅ 一字形编队设置完成，间距 {spacing}m")
        
        # 发送位置设定点
        self._send_formation_positions()
        
    def set_formation_circle(self, uav_ids: Set[int], radius: float = 5.0,
                           center_position: Tuple[float, float, float] = None):
        """设置圆形编队 - 重构版：确保正确的圆形排列
        
        Args:
            uav_ids: 参与编队的无人机ID集合
            radius: 圆半径（米），增加半径避免碰撞
            center_position: 圆心位置 (x, y, z)，如果为None则使用第一个无人机的位置
        """
        print(f"🎯 设置圆形编队: {sorted(uav_ids)} 台无人机")
        
        if len(uav_ids) < 2:
            print("⚠️ 圆形编队至少需要2台无人机")
            return
        
        # 获取无人机位置数据
        uav_positions = {}
        for uav_id in uav_ids:
            uav_data = self.uav_controller.get_uav_data(uav_id)
            if uav_data and uav_data.get('connected', False):
                uav_positions[uav_id] = (
                    uav_data.get('local_x', 0),
                    uav_data.get('local_y', 0),
                    uav_data.get('local_z', 0)
                )
        
        if not uav_positions:
            print("❌ 错误：没有可用的无人机位置数据")
            return
        
        # 确定圆心
        if center_position is None:
            # 使用所有无人机的平均位置作为圆心
            all_x = [pos[0] for pos in uav_positions.values()]
            all_y = [pos[1] for pos in uav_positions.values()]
            all_z = [pos[2] for pos in uav_positions.values()]
            center_x = sum(all_x) / len(all_x)
            center_y = sum(all_y) / len(all_y)
            center_z = sum(all_z) / len(all_z)
        else:
            center_x, center_y, center_z = center_position
        
        # 计算圆形编队位置 - 确保正确的圆形排列
        sorted_uav_ids = sorted(uav_ids)
        formation_positions = {}
        num_uavs = len(sorted_uav_ids)
        
        # 计算每个无人机的角度和位置
        for i, uav_id in enumerate(sorted_uav_ids):
            # 计算角度（均匀分布在圆周上，从北方向开始顺时针）
            angle = 2 * math.pi * i / num_uavs
            
            # 计算位置（NED坐标系：x=北，y=东）
            target_x = center_x + radius * math.cos(angle)
            target_y = center_y + radius * math.sin(angle)
            target_z = center_z
            
            formation_positions[uav_id] = (target_x, target_y, target_z)
            print(f"  🚁 UAV {uav_id}: 角度 {math.degrees(angle):.1f}°, 目标位置 [{target_x:.2f}, {target_y:.2f}, {target_z:.2f}]")
        
        # 碰撞避免检查
        if self._check_collisions(formation_positions):
            print("⚠️ 检测到碰撞风险，正在调整位置...")
            formation_positions = self._adjust_positions_for_collision_avoidance(formation_positions)
        
        self.current_formation = 'circle'
        self.formation_parameters = {
            'type': 'circle',
            'positions': formation_positions,
            'radius': radius,
            'center': (center_x, center_y, center_z)
        }
        
        print(f"✅ 圆形编队设置完成，半径 {radius}m")
        
        # 发送位置设定点
        self._send_formation_positions()
        
    def set_formation_square(self, uav_ids: Set[int], side_length: float = 6.0,
                           center_position: Tuple[float, float, float] = None):
        """设置正方形编队 - 重构版：确保正确的正方形排列
        
        Args:
            uav_ids: 参与编队的无人机ID集合
            side_length: 正方形边长（米），增加边长避免碰撞
            center_position: 正方形中心位置 (x, y, z)，如果为None则使用第一个无人机的位置
        """
        print(f"🎯 设置正方形编队: {sorted(uav_ids)} 台无人机")
        
        if len(uav_ids) < 2:
            print("⚠️ 正方形编队至少需要2台无人机")
            return
        
        # 获取无人机位置数据
        uav_positions = {}
        for uav_id in uav_ids:
            uav_data = self.uav_controller.get_uav_data(uav_id)
            if uav_data and uav_data.get('connected', False):
                uav_positions[uav_id] = (
                    uav_data.get('local_x', 0),
                    uav_data.get('local_y', 0),
                    uav_data.get('local_z', 0)
                )
        
        if not uav_positions:
            print("❌ 错误：没有可用的无人机位置数据")
            return
        
        # 确定中心位置
        if center_position is None:
            # 使用所有无人机的平均位置作为中心
            all_x = [pos[0] for pos in uav_positions.values()]
            all_y = [pos[1] for pos in uav_positions.values()]
            all_z = [pos[2] for pos in uav_positions.values()]
            center_x = sum(all_x) / len(all_x)
            center_y = sum(all_y) / len(all_y)
            center_z = sum(all_z) / len(all_z)
        else:
            center_x, center_y, center_z = center_position
        
        # 计算正方形顶点位置 - 确保正确的正方形排列
        half_side = side_length / 2
        # 创建正方形的四个顶点（NED坐标系：x=北，y=东）
        # 顶点顺序：西北、东北、东南、西南
        square_vertices = [
            (center_x - half_side, center_y - half_side, center_z),  # 西北
            (center_x + half_side, center_y - half_side, center_z),  # 东北
            (center_x + half_side, center_y + half_side, center_z),  # 东南
            (center_x - half_side, center_y + half_side, center_z)   # 西南
        ]
        
        # 分配位置给无人机 - 确保正确的分配策略
        sorted_uav_ids = sorted(uav_ids)
        formation_positions = {}
        
        # 根据无人机数量选择分配策略
        if len(sorted_uav_ids) == 2:
            # 2台无人机：使用对角顶点
            formation_positions[sorted_uav_ids[0]] = square_vertices[0]  # 西北
            formation_positions[sorted_uav_ids[1]] = square_vertices[2]  # 东南
        elif len(sorted_uav_ids) == 3:
            # 3台无人机：使用三个顶点形成明显的三角形
            formation_positions[sorted_uav_ids[0]] = square_vertices[0]  # 西北
            formation_positions[sorted_uav_ids[1]] = square_vertices[1]  # 东北
            formation_positions[sorted_uav_ids[2]] = square_vertices[2]  # 东南
        elif len(sorted_uav_ids) == 4:
            # 4台无人机：使用全部四个顶点
            for i, uav_id in enumerate(sorted_uav_ids):
                formation_positions[uav_id] = square_vertices[i]
        else:
            # 对于其他数量的无人机，按顺序分配顶点，多余的均匀分布
            for i, uav_id in enumerate(sorted_uav_ids):
                if i < len(square_vertices):
                    # 使用正方形顶点
                    target_x, target_y, target_z = square_vertices[i]
                else:
                    # 如果无人机数量超过4个，在正方形内部均匀分布
                    extra_uav_index = i - len(square_vertices)
                    num_extra = len(sorted_uav_ids) - len(square_vertices)
                    
                    # 在正方形内部创建网格
                    grid_size = math.ceil(math.sqrt(num_extra))
                    if grid_size > 0:
                        row = extra_uav_index // grid_size
                        col = extra_uav_index % grid_size
                        
                        # 在正方形内部均匀分布
                        inner_spacing = side_length / (grid_size + 1)
                        target_x = center_x - half_side + (col + 1) * inner_spacing
                        target_y = center_y - half_side + (row + 1) * inner_spacing
                        target_z = center_z
                    else:
                        target_x = center_x
                        target_y = center_y
                        target_z = center_z
                
                formation_positions[uav_id] = (target_x, target_y, target_z)
        
        # 碰撞避免检查
        if self._check_collisions(formation_positions):
            print("⚠️ 检测到碰撞风险，正在调整位置...")
            formation_positions = self._adjust_positions_for_collision_avoidance(formation_positions)
        
        self.current_formation = 'square'
        self.formation_parameters = {
            'type': 'square',
            'positions': formation_positions,
            'side_length': side_length,
            'center': (center_x, center_y, center_z)
        }
        
        print(f"✅ 正方形编队设置完成，边长 {side_length}m")
        
        # 发送位置设定点
        self._send_formation_positions()
        
    def set_formation_triangle(self, uav_ids: Set[int], side_length: float = 6.0,
                             center_position: Tuple[float, float, float] = None):
        """设置三角形编队 - 重构版：确保正确的等边三角形排列
        
        Args:
            uav_ids: 参与编队的无人机ID集合
            side_length: 三角形边长（米），增加边长避免碰撞
            center_position: 三角形中心位置 (x, y, z)，如果为None则使用第一个无人机的位置
        """
        print(f"🎯 设置三角形编队: {sorted(uav_ids)} 台无人机")
        
        if len(uav_ids) < 2:
            print("⚠️ 三角形编队至少需要2台无人机")
            return
        
        # 获取无人机位置数据
        uav_positions = {}
        for uav_id in uav_ids:
            uav_data = self.uav_controller.get_uav_data(uav_id)
            if uav_data and uav_data.get('connected', False):
                uav_positions[uav_id] = (
                    uav_data.get('local_x', 0),
                    uav_data.get('local_y', 0),
                    uav_data.get('local_z', 0)
                )
        
        if not uav_positions:
            print("❌ 错误：没有可用的无人机位置数据")
            return
        
        # 确定中心位置
        if center_position is None:
            # 使用所有无人机的平均位置作为中心
            all_x = [pos[0] for pos in uav_positions.values()]
            all_y = [pos[1] for pos in uav_positions.values()]
            all_z = [pos[2] for pos in uav_positions.values()]
            center_x = sum(all_x) / len(all_x)
            center_y = sum(all_y) / len(all_y)
            center_z = sum(all_z) / len(all_z)
        else:
            center_x, center_y, center_z = center_position
        
        # 计算等边三角形顶点位置 - 确保正确的等边三角形排列
        height = side_length * math.sqrt(3) / 2  # 等边三角形高度
        
        # 创建等边三角形的三个顶点（NED坐标系：x=北，y=东）
        # 顶点1：北顶点
        # 顶点2：西南顶点
        # 顶点3：东南顶点
        triangle_vertices = [
            (center_x, center_y + height/2, center_z),           # 北顶点
            (center_x - side_length/2, center_y - height/2, center_z),  # 西南顶点
            (center_x + side_length/2, center_y - height/2, center_z)   # 东南顶点
        ]
        
        # 分配位置给无人机 - 确保正确的分配策略
        sorted_uav_ids = sorted(uav_ids)
        formation_positions = {}
        
        # 根据无人机数量选择分配策略
        if len(sorted_uav_ids) == 2:
            # 2台无人机：使用两个顶点
            formation_positions[sorted_uav_ids[0]] = triangle_vertices[0]  # 北顶点
            formation_positions[sorted_uav_ids[1]] = triangle_vertices[1]  # 西南顶点
        elif len(sorted_uav_ids) == 3:
            # 3台无人机：使用全部三个顶点
            for i, uav_id in enumerate(sorted_uav_ids):
                formation_positions[uav_id] = triangle_vertices[i]
        else:
            # 对于其他数量的无人机，按顺序分配顶点，多余的均匀分布
            for i, uav_id in enumerate(sorted_uav_ids):
                if i < len(triangle_vertices):
                    # 使用三角形顶点
                    target_x, target_y, target_z = triangle_vertices[i]
                else:
                    # 如果无人机数量超过3个，在三角形内部均匀分布
                    extra_uav_index = i - len(triangle_vertices)
                    num_extra = len(sorted_uav_ids) - len(triangle_vertices)
                    
                    # 在三角形内部创建网格
                    grid_size = math.ceil(math.sqrt(num_extra))
                    if grid_size > 0:
                        row = extra_uav_index // grid_size
                        col = extra_uav_index % grid_size
                        
                        # 在三角形内部均匀分布（简化方法）
                        inner_spacing = side_length / (grid_size + 1)
                        target_x = center_x - side_length/4 + col * inner_spacing
                        target_y = center_y - height/4 + row * inner_spacing
                        target_z = center_z
                    else:
                        target_x = center_x
                        target_y = center_y
                        target_z = center_z
                
                formation_positions[uav_id] = (target_x, target_y, target_z)
        
        # 碰撞避免检查
        if self._check_collisions(formation_positions):
            print("⚠️ 检测到碰撞风险，正在调整位置...")
            formation_positions = self._adjust_positions_for_collision_avoidance(formation_positions)
        
        self.current_formation = 'triangle'
        self.formation_parameters = {
            'type': 'triangle',
            'positions': formation_positions,
            'side_length': side_length,
            'center': (center_x, center_y, center_z)
        }
        
        print(f"✅ 三角形编队设置完成，边长 {side_length}m")
        
        # 发送位置设定点
        self._send_formation_positions()
        
    def rotate_formation(self, angle_degrees: float):
        """旋转当前编队
        
        Args:
            angle_degrees: 旋转角度（度）
        """
        if self.current_formation is None:
            print("错误：没有激活的编队")
            return
        
        print(f"旋转编队 {angle_degrees} 度")
        
        formation_type = self.formation_parameters.get('type')
        center = self.formation_parameters.get('center', (0, 0, 0))
        center_x, center_y, center_z = center
        
        # 转换为弧度
        angle_rad = math.radians(angle_degrees)
        
        # 旋转所有位置
        new_positions = {}
        for uav_id, (x, y, z) in self.formation_parameters['positions'].items():
            # 计算相对于中心的位置
            rel_x = x - center_x
            rel_y = y - center_y
            
            # 应用旋转矩阵
            new_rel_x = rel_x * math.cos(angle_rad) - rel_y * math.sin(angle_rad)
            new_rel_y = rel_x * math.sin(angle_rad) + rel_y * math.cos(angle_rad)
            
            # 计算新的绝对位置
            new_x = center_x + new_rel_x
            new_y = center_y + new_rel_y
            new_z = z
            
            new_positions[uav_id] = (new_x, new_y, new_z)
            
        # 碰撞避免检查
        if self._check_collisions(new_positions):
            new_positions = self._adjust_positions_for_collision_avoidance(new_positions)
        
        # 更新编队参数
        self.formation_parameters['positions'] = new_positions
        
        # 发送新的位置设定点
        self._send_formation_positions()
        
    def move_formation(self, delta_x: float, delta_y: float, delta_z: float = 0):
        """移动当前编队
        
        Args:
            delta_x: X方向移动距离（米）
            delta_y: Y方向移动距离（米）
            delta_z: Z方向移动距离（米）
        """
        if self.current_formation is None:
            print("错误：没有激活的编队")
            return
        
        print(f"🔄 移动编队: ΔX={delta_x:.2f}, ΔY={delta_y:.2f}, ΔZ={delta_z:.2f}")
        
        # 移动所有位置
        new_positions = {}
        for uav_id, (x, y, z) in self.formation_parameters['positions'].items():
            new_x = x + delta_x
            new_y = y + delta_y
            new_z = z + delta_z
            
            new_positions[uav_id] = (new_x, new_y, new_z)
            
            print(f"UAV {uav_id}: 移动后位置 [{new_x:.2f}, {new_y:.2f}, {new_z:.2f}]")
        
        # 碰撞避免检查
        if self._check_collisions(new_positions):
            print("🔄 检测到碰撞风险，正在调整位置...")
            new_positions = self._adjust_positions_for_collision_avoidance(new_positions)
        
        # 更新编队参数
        self.formation_parameters['positions'] = new_positions
        
        # 更新中心位置
        if 'center' in self.formation_parameters:
            center_x, center_y, center_z = self.formation_parameters['center']
            self.formation_parameters['center'] = (center_x + delta_x, center_y + delta_y, center_z + delta_z)
        
        # 发送新的位置设定点
        self._send_formation_positions()
        
    def _send_formation_positions(self):
        """发送编队位置设定点"""
        if 'positions' not in self.formation_parameters:
            return
        
        positions = self.formation_parameters['positions']
        
        print(f"📡 发送编队位置设定点到 {len(positions)} 台无人机:")
        for uav_id, (target_x, target_y, target_z) in positions.items():
            # 发送位置设定点（会自动启动 Offboard）
            self.uav_controller.goto_ned(uav_id, target_x, target_y, target_z)
            print(f"  🚁 UAV {uav_id}: [{target_x:.2f}, {target_y:.2f}, {target_z:.2f}]")
        
        print("🚀 编队控制已激活")
        
    def get_formation_info(self) -> Dict:
        """获取当前编队信息"""
        return {
            'current_formation': self.current_formation,
            'parameters': self.formation_parameters,
            'collision_avoidance_enabled': self.collision_avoidance_enabled,
            'min_safe_distance': self.min_safe_distance
        }
        
    def set_collision_avoidance(self, enabled: bool):
        """设置碰撞避免功能
        
        Args:
            enabled: 是否启用碰撞避免
        """
        self.collision_avoidance_enabled = enabled
        status = "启用" if enabled else "禁用"
        print(f"🔄 碰撞避免功能已{status}")
        
    def set_min_safe_distance(self, distance: float):
        """设置最小安全距离
        
        Args:
            distance: 最小安全距离（米）
        """
        self.min_safe_distance = distance
        print(f"🔄 最小安全距离已设置为 {distance:.2f} 米")
        
    def clear_formation(self):
        """清除当前编队"""
        self.current_formation = None
        self.formation_parameters = {}
        self.formation_active = False
        print("🔄 编队已清除")
        
    def _start_formation_control_thread(self):
        """启动编队控制持续发送线程"""
        if self.formation_active:
            return
            
        self.formation_active = True
        
        import threading
        def formation_control_loop():
            counter = 0
            while self.formation_active and self.current_formation is not None:
                try:
                    # 持续发送编队位置设定点
                    if 'positions' in self.formation_parameters:
                        positions = self.formation_parameters['positions']
                        
                        for uav_id, (target_x, target_y, target_z) in positions.items():
                            # 持续发送位置设定点
                            self.uav_controller.goto_ned(uav_id, target_x, target_y, target_z)
                        
                        counter += 1
                        if counter % 50 == 0:  # 每50次打印一次状态
                            print(f"📡 编队控制持续发送信号 ({counter}次) - {len(positions)} 台无人机")
                    
                    # 控制频率：10Hz（符合PX4要求）
                    import time
                    time.sleep(0.1)
                    
                except Exception as e:
                    print(f"编队控制线程错误: {e}")
                    break
        
        # 启动编队控制线程
        formation_thread = threading.Thread(target=formation_control_loop, daemon=True)
        formation_thread.start()
        print("🚀 编队控制持续发送线程已启动")