"""
Map middleware main class
"""
from typing import Tuple, Optional, List
import yaml
import os
import numpy as np
from scipy.spatial.distance import cdist
from scipy.spatial import KDTree
from .data_structures import Scene, Object, Trajectory, OccupancyMapConfig
from .scene_builder import SceneBuilder
from .map_generator import MapGenerator
from .visualizer import Visualizer


class MapMiddleware:
    """Map middleware main class"""
    
    def __init__(self, config_path: str = "config.yaml"):
        """Initialize map middleware
        
        Args:
            config_path: Configuration file path
        """
        self.config_path = config_path
        with open(config_path, 'r', encoding='utf-8') as f:
            self.config = yaml.safe_load(f)
        
        # Initialize components
        self.scene_builder = SceneBuilder(config_path)
        self.map_generator = MapGenerator(config_path)
        self.visualizer = Visualizer(config_path)
        
        # Initialize optimized path finder
        self.optimized_path_finder = OptimizedPathFinder(self)
    
    def create_scene(self, label: str, size: Tuple[float, float, float] = None) -> Scene:
        """Create new scene
        
        Args:
            label: Scene label
            size: Scene size, defaults to configured default size
            
        Returns:
            Scene: Newly created scene
        """
        return self.scene_builder.create_scene(label, size)
    
    def add_object_to_scene(self, scene: Scene, obj: Object, 
                           x_min: float, y_min: float, z_min: float):
        """Add object to scene
        
        Args:
            scene: Target scene
            obj: Object to add
            x_min, y_min, z_min: Minimum coordinates of object (can be negative)
        """
        return self.scene_builder.add_object_to_scene(scene, obj, x_min, y_min, z_min)
    
    def auto_resize_scene(self, scene: Scene, margin: float = None) -> Scene:
        """Automatically resize scene to fit all objects
        
        Args:
            scene: Scene to resize
            margin: Additional margin around objects
            
        Returns:
            Scene: Resized scene
        """
        return self.scene_builder.resize_scene_to_objects(scene, margin)
    
    def resize_scene_to_objects(self, scene: Scene) -> Scene:
        """Resize scene based on objects
        
        Args:
            scene: Scene to resize
            
        Returns:
            Scene: Resized scene
        """
        return self.scene_builder.resize_scene_to_objects(scene)
    
    def save_scene(self, scene: Scene, save_path: str = None) -> str:
        """Save scene to JSON file
        
        Args:
            scene: Scene to save
            save_path: Save path, defaults to configured default path
            
        Returns:
            str: Actual saved file path
        """
        return self.scene_builder.save_scene(scene, save_path)
    
    def load_scene(self, scene_path: str) -> Scene:
        """Load scene from JSON file
        
        Args:
            scene_path: Scene file path
            
        Returns:
            Scene: Loaded scene
        """
        return self.scene_builder.load_scene(scene_path)
    
    def generate_occupancy_map(self, scene: Scene, scale: float = None, 
                              save_path: str = None) -> Tuple[object, OccupancyMapConfig]:
        """Generate occupancy map from scene
        
        Args:
            scene: Scene
            scale: Grid resolution, defaults to configured default value
            save_path: Save path prefix, defaults to configured default path
            
        Returns:
            Tuple[np.ndarray, OccupancyMapConfig]: Occupancy map and configuration
        """
        return self.map_generator.generate_occupancy_map(scene, scale, save_path)
    
    def load_occupancy_map(self, scene_label: str) -> Tuple[object, OccupancyMapConfig]:
        """Load occupancy map
        
        Args:
            scene_label: Scene label
            
        Returns:
            Tuple[np.ndarray, OccupancyMapConfig]: Occupancy map and configuration
        """
        return self.map_generator.load_occupancy_map(scene_label)
    
    def load_occupancy_map_from_path(self, scene_dir: str) -> Tuple[object, OccupancyMapConfig]:
        """Load occupancy map from complete scene directory path
        
        Args:
            scene_dir: Complete path to scene directory
            
        Returns:
            Tuple[np.ndarray, OccupancyMapConfig]: Occupancy map and configuration
        """
        return self.map_generator.load_occupancy_map_from_path(scene_dir)
    
    def find_optimal_path(self, start: Tuple[float, float], end: Tuple[float, float],
                         occupancy_map: object, scale: float, min_coords: Tuple[float, float],
                         safe_distance: float = None) -> Trajectory:
        """Calculate optimal path
        
        Args:
            start: Start coordinates (x, y)
            end: End coordinates (x, y)
            occupancy_map: Occupancy map
            scale: Grid resolution
            min_coords: Physical coordinates corresponding to map origin
            safe_distance: Safe distance, defaults to configured default value
            
        Returns:
            Trajectory: Optimal path trajectory
        """
        return self.map_generator.find_optimal_path(
            start, end, occupancy_map, scale, min_coords, safe_distance
        )
    
    def visualize_scene(self, scene: Scene, show_grid: bool = True, title: str = None):
        """Visualize scene
        
        Args:
            scene: Scene to visualize
            show_grid: Whether to show grid
            title: Figure title
        """
        self.visualizer.visualize_scene(scene, show_grid=show_grid, title=title)
        self.visualizer.show_plot()
    
    def visualize_occupancy_map(self, occupancy_map: object, config: OccupancyMapConfig,
                               title: str = None, origin: str = 'lower'):
        """Visualize occupancy map
        
        Args:
            occupancy_map: Occupancy map
            config: Occupancy map configuration
            title: Figure title
            origin: Origin of the image coordinate system ('upper' or 'lower')
        """
        self.visualizer.visualize_occupancy_map(occupancy_map, config, title=title, origin=origin)
        self.visualizer.show_plot()
    
    def visualize_trajectory(self, trajectory: Trajectory, color: str = None,
                           linewidth: float = 2, marker: str = 'o', markersize: float = 4,
                           label: str = None):
        """Visualize trajectory
        
        Args:
            trajectory: Trajectory to visualize
            color: Trajectory color
            linewidth: Line width
            marker: Marker style
            markersize: Marker size
            label: Legend label
        """
        self.visualizer.visualize_trajectory(
            trajectory, color=color, linewidth=linewidth, 
            marker=marker, markersize=markersize, label=label
        )
        self.visualizer.show_plot()
    
    def visualize_scene_with_trajectory(self, scene: Scene, trajectory: Trajectory,
                                      title: str = None):
        """Visualize trajectory in scene
        
        Args:
            scene: Scene
            trajectory: Trajectory
            title: Figure title
        """
        self.visualizer.visualize_scene_with_trajectory(scene, trajectory, title=title)
        self.visualizer.show_plot()
    
    def visualize_occupancy_with_trajectory(self, occupancy_map: object,
                                          config: OccupancyMapConfig,
                                          trajectory: Trajectory, title: str = None, origin: str = 'lower'):
        """Visualize trajectory in occupancy map
        
        Args:
            occupancy_map: Occupancy map
            config: Occupancy map configuration
            trajectory: Trajectory
            title: Figure title
            origin: Origin of the image coordinate system ('upper' or 'lower')
        """
        self.visualizer.visualize_occupancy_with_trajectory(
            occupancy_map, config, trajectory, title=title, origin=origin
        )
        self.visualizer.show_plot()
    
    def create_comparison_plot(self, scene: Scene, occupancy_map: object,
                             config: OccupancyMapConfig, trajectory: Trajectory = None,
                             title: str = None, save_path: str = None, origin: str = 'lower', cmap: str = 'gray'):
        """创建对比图：场景和占用地图
        
        Args:
            scene: 场景
            occupancy_map: 占用地图
            config: 占用地图配置
            trajectory: 可选的轨迹
            title: 图形标题
            save_path: 保存路径，如果提供则保存图形
            origin: Origin of the image coordinate system ('upper' or 'lower')
            cmap: Colormap for occupancy map
        """
        fig = self.visualizer.create_comparison_plot(
            scene, occupancy_map, config, trajectory, title, origin, cmap
        )
        
        if save_path:
            self.visualizer.save_plot(fig, save_path)
        
        self.visualizer.show_plot()
    
    def process_scene_to_map(self, scene: Scene, scale: float = None,
                           save_visualization: bool = False) -> Tuple[object, OccupancyMapConfig]:
        """Complete scene to map processing workflow
        
        Args:
            scene: Scene
            scale: Grid resolution
            save_visualization: Whether to save visualization results (disabled by default)
            
        Returns:
            Tuple[np.ndarray, OccupancyMapConfig]: Occupancy map and configuration
        """
        # 1. Resize scene
        scene = self.resize_scene_to_objects(scene)
        
        # 2. Save scene (only essential file: scene.json)
        self.save_scene(scene)
        
        # 3. Generate occupancy map (saves occupancy.png and occupancy.json)
        occupancy_map, config = self.generate_occupancy_map(scene, scale)
        
        # 4. Visualization (disabled by default to avoid extra files)
        if save_visualization:
            scene_dir = os.path.join(self.config['paths']['scenes_dir'], scene.label)
            os.makedirs(scene_dir, exist_ok=True)
            save_path = os.path.join(scene_dir, self.config['paths']['visualization_png'])
            self.create_comparison_plot(
                scene, occupancy_map, config,
                title=f"Scene '{scene.label}' Processing Results",
                save_path=save_path
            )
        
        return occupancy_map, config
    
    def plan_path_for_scene(self, scene_label: str, start: Tuple[float, float],
                           end: Tuple[float, float], safe_distance: float = None,
                           save_visualization: bool = False) -> Trajectory:
        """Plan path for scene
        
        Args:
            scene_label: Scene label
            start: Start coordinates
            end: End coordinates
            safe_distance: Safe distance
            save_visualization: Whether to save visualization results (disabled by default)
            
        Returns:
            Trajectory: Planned path
        """
        # 1. Load scene and occupancy map
        scene = self.load_scene(f"data/scenes/{scene_label}/scene.json")
        occupancy_map, config = self.load_occupancy_map(scene_label)
        
        # 2. Plan path
        trajectory = self.find_optimal_path(
            start, end, occupancy_map, config.scale, config.min_coords, safe_distance
        )
        
        # 3. Visualization (disabled by default to avoid extra files)
        if save_visualization:
            scene_dir = os.path.join(self.config['paths']['scenes_dir'], scene_label)
            os.makedirs(scene_dir, exist_ok=True)
            save_path = os.path.join(scene_dir, self.config['paths']['path_planning_png'])
            self.create_comparison_plot(
                scene, occupancy_map, config, trajectory,
                title=f"Scene '{scene_label}' Path Planning",
                save_path=save_path
            )
        
        return trajectory 

    def find_path_between_objects(self, scene: Scene, start_object_label: str, 
                                 end_object_label: str, occupancy_map: object, 
                                 config: OccupancyMapConfig, max_search_distance: float = 0.5,
                                 safe_distance: float = None) -> Optional[Trajectory]:
        """在物体之间寻找路径
        
        Args:
            scene: 场景
            start_object_label: 起始物体标签
            end_object_label: 目标物体标签
            occupancy_map: 占用地图
            config: 占用地图配置
            max_search_distance: 最远判定距离，默认0.5米
            safe_distance: 安全距离，默认为配置中的默认值
            
        Returns:
            Optional[Trajectory]: 找到的路径，如果没有找到则返回None
        """
        if safe_distance is None:
            safe_distance = self.config['map']['default_safe_distance']
        
        # 查找起始和目标物体
        start_obj = None
        end_obj = None
        for obj in scene.objects:
            if obj.label == start_object_label:
                start_obj = obj
            elif obj.label == end_object_label:
                end_obj = obj
        
        if start_obj is None or end_obj is None:
            raise ValueError(f"未找到物体: {start_object_label} 或 {end_object_label}")
        
        # 生成起始物体周围的候选起点
        start_candidates = self._generate_candidates_around_object(
            start_obj, occupancy_map, config, safe_distance, max_search_distance
        )
        
        # 生成目标物体周围的候选终点
        end_candidates = self._generate_candidates_around_object(
            end_obj, occupancy_map, config, safe_distance, max_search_distance
        )
        
        # 按距离排序候选点（从近到远）
        start_candidates.sort(key=lambda p: self._distance_to_object(p, start_obj))
        end_candidates.sort(key=lambda p: self._distance_to_object(p, end_obj))
        
        # 尝试所有候选点组合，从最近的开始
        for start_point in start_candidates:
            for end_point in end_candidates:
                try:
                    trajectory = self.find_optimal_path(
                        start_point, end_point, occupancy_map, config.scale, 
                        config.min_coords, safe_distance
                    )
                    if trajectory and len(trajectory.points) > 0:
                        return trajectory
                except (ValueError, Exception):
                    # 如果这个组合无法找到路径，继续尝试下一个
                    continue
        
        # 没有找到有效路径
        return None
    
    def find_path_between_objects_optimized(self, scene: Scene, start_object_label: str, 
                                          end_object_label: str, occupancy_map: object, 
                                          config: OccupancyMapConfig, max_search_distance: float = 0.5,
                                          safe_distance: float = None, 
                                          max_candidates: int = 15) -> Optional[Trajectory]:
        """优化后的物体间路径查找
        
        Args:
            scene: 场景
            start_object_label: 起始物体标签
            end_object_label: 目标物体标签
            occupancy_map: 占用地图
            config: 占用地图配置
            max_search_distance: 最远判定距离，默认0.5米
            safe_distance: 安全距离，默认为配置中的默认值
            max_candidates: 每个物体最多生成的候选点数量，默认15个
            
        Returns:
            Optional[Trajectory]: 找到的路径，如果没有找到则返回None
        """
        return self.optimized_path_finder.find_path_between_objects(
            scene, start_object_label, end_object_label, occupancy_map, 
            config, max_search_distance, safe_distance, max_candidates
        )
    
    def _generate_candidates_around_object(self, obj: Object, occupancy_map: object,
                                         config: OccupancyMapConfig, safe_distance: float,
                                         max_search_distance: float) -> List[Tuple[float, float]]:
        """在物体周围生成候选点
        
        Args:
            obj: 场景物体
            occupancy_map: 占用地图
            config: 占用地图配置
            safe_distance: 安全距离
            max_search_distance: 最大搜索距离
            
        Returns:
            List[Tuple[float, float]]: 候选点列表
        """
        candidates = []
        
        # 计算物体边界框
        bbox = obj.bbox
        margin = safe_distance + max_search_distance
        
        # 在物体周围生成网格点
        x_start = bbox.x_min - margin
        x_end = bbox.x_max + margin
        y_start = bbox.y_min - margin
        y_end = bbox.y_max + margin
        
        # 使用较小的步长生成候选点
        step = config.scale * 0.5  # 使用网格分辨率的一半作为步长
        
        x_coords = np.arange(x_start, x_end + step, step)
        y_coords = np.arange(y_start, y_end + step, step)
        
        for x in x_coords:
            for y in y_coords:
                # 检查点是否在物体的安全距离外
                if self._is_point_safe_from_object((x, y), obj, safe_distance):
                    # 检查点是否在地图边界内
                    try:
                        grid_i, grid_j = config.world_to_grid(x, y)
                        if (0 <= grid_i < occupancy_map.shape[0] and 
                            0 <= grid_j < occupancy_map.shape[1]):
                            # 检查点是否可通行（不在障碍物内）
                            if occupancy_map[grid_i, grid_j] == 0:
                                candidates.append((x, y))
                    except (ValueError, IndexError):
                        continue
        
        return candidates
    
    def _is_point_safe_from_object(self, point: Tuple[float, float], 
                                  obj: Object, safe_distance: float) -> bool:
        """检查点是否与物体保持安全距离
        
        Args:
            point: 待检查的点 (x, y)
            obj: 场景物体
            safe_distance: 安全距离
            
        Returns:
            bool: 是否安全
        """
        x, y = point
        bbox = obj.bbox
        
        # 计算点到物体边界框的最短距离
        dx = max(bbox.x_min - x, 0, x - bbox.x_max)
        dy = max(bbox.y_min - y, 0, y - bbox.y_max)
        
        # 如果点在边界框内，距离为0
        if dx == 0 and dy == 0:
            return False
        
        # 计算欧几里得距离
        distance = (dx * dx + dy * dy) ** 0.5
        
        return distance >= safe_distance
    
    def _distance_to_object(self, point: Tuple[float, float], obj: Object) -> float:
        """计算点到物体的距离
        
        Args:
            point: 点坐标 (x, y)
            obj: 场景物体
            
        Returns:
            float: 距离
        """
        x, y = point
        bbox = obj.bbox
        
        # 计算点到物体边界框的最短距离
        dx = max(bbox.x_min - x, 0, x - bbox.x_max)
        dy = max(bbox.y_min - y, 0, y - bbox.y_max)
        
        # 如果点在边界框内，距离为0
        if dx == 0 and dy == 0:
            return 0.0
        
        # 计算欧几里得距离
        distance = (dx * dx + dy * dy) ** 0.5
        
        return distance 


