masa-agent/uav_langchain_tools.py

"""
LangChain Tools for UAV Control
Wraps the UAV API client as LangChain tools using @tool decorator
All tools accept JSON string input for consistent parameter handling
"""
from langchain.tools import tool
from uav_api_client import UAVAPIClient
import math
import heapq
import json
from typing import List, Dict, Any, Optional

# --- 构建地图scan points ---

class ScanPointInfo:
    def __init__(self, x: float, y: float):
        self.x: float = x
        self.y: float = y
        self.visited: bool = False
        self.drones_visited: List[str] = []

    def to_dict(self):
        return {
            "x": self.x,
            "y": self.y,
            "visited": self.visited
        }

def map_to_drones_points(map_width=1024.0, map_height=768.0) -> List[ScanPointInfo]:
    """Generate a list of scan points using a hexagonal grid pattern."""
    # 核心参数
    r = 150  # 侦察半径(m)
    dx = r * math.sqrt(3)  # 横向点间距
    dy = 1.5 * r  # 纵向行间距

    # 确定坐标范围，处理负数、一正一负等情况
    min_x, max_x = sorted([0.0, float(map_width)])
    min_y, max_y = sorted([0.0, float(map_height)])

    # 正六边形网格侦察点坐标列表 (x, y)
    points = []
    y = min_y + r
    row = 1
    while y < max_y + r:
        # 奇数行偏移 dx/2，偶数行偏移 dx
        x_start = min_x + ((dx / 2) if row % 2 != 0 else dx)
        x = x_start
        while x < max_x + r:
            points.append(ScanPointInfo(round(x, 4), round(y, 4)))
            x += dx
        y += dy
        row += 1
    return points

#初始地图
scan_points = map_to_drones_points(1024,768)


def scan_point_is_reached(scan_point: ScanPointInfo, drone_pos: tuple[float, float], threshold: float = 30.0) -> bool:
    """判断当前位置在侦察点附近"""
    dx = scan_point.x - drone_pos[0]
    dy = scan_point.y - drone_pos[1]
    dist_sq = dx * dx + dy * dy
    return dist_sq <= threshold * threshold

def scan_point_is_reached_by_drones(drone_id: str, x: float, y: float, threshold: float = 30.0) -> bool:
    """判断无人机当前位置是否到达侦察点"""
    global scan_points
    for i in scan_points:
        if scan_point_is_reached(i, (x, y), threshold):
            i.visited = True
            if drone_id not in i.drones_visited:
                i.drones_visited.append(drone_id)
            print(f"Drone {drone_id} reached scan point ({i.x}, {i.y})")
            break

class TargetInfo:
    def __init__(self, data: Dict[str, Any]):
        self.id: str = data.get("id")
        self.name: str = data.get("name")
        self.type: str = data.get("type")
        self.position: Dict[str, float] = data.get("position")
        self.description: str = data.get("description")
        self.velocity: Optional[Dict[str, float]] = data.get("velocity")
        self.radius: Optional[float] = data.get("radius")
        self.created_at: float = data.get("created_at")
        self.last_updated: float = data.get("last_updated")
        self.moving_path: Optional[List[Dict[str, float]]] = data.get("moving_path")
        self.moving_duration: Optional[float] = data.get("moving_duration")
        self.current_path_index: Optional[int] = data.get("current_path_index")
        self.path_direction: Optional[int] = data.get("path_direction")
        self.time_in_direction: Optional[float] = data.get("time_in_direction")
        self.calculated_speed: Optional[float] = data.get("calculated_speed")
        self.charge_amount: Optional[float] = data.get("charge_amount")
        self.vertices: Optional[List[Dict[str, float]]] = data.get("vertices")
        self.is_reached: bool = data.get("is_reached")
        self.reached_by: List[str] = data.get("reached_by")

    def __hash__(self):
        return hash(self.id)

    def __eq__(self, other):
        return isinstance(other, TargetInfo) and self.id == other.id
    
    def to_dict(self):
        return {
            "id": self.id,
            "name": self.name,
            "type": self.type,
            "position": self.position,
            "description": self.description,
            "velocity": self.velocity,
            "radius": self.radius,
            "created_at": self.created_at,
            "last_updated": self.last_updated,
            "moving_path": self.moving_path,
            "moving_duration": self.moving_duration,
            "current_path_index": self.current_path_index,
            "path_direction": self.path_direction,
            "time_in_direction": self.time_in_direction,
            "calculated_speed": self.calculated_speed,
            "charge_amount": self.charge_amount,
            "vertices": self.vertices,
            "is_reached": self.is_reached,
            "reached_by": self.reached_by,
        }

class ObstacleInfo:
    def __init__(self, data: Dict[str, Any]):
        self.id: str = data.get("id")
        self.name: str = data.get("name")
        self.type: str = data.get("type")
        self.position: Dict[str, float] = data.get("position")
        self.description: str = data.get("description")
        self.radius: Optional[float] = data.get("radius")
        self.vertices: Optional[List[Dict[str, float]]] = data.get("vertices")
        self.width: Optional[float] = data.get("width")
        self.length: Optional[float] = data.get("length")
        self.height: Optional[float] = data.get("height")
        self.area: Optional[float] = data.get("area")
        self.created_at: float = data.get("created_at")
        self.last_updated: float = data.get("last_updated")

    def __hash__(self):
        return hash(self.id)

    def __eq__(self, other):
        return isinstance(other, ObstacleInfo) and self.id == other.id

    def to_dict(self):
        return {
            "id": self.id,
            "name": self.name,
            "type": self.type,
            "position": self.position,
            "description": self.description,
            "radius": self.radius,
            "vertices": self.vertices,
            "width": self.width,
            "length": self.length,
            "height": self.height,
            "area": self.area,
            "created_at": self.created_at,
            "last_updated": self.last_updated,
        }

targets_expolred : set[TargetInfo] = set()
obstacles_detected : set[ObstacleInfo] = set()

# --- 内部几何算法类 ---
class GeometryUtils:
    @staticmethod
    def point_to_segment_dist_sq(p, a, b):
        """计算点 p 到线段 ab 的最短距离的平方"""
        px, py = p[0], p[1]
        ax, ay = a[0], a[1]
        bx, by = b[0], b[1]
        l2 = (ax - bx)**2 + (ay - by)**2
        if l2 == 0: return (px - ax)**2 + (py - ay)**2
        t = ((px - ax) * (bx - ax) + (py - ay) * (by - ay)) / l2
        t = max(0, min(1, t))
        proj_x = ax + t * (bx - ax)
        proj_y = ay + t * (by - ay)
        return (px - proj_x)**2 + (py - proj_y)**2
    @staticmethod
    def segments_intersect(a1, a2, b1, b2):
        """判断线段 a1-a2 与 b1-b2 是否相交"""
        def ccw(A, B, C):
            return (C[1]-A[1]) * (B[0]-A[0]) > (B[1]-A[1]) * (C[0]-A[0])
        return ccw(a1, b1, b2) != ccw(a2, b1, b2) and ccw(a1, a2, b1) != ccw(a1, a2, b2)
    @staticmethod
    def is_point_in_polygon(p, vertices):
        x, y = p[0], p[1]
        inside = False
        j = len(vertices) - 1
        for i in range(len(vertices)):
            xi, yi = vertices[i]['x'], vertices[i]['y']
            xj, yj = vertices[j]['x'], vertices[j]['y']
            intersect = ((yi > y) != (yj > y)) and \
                        (x < (xj - xi) * (y - yi) / (yj - yi + 1e-9) + xi)
            if intersect:
                inside = not inside
            j = i
        return inside
    @staticmethod
    def check_collision(p1, p2, obs, safety_buffer=2.0):
        """
        检测线段 p1-p2 是否与障碍物 obs 碰撞。
        返回: (Boolean 是否碰撞, Float 障碍物高度)
        """
        otype = obs['type']
        opos = obs['position']
        ox, oy = opos['x'], opos['y']
        obs_height = obs.get('height', 0)
        # 1. 圆形/点
        if otype in ['circle', 'point']:
            r = obs.get('radius', 0)
            limit = r + safety_buffer
            dist_sq = GeometryUtils.point_to_segment_dist_sq((ox, oy), p1, p2)
            if dist_sq < limit**2:
                return True, obs_height
        # 2. 椭圆 (坐标变换法)
        elif otype == 'ellipse':
            w = obs.get('width', 0)
            l = obs.get('length', 0)
            semi_axis_x = w + safety_buffer
            semi_axis_y = l + safety_buffer
            
            def to_unit_space(point):
                dx = point[0] - ox
                dy = point[1] - oy
                return (dx / semi_axis_x, dy / semi_axis_y)
            u_p1 = to_unit_space(p1)
            u_p2 = to_unit_space(p2)
            dist_sq_in_unit_space = GeometryUtils.point_to_segment_dist_sq((0,0), u_p1, u_p2)
            
            if dist_sq_in_unit_space < 1.0:
                return True, obs_height
        # 3. 多边形
        elif otype == 'polygon':
            verts = obs['vertices']
            if not verts: return False, 0
            for i in range(len(verts)):
                v1 = (verts[i]['x'], verts[i]['y'])
                v2 = (verts[(i + 1) % len(verts)]['x'], verts[(i + 1) % len(verts)]['y'])
                if GeometryUtils.segments_intersect(p1, p2, v1, v2):
                    return True, obs_height
            if GeometryUtils.is_point_in_polygon(p1, verts) or GeometryUtils.is_point_in_polygon(p2, verts):
                return True, obs_height
        
        return False, 0
    
    # --- 新增方法: 仅检测单点是否在障碍物内 ---
    @staticmethod
    def check_point_overlap(point, obs, safety_buffer=2.0):
        """
        检测单个坐标点 point(x,y) 是否落在障碍物范围内
        """
        px, py = point
        otype = obs['type']
        opos = obs['position']
        ox, oy = opos['x'], opos['y']
        obs_height = obs.get('height', 0.0)
        
        # 1. 圆形/点 (原有逻辑正确)
        if otype in ['point', 'circle']:
            r = obs.get('radius', 0.0)
            if otype == 'point' and r is None: r = 0.5 
            limit = r + safety_buffer
            if (px - ox)**2 + (py - oy)**2 <= limit**2:
                return True, obs_height

        # 2. 椭圆 (原有逻辑正确)
        elif otype == 'ellipse':
            semi_x = (obs.get('width', 0) / 2.0) + safety_buffer
            semi_y = (obs.get('length', 0) / 2.0) + safety_buffer
            if semi_x > 0 and semi_y > 0:
                if ((px - ox)**2 / semi_x**2) + ((py - oy)**2 / semi_y**2) <= 1.0:
                    return True, obs_height

        # 3. 多边形 (修正版：增加 buffer 判定)
        elif otype == 'polygon':
            verts = obs.get('vertices', [])
            
            # A. 严格内部检测
            if GeometryUtils.is_point_in_polygon((px, py), verts):
                return True, obs_height
            
            # B. [新增] 边缘缓冲检测
            # 即使点在几何外部，如果距离任意一条边的距离小于 buffer，视为重叠
            buffer_sq = safety_buffer**2
            for i in range(len(verts)):
                v1 = (verts[i]['x'], verts[i]['y'])
                v2 = (verts[(i + 1) % len(verts)]['x'], verts[(i + 1) % len(verts)]['y'])
                # 计算点到边线段的距离平方
                dist_sq = GeometryUtils.point_to_segment_dist_sq((px, py), v1, v2)
                if dist_sq < buffer_sq:
                    return True, obs_height
        
        return False, 0.0

class ToolStates:
    def __init__(self):
        self.explored_count = 0

tool_states = ToolStates()

