"""
地形路径和路网系统
提供完整的路径创建、编辑、导航、寻路算法和可视化功能
"""

from panda3d.core import NodePath, Vec3, Point3, BitMask32
from panda3d.core import CardMaker, LineSegs
from panda3d.core import Texture, TextureStage
from panda3d.ai import AIPathFinder, AIPath, AICharacter
import math
import json
import os
import heapq

class PathSystem:
    """
    地形路径和路网系统类
    提供路径创建、编辑、导航、寻路算法和可视化功能
    """
    
    def __init__(self, world):
        self.world = world
        self.paths = []  # 存储路径
        self.waypoints = []  # 存储路径点
        self.roads = []  # 存储道路
        self.navigation_mesh = None  # 导航网格
        self.pathfinding_enabled = True
        self.graph = {}  # 寻路图
        self.traffic_system = {}  # 交通系统
        self.pathfinding_algorithm = 'a*'  # 寻路算法 ('a*', 'dijkstra', 'rrt')
        
    def create_path(self, name, points=None):
        """
        创建路径
        name: 路径名称
        points: 路径点列表
        """
        try:
            # 创建路径节点
            path_node = self.world.render.attachNewNode(f"path_{name}")
            
            # 创建路径信息
            path_info = {
                'name': name,
                'node': path_node,
                'points': points if points else [],
                'waypoints': [],  # 路径点对象
                'segments': [],  # 路段信息
                'properties': {
                    'width': 2.0,
                    'type': 'generic',  # generic, road, trail, river, etc.
                    'speed_limit': 50.0,  # km/h
                    'one_way': False,
                    'lanes': 1,
                    'difficulty': 1.0,  # 通行难度系数
                    'traffic_density': 0.0  # 交通密度
                }
            }
            
            # 如果提供了点，创建路径点
            if points:
                self._create_waypoints(path_info, points)
                
            # 设置标签
            path_node.setTag("path_name", name)
            path_node.setTag("is_scene_element", "1")
            path_node.setTag("tree_item_type", "PATH_NODE")
            
            # 添加到路径列表
            self.paths.append(path_info)
            
            # 更新寻路图
            self._update_pathfinding_graph()
            
            print(f"创建路径: {name}")
            return path_info
            
        except Exception as e:
            print(f"创建路径时出错: {e}")
            return None
            
    def _create_waypoints(self, path_info, points):
        """
        创建路径点
        """
        try:
            waypoints = []
            
            for i, point in enumerate(points):
                # 创建路径点节点
                waypoint_node = path_info['node'].attachNewNode(f"waypoint_{i}")
                waypoint_node.setPos(point)
                
                # 创建路径点信息
                waypoint_info = {
                    'node': waypoint_node,
                    'position': point,
                    'index': i,
                    'connections': [],  # 连接的其他路径点
                    'properties': {
                        'speed_limit': path_info['properties']['speed_limit'],
                        'stop_sign': False,
                        'traffic_light': False,
                        'priority': 1.0,  # 路径点优先级
                        'wait_time': 0.0  # 等待时间
                    }
                }
                
                waypoints.append(waypoint_info)
                self.waypoints.append(waypoint_info)
                
            path_info['waypoints'] = waypoints
            
            # 创建路径段
            self._create_path_segments(path_info)
            
            # 可视化路径
            self._visualize_path(path_info)
            
        except Exception as e:
            print(f"创建路径点时出错: {e}")
            
    def _create_path_segments(self, path_info):
        """
        创建路径段
        """
        try:
            waypoints = path_info['waypoints']
            segments = []
            
            for i in range(len(waypoints) - 1):
                start_waypoint = waypoints[i]
                end_waypoint = waypoints[i + 1]
                
                # 计算路段信息
                segment_length = (end_waypoint['position'] - start_waypoint['position']).length()
                segment_direction = (end_waypoint['position'] - start_waypoint['position']).normalized()
                segment_slope = self._calculate_slope(start_waypoint['position'], end_waypoint['position'])
                
                segment_info = {
                    'start': start_waypoint,
                    'end': end_waypoint,
                    'length': segment_length,
                    'direction': segment_direction,
                    'slope': segment_slope,
                    'difficulty': 1.0 + abs(segment_slope) * 0.1,  # 坡度影响通行难度
                    'traffic_flow': 0.0,  # 交通流量
                    'last_update': self.world.globalClock.getFrameTime()
                }
                
                segments.append(segment_info)
                
                # 建立连接关系
                start_waypoint['connections'].append(end_waypoint)
                if not path_info['properties']['one_way']:
                    end_waypoint['connections'].append(start_waypoint)
                    
            path_info['segments'] = segments
            
        except Exception as e:
            print(f"创建路径段时出错: {e}")
            
    def _calculate_slope(self, start_pos, end_pos):
        """
        计算两点间的坡度
        """
        try:
            horizontal_distance = Vec3(end_pos.getX() - start_pos.getX(), 
                                     end_pos.getY() - start_pos.getY(), 0).length()
            vertical_distance = end_pos.getZ() - start_pos.getZ()
            
            if horizontal_distance > 0:
                slope = math.degrees(math.atan(vertical_distance / horizontal_distance))
                return slope
            else:
                return 0.0
                
        except:
            return 0.0
            
    def _visualize_path(self, path_info):
        """
        可视化路径
        """
        try:
            # 移除现有的可视化节点
            if 'visualization' in path_info:
                if path_info['visualization'] and not path_info['visualization'].isEmpty():
                    path_info['visualization'].removeNode()
                    
            # 创建线条段
            ls = LineSegs()
            ls.setThickness(3.0)
            
            # 根据路径类型和交通状况设置颜色
            path_type = path_info['properties']['type']
            traffic_density = path_info['properties']['traffic_density']
            
            # 基础颜色
            if path_type == 'road':
                base_color = Vec3(0.3, 0.3, 0.3)  # 灰色
            elif path_type == 'trail':
                base_color = Vec3(0.5, 0.3, 0.1)  # 棕色
            elif path_type == 'river':
                base_color = Vec3(0.0, 0.0, 1.0)  # 蓝色
            else:
                base_color = Vec3(1.0, 1.0, 0.0)  # 黄色
                
            # 交通密度影响颜色
            if traffic_density > 0.8:
                color = Vec3(1.0, 0.0, 0.0)  # 红色 - 拥堵
            elif traffic_density > 0.5:
                color = Vec3(1.0, 0.5, 0.0)  # 橙色 - 繁忙
            else:
                color = base_color  # 正常
                
            ls.setColor(color[0], color[1], color[2], 1.0)
                
            # 添加线条点
            for waypoint in path_info['waypoints']:
                ls.moveTo(waypoint['position'])
                
            # 创建可视化节点
            visualization_node = self.world.render.attachNewNode(ls.create())
            visualization_node.reparentTo(path_info['node'])
            
            # 保存可视化节点
            path_info['visualization'] = visualization_node
            
        except Exception as e:
            print(f"可视化路径时出错: {e}")
            
    def add_waypoint(self, path_info, position, index=None):
        """
        添加路径点
        """
        try:
            waypoints = path_info['waypoints']
            
            # 确定插入位置
            if index is None:
                index = len(waypoints)
                
            # 创建路径点节点
            waypoint_node = path_info['node'].attachNewNode(f"waypoint_{index}")
            waypoint_node.setPos(position)
            
            # 创建路径点信息
            waypoint_info = {
                'node': waypoint_node,
                'position': position,
                'index': index,
                'connections': [],
                'properties': {
                    'speed_limit': path_info['properties']['speed_limit'],
                    'stop_sign': False,
                    'traffic_light': False,
                    'priority': 1.0,
                    'wait_time': 0.0
                }
            }
            
            # 插入到路径点列表
            waypoints.insert(index, waypoint_info)
            self.waypoints.append(waypoint_info)
            
            # 更新索引
            for i, wp in enumerate(waypoints):
                wp['index'] = i
                
            # 重新创建路径段
            self._create_path_segments(path_info)
            
            # 重新可视化路径
            self._visualize_path(path_info)
            
            # 更新寻路图
            self._update_pathfinding_graph()
            
            print(f"添加路径点到路径 {path_info['name']}")
            return waypoint_info
            
        except Exception as e:
            print(f"添加路径点时出错: {e}")
            return None
            
    def remove_waypoint(self, path_info, waypoint_info):
        """
        移除路径点
        """
        try:
            # 从路径中移除
            if waypoint_info in path_info['waypoints']:
                path_info['waypoints'].remove(waypoint_info)
                
            # 从全局列表中移除
            if waypoint_info in self.waypoints:
                self.waypoints.remove(waypoint_info)
                
            # 移除节点
            if waypoint_info['node'] and not waypoint_info['node'].isEmpty():
                waypoint_info['node'].removeNode()
                
            # 重新创建路径段
            self._create_path_segments(path_info)
            
            # 重新可视化路径
            self._visualize_path(path_info)
            
            # 更新寻路图
            self._update_pathfinding_graph()
            
            print(f"从路径 {path_info['name']} 移除路径点")
            return True
            
        except Exception as e:
            print(f"移除路径点时出错: {e}")
            return False
            
    def create_road(self, name, start_point, end_point, width=5.0, lanes=2):
        """
        创建道路
        """
        try:
            # 创建道路节点
            road_node = self.world.render.attachNewNode(f"road_{name}")
            
            # 计算道路方向和长度
            direction = (end_point - start_point).normalized()
            length = (end_point - start_point).length()
            
            # 创建道路几何体
            cm = CardMaker('road_surface')
            cm.setFrame(0, length, -width/2, width/2)
            road_surface = road_node.attachNewNode(cm.generate())
            
            # 设置道路方向和位置
            road_node.setPos(start_point)
            road_node.lookAt(end_point)
            
            # 创建道路信息
            road_info = {
                'name': name,
                'node': road_node,
                'surface': road_surface,
                'start_point': start_point,
                'end_point': end_point,
                'direction': direction,
                'length': length,
                'width': width,
                'lanes': lanes,
                'properties': {
                    'surface_type': 'asphalt',  # asphalt, concrete, dirt, etc.
                    'speed_limit': 50.0,
                    'one_way': False,
                    'traffic_lights': [],  # 交通灯列表
                    'road_markings': True  # 道路标记
                }
            }
            
            # 应用道路材质
            self._setup_road_material(road_info)
            
            # 设置标签
            road_node.setTag("road_name", name)
            road_node.setTag("is_scene_element", "1")
            road_node.setTag("tree_item_type", "ROAD_NODE")
            
            # 添加到道路列表
            self.roads.append(road_info)
            
            # 创建对应路径
            path_points = [start_point, end_point]
            path_info = self.create_path(f"path_for_{name}", path_points)
            if path_info:
                path_info['properties']['type'] = 'road'
                path_info['properties']['width'] = width
                path_info['properties']['lanes'] = lanes
                road_info['path'] = path_info
                
            print(f"创建道路: {name}")
            return road_info
            
        except Exception as e:
            print(f"创建道路时出错: {e}")
            return None
            
    def _setup_road_material(self, road_info):
        """
        设置道路材质
        """
        try:
            surface = road_info['surface']
            
            # 创建道路材质
            from panda3d.core import Material
            road_material = Material()
            road_material.setAmbient((0.3, 0.3, 0.3, 1.0))
            road_material.setDiffuse((0.4, 0.4, 0.4, 1.0))
            road_material.setSpecular((0.1, 0.1, 0.1, 1.0))
            road_material.setShininess(10.0)
            
            surface.setMaterial(road_material)
            
            # 如果有纹理系统，应用道路纹理
            # 这里可以集成纹理系统
            
        except Exception as e:
            print(f"设置道路材质时出错: {e}")
            
    def generate_navigation_mesh(self, terrain_info):
        """
        生成导航网格
        """
        try:
            # 基于地形生成导航网格
            # 包括可行走区域、障碍物识别等
            
            # 获取地形信息
            heightfield = terrain_info['heightfield']
            if not heightfield:
                print("无法获取地形高度图数据")
                return None
                
            # 创建导航网格数据结构
            nav_mesh = {
                'triangles': [],      # 三角形网格
                'vertices': [],       # 顶点
                'obstacles': [],      # 障碍物
                'walkable_areas': [], # 可行走区域
                'terrain_info': terrain_info
            }
            
            # 生成三角形网格（简化实现）
            width = heightfield.getXSize()
            height = heightfield.getYSize()
            
            # 创建顶点
            for y in range(0, height, 4):  # 降低分辨率
                for x in range(0, width, 4):
                    # 获取高度值
                    height_value = heightfield.getRed(x, y)
                    
                    # 计算世界坐标
                    terrain_node = terrain_info['node']
                    terrain_pos = terrain_node.getPos()
                    terrain_scale = terrain_node.getScale()
                    center_offset = (width - 1) / 2
                    
                    world_x = (x - center_offset) * terrain_scale.getX() + terrain_pos.getX()
                    world_y = (y - center_offset) * terrain_scale.getY() + terrain_pos.getY()
                    world_z = height_value * terrain_scale.getZ() + terrain_pos.getZ()
                    
                    nav_mesh['vertices'].append(Point3(world_x, world_y, world_z))
                    
            # 创建三角形（简化实现）
            vertex_count = len(nav_mesh['vertices'])
            for i in range(0, vertex_count - 1, 2):
                if i + 2 < vertex_count:
                    nav_mesh['triangles'].append([i, i + 1, i + 2])
                    
            # 识别可行走区域（基于坡度）
            for i, vertex in enumerate(nav_mesh['vertices']):
                # 检查周围点的坡度
                if i + 1 < len(nav_mesh['vertices']):
                    next_vertex = nav_mesh['vertices'][i + 1]
                    slope = self._calculate_slope(vertex, next_vertex)
                    if slope < 30.0:  # 坡度小于30度认为可行走
                        nav_mesh['walkable_areas'].append(i)
                        
            print(f"生成导航网格: {vertex_count} 个顶点, {len(nav_mesh['triangles'])} 个三角形")
            self.navigation_mesh = nav_mesh
            return nav_mesh
            
        except Exception as e:
            print(f"生成导航网格时出错: {e}")
            return None
            
    def find_path(self, start_pos, end_pos, algorithm=None):
        """
        寻找路径
        """
        try:
            if not self.pathfinding_enabled:
                print("路径寻找已禁用")
                return None
                
            # 选择寻路算法
            if algorithm is None:
                algorithm = self.pathfinding_algorithm
                
            # 执行寻路
            if algorithm == 'a*':
                path = self._a_star_pathfinding(start_pos, end_pos)
            elif algorithm == 'dijkstra':
                path = self._dijkstra_pathfinding(start_pos, end_pos)
            elif algorithm == 'rrt':
                path = self._rrt_pathfinding(start_pos, end_pos)
            else:
                # 默认使用A*
                path = self._a_star_pathfinding(start_pos, end_pos)
                
            if path:
                print(f"找到路径: {start_pos} -> {end_pos} (长度: {len(path)} 个点)")
                return path
            else:
                print(f"无法找到路径: {start_pos} -> {end_pos}")
                return None
                
        except Exception as e:
            print(f"寻找路径时出错: {e}")
            return None
            
    def _a_star_pathfinding(self, start_pos, end_pos):
        """
        A*寻路算法
        """
        try:
            # 如果没有寻路图，使用路径点
            if not self.graph or len(self.graph) == 0:
                return self._simple_a_star(start_pos, end_pos)
                
            # A*算法实现
            open_set = []
            closed_set = set()
            came_from = {}
            g_score = {start_pos: 0}
            f_score = {start_pos: self._heuristic(start_pos, end_pos)}
            
            heapq.heappush(open_set, (f_score[start_pos], start_pos))
            
            while open_set:
                current = heapq.heappop(open_set)[1]
                
                if self._is_close(current, end_pos, 1.0):
                    # 重构路径
                    path = []
                    while current in came_from:
                        path.append(current)
                        current = came_from[current]
                    path.append(start_pos)
                    path.reverse()
                    return path
                    
                closed_set.add(current)
                
                # 检查邻居
                for neighbor in self._get_neighbors(current):
                    if neighbor in closed_set:
                        continue
                        
                    tentative_g_score = g_score[current] + (neighbor - current).length()
                    
                    if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
                        came_from[neighbor] = current
                        g_score[neighbor] = tentative_g_score
                        f_score[neighbor] = g_score[neighbor] + self._heuristic(neighbor, end_pos)
                        
                        if neighbor not in [i[1] for i in open_set]:
                            heapq.heappush(open_set, (f_score[neighbor], neighbor))
                            
            return None  # 未找到路径
            
        except Exception as e:
            print(f"A*寻路时出错: {e}")
            return None
            
    def _dijkstra_pathfinding(self, start_pos, end_pos):
        """
        Dijkstra寻路算法
        """
        try:
            # Dijkstra算法实现
            distances = {start_pos: 0}
            previous = {}
            unvisited = set()
            
            # 初始化
            for waypoint in self.waypoints:
                pos = waypoint['position']
                distances[pos] = float('inf')
                unvisited.add(pos)
            distances[start_pos] = 0
            unvisited.add(start_pos)
            
            while unvisited:
                # 找到距离最小的节点
                current = min(unvisited, key=lambda x: distances[x])
                unvisited.remove(current)
                
                if self._is_close(current, end_pos, 1.0):
                    # 重构路径
                    path = []
                    while current in previous:
                        path.append(current)
                        current = previous[current]
                    path.append(start_pos)
                    path.reverse()
                    return path
                    
                # 更新邻居距离
                for neighbor in self._get_neighbors(current):
                    if neighbor in unvisited:
                        alt = distances[current] + (neighbor - current).length()
                        if alt < distances[neighbor]:
                            distances[neighbor] = alt
                            previous[neighbor] = current
                            
            return None  # 未找到路径
            
        except Exception as e:
            print(f"Dijkstra寻路时出错: {e}")
            return None
            
    def _rrt_pathfinding(self, start_pos, end_pos):
        """
        RRT (快速随机树) 寻路算法
        """
        try:
            # RRT算法简化实现
            nodes = [start_pos]
            parent = {start_pos: None}
            max_iterations = 1000
            step_size = 5.0
            
            for i in range(max_iterations):
                # 随机采样或偏向目标
                if random.random() < 0.1:
                    random_point = end_pos
                else:
                    # 随机点
                    random_point = Point3(
                        start_pos.getX() + random.uniform(-50, 50),
                        start_pos.getY() + random.uniform(-50, 50),
                        start_pos.getZ()
                    )
                    
                # 找到最近的节点
                nearest_node = min(nodes, key=lambda x: (x - random_point).length())
                
                # 向随机点延伸
                direction = (random_point - nearest_node).normalized()
                new_point = nearest_node + direction * step_size
                
                # 检查是否与障碍物碰撞（简化检查）
                if self._is_valid_point(new_point):
                    nodes.append(new_point)
                    parent[new_point] = nearest_node
                    
                    # 检查是否到达目标附近
                    if (new_point - end_pos).length() < step_size:
                        # 重构路径
                        path = [end_pos]
                        current = new_point
                        while current is not None:
                            path.append(current)
                            current = parent.get(current)
                        path.reverse()
                        return path
                        
            return None  # 未找到路径
            
        except Exception as e:
            print(f"RRT寻路时出错: {e}")
            return None
            
    def _simple_a_star(self, start_pos, end_pos):
        """
        简化版A*（基于路径点）
        """
        try:
            if not self.waypoints:
                return [start_pos, end_pos]
                
            # 找到最近的路径点作为起点和终点
            start_waypoint = min(self.waypoints, key=lambda x: (x['position'] - start_pos).length())
            end_waypoint = min(self.waypoints, key=lambda x: (x['position'] - end_pos).length())
            
            # 在路径点间寻路
            waypoint_path = self._waypoint_a_star(start_waypoint, end_waypoint)
            if not waypoint_path:
                return None
                
            # 构造完整路径
            path = [start_pos]
            for waypoint in waypoint_path:
                path.append(waypoint['position'])
            path.append(end_pos)
            
            return path
            
        except Exception as e:
            print(f"简化A*寻路时出错: {e}")
            return None
            
    def _waypoint_a_star(self, start_waypoint, end_waypoint):
        """
        路径点间的A*寻路
        """
        try:
            open_set = []
            closed_set = set()
            came_from = {}
            g_score = {start_waypoint: 0}
            f_score = {start_waypoint: self._waypoint_heuristic(start_waypoint, end_waypoint)}
            
            heapq.heappush(open_set, (f_score[start_waypoint], id(start_waypoint), start_waypoint))
            
            while open_set:
                current = heapq.heappop(open_set)[2]
                
                if current == end_waypoint:
                    # 重构路径
                    path = []
                    while current in came_from:
                        path.append(current)
                        current = came_from[current]
                    path.reverse()
                    return path
                    
                closed_set.add(current)
                
                # 检查连接的路径点
                for neighbor in current['connections']:
                    if neighbor in closed_set:
                        continue
                        
                    # 计算通行成本
                    distance = (neighbor['position'] - current['position']).length()
                    difficulty = 1.0  # 可以从路径属性获取
                    tentative_g_score = g_score[current] + distance * difficulty
                    
                    if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
                        came_from[neighbor] = current
                        g_score[neighbor] = tentative_g_score
                        f_score[neighbor] = g_score[neighbor] + self._waypoint_heuristic(neighbor, end_waypoint)
                        
                        if neighbor not in [item[2] for item in open_set]:
                            heapq.heappush(open_set, (f_score[neighbor], id(neighbor), neighbor))
                            
            return None  # 未找到路径
            
        except Exception as e:
            print(f"路径点A*寻路时出错: {e}")
            return None
            
    def _heuristic(self, pos1, pos2):
        """
        启发式函数（欧几里得距离）
        """
        return (pos1 - pos2).length()
        
    def _waypoint_heuristic(self, waypoint1, waypoint2):
        """
        路径点启发式函数
        """
        return (waypoint1['position'] - waypoint2['position']).length()
        
    def _is_close(self, pos1, pos2, threshold):
        """
        检查两点是否接近
        """
        return (pos1 - pos2).length() <= threshold
        
    def _get_neighbors(self, pos):
        """
        获取位置的邻居点
        """
        # 简化实现：返回附近的路径点
        neighbors = []
        for waypoint in self.waypoints:
            distance = (waypoint['position'] - pos).length()
            if 0.1 < distance < 10.0:  # 距离阈值
                neighbors.append(waypoint['position'])
        return neighbors
        
    def _is_valid_point(self, point):
        """
        检查点是否有效（不与障碍物碰撞）
        """
        # 简化实现：检查是否在合理高度范围内
        return -10.0 <= point.getZ() <= 100.0
        
    def create_ai_character(self, name, position, path=None):
        """
        创建AI角色
        """
        try:
            # 创建AI角色
            ai_char = AICharacter(name, position, 50, 0.05, 5)
            
            # 如果提供了路径，设置路径跟随
            if path:
                ai_path = AIPath()
                for point in path:
                    ai_path.addPoint(point)
                ai_char.setAiPath(ai_path)
                
            print(f"创建AI角色: {name}")
            return ai_char
            
        except Exception as e:
            print(f"创建AI角色时出错: {e}")
            return None
            
    def _update_pathfinding_graph(self):
        """
        更新寻路图
        """
        try:
            # 基于路径点构建寻路图
            self.graph = {}
            
            for waypoint in self.waypoints:
                pos = waypoint['position']
                self.graph[pos] = []
                
                # 添加连接的邻居
                for connection in waypoint['connections']:
                    neighbor_pos = connection['position']
                    self.graph[pos].append(neighbor_pos)
                    
            print(f"更新寻路图: {len(self.graph)} 个节点")
            
        except Exception as e:
            print(f"更新寻路图时出错: {e}")
            
    def update_path_system(self, time_delta):
        """
        更新路径系统
        """
        try:
            # 更新交通系统
            self._update_traffic_system(time_delta)
            
            # 更新路径可视化
            self._update_path_visualization()
            
            # 更新AI角色
            # 这里可以更新所有AI角色的状态
            
        except Exception as e:
            print(f"更新路径系统时出错: {e}")
            
    def _update_traffic_system(self, time_delta):
        """
        更新交通系统
        """
        try:
            current_time = self.world.globalClock.getFrameTime()
            
            # 更新每条路径的交通密度
            for path_info in self.paths:
                # 模拟交通流量变化
                if 'traffic_density' in path_info['properties']:
                    # 简单的交通流量模拟
                    base_density = 0.3
                    time_factor = (math.sin(current_time * 0.1) + 1.0) * 0.5
                    path_info['properties']['traffic_density'] = base_density + time_factor * 0.4
                    
        except Exception as e:
            print(f"更新交通系统时出错: {e}")
            
    def _update_path_visualization(self):
        """
        更新路径可视化
        """
        try:
            # 重新可视化所有路径以反映交通状况
            for path_info in self.paths:
                self._visualize_path(path_info)
                
        except Exception as e:
            print(f"更新路径可视化时出错: {e}")
            
    def set_path_properties(self, path_info, properties):
        """
        设置路径属性
        """
        try:
            # 更新路径属性
            path_info['properties'].update(properties)
            
            # 重新可视化路径
            self._visualize_path(path_info)
            
            # 更新寻路图（如果影响通行难度）
            if 'difficulty' in properties or 'speed_limit' in properties:
                self._update_pathfinding_graph()
            
            print(f"更新路径 {path_info['name']} 属性")
            return True
            
        except Exception as e:
            print(f"设置路径属性时出错: {e}")
            return False
            
    def get_path_stats(self):
        """
        获取路径统计信息
        """
        stats = {
            'total_paths': len(self.paths),
            'total_waypoints': len(self.waypoints),
            'total_roads': len(self.roads),
            'path_types': {},
            'total_length': 0,
            'algorithm': self.pathfinding_algorithm,
            'traffic_stats': {
                'total_density': 0.0,
                'avg_density': 0.0
            }
        }
        
        # 统计路径类型和总长度
        total_density = 0.0
        for path_info in self.paths:
            path_type = path_info['properties']['type']
            if path_type in stats['path_types']:
                stats['path_types'][path_type] += 1
            else:
                stats['path_types'][path_type] = 1
                
            # 计算路径长度
            for segment in path_info['segments']:
                stats['total_length'] += segment['length']
                
            # 累计交通密度
            total_density += path_info['properties'].get('traffic_density', 0.0)
                
        # 计算平均交通密度
        if len(self.paths) > 0:
            stats['traffic_stats']['avg_density'] = total_density / len(self.paths)
        stats['traffic_stats']['total_density'] = total_density
        
        return stats
        
    def save_path_data(self, output_path):
        """
        保存路径数据
        """
        try:
            # 收集路径数据
            path_data = {
                'paths': [],
                'roads': [],
                'settings': {
                    'pathfinding_algorithm': self.pathfinding_algorithm,
                    'pathfinding_enabled': self.pathfinding_enabled
                }
            }
            
            # 保存路径信息
            for path_info in self.paths:
                path_dict = {
                    'name': path_info['name'],
                    'points': [],
                    'properties': path_info['properties']
                }
                
                # 保存路径点
                for waypoint in path_info['waypoints']:
                    point_dict = {
                        'position': [waypoint['position'].x,
                                   waypoint['position'].y,
                                   waypoint['position'].z],
                        'properties': waypoint['properties']
                    }
                    path_dict['points'].append(point_dict)
                    
                path_data['paths'].append(path_dict)
                
            # 保存道路信息
            for road_info in self.roads:
                road_dict = {
                    'name': road_info['name'],
                    'start_point': [road_info['start_point'].x,
                                  road_info['start_point'].y,
                                  road_info['start_point'].z],
                    'end_point': [road_info['end_point'].x,
                                road_info['end_point'].y,
                                road_info['end_point'].z],
                    'width': road_info['width'],
                    'lanes': road_info['lanes'],
                    'properties': road_info['properties']
                }
                path_data['roads'].append(road_dict)
                
            # 确保输出目录存在
            output_dir = os.path.dirname(output_path)
            if not os.path.exists(output_dir):
                os.makedirs(output_dir)
                
            # 写入JSON文件
            with open(output_path, 'w', encoding='utf-8') as f:
                json.dump(path_data, f, indent=2, ensure_ascii=False)
                
            print(f"路径数据保存成功: {output_path}")
            return True
            
        except Exception as e:
            print(f"保存路径数据时出错: {e}")
            return False
            
    def load_path_data(self, input_path):
        """
        加载路径数据
        """
        try:
            if not os.path.exists(input_path):
                print(f"路径数据文件不存在: {input_path}")
                return False
                
            # 读取JSON文件
            with open(input_path, 'r', encoding='utf-8') as f:
                path_data = json.load(f)
                
            # 清除现有路径
            self.clear_all_paths()
            
            # 恢复设置
            if 'settings' in path_data:
                settings = path_data['settings']
                self.pathfinding_algorithm = settings.get('pathfinding_algorithm', 'a*')
                self.pathfinding_enabled = settings.get('pathfinding_enabled', True)
                
            # 创建路径
            for path_dict in path_data['paths']:
                points = []
                for point_dict in path_dict['points']:
                    position = Point3(point_dict['position'][0],
                                    point_dict['position'][1],
                                    point_dict['position'][2])
                    points.append(position)
                    
                path_info = self.create_path(path_dict['name'], points)
                if path_info:
                    self.set_path_properties(path_info, path_dict['properties'])
                    
            # 创建道路
            for road_dict in path_data['roads']:
                start_point = Point3(road_dict['start_point'][0],
                                   road_dict['start_point'][1],
                                   road_dict['start_point'][2])
                end_point = Point3(road_dict['end_point'][0],
                                 road_dict['end_point'][1],
                                 road_dict['end_point'][2])
                                 
                self.create_road(road_dict['name'], start_point, end_point,
                               road_dict['width'], road_dict['lanes'])
                               
            print(f"路径数据加载成功: {input_path}")
            return True
            
        except Exception as e:
            print(f"加载路径数据时出错: {e}")
            return False
            
    def clear_all_paths(self):
        """
        清除所有路径
        """
        try:
            # 移除所有路径节点
            for path_info in self.paths:
                if path_info['node'] and not path_info['node'].isEmpty():
                    path_info['node'].removeNode()
                    
            # 移除所有道路节点
            for road_info in self.roads:
                if road_info['node'] and not road_info['node'].isEmpty():
                    road_info['node'].removeNode()
                    
            # 清空数据结构
            self.paths = []
            self.waypoints = []
            self.roads = []
            self.graph = {}
            
            print("清除所有路径")
            return True
            
        except Exception as e:
            print(f"清除所有路径时出错: {e}")
            return False
            
    def paint_path(self, start_point, end_point, path_type='generic', width=2.0):
        """
        绘制路径（笔刷工具）
        """
        try:
            # 创建路径名称
            path_name = f"painted_path_{len(self.paths)}"
            
            # 创建路径点
            points = [start_point, end_point]
            
            # 创建路径
            path_info = self.create_path(path_name, points)
            if path_info:
                # 设置路径属性
                properties = {
                    'type': path_type,
                    'width': width
                }
                self.set_path_properties(path_info, properties)
                
                print(f"绘制路径: {path_name}")
                return path_info
                
        except Exception as e:
            print(f"绘制路径时出错: {e}")
            return None
            
    def set_pathfinding_algorithm(self, algorithm):
        """
        设置寻路算法
        """
        valid_algorithms = ['a*', 'dijkstra', 'rrt']
        if algorithm in valid_algorithms:
            self.pathfinding_algorithm = algorithm
            print(f"寻路算法设置为: {algorithm}")
            return True
        else:
            print(f"无效的寻路算法: {algorithm}")
            return False
            
    def get_navigation_mesh_stats(self):
        """
        获取导航网格统计信息
        """
        if not self.navigation_mesh:
            return {'generated': False}
            
        return {
            'generated': True,
            'vertices': len(self.navigation_mesh['vertices']),
            'triangles': len(self.navigation_mesh['triangles']),
            'obstacles': len(self.navigation_mesh['obstacles']),
            'walkable_areas': len(self.navigation_mesh['walkable_areas'])
        }