class OptimizedPathFinder:
    """优化后的路径查找类"""
    
    def __init__(self, map_middleware):
        self.middleware = map_middleware
        # 预计算自由空间点的KDTree以加速查找
        self._free_space_kdtree = None
        self._free_space_points = None
    
    def find_path_between_objects(self, scene, start_object_label: str, 
                                 end_object_label: str, occupancy_map, 
                                 config, max_search_distance: float = 0.5,
                                 safe_distance: float = None, 
                                 max_candidates: int = 20) -> Optional:
        """优化后的物体间路径查找
        
        Args:
            scene: 场景
            start_object_label: 起始物体标签
            end_object_label: 目标物体标签
            occupancy_map: 占用地图
            config: 占用地图配置
            max_search_distance: 最远判定距离
            safe_distance: 安全距离
            max_candidates: 每个物体最多生成的候选点数量
            
        Returns:
            Optional[Trajectory]: 找到的路径
        """
        if safe_distance is None:
            safe_distance = self.middleware.config['map']['default_safe_distance']
        
        # 查找物体
        start_obj, end_obj = self._find_objects(scene, start_object_label, end_object_label)
        if start_obj is None or end_obj is None:
            raise ValueError(f"未找到物体: {start_object_label} 或 {end_object_label}")
        
        # 构建自由空间KDTree（如果还没有构建）
        if self._free_space_kdtree is None:
            self._build_free_space_kdtree(occupancy_map, config)
        
        # 智能生成候选点（数量限制）
        start_candidates = self._generate_smart_candidates(
            start_obj, occupancy_map, config, safe_distance, 
            max_search_distance, max_candidates
        )
        end_candidates = self._generate_smart_candidates(
            end_obj, occupancy_map, config, safe_distance, 
            max_search_distance, max_candidates
        )
        
        if not start_candidates or not end_candidates:
            return None
        
        # 使用启发式搜索策略
        return self._heuristic_path_search(
            start_candidates, end_candidates, start_obj, end_obj,
            occupancy_map, config, safe_distance
        )
    
    def _find_objects(self, scene, start_label: str, end_label: str):
        """查找物体"""
        start_obj = end_obj = None
        for obj in scene.objects:
            if obj.label == start_label:
                start_obj = obj
            elif obj.label == end_label:
                end_obj = obj
            if start_obj and end_obj:  # 找到后提前退出
                break
        return start_obj, end_obj
    
    def _build_free_space_kdtree(self, occupancy_map, config):
        """构建自由空间的KDTree用于快速最近邻查询"""
        free_indices = np.where(occupancy_map == 0)
        free_points = []
        
        # 转换网格坐标到世界坐标
        for i, j in zip(free_indices[0], free_indices[1]):
            x, y = config.grid_to_world(i, j)
            free_points.append((x, y))
        
        if free_points:
            self._free_space_points = np.array(free_points)
            self._free_space_kdtree = KDTree(self._free_space_points)
    
    def _generate_smart_candidates(self, obj, occupancy_map, config, 
                                 safe_distance: float, max_search_distance: float, 
                                 max_candidates: int) -> List[Tuple[float, float]]:
        """智能生成候选点"""
        bbox = obj.bbox
        center_x = (bbox.x_min + bbox.x_max) / 2
        center_y = (bbox.y_min + bbox.y_max) / 2
        
        candidates = []
        
        # 策略1: 在物体边缘的关键方向生成候选点
        edge_candidates = self._generate_edge_candidates(
            obj, safe_distance, max_search_distance
        )
        
        # 策略2: 使用KDTree查找物体附近的自由空间点
        if self._free_space_kdtree is not None:
            nearby_candidates = self._find_nearby_free_points(
                center_x, center_y, obj, safe_distance, max_search_distance
            )
            candidates.extend(nearby_candidates)
        
        candidates.extend(edge_candidates)
        
        # 过滤和验证候选点
        valid_candidates = []
        for point in candidates:
            if self._is_point_valid(point, obj, occupancy_map, config, safe_distance):
                valid_candidates.append(point)
        
        # 按距离物体中心排序，选择最优的candidates
        if len(valid_candidates) > max_candidates:
            distances = [((p[0] - center_x)**2 + (p[1] - center_y)**2)**0.5 
                        for p in valid_candidates]
            sorted_indices = np.argsort(distances)
            valid_candidates = [valid_candidates[i] for i in sorted_indices[:max_candidates]]
        
        return valid_candidates
    
    def _generate_edge_candidates(self, obj, safe_distance: float, 
                                max_search_distance: float) -> List[Tuple[float, float]]:
        """在物体边缘的关键方向生成候选点"""
        bbox = obj.bbox
        candidates = []
        
        # 8个主要方向
        directions = [
            (1, 0), (-1, 0), (0, 1), (0, -1),  # 四个主方向
            (1, 1), (1, -1), (-1, 1), (-1, -1)  # 四个对角方向
        ]
        
        center_x = (bbox.x_min + bbox.x_max) / 2
        center_y = (bbox.y_min + bbox.y_max) / 2
        
        # 物体的半径（用于估算）
        obj_radius = max(bbox.x_max - bbox.x_min, bbox.y_max - bbox.y_min) / 2
        
        for dx, dy in directions:
            # 在每个方向上生成多个候选点
            for distance in np.linspace(safe_distance + obj_radius, 
                                      safe_distance + obj_radius + max_search_distance, 5):
                x = center_x + dx * distance
                y = center_y + dy * distance
                candidates.append((x, y))
        
        return candidates
    
    def _find_nearby_free_points(self, center_x: float, center_y: float, 
                               obj, safe_distance: float, 
                               max_search_distance: float) -> List[Tuple[float, float]]:
        """使用KDTree查找物体附近的自由空间点"""
        if self._free_space_kdtree is None:
            return []
        
        # 查找中心点附近的自由空间点
        query_point = np.array([[center_x, center_y]])
        distances, indices = self._free_space_kdtree.query(
            query_point, k=min(100, len(self._free_space_points)), 
            distance_upper_bound=max_search_distance * 2
        )
        
        candidates = []
        for dist, idx in zip(distances[0], indices[0]):
            if dist < np.inf:  # 有效点
                point = tuple(self._free_space_points[idx])
                # 检查是否与物体保持安全距离
                if self._is_point_safe_from_object(point, obj, safe_distance):
                    candidates.append(point)
        
        return candidates
    
    def _heuristic_path_search(self, start_candidates, end_candidates, 
                             start_obj, end_obj, occupancy_map, config, safe_distance):
        """使用启发式策略搜索路径"""
        
        # 计算物体间的直线距离，用于启发式排序
        start_center = ((start_obj.bbox.x_min + start_obj.bbox.x_max) / 2,
                       (start_obj.bbox.y_min + start_obj.bbox.y_max) / 2)
        end_center = ((end_obj.bbox.x_min + end_obj.bbox.x_max) / 2,
                     (end_obj.bbox.y_min + end_obj.bbox.y_max) / 2)
        
        # 为候选点组合计算启发式分数
        combinations = []
        for start_point in start_candidates:
            for end_point in end_candidates:
                # 启发式分数：起点到起始物体距离 + 终点到目标物体距离 + 起点终点间距离
                score = (
                    self._point_distance(start_point, start_center) +
                    self._point_distance(end_point, end_center) +
                    self._point_distance(start_point, end_point)
                )
                combinations.append((score, start_point, end_point))
        
        # 按分数排序，优先尝试最有希望的组合
        combinations.sort(key=lambda x: x[0])
        
        # 尝试前N个最优组合
        max_attempts = min(10, len(combinations))  # 限制尝试次数
        
        for i in range(max_attempts):
            _, start_point, end_point = combinations[i]
            try:
                trajectory = self.middleware.find_optimal_path(
                    start_point, end_point, occupancy_map, config.scale, 
                    config.min_coords, safe_distance
                )
                if trajectory and len(trajectory.points) > 0:
                    return trajectory
            except (ValueError, Exception):
                continue
        
        return None
    
    def _is_point_valid(self, point: Tuple[float, float], obj, 
                       occupancy_map, config, safe_distance: float) -> bool:
        """检查点是否有效"""
        try:
            # 检查是否在地图边界内
            grid_i, grid_j = config.world_to_grid(point[0], point[1])
            if not (0 <= grid_i < occupancy_map.shape[0] and 
                   0 <= grid_j < occupancy_map.shape[1]):
                return False
            
            # 检查是否可通行
            if occupancy_map[grid_i, grid_j] != 0:
                return False
            
            # 检查安全距离
            return self._is_point_safe_from_object(point, obj, safe_distance)
        
        except (ValueError, IndexError):
            return False
    
    def _is_point_safe_from_object(self, point: Tuple[float, float], 
                                  obj, safe_distance: float) -> bool:
        """检查点是否与物体保持安全距离"""
        x, y = point
        bbox = obj.bbox
        
        # 计算点到物体边界框的最短距离
        dx = max(bbox.x_min - x, 0, x - bbox.x_max)
        dy = max(bbox.y_min - y, 0, y - bbox.y_max)
        
        if dx == 0 and dy == 0:
            return False  # 点在边界框内
        
        distance = (dx * dx + dy * dy) ** 0.5
        return distance >= safe_distance
    
    def _point_distance(self, p1: Tuple[float, float], 
                       p2: Tuple[float, float]) -> float:
        """计算两点间距离"""
        return ((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)**0.5


class MapMiddlewareOptimized:
    """在原有MapMiddleware基础上添加优化方法"""
    
    def __init__(self, original_middleware):
        self.middleware = original_middleware
        self.path_finder = OptimizedPathFinder(original_middleware)
    
    def find_path_between_objects_optimized(self, scene, start_object_label: str, 
                                          end_object_label: str, occupancy_map, 
                                          config, max_search_distance: float = 0.5,
                                          safe_distance: float = None, 
                                          max_candidates: int = 15):
        """优化后的物体间路径查找"""
        return self.path_finder.find_path_between_objects(
            scene, start_object_label, end_object_label, occupancy_map, 
            config, max_search_distance, safe_distance, max_candidates
        )