def create_uav_tools(client: UAVAPIClient) -> list:
    """
    Create all UAV control tools for LangChain agent using @tool decorator
    All tools that require parameters accept a JSON string input
    """

    # ========== Information Gathering Tools (No Parameters) ==========

    @tool
    def list_drones() -> str:
        """List all available drones in the current session with their status, battery level, and position.
        Use this to see what drones are available before trying to control them.

        No input required."""
        try:
            drones = client.list_drones()
            return json.dumps(drones, indent=2)
        except Exception as e:
            return f"Error listing drones: {str(e)}"
    
    def env_perception() -> bool:
        global obstacles_detected
        drones_data = client.list_drones()
        drone_ids = [drone["id"] for drone in drones_data]
        flag = False
        for drone_id in drone_ids:
            nearby = client.get_nearby_entities(drone_id)
            # Update explored targets
            _copied_targets_expolred = [x.id for x in targets_expolred]
            if 'targets' in nearby:
                for target_data in nearby['targets']:
                    targets_expolred.add(TargetInfo(target_data))
                    if target_data['id'] not in _copied_targets_expolred:
                        flag = True

            # Update detected obstacles
            _copied_obstacles_detected = [x.id for x in obstacles_detected]
            if 'obstacles' in nearby:
                for obstacle_data in nearby['obstacles']:
                    obstacles_detected.add(ObstacleInfo(obstacle_data))
                    if obstacle_data['id'] not in _copied_obstacles_detected:
                        flag = True
        return flag

    @tool
    def get_session_info() -> str:
        """Get current session information including task type, statistics, and status.
        Use this to understand what mission you need to complete.

        No input required."""
        try:
            session = client.get_current_session()
            env_changed = env_perception()
            if env_changed:
                session["tips"] = "Target/Obstacle status changed. Use `get_obstacles` or `get_targets` to get new information."
            return json.dumps(session, indent=2)
        except Exception as e:
            return f"Error getting session info: {str(e)}"
    
    @tool
    def get_session_data() -> str:
        """Get all session data including drones, targets, and obstacles.
        Use this to understand the environment and plan your mission.

        No input required."""
        try:
            session_data = client.get_session_data()
            return json.dumps(session_data, indent=2)
        except Exception as e:
            return f"Error getting session data: {str(e)}"
        
    @tool
    def get_task_progress() -> str:
        """Get mission task progress including completion percentage and status.
        Use this to track mission completion and see how close you are to finishing.

        No input required."""
        try:
            progress = client.get_task_progress()
            return json.dumps(progress, indent=2)
        except Exception as e:
            return f"Error getting task progress: {str(e)}"

    @tool
    def get_weather() -> str:
        """Get current weather conditions including wind speed, visibility, and weather type.
        Check this before takeoff to ensure safe flying conditions.

        No input required."""
        try:
            weather = client.get_weather()
            return json.dumps(weather, indent=2)
        except Exception as e:
            return f"Error getting weather: {str(e)}"

    @tool
    def get_targets_in_session() -> str:
        """Get all targets in the session including fixed, moving, waypoint, circle and polygon to search or patrol.
        Use this to see what targets you need to visit.

        No input required."""
        try:
            targets = client.get_targets()
            return json.dumps(targets, indent=2)
        except Exception as e:
            return f"Error getting targets: {str(e)}"

    @tool
    def get_targets() -> str:
        """Get all explored targets that have been detected so far.
        This returns targets from the agent's memory.

        No input required."""
        global targets_expolred
        try:
            env_changed = env_perception()
            targets_list = [target.to_dict() for target in targets_expolred]
            print([tgt.name for tgt in targets_expolred])
            return json.dumps(targets_list, indent=2)
        except Exception as e:
            return f"Error getting explored targets: {str(e)}"

    @tool
    def get_all_waypoints() -> str:
        """Get all waypoints in the session including coordinates and altitude.
        Use this to understand the where to charge that drones will follow.

        No input required."""
        try:
            waypoints = client.get_all_waypoints()
            return json.dumps(waypoints, indent=2)
        except Exception as e:
            return f"Error getting waypoints: {str(e)}"

    @tool
    def get_obstacles_in_session() -> str:
        """Get all obstacles in the session that drones must avoid.
        Use this to understand what obstacles exist in the environment.

        No input required."""
        try:
            obstacles = client.get_obstacles()
            return json.dumps(obstacles, indent=2)
        except Exception as e:
            return f"Error getting detected obstacles: {str(e)}"
        
    @tool
    def get_obstacles() -> str:
        """Get all obstacles that have been detected so far.
        This returns obstacles from the agent's memory.

        No input required."""
        global obstacles_detected
        try:
            env_changed = env_perception()
            obstacles_list = [obstacle.to_dict() for obstacle in obstacles_detected]
            print([obs.name for obs in obstacles_detected])
            return json.dumps(obstacles_list, indent=2)
        except Exception as e:
            return f"Error getting detected obstacles: {str(e)}"

    @tool
    def get_drone_status(input_json: str) -> str:
        """Get detailed status of a specific drone including position, battery, heading, and visited targets.

        Input should be a JSON string with:
        - drone_id: The ID of the drone (required, get from Action list_drones)

        Example: {{"drone_id": "04d6cfe7"}}
        """
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')

            if not drone_id:
                return "Error: drone_id is required"

            status = client.get_drone_status(drone_id)
            return json.dumps(status, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\"}}"
        except Exception as e:
            return f"Error getting drone status: {str(e)}"

    @tool
    def get_nearby_entities(input_json: str) -> str:
        """Get drones, targets, and obstacles near a specific drone (within its perception radius).
        This also updates the internal sets of explored targets and detected obstacles."""
        global targets_expolred, obstacles_detected
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')

            if not drone_id:
                return "Error: drone_id is required"

            nearby = client.get_nearby_entities(drone_id)

            # Update explored targets
            if 'targets' in nearby:
                for target_data in nearby['targets']:
                    targets_expolred.add(TargetInfo(target_data))

            # Update detected obstacles
            if 'obstacles' in nearby:
                for obstacle_data in nearby['obstacles']:
                    obstacles_detected.add(ObstacleInfo(obstacle_data))

            return json.dumps(nearby, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\"}}"
        except Exception as e:
            return f"Error getting nearby entities: {str(e)}"

    @tool
    def land(input_json: str) -> str:
        """Command a drone to land at its current position.

        Input should be a JSON string with:
        - drone_id: The ID of the drone (required, get from Action list_drones)

        Example: {{"drone_id": "drone-001"}}
        """
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')

            if not drone_id:
                return "Error: drone_id is required"

            result = client.land(drone_id)
            return json.dumps(result, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\"}}"
        except Exception as e:
            return f"Error during landing: {str(e)}"

    @tool
    def hover(input_json: str) -> str:
        """Command a drone to hover at its current position.

        Input should be a JSON string with:
        - drone_id: The ID of the drone (required, get from Action list_drones)
        - duration: Optional duration in seconds to hover (optional)

        Example: {{"drone_id": "drone-001", "duration": 5.0}}
        """
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')
            duration = params.get('duration')

            if not drone_id:
                return "Error: drone_id is required"

            result = client.hover(drone_id, duration)
            return json.dumps(result, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\"}}"
        except Exception as e:
            return f"Error hovering: {str(e)}"

    @tool
    def return_home(input_json: str) -> str:
        """Command a drone to return to its home position.

        Input should be a JSON string with:
        - drone_id: The ID of the drone (required, get from Action list_drones)

        Example: {{"drone_id": "drone-001"}}
        """
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')

            if not drone_id:
                return "Error: drone_id is required"

            result = client.return_home(drone_id)
            if result["status"] == "error":
                result["info"] = "If the drone is IDLE, first take off, then return home."
            return json.dumps(result, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\"}}"
        except Exception as e:
            return f"Error returning home: {str(e)}"

    @tool
    def set_home(input_json: str) -> str:
        """Set the drone's current position as its new home position.

        Input should be a JSON string with:
        - drone_id: The ID of the drone (required, get from Action list_drones)

        Example: {{"drone_id": "drone-001"}}
        """
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')

            if not drone_id:
                return "Error: drone_id is required"

            result = client.set_home(drone_id)
            return json.dumps(result, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\"}}"
        except Exception as e:
            return f"Error setting home: {str(e)}"

    @tool
    def calibrate(input_json: str) -> str:
        """Calibrate the drone's sensors.

        Input should be a JSON string with:
        - drone_id: The ID of the drone (required, get from Action list_drones)

        Example: {{"drone_id": "drone-001"}}
        """
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')

            if not drone_id:
                return "Error: drone_id is required"

            result = client.calibrate(drone_id)
            return json.dumps(result, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\"}}"
        except Exception as e:
            return f"Error calibrating: {str(e)}"

    @tool
    def take_photo(input_json: str) -> str:
        """Command a drone to take a photo.

        Input should be a JSON string with:
        - drone_id: The ID of the drone (required, get from Action list_drones)

        Example: {{"drone_id": "drone-001"}}
        """
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')

            if not drone_id:
                return "Error: drone_id is required"

            result = client.take_photo(drone_id)
            return json.dumps(result, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\"}}"
        except Exception as e:
            return f"Error taking photo: {str(e)}"

    # ========== Two Parameter Tools ==========

    @tool
    def take_off(input_json: str) -> str:
        """Command a drone to take off to a specified altitude.
        Drone must be on ground (idle or ready status).

        Input should be a JSON string with:
        - drone_id: The ID of the drone (required, get from Action list_drones)
        - altitude: Target altitude in meters (optional, default: 10.0)

        Example: {{"drone_id": "drone-001", "altitude": 15.0}}
        """
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')
            altitude = params.get('altitude', 10.0)

            if not drone_id:
                return "Error: drone_id is required"

            result = client.take_off(drone_id, altitude)
            if result["status"] == "success":
                result["message"] += f" Using `get_drone_status` to check if the drone\'s current position is start point. If not, fly to the start point first."
            return json.dumps(result, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\", \"altitude\": 15.0}}"
        except Exception as e:
            return f"Error during takeoff: {str(e)}"

    @tool
    def change_altitude(input_json: str) -> str:
        """Change a drone's altitude while maintaining X/Y position.

        Input should be a JSON string with:
        - drone_id: The ID of the drone (required, get from Action list_drones)
        - altitude: Target altitude in meters (required)

        Example: {{"drone_id": "drone-001", "altitude": 20.0}}
        """
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')
            altitude = params.get('altitude')

            if not drone_id:
                return "Error: drone_id is required"
            if altitude is None:
                return "Error: altitude is required"

            result = client.change_altitude(drone_id, altitude)
            return json.dumps(result, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\", \"altitude\": 20.0}}"
        except Exception as e:
            return f"Error changing altitude: {str(e)}"

    @tool
    def rotate(input_json: str) -> str:
        """Rotate a drone to face a specific direction.
        0=North, 90=East, 180=South, 270=West.

        Input should be a JSON string with:
        - drone_id: The ID of the drone (required, get from Action list_drones)
        - heading: Target heading in degrees 0-360 (required)

        Example: {{"drone_id": "drone-001", "heading": 90.0}}
        """
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')
            heading = params.get('heading')

            if not drone_id:
                return "Error: drone_id is required"
            if heading is None:
                return "Error: heading is required"

            result = client.rotate(drone_id, heading)
            return json.dumps(result, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\", \"heading\": 90.0}}"
        except Exception as e:
            return f"Error rotating: {str(e)}"

    @tool
    def send_message(input_json: str) -> str:
        """Send a message from one drone to another.

        Input should be a JSON string with:
        - drone_id: The ID of the sender drone (required, get from Action list_drones)
        - target_drone_id: The ID of the recipient drone (required, get from Action list_drones)
        - message: The message content (required)

        Example: {{"drone_id": "drone-001", "target_drone_id": "drone-002", "message": "Hello"}}
        """
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')
            target_drone_id = params.get('target_drone_id')
            message = params.get('message')

            if not drone_id:
                return "Error: drone_id is required"
            if not target_drone_id:
                return "Error: target_drone_id is required"
            if not message:
                return "Error: message is required"

            result = client.send_message(drone_id, target_drone_id, message)
            return json.dumps(result, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\", \"target_drone_id\": \"drone-002\", \"message\": \"...\"}}"
        except Exception as e:
            return f"Error sending message: {str(e)}"

    @tool
    def broadcast(input_json: str) -> str:
        """Broadcast a message from one drone to all other drones.

        Input should be a JSON string with:
        - drone_id: The ID of the sender drone (required, get from Action list_drones)
        - message: The message content (required)

        Example: {{"drone_id": "drone-001", "message": "Alert"}}
        """
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')
            message = params.get('message')

            if not drone_id:
                return "Error: drone_id is required"
            if not message:
                return "Error: message is required"

            result = client.broadcast(drone_id, message)
            return json.dumps(result, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\", \"message\": \"...\"}}"
        except Exception as e:
            return f"Error broadcasting: {str(e)}"

    @tool
    def charge(input_json: str) -> str:
        """Command a drone to charge its battery.
        Drone must be landed at a charging station.

        Input should be a JSON string with:
        - drone_id: The ID of the drone (required, get from Action list_drones)
        - charge_amount: Amount to charge in percent (required)

        Example: {{"drone_id": "drone-001", "charge_amount": 25.0}}
        """
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')
            charge_amount = params.get('charge_amount')

            if not drone_id:
                return "Error: drone_id is required"
            if charge_amount is None:
                return "Error: charge_amount is required"

            result = client.charge(drone_id, charge_amount)
            return json.dumps(result, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\", \"charge_amount\": 25.0}}"
        except Exception as e:
            return f"Error charging: {str(e)}"

    @tool
    def move_towards(input_json: str) -> str:
        """Move a drone a specific distance in a direction.
        Tips: Prefer using `auto_navigate_towards`. If it fails, use this tool and dynamically plan the destination.

        Input should be a JSON string with:
        - drone_id: The ID of the drone (required, get from Action list_drones)
        - distance: Distance to move in meters (required)
        - heading: Heading direction in degrees 0-360 (optional, default: current heading)
        - dz: Vertical component in meters (optional)

        Example: {{"drone_id": "drone-001", "distance": 10.0, "heading": 90.0}}
        """
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')
            distance = params.get('distance')
            heading = params.get('heading')
            dz = params.get('dz')

            if not drone_id:
                return "Error: drone_id is required"
            if distance is None:
                return "Error: distance is required"

            result = client.move_towards(drone_id, distance, heading, dz)
            if result["status"] == "error":
                result["message"] += "(1) If the task is to move a certain distance in a specific direction but the path is blocked by an obstacle, first move to a position where there is no obstacle in that direction, and then move the specified distance along that direction. (2) If the obstacle’s height is lower than the maximum altitude the drone can reach, the drone may ascend to an altitude higher than the obstacle and fly over it. If the obstacle's height is 0, then it indicates no drone can fly over it (In this case you need to detour). (3) Try other tools like `move_to` or `auto_navigate_to` (4) Do not move so far since the perception range is 150m."
            else:
                env_changed = env_perception()
                if env_changed:
                    result["tips"] = "Target/Obstacle status changed. Use `get_obstacles` or `get_targets` to get new information."
            return json.dumps(result, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\", \"distance\": 10.0}}"
        except Exception as e:
            return f"Error moving towards: {str(e)}"
        
    @tool
    def auto_navigate_towards(input_json: str) -> str:
        """Automatically find a route that bypasses obstacles and allows flying the specified distance in the given direction.
        
        Input should be a JSON string with:
        - drone_id: The ID of the drone (required)
        - distance: Distance to move in meters (required)
        - direction: Direction in degrees 0, 90, 180, 270 (required)
        """
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')
            direction = params.get('direction') # 0, 90, 180, 270
            min_distance = float(params.get('distance', 0.0))

            if not drone_id or direction is None:
                return "Error: drone_id and direction are required."

            # 获取状态
            drone_state = client.get_drone_status(drone_id)
            obstacles = [x.to_dict() for x in obstacles_detected]
            start_x = drone_state['position']['x']
            start_y = drone_state['position']['y']
            start_z = drone_state['position']['z']
            max_h = drone_state.get('max_altitude', 100.0)

            direction_map = {
                0: (0.0, 1.0),    # North
                180: (0.0, -1.0), # South
                90:  (1.0, 0.0),  # East
                270: (-1.0, 0.0)  # West
            }
            if direction not in direction_map:
                return json.dumps({"error": f"Invalid direction: {direction}. Must be 0, 90, 180, or 270."})
            
            dir_x, dir_y = direction_map[direction]
            
            # --- 核心修复 1: 更好的步长控制 ---
            current_distance = min_distance
            step_size = 5.0    # 减小步长以获得更精细的落点
            max_iterations = 750 
            safety_margin = 2.5 # 稍微加大缓冲，确保落点绝对安全
            height_buffer = 5.0 

            # --- 核心修复 2: 移除错误的 (0,0) 初始化 ---
            found_valid_target = False
            final_target_x = 0.0
            final_target_y = 0.0
            final_target_z = start_z

            for _ in range(max_iterations):
                # 计算当前尝试的坐标
                test_x = start_x + dir_x * current_distance
                test_y = start_y + dir_y * current_distance
                
                is_location_safe = True
                required_z = start_z 
                
                # 检查该点相对于所有障碍物的情况
                for obs in obstacles:
                    # 使用修复后的 check_point_overlap (必须包含 buffer 检查!)
                    is_inside, obs_h = GeometryUtils.check_point_overlap(
                        (test_x, test_y), obs, safety_buffer=safety_margin
                    )
                    
                    if is_inside:
                        # 冲突：检查是否可以飞越
                        # 如果障碍物过高 OR 是禁飞区(height=0) -> 该点不可用
                        if obs_h <= 0 or obs_h + height_buffer >= max_h:
                            is_location_safe = False
                            break 
                        else:
                            # 可以飞越 -> 推高 Z 轴
                            required_z = max(required_z, obs_h + height_buffer)
                
                if is_location_safe:
                    # 找到合法点
                    final_target_x = test_x
                    final_target_y = test_y
                    final_target_z = required_z
                    found_valid_target = True
                    break
                else:
                    # 当前距离的点不安全，继续向远处延伸
                    current_distance += step_size

            if not found_valid_target:
                return json.dumps({
                    "status": "error", 
                    "message": f"Could not find a safe target point in direction {direction} even after extending distance to {current_distance}m. Path is completely blocked."
                })

            # 生成导航指令
            nav_payload = json.dumps({
                "drone_id": drone_id,
                "x": final_target_x,
                "y": final_target_y,
                "z": final_target_z
            })

            # return f"Finding a destination successfully (dist={current_distance}m)... Now use `auto_navigate_to({nav_payload})` to move the drone. This tool safely brings drone to the destination and detour obstacles. First try it with exactly input {nav_payload}, if it fails, then adjust the positions."
            return auto_navigate_to_non_tool(nav_payload, move_towards_flag=True, move_towards_direction=direction, move_towards_distance=min_distance)
            
        except Exception as e:
            return json.dumps({"status": "error", "message": f"Error executing auto_navigate_towards: {str(e)}"})



    # @tool
    # def move_along_path(input_json: str) -> str:
    #     """Move a drone along a path of waypoints.

    #     Input should be a JSON string with:
    #     - drone_id: The ID of the drone (required)
    #     - waypoints: List of points with x, y, z coordinates (required)

    #     Example: {{"drone_id": "drone-001", "waypoints": [{{"x": 10, "y": 10, "z": 10}}, {{"x": 20, "y": 20, "z": 10}}]}}
    #     """
    #     try:
    #         params = json.loads(input_json) if isinstance(input_json, str) else input_json
    #         drone_id = params.get('drone_id')
    #         waypoints = params.get('waypoints')

    #         if not drone_id:
    #             return "Error: drone_id is required"
    #         if not waypoints:
    #             return "Error: waypoints list is required"

    #         result = client.move_along_path(drone_id, waypoints)
    #         return json.dumps(result, indent=2)
    #     except json.JSONDecodeError as e:
    #         return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\", \"waypoints\": [...]}}"
    #     except Exception as e:
    #         return f"Error moving along path: {str(e)}"

    # ========== Multi-Parameter Tools ==========

    @tool
    def get_nearest_waypoint(input_json: str) -> str:
        """Get the nearest waypoint to a specific drone.
        Input should be a JSON string with:
        - x: The x-coordinate of the drone (required)
        - y: The y-coordinate of the drone (required)
        - z: The z-coordinate of the drone (required)

        Example: {{"x": 0.0, "y": 0.0, "z": 0.0}}"""
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            x = params.get('x')
            y = params.get('y')
            z = params.get('z')

            if x is None or y is None or z is None:
                return "Error: x, y, and z coordinates are required"
            nearest = client.get_nearest_waypoint(x, y, z)
            return json.dumps(nearest, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"x\": 0.0, \"y\": 0.0, \"z\": 0.0}}"
        except Exception as e:
            return f"Error getting nearest waypoint: {str(e)}"
        
    @tool
    def move_to(input_json: str) -> str:
        """Move a drone to specific 3D coordinates (x, y, z).
        Always check for collisions first using check_path_collision.
        Tips: Prefer using `auto_navigate_to`. If it fails, use this tool and dynamically plan the destination.

        Input should be a JSON string with:
        - drone_id: The ID of the drone (required)
        - x: Target X coordinate in meters (required)
        - y: Target Y coordinate in meters (required)
        - z: Target Z coordinate (altitude) in meters (required)

        Example: {{"drone_id": "drone-001", "x": 100.0, "y": 50.0, "z": 20.0}}
        """
        try:
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')
            x = params.get('x')
            y = params.get('y')
            z = params.get('z')

            if not drone_id:
                return "Error: drone_id is required"
            if x is None or y is None or z is None:
                return "Error: x, y, and z coordinates are required"

            result = client.move_to(drone_id, x, y, z)
            env_changed = env_perception()
            if env_changed:
                result["tips"] = "Target/Obstacle status changed. Use `get_obstacles` or `get_targets` to get new information."
            if result["status"] == "error":
                result["message"] += "(1) If the task is to move a certain distance in a specific direction but the path is blocked by an obstacle, first move to a position where there is no obstacle in that direction, and then move the specified distance along that direction. (2) If the obstacle’s height is lower than the maximum altitude the drone can reach, the drone may ascend to an altitude higher than the obstacle and fly over it. If the obstacle's height is 0, then it indicates no drone can fly over it (In this case you need to detour). (3) You can use `get_obstacles` tool to see the obstacle, and pay attention to the obstacle\'s shape and size. (4) You may try `auto_navigate_to` tool to detour if it works. (5) Do not move so far since the perception range is 150m."
            
            # check if the point is reached by any drone
            scan_point_is_reached_by_drones(drone_id, x, y)
            
            return json.dumps(result, indent=2)
        except json.JSONDecodeError as e:
            return f"Error parsing JSON input: {str(e)}. Expected format: {{\"drone_id\": \"drone-001\", \"x\": 100.0, \"y\": 50.0, \"z\": 20.0}}"
        except Exception as e:
            return f"Error moving drone: {str(e)}"

    @tool
    def auto_navigate_to(input_json: str) -> str:
        """
        Plan an obstacle-avoiding path using analytic geometry for precise collision detection.
        
        Input should be a JSON string with:
        - drone_id: The ID of the drone (required)
        - x: Target X coordinate in meters (required)
        - y: Target Y coordinate in meters (required)
        - z: Target Z coordinate (altitude) in meters (required)

        Example: {{"drone_id": "drone-001", "x": 100.0, "y": 50.0, "z": 20.0}}
        """
        # ---------------------------------------------------------
        result = auto_navigate_to_non_tool(input_json)
        return result
    
    def auto_navigate_to_non_tool(input_json: str, move_towards_flag: bool = False, move_towards_distance: float = 0.0, move_towards_direction: float = 0.0) -> str:
        global obstacles_detected
        try:
            # 1. 解析参数
            params = json.loads(input_json) if isinstance(input_json, str) else input_json
            drone_id = params.get('drone_id')
            tx, ty, tz = params.get('x'), params.get('y'), params.get('z')

            if not drone_id or tx is None or ty is None or tz is None:
                return json.dumps({"status": "error", "message": "drone_id, x, y, z are required"})
            
            # 2. 获取状态
            status = client.get_drone_status(drone_id)
            start_pos = status['position']
            sx, sy, sz = start_pos['x'], start_pos['y'], start_pos['z']
            drone_max_alt = status.get('max_altitude', 100.0)
            all_obstacles = [obs.to_dict() for obs in obstacles_detected]

            # 3. 分类
            mandatory_avoid = []
            fly_over_candidates = []
            for obs in all_obstacles:
                h = obs.get('height', 0)
                if h == 0 or h >= drone_max_alt:
                    mandatory_avoid.append(obs)
                else:
                    fly_over_candidates.append(obs)

            target_point = (tx, ty)
            start_point = (sx, sy)

            # === 核心修复 1: 端点合法性预检查 ===
            for obs in mandatory_avoid:
                # 检查终点是否在严格障碍物内 (buffer=0)
                in_obs, _ = GeometryUtils.check_collision(target_point, target_point, obs, safety_buffer=0.0)
                if in_obs:
                    return json.dumps({"status": f"Failed: Target position is inside obstacle '{obs.get('name', 'Unknown')}'. Path planning aborted."})
                
                # 检查起点是否在严格障碍物内
                in_obs_start, _ = GeometryUtils.check_collision(start_point, start_point, obs, safety_buffer=0.0)
                if in_obs_start:
                    # 如果起点在障碍物内，尝试飞出来（允许）
                    pass 

            # 4. 2D 路径规划
            
            # 4.1 生成节点
            nodes = [start_point, target_point]
            safety_margin = 5.0
            
            for obs in mandatory_avoid:
                opos = obs['position']
                ox, oy = opos['x'], opos['y']
                
                if obs['type'] == 'polygon':
                    center_x = sum(v['x'] for v in obs['vertices']) / len(obs['vertices'])
                    center_y = sum(v['y'] for v in obs['vertices']) / len(obs['vertices'])
                    for v in obs['vertices']:
                        vx, vy = v['x'], v['y']
                        vec_len = math.hypot(vx - center_x, vy - center_y)
                        if vec_len > 0:
                            scale = (vec_len + safety_margin) / vec_len
                            nx = center_x + (vx - center_x) * scale
                            ny = center_y + (vy - center_y) * scale
                            nodes.append((nx, ny))
                
                elif obs['type'] == 'ellipse':
                    w = obs.get('width', 0)
                    l = obs.get('length', 0)
                    gen_w = w + safety_margin
                    gen_l = l + safety_margin
                    num_steps = max(8, int(max(w, l) / 3.0))
                    for i in range(num_steps):
                        angle = i * (2 * math.pi / num_steps)
                        nx = ox + gen_w * math.cos(angle)
                        ny = oy + gen_l * math.sin(angle)
                        nodes.append((nx, ny))
                        
                elif obs['type'] in ['circle', 'point']:
                    r = obs.get('radius', 0)
                    gen_r = r + safety_margin
                    num_steps = max(8, int(r / 3.0))
                    for i in range(num_steps):
                        angle = i * (2 * math.pi / num_steps)
                        nodes.append((ox + gen_r * math.cos(angle), oy + gen_r * math.sin(angle)))

            # 4.2 过滤非法节点
            valid_nodes = []
            for node in nodes:
                is_bad = False
                for obs in mandatory_avoid:
                    collided, _ = GeometryUtils.check_collision(node, node, obs, safety_buffer=0.5)
                    if collided:
                        is_bad = True
                        break
                if not is_bad:
                    valid_nodes.append(node)
            
            # 强制加回起点终点 (即使它们在缓冲区内)
            if start_point not in valid_nodes: valid_nodes.insert(0, start_point)
            if target_point not in valid_nodes: valid_nodes.append(target_point)
            start_idx = valid_nodes.index(start_point)
            target_idx = valid_nodes.index(target_point)
            
            # 4.3 构建图
            adj = {i: [] for i in range(len(valid_nodes))}
            for i in range(len(valid_nodes)):
                for j in range(i + 1, len(valid_nodes)):
                    u, v = valid_nodes[i], valid_nodes[j]
                    
                    # === 核心修复 2: 动态缓冲区策略 ===
                    # 如果线段连接的是起点或终点，使用“宽松缓冲区”(0.1m)
                    # 允许无人机在安全的情况下起飞或降落到靠近障碍物的地方
                    is_start_or_end_edge = (i == start_idx or i == target_idx or j == start_idx or j == target_idx)
                    current_buffer = 0.1 if is_start_or_end_edge else 2.0
                    
                    path_blocked = False
                    for obs in mandatory_avoid:
                        hit, _ = GeometryUtils.check_collision(u, v, obs, safety_buffer=current_buffer)
                        if hit:
                            path_blocked = True
                            break
                    
                    if not path_blocked:
                        dist = math.hypot(u[0]-v[0], u[1]-v[1])
                        adj[i].append((j, dist))
                        adj[j].append((i, dist))

            # 4.4 Dijkstra
            pq = [(0.0, start_idx, [valid_nodes[start_idx]])]
            visited = set()
            path_2d = []
            
            while pq:
                cost, u, path = heapq.heappop(pq)
                if u == target_idx:
                    path_2d = path
                    break
                if u in visited: continue
                visited.add(u)
                
                for v_idx, w in adj[u]:
                    if v_idx not in visited:
                        heapq.heappush(pq, (cost + w, v_idx, path + [valid_nodes[v_idx]]))
            
            if not path_2d:
                # 如果依然找不到路径，说明真的被封死了
                return json.dumps({"status": "Failed: No 2D path found. Target is reachable but likely fully enclosed by obstacles."})

            # 5. 高度计算
            safe_base_alt = max(sz, tz) 
            path_max_obs_height = 0.0
            for i in range(len(path_2d) - 1):
                p1 = path_2d[i]
                p2 = path_2d[i+1]
                for obs in fly_over_candidates:
                    hit, obs_h = GeometryUtils.check_collision(p1, p2, obs, safety_buffer=2.0)
                    if hit:
                        path_max_obs_height = max(path_max_obs_height, obs_h)

            if path_max_obs_height > 0:
                cruise_alt = path_max_obs_height + 2.0
            else:
                cruise_alt = safe_base_alt

            cruise_alt = max(cruise_alt, sz, tz)
            if cruise_alt > drone_max_alt:
                return json.dumps({"status": "Failed: Required altitude exceeds drone capability."})

            # 6. 生成航点
            waypoints = []
            waypoints.append((sx, sy, sz))
            if cruise_alt > sz + 0.5:
                waypoints.append((sx, sy, cruise_alt))
            for i in range(len(path_2d)):
                node = path_2d[i]
                if i == 0 and math.hypot(node[0]-sx, node[1]-sy) < 0.1:
                    continue
                waypoints.append((node[0], node[1], cruise_alt))
            last_wp = waypoints[-1]
            if abs(last_wp[2] - tz) > 0.5 or math.hypot(last_wp[0]-tx, last_wp[1]-ty) > 0.1:
                waypoints.append((tx, ty, tz))
                
            # 7. 执行
            final_msg = "Success"
            for wp in waypoints:
                if wp == waypoints[0] and len(waypoints) > 1:
                    continue
                waypoint_move_result = client.move_to(drone_id, wp[0], wp[1], wp[2])
                env_changed = env_perception()
                if env_changed:
                    waypoint_move_result["tips"] = "Target/Obstacle status changed. Use `get_obstacles` or `get_targets` to get new information."
                if waypoint_move_result["status"] == "error":
                    error_str = f"Error automatically navigate to waypoint {wp}: {waypoint_move_result['message']}"
                    if env_changed:
                        error_str += f" (Target/Obstacle status changed. Use `get_obstacles` or `get_targets` to get new information.) If fails too many times of automatic navigation, consider dynamically plan a way."
                    else:
                        error_str += f" Try not move so far."
                    return json.dumps({"status": "error", "message": error_str})
                
                # check if the point is reached by any drone
                scan_point_is_reached_by_drones(drone_id, wp[0], wp[1])

                final_msg = waypoint_move_result.get("message", "Success")
            
            if len(waypoints) > 1 and move_towards_flag:
                client.move_towards(drone_id, move_towards_distance, move_towards_direction)
                env_changed = env_perception()
                if env_changed:
                    additional_info = "Target/Obstacle status changed. Use `get_obstacles` or `get_targets` to get new information."
                    return json.dumps({"status": "success", "path": waypoints, "message": final_msg, "tips": additional_info})

            return json.dumps({"status": "success", "path": waypoints, "message": final_msg})

        except Exception as e:
            return json.dumps({"status": "error", "message": f"Error executing path finding: {str(e)}"})
    
    @tool
    def auto_explore(input_json: str) -> str:
        """
        Automatically move the drone to achieve a specified coverage ratio with respect to a given target.
        
        Input should be a JSON string with:
        - drone_id: The ID of the drone (required)
        - target_id: The ID of the target (required)
        - coverage: Target coverage ratio (required, e.g., 0.95 or 95)

        Example: {{"drone_id": "drone-001", "target_id": "target-001", "coverage": 0.95}}
        """
        import json
        import math

        # ================= 1. 内部几何判定函数 =================
        def check_drone_in_target(drone_id: str, target_id: str):
            # 获取数据
            drone = client.get_drone_status(drone_id)
            target = client.get_target_status(target_id)
            
            # 提取无人机坐标
            d_pos = drone['position']
            d_x, d_y, d_z = d_pos['x'], d_pos['y'], d_pos['z']
            
            # 提取目标坐标信息
            t_pos = target['position']
            t_z = t_pos['z']
            
            task_radius = drone.get('task_radius', 10.0)
            buffer = task_radius * 0.5

            # --- 判定条件 1: Z 坐标相同 ---(无需判断)
            # if not math.isclose(d_z, t_z, abs_tol=0.1):
            #     return False, drone, target # 返回数据以便复用

            # --- 判定条件 2: X、Y 坐标在平面内 ---
            t_type = target['type']
            inside = False
            
            # 情况 A: 多边形 (Polygon)
            if t_type == 'polygon':
                vertices = target.get('vertices', [])
                if not vertices:
                    return False, drone, target
                    
                # 射线法
                j = len(vertices) - 1
                for i in range(len(vertices)):
                    xi, yi = vertices[i]['x'], vertices[i]['y']
                    xj, yj = vertices[j]['x'], vertices[j]['y']
                    
                    intersect = ((yi > d_y) != (yj > d_y)) and \
                                (d_x < (xj - xi) * (d_y - yi) / (yj - yi + 1e-9) + xi)
                    if intersect:
                        inside = not inside
                    j = i
                if not inside and buffer > 0:
                    for i in range(len(vertices)):
                        v1 = (vertices[i]['x'], vertices[i]['y'])
                        v2 = (vertices[(i + 1) % len(vertices)]['x'], vertices[(i + 1) % len(vertices)]['y'])
                        if GeometryUtils.point_to_segment_dist_sq((d_x, d_y), v1, v2) <= buffer**2:
                            inside = True
                            break
            
            # 情况 B: 圆形区域
            elif t_type in ['circle', 'waypoint', 'fixed']:
                t_x, t_y = t_pos['x'], t_pos['y']
                radius = target.get('radius', 0.0) + buffer
                distance = math.sqrt((d_x - t_x)**2 + (d_y - t_y)**2)
                inside = distance <= radius
            
            return inside, drone, target

        # ================= 2. 辅助函数：点是否在目标内 =================
        def is_point_in_target(x, y, target_data, buffer=0.0):
            t_type = target_data['type']
            if t_type == 'polygon':
                vertices = target_data.get('vertices', [])
                inside = False
                j = len(vertices) - 1
                for i in range(len(vertices)):
                    xi, yi = vertices[i]['x'], vertices[i]['y']
                    xj, yj = vertices[j]['x'], vertices[j]['y']
                    intersect = ((yi > y) != (yj > y)) and \
                                (x < (xj - xi) * (y - yi) / (yj - yi + 1e-9) + xi)
                    if intersect:
                        inside = not inside
                    j = i
                if inside: return True
                if buffer > 0:
                    for i in range(len(vertices)):
                        v1 = (vertices[i]['x'], vertices[i]['y'])
                        v2 = (vertices[(i + 1) % len(vertices)]['x'], vertices[(i + 1) % len(vertices)]['y'])
                        if GeometryUtils.point_to_segment_dist_sq((x, y), v1, v2) <= buffer**2:
                            return True
                return False
            else: # Circle based
                t_x, t_y = target_data['position']['x'], target_data['position']['y']
                radius = target_data.get('radius', 0.0) + buffer
                return math.sqrt((x - t_x)**2 + (y - t_y)**2) <= radius

        # ================= 3. 主逻辑 =================
        try:
            data = json.loads(input_json)
            drone_id = data['drone_id']
            # 注意：这里我们假设输入包含 target_id，因为单纯的 x,y,z 无法描述多边形形状
            # 如果必须使用 x,y,z 寻找 target，则需要额外的逻辑去匹配 target_id
            target_id = data.get('target_id', 'unknown_target') 
            required_coverage = data.get('coverage', 0.95)
            # 归一化覆盖率输入 (处理 95 为 0.95)
            if required_coverage > 1:
                required_coverage /= 100.0
        except Exception as e:
            return f"Error parsing input: {str(e)}"

        # 3.1 初始位置检查
        is_inside, drone_data, target_data = check_drone_in_target(drone_id, target_id)
        if not is_inside:
            return f"Error: Drone {drone_id} is not inside target {target_id}. Please move inside first."

        # 3.2 路径规划准备
        task_radius = drone_data.get('task_radius', 10.0)
        current_z = drone_data['position']['z']
        
        # 增加覆盖缓冲区，确保边缘覆盖 (取半径的一半作为外扩)
        coverage_buffer = task_radius * 0.5 
        
        # 计算 Bounding Box (边界框)，并向外扩张缓冲区
        if target_data['type'] == 'polygon':
            vx = [v['x'] for v in target_data['vertices']]
            vy = [v['y'] for v in target_data['vertices']]
            min_x, max_x = min(vx) - coverage_buffer, max(vx) + coverage_buffer
            min_y, max_y = min(vy) - coverage_buffer, max(vy) + coverage_buffer
        else:
            tx, ty = target_data['position']['x'], target_data['position']['y']
            r = target_data['radius'] + coverage_buffer
            min_x, max_x = tx - r, tx + r
            min_y, max_y = ty - r, ty + r

        # 3.3 网格化路径生成 (Grid Decomposition)
        # 步长设定：为了保证覆盖率，步长通常设为 task_radius 的 √2 倍或更小
        # 这里设为 task_radius 确保每个网格点代表的圆都有重叠，保证无缝隙
        step_size = task_radius * 1.414
        
        valid_waypoints = [] # 存储所有位于目标内部的网格点
        
        # 扫描 Bounding Box
        x_cursor = min_x
        while x_cursor <= max_x:
            y_cursor = min_y
            col_points = []
            while y_cursor <= max_y:
                # 使用带缓冲区的判定，确保边缘点也被纳入扫描路径
                if is_point_in_target(x_cursor, y_cursor, target_data, coverage_buffer):
                    col_points.append({'x': x_cursor, 'y': y_cursor})
                y_cursor += step_size
            
            # 蛇形排序 (Boustrophedon): 偶数列正向，奇数列反向
            # 这样可以最小化无人机换列时的飞行距离
            if col_points:
                # 根据当前的列数决定方向，这里用 valid_waypoints 已有长度估算列是不准的
                # 简单做法：利用 x_cursor 的归一化索引，或者直接交替 append
                pass # 后续统一处理
            
            valid_waypoints.append(col_points)
            x_cursor += step_size

        # 展平并执行蛇形排序
        final_path = []
        for i, col in enumerate(valid_waypoints):
            if i % 2 == 1:
                final_path.extend(reversed(col))
            else:
                final_path.extend(col)

        total_points = len(final_path)
        if total_points == 0:
            return "Error: Target area is too small or invalid geometry found."

        # 3.4 执行探索
        # 根据已探索进度初始化覆盖率
        current_coverage = tool_states.explored_count / total_points
        first_warning = 0
        for idx, wp in enumerate(final_path):
            if idx < tool_states.explored_count:
                continue
                
            # 检查无人机电池电量
            drone_status = client.get_drone_status(drone_id)
            battery_level = drone_status.get('battery_level', 100.0)
            
            # 如果电量低于30%，则前往最近的充电站

            if battery_level < 30.0:
                # 查找最近的充电站
                try:
                    waypoints = client.get_all_waypoints()
                    charging_stations = [wp for wp in waypoints if wp.get('charge_amount', 0) > 0]
                    
                    if charging_stations:
                        #返回后交给充电工具处理, 充电完成后会再继续执行
                        return f" {drone_id} is low on battery ({battery_level:.1f}%)! Please charge at the nearest station. Then continue auto exploring."

                    else:
                        if first_warning == 0:
                            print("Warning: Low battery but no charging stations found!")
                            first_warning = 1
                            return "Wait for a while and continue calling this function! return [TASK DONE] this time"
                except Exception as e:
                    print(f"Error during charging process: {str(e)}")
            
            # 移动无人机
            client.move_to(drone_id, wp['x'], wp['y'], current_z)
            
            tool_states.explored_count += 1
            current_coverage = tool_states.explored_count / total_points
            
            # 检查是否达标
            if current_coverage >= required_coverage:
                explored_count = tool_states.explored_count
                tool_states.explored_count = 0
                return f"Success: Target explored with coverage {current_coverage:.2%} (Visited {explored_count}/{total_points} grid points)"
        if math.isclose(current_coverage, 0.0):
            return f"Finished path. Final coverage: {current_coverage:.2%}. Please try call this tool again to continue exploring."
        else:
            return f"Finished path. Final coverage: {current_coverage:.2%}. Wait for a while and continue calling this function! return [TASK DONE] this time"
    
    @tool
    def auto_scan_all_environment() -> str:
        """
        Command all available drones to collaboratively scan the area (0,0) to (1024,1024) using obstacle avoidance.
        Drones will plan safe paths to grid points, avoiding known obstacles, and detecting new ones along the way.
        
        No input required.
        """
        global targets_expolred, obstacles_detected, scan_points
        import math
        import json

        try:
            # 1. 初始化：获取无人机并起飞
            drones = client.list_drones()
            if not drones:
                return "Error: No drones available for scanning."
            
            # 获取无人机 ID
            drone_ids = [d['id'] for d in drones]
            active_drones = []
            
            # 起飞高度设置为 20m，既能避开低矮障碍，也能获得较好视野
            scan_altitude = 20.0
            
            for did in drone_ids:
                status = client.get_drone_status(did)
                if status.get('state') == 'ground' or status['position']['z'] < 1.0:
                    client.take_off(did, altitude=scan_altitude)
                active_drones.append(did)
            
            # 初始感知：起飞后先看一眼，建立初步地图
            env_perception()
            
            # # 2. 生成扫描网格 (Grid Generation)
            # # 区域 1024x1024。步长设为 250m，平衡覆盖率与时间成本
            # scan_points = []
            # area_size = 1024.0
            # step_size = 250.0 
            
            # x = step_size / 2
            # while x < area_size:
            #     y = step_size / 2
            #     while y < area_size:
            #         scan_points.append((x, y))
            #         y += step_size
            #     x += step_size
            
            # 2. 从scan_points中获得需要未探索的点
            unvisited_points = [point for point in scan_points if not point.visited]

            # 3. 任务分配 (基于区域：左上、左下、右上、右下)
            cx, cy = 512, 384
            quadrants_points = {"TL": [], "BL": [], "TR": [], "BR": []}
            for p in unvisited_points:
                if p.x < cx:
                    if p.y >= cy: quadrants_points["TL"].append(p)
                    else: quadrants_points["BL"].append(p)
                else:
                    if p.y >= cy: quadrants_points["TR"].append(p)
                    else: quadrants_points["BR"].append(p)
            
            tasks = {did: [] for did in active_drones}
            num_drones = len(active_drones)
            drone_quadrants = {}
            for did in active_drones:
                d_status = client.get_drone_status(did)
                pos = d_status['position']
                q = "TL" if pos['x'] < cx and pos['y'] >= cy else \
                    "BL" if pos['x'] < cx and pos['y'] < cy else \
                    "TR" if pos['x'] >= cx and pos['y'] >= cy else "BR"
                drone_quadrants.setdefault(q, []).append(did)
            
            assigned_points = set()
            for q, q_points in quadrants_points.items():
                q_drones = drone_quadrants.get(q, [])
                if q_drones:
                    for i, p in enumerate(q_points):
                        tasks[q_drones[i % len(q_drones)]].append(p)
                        assigned_points.add(p)

            # print("Drone Quadrants:", drone_quadrants)
            # print("Quadrants Points:", quadrants_points)
            
            # 兜底：分配那些所在区域没有无人机的点
            remaining_points = [p for p in unvisited_points if p not in assigned_points]
            # print("Remaining points:", remaining_points)
            if remaining_points:
                for i, p in enumerate(remaining_points):
                    tasks[active_drones[i % num_drones]].append(p)
                
            scan_log = []
            max_points = max([len(t) for t in tasks.values()]) if tasks else 0
            
            # 4. 执行循环：规划 -> 移动 -> 感知
            for i in range(max_points):
                for did in active_drones:
                    if i < len(tasks[did]):
                        point = tasks[did][i]
                        tx, ty = point.x, point.y
                        
                        # 构造导航参数
                        nav_payload = json.dumps({
                            "drone_id": did,
                            "x": tx,
                            "y": ty,
                            "z": scan_altitude
                        })
                        
                        # === 核心修改：调用 auto_navigate_to_non_tool 进行路径规划 ===
                        # 该函数会利用当前 obstacles_detected 里的数据计算避障路径
                        # 如果目标点在障碍物内，它会返回错误信息，我们只需记录并跳过
                        try:
                            # 1. 尝试导航
                            nav_result_str = auto_navigate_to_non_tool(nav_payload)
                            nav_result = json.loads(nav_result_str)
                            
                            status = nav_result.get("status", "error")
                            if status == "success":
                                # 标记该点已访问
                                scan_point_is_reached_by_drones(did, tx, ty)
                            
                            # 2. 无论导航成功与否（可能半路停下），都进行感知
                            # 这对于"一边撞墙一边开图"的探索过程至关重要
                            new_entities_found = env_perception()
                            
                            log_msg = f"Drone {did} -> ({tx:.1f}, {ty:.1f}): {status}"
                            if new_entities_found:
                                log_msg += " [New Entities Detected]"
                            scan_log.append(log_msg)
                            
                        except Exception as nav_err:
                            scan_log.append(f"Drone {did} nav error: {str(nav_err)}")
                            # 即使出错，也要尝试感知当前位置
                            env_perception()
            unvisited_points = [point for point in scan_points if not point.visited]
            # 5. 汇总结果
            return json.dumps({
                "status": "success",
                "message": f"Global smart scan completed with {num_drones} drones.",
                "total_targets_detected": len(targets_expolred),
                "total_obstacles_detected": len(obstacles_detected),
                "scan_details": scan_log,
                "scan_all_environment_completed": (len(unvisited_points) <= 0)
            }, indent=2)

        except Exception as e:
            return f"Error during smart scanning: {str(e)}"
    
    @tool
    def get_scan_points() -> str:
        """Get the list of scan points categorized by visited status and current drone info(pos,status).

        No input required."""
        try:
            visited_points = [p.to_dict() for p in scan_points if p.visited]
            unvisited_points = [p.to_dict() for p in scan_points if not p.visited]

            drones = client.list_drones()
            drones_info = {d['id']: {'pos': d['position'], 'status': d['status']} for d in drones}

            result = {
                "visited_points": visited_points,
                "unvisited_points": unvisited_points,
                "drones_info": drones_info
            }
            return json.dumps(result, indent=2)
        except Exception as e:
            return f"Error getting scan points: {str(e)}"

    @tool
    def get_more_possible_scan_points() -> str:
        """
        When scan_points are all visited, but cannot find the target, use this function to get more possible scan points, which x or y is negative.
        
        No input required.
        """
        global scan_points
        scan_points += map_to_drones_points(1024,-768)
        scan_points += map_to_drones_points(-1024,768)
        scan_points += map_to_drones_points(-1024,-768)
        return """SUCCESS: Search area expanded with negative coordinate grids. 
        ACTION REQUIRED: Call 'get_scan_points' to retrieve the new unvisited points. 
        STRATEGY: Use `auto_scan_all_environment` first.Then Prioritize unvisited points within a 300m radius of (0,0), like (-157,57), (-157,-256),(256,-157).et.
        """
    # Return all tools
    return [
        list_drones,
        get_drone_status,
        get_session_info,
        # get_session_data,
        # get_task_progress,
        get_weather,
        auto_explore,
        # get_targets,
        get_obstacles,
        # get_nearby_entities,
        take_off,
        land,
        move_to,
        auto_navigate_to,
        auto_navigate_towards,
        move_towards,
        change_altitude,
        hover,
        rotate,
        return_home,
        set_home,
        calibrate,
        take_photo,
        send_message,
        broadcast,
        charge,
        get_nearest_waypoint,
        get_all_waypoints,
        get_targets,
        auto_scan_all_environment,
        get_scan_points,
        get_more_possible_scan_points
    ]