// ================================
// A*航线规划模块 - 基于3D栅格索引
// ================================
import * as Cesium from 'cesium'

class PathPlanning {
  constructor(viewer, occupancyGrid) {
    this.viewer = viewer;
    this.occupancyGrid = occupancyGrid;
    this.pathEntities = [];
    this.isEnabled = false;

    // 路径配置
    this.config = {
      pathColor: Cesium.Color.YELLOW.withAlpha(0.8), // 路径颜色（黄色）
      pathOutlineColor: Cesium.Color.ORANGE, // 路径边框颜色
      startPointColor: Cesium.Color.BLUE.withAlpha(0.9), // 起点颜色（蓝色）
      endPointColor: Cesium.Color.BLUE.withAlpha(0.9), // 终点颜色（蓝色）
      startEndOutlineColor: Cesium.Color.DARKBLUE, // 起点终点边框颜色
      outlineWidth: 2, // 边框宽度
      enableDiagonal: true, // 是否允许对角线移动（3D中为26连通性）
      verticalWeight: 1.2 // 垂直移动权重（略高于水平移动）
    };

    // 显示控制
    this.showOnlyPath = false; // 是否只显示路径单元
    this.hasActivePath = false; // 是否有活跃的路径

    // 3D栅格数据结构
    this.grid3D = null; // 3维数组存储占用状态
    this.gridDimensions = { width: 0, height: 0, depth: 0 }; // 栅格尺寸
    this.gridEntityMap = new Map(); // 栅格索引(字符串)到实体的映射
    this.indexToEntityMap = new Map(); // 栅格索引对象到实体的映射

    // A*算法相关
    this.openList = []; // 开放列表
    this.closedList = []; // 关闭列表
  }

  // ================================
  // 启用/禁用路径规划功能
  // ================================
  setEnabled(enabled) {
    this.isEnabled = enabled;
    if (!enabled) {
      this.clearPath();
    }
  }

  // ================================
  // A*路径规划主函数 - 完全基于3D栅格索引
  // ================================
  async planPath() {
    if (!this.isEnabled || !this.occupancyGrid.gridEntities.length) {
      console.warn('路径规划未启用或网格未生成');
      return;
    }

    try {
      console.log('开始基于3D栅格索引的A*路径规划...');

      // 1. 构建3D栅格索引结构和占用状态数组
      this.build3DGridStructure();

      // 2. 通过最近邻查找获取航线起点和终点对应的3D栅格索引
      const { startIndex, endIndex } = await this.findWaypointGridIndices();

      if (!startIndex || !endIndex) {
        console.warn('无法找到航线起点或终点对应的栅格索引');
        alert('无法找到航线起点或终点对应的栅格索引！');
        return;
      }

      console.log('起点栅格索引:', startIndex);
      console.log('终点栅格索引:', endIndex);
      console.log('3D栅格维度:', this.gridDimensions);

      // 3. 首先显示起点和终点（蓝色）
      console.log('首先显示起点和终点...');
      this.visualizeStartEndPoints(startIndex, endIndex);

      // 等待一小段时间让用户看到起点和终点
      await new Promise(resolve => setTimeout(resolve, 1000));

      // 4. 在三维数组上执行A*路径规划
      console.log('开始执行A*路径规划算法...');
      const pathIndices = this.aStarOnGrid3D(startIndex, endIndex);

      if (pathIndices && pathIndices.length > 0) {
        // 5. 通过索引找到对应的3D占用网格单元，复制并修改颜色（包含起点终点的蓝色）
        this.visualizePathByGridCopy(pathIndices, startIndex, endIndex);
        console.log(`路径规划成功，路径长度：${pathIndices.length}个栅格索引`);

        // 更新UI
        this.updatePathInfo(pathIndices);
      } else {
        console.warn('未找到可行路径');
        alert('未找到从起点到终点的可行路径！');
        // 即使没找到路径，也保持起点终点的显示
      }

    } catch (error) {
      console.error('路径规划错误：', error);
      alert('路径规划过程中发生错误，请查看控制台日志。');
    }
  }

  // ================================
  // 构建3D栅格索引结构和占用状态数组 - 直接使用网格实体properties数据
  // ================================
  build3DGridStructure() {
    console.log('开始构建3D栅格索引结构，直接使用网格实体properties数据...');

    if (!this.occupancyGrid.gridEntities.length) {
      throw new Error('没有可用的占用网格实体');
    }

    // 第一步：分析所有网格实体，从properties中提取gridIndex，确定栅格维度
    let minX = Infinity, minY = Infinity, minZ = Infinity;
    let maxX = -Infinity, maxY = -Infinity, maxZ = -Infinity;

    const validEntities = [];

    // 遍历所有网格实体，提取gridIndex信息
    for (const entity of this.occupancyGrid.gridEntities) {
      // 直接从properties中获取gridIndex
      if (entity.properties && entity.properties.gridIndex) {
        const gridIndex = entity.properties.gridIndex.getValue();

        if (gridIndex && typeof gridIndex.x === 'number' &&
            typeof gridIndex.y === 'number' && typeof gridIndex.z === 'number') {

          // 更新维度范围
          minX = Math.min(minX, gridIndex.x);
          maxX = Math.max(maxX, gridIndex.x);
          minY = Math.min(minY, gridIndex.y);
          maxY = Math.max(maxY, gridIndex.y);
          minZ = Math.min(minZ, gridIndex.z);
          maxZ = Math.max(maxZ, gridIndex.z);

          validEntities.push(entity);
        } else {
          console.warn('网格实体properties中的gridIndex格式不正确:', gridIndex);
        }
      } else {
        console.warn('网格实体缺少gridIndex属性');
      }
    }

    if (validEntities.length === 0) {
      throw new Error('没有找到包含有效gridIndex的网格实体');
    }

    // 计算栅格维度（基于实际的索引范围）
    this.gridDimensions = {
      width: maxX - minX + 1,
      height: maxY - minY + 1,
      depth: maxZ - minZ + 1
    };

    console.log(`从properties提取的3D栅格维度: ${this.gridDimensions.width} x ${this.gridDimensions.height} x ${this.gridDimensions.depth}`);
    console.log(`索引范围: X[${minX}, ${maxX}], Y[${minY}, ${maxY}], Z[${minZ}, ${maxZ}]`);

    // 第二步：初始化3D占用状态数组
    this.grid3D = new Array(this.gridDimensions.width);
    for (let x = 0; x < this.gridDimensions.width; x++) {
      this.grid3D[x] = new Array(this.gridDimensions.height);
      for (let y = 0; y < this.gridDimensions.height; y++) {
        this.grid3D[x][y] = new Array(this.gridDimensions.depth);
        for (let z = 0; z < this.gridDimensions.depth; z++) {
          this.grid3D[x][y][z] = {
            occupied: false,
            entity: null,
            exists: false
          };
        }
      }
    }

    // 第三步：直接使用实体properties数据填充3D数组
    this.gridEntityMap.clear();
    this.indexToEntityMap.clear();

    let mappedCount = 0;
    let occupiedCount = 0;
    let freeCount = 0;

    // 记录索引偏移量，将原始索引映射到数组索引
    this.indexOffset = { x: minX, y: minY, z: minZ };

    for (const entity of validEntities) {
      const gridIndex = entity.properties.gridIndex.getValue();
      const isOccupied = entity.properties.isOccupied ? entity.properties.isOccupied.getValue() : false;

      // 转换为数组索引（相对于最小值的偏移）
      const arrayX = gridIndex.x - minX;
      const arrayY = gridIndex.y - minY;
      const arrayZ = gridIndex.z - minZ;

      // 确保索引在有效范围内
      if (arrayX >= 0 && arrayX < this.gridDimensions.width &&
          arrayY >= 0 && arrayY < this.gridDimensions.height &&
          arrayZ >= 0 && arrayZ < this.gridDimensions.depth) {

        // 存储到3D数组
        this.grid3D[arrayX][arrayY][arrayZ] = {
          occupied: isOccupied,
          entity: entity,
          exists: true,
          originalIndex: { x: gridIndex.x, y: gridIndex.y, z: gridIndex.z } // 保存原始索引
        };

        // 建立映射关系
        const gridKey = `${arrayX},${arrayY},${arrayZ}`;
        this.gridEntityMap.set(gridKey, entity);
        this.indexToEntityMap.set(`${arrayX}_${arrayY}_${arrayZ}`, entity);

        mappedCount++;
        if (isOccupied) {
          occupiedCount++;
        } else {
          freeCount++;
        }
      } else {
        console.warn(`网格索引超出范围: 原始(${gridIndex.x}, ${gridIndex.y}, ${gridIndex.z}) -> 数组(${arrayX}, ${arrayY}, ${arrayZ})`);
      }
    }

    console.log(`3D栅格结构构建完成:`);
    console.log(`- 栅格维度: ${this.gridDimensions.width} x ${this.gridDimensions.height} x ${this.gridDimensions.depth}`);
    console.log(`- 索引偏移: (${this.indexOffset.x}, ${this.indexOffset.y}, ${this.indexOffset.z})`);
    console.log(`- 映射实体数: ${mappedCount}`);
    console.log(`- 占用栅格: ${occupiedCount}`);
    console.log(`- 空闲栅格: ${freeCount}`);
    console.log(`- 占用率: ${mappedCount > 0 ? ((occupiedCount / mappedCount) * 100).toFixed(1) : 0}%`);
  }

  // ================================
  // 通过最近邻查找获取航线起点和终点对应的3D栅格索引
  // ================================
  async findWaypointGridIndices() {
    console.log('开始通过最近邻查找航线起点和终点对应的栅格索引...');

    // 获取航线航点
    const waypoints = this.getFlightWaypoints();

    if (!waypoints || waypoints.length < 2) {
      console.warn('航线航点不足，无法确定起点和终点');
      return { startIndex: null, endIndex: null };
    }

    const startWaypoint = waypoints[0];
    const endWaypoint = waypoints[waypoints.length - 1];

    console.log('航线起点位置:', startWaypoint.position);
    console.log('航线终点位置:', endWaypoint.position);

    // 使用最近邻搜索找到对应的栅格索引
    const startIndex = this.findNearestGridIndex(startWaypoint.position);
    const endIndex = this.findNearestGridIndex(endWaypoint.position);

    if (!startIndex) {
      console.error('无法找到航线起点对应的栅格索引');
      return { startIndex: null, endIndex: null };
    }

    if (!endIndex) {
      console.error('无法找到航线终点对应的栅格索引');
      return { startIndex: null, endIndex: null };
    }

    // 验证找到的栅格索引是否可通行
    if (this.grid3D[startIndex.x][startIndex.y][startIndex.z].occupied) {
      console.warn('起点栅格被占用，尝试寻找附近的空闲栅格...');
      const freeStartIndex = this.findNearestFreeGrid(startIndex);
      if (freeStartIndex) {
        console.log(`找到起点附近的空闲栅格: (${freeStartIndex.x}, ${freeStartIndex.y}, ${freeStartIndex.z})`);
        startIndex.x = freeStartIndex.x;
        startIndex.y = freeStartIndex.y;
        startIndex.z = freeStartIndex.z;
      } else {
        console.error('起点附近没有可通行的栅格');
        return { startIndex: null, endIndex: null };
      }
    }

    if (this.grid3D[endIndex.x][endIndex.y][endIndex.z].occupied) {
      console.warn('终点栅格被占用，尝试寻找附近的空闲栅格...');
      const freeEndIndex = this.findNearestFreeGrid(endIndex);
      if (freeEndIndex) {
        console.log(`找到终点附近的空闲栅格: (${freeEndIndex.x}, ${freeEndIndex.y}, ${freeEndIndex.z})`);
        endIndex.x = freeEndIndex.x;
        endIndex.y = freeEndIndex.y;
        endIndex.z = freeEndIndex.z;
      } else {
        console.error('终点附近没有可通行的栅格');
        return { startIndex: null, endIndex: null };
      }
    }

    console.log(`成功找到起点栅格索引: (${startIndex.x}, ${startIndex.y}, ${startIndex.z})`);
    console.log(`成功找到终点栅格索引: (${endIndex.x}, ${endIndex.y}, ${endIndex.z})`);

    return { startIndex, endIndex };
  }

  // ================================
  // 最近邻搜索：找到距离指定世界坐标最近的栅格索引 - 直接使用properties数据
  // ================================
  findNearestGridIndex(targetPosition) {
    let nearestIndex = null;
    let minDistance = Infinity;
    let nearestEntity = null;

    // 直接遍历所有网格实体，使用properties中的gridIndex
    for (const entity of this.occupancyGrid.gridEntities) {
      if (entity.properties && entity.properties.gridIndex && entity.position) {
        try {
          // 获取栅格实体的位置
          const entityPosition = entity.position.getValue(this.viewer.clock.currentTime);

          // 计算距离
          const distance = Cesium.Cartesian3.distance(targetPosition, entityPosition);

          if (distance < minDistance) {
            minDistance = distance;
            nearestEntity = entity;

            // 直接从properties获取gridIndex，并转换为数组索引
            const originalIndex = entity.properties.gridIndex.getValue();
            nearestIndex = {
              x: originalIndex.x - this.indexOffset.x,
              y: originalIndex.y - this.indexOffset.y,
              z: originalIndex.z - this.indexOffset.z,
              original: originalIndex // 保存原始索引以便调试
            };
          }
        } catch (error) {
          console.warn('处理网格实体时出错:', error);
        }
      }
    }

    if (nearestIndex) {
      console.log(`找到最近栅格索引: 数组索引(${nearestIndex.x}, ${nearestIndex.y}, ${nearestIndex.z}), 原始索引(${nearestIndex.original.x}, ${nearestIndex.original.y}, ${nearestIndex.original.z}), 距离: ${minDistance.toFixed(2)}m`);

      // 验证找到的索引是否有效
      if (!this.isValidGridIndex(nearestIndex)) {
        console.error('找到的栅格索引无效');
        return null;
      }

      // 移除original属性，只返回数组索引
      return { x: nearestIndex.x, y: nearestIndex.y, z: nearestIndex.z };
    } else {
      console.warn('未找到最近的栅格索引');
      return null;
    }
  }

  // ================================
  // 寻找指定栅格索引附近的空闲（可通行）栅格
  // ================================
  findNearestFreeGrid(centerIndex) {
    const searchRadius = 3; // 搜索半径

    for (let radius = 1; radius <= searchRadius; radius++) {
      for (let dx = -radius; dx <= radius; dx++) {
        for (let dy = -radius; dy <= radius; dy++) {
          for (let dz = -radius; dz <= radius; dz++) {
            const x = centerIndex.x + dx;
            const y = centerIndex.y + dy;
            const z = centerIndex.z + dz;

            if (this.isValidGridIndex({ x, y, z })) {
              const cell = this.grid3D[x][y][z];
              if (cell.exists && !cell.occupied) {
                return { x, y, z };
              }
            }
          }
        }
      }
    }

    return null; // 未找到空闲栅格
  }

  // ================================
  // 获取航线航点
  // ================================
  getFlightWaypoints() {
    // 尝试从UAVApp获取航线航点
    if (window.uavApp && window.uavApp.waypoints && window.uavApp.waypoints.length > 0) {
      return window.uavApp.waypoints;
    }

    console.warn('未找到航线航点，将无法进行基于航线的路径规划');
    return [];
  }

  // ================================
  // 在三维数组上执行A*路径规划算法
  // ================================
  aStarOnGrid3D(startIndex, endIndex) {
    console.log('开始在三维数组上执行A*路径规划算法...');
    console.log(`起点: (${startIndex.x}, ${startIndex.y}, ${startIndex.z})`);
    console.log(`终点: (${endIndex.x}, ${endIndex.y}, ${endIndex.z})`);

    // 清理算法数据结构
    this.openList = [];
    this.closedList = [];

    // 创建起始节点
    const startNode = {
      x: startIndex.x,
      y: startIndex.y,
      z: startIndex.z,
      g: 0,
      h: this.calculateHeuristic(startIndex, endIndex),
      f: 0,
      parent: null
    };
    startNode.f = startNode.g + startNode.h;

    this.openList.push(startNode);

    let iterations = 0;
    const maxIterations = 50000;
    let verticalMoves = 0;

    while (this.openList.length > 0 && iterations < maxIterations) {
      iterations++;

      // 找到f值最小的节点
      this.openList.sort((a, b) => a.f - b.f);
      const currentNode = this.openList.shift();

      // 如果到达终点
      if (currentNode.x === endIndex.x &&
          currentNode.y === endIndex.y &&
          currentNode.z === endIndex.z) {
        console.log(`A*算法完成，迭代次数：${iterations}`);
        console.log(`垂直移动次数：${verticalMoves}`);
        return this.reconstructPath(currentNode);
      }

      // 将当前节点移到关闭列表
      this.closedList.push(currentNode);

      // 获取邻居节点
      const neighbors = this.getNeighbors(currentNode);

      for (const neighbor of neighbors) {
        // 跳过已在关闭列表中的节点
        if (this.isInClosedList(neighbor)) {
          continue;
        }

        // 检查是否可通行
        if (!this.isTraversable(neighbor.x, neighbor.y, neighbor.z)) {
          continue;
        }

        // 计算移动成本
        const moveCost = this.getMoveCost(currentNode, neighbor);
        const tentativeG = currentNode.g + moveCost;

        // 统计垂直移动
        if (neighbor.z !== currentNode.z) {
          verticalMoves++;
        }

        // 检查是否已在开放列表中
        const existingNode = this.findInOpenList(neighbor);
        if (existingNode) {
          if (tentativeG < existingNode.g) {
            existingNode.g = tentativeG;
            existingNode.h = this.calculateHeuristic(neighbor, endIndex);
            existingNode.f = existingNode.g + existingNode.h;
            existingNode.parent = currentNode;
          }
        } else {
          const newNode = {
            x: neighbor.x,
            y: neighbor.y,
            z: neighbor.z,
            g: tentativeG,
            h: this.calculateHeuristic(neighbor, endIndex),
            f: 0,
            parent: currentNode
          };
          newNode.f = newNode.g + newNode.h;
          this.openList.push(newNode);
        }
      }

      // 每1000次迭代输出一次进度
      if (iterations % 1000 === 0) {
        const openCount = this.openList.length;
        const closedCount = this.closedList.length;
        console.log(`A*算法进度: ${iterations}次迭代, 开放列表: ${openCount}, 关闭列表: ${closedCount}`);
      }
    }

    console.warn(`A*算法未找到路径，迭代次数：${iterations}`);
    return null;
  }

  // ================================
  // 重构路径
  // ================================
  reconstructPath(endNode) {
    const path = [];
    let currentNode = endNode;

    while (currentNode) {
      path.unshift({
        x: currentNode.x,
        y: currentNode.y,
        z: currentNode.z
      });
      currentNode = currentNode.parent;
    }

    console.log(`路径重构完成，路径长度: ${path.length}个节点`);
    return path;
  }

  // ================================
  // 计算启发式函数（3D曼哈顿距离）
  // ================================
  calculateHeuristic(index1, index2) {
    const dx = Math.abs(index1.x - index2.x);
    const dy = Math.abs(index1.y - index2.y);
    const dz = Math.abs(index1.z - index2.z);

    // 使用3D曼哈顿距离，给垂直移动适当权重
    return dx + dy + (dz * this.config.verticalWeight);
  }

  // ================================
  // 获取邻居节点（3D：26连通性）
  // ================================
  getNeighbors(currentNode) {
    const neighbors = [];

    // 3D搜索：包括所有26个方向的邻居
    for (let dx = -1; dx <= 1; dx++) {
      for (let dy = -1; dy <= 1; dy++) {
        for (let dz = -1; dz <= 1; dz++) {
          if (dx === 0 && dy === 0 && dz === 0) continue;

          // 如果不允许对角线移动，只使用6连通性
          if (!this.config.enableDiagonal) {
            const isDirectional = (Math.abs(dx) + Math.abs(dy) + Math.abs(dz)) === 1;
            if (!isDirectional) continue;
          }

          const newX = currentNode.x + dx;
          const newY = currentNode.y + dy;
          const newZ = currentNode.z + dz;

          // 检查边界
          if (this.isValidGridIndex({ x: newX, y: newY, z: newZ })) {
            neighbors.push({ x: newX, y: newY, z: newZ });
          }
        }
      }
    }

    return neighbors;
  }

  // ================================
  // A*算法辅助方法
  // ================================
  isInClosedList(node) {
    return this.closedList.some(n =>
      n.x === node.x && n.y === node.y && n.z === node.z
    );
  }

  findInOpenList(node) {
    return this.openList.find(n =>
      n.x === node.x && n.y === node.y && n.z === node.z
    );
  }

  // ================================
  // 检查位置是否可通行
  // ================================
  isTraversable(x, y, z) {
    if (!this.isValidGridIndex({ x, y, z })) {
      return false;
    }

    const cell = this.grid3D[x][y][z];

    // 如果该位置不存在网格实体，则不可通行
    if (!cell.exists) {
      return false;
    }

    // 如果有实体但未被占用，可通行
    return !cell.occupied;
  }

  // ================================
  // 计算移动成本
  // ================================
  getMoveCost(fromNode, toNode) {
    const dx = Math.abs(toNode.x - fromNode.x);
    const dy = Math.abs(toNode.y - fromNode.y);
    const dz = Math.abs(toNode.z - fromNode.z);

    // 计算基础移动成本
    let baseCost;
    if (dx && dy && dz) {
      baseCost = Math.sqrt(3); // 3D对角线移动
    } else if ((dx && dy) || (dx && dz) || (dy && dz)) {
      baseCost = Math.sqrt(2); // 2D对角线移动
    } else {
      baseCost = 1; // 直线移动
    }

    // 如果是垂直移动，应用垂直权重
    if (dz > 0) {
      baseCost *= this.config.verticalWeight;
    }

    return baseCost;
  }

  // ================================
  // 通过索引找到对应的3D占用网格单元，复制并修改为黄色 - 使用properties数据
  // ================================
  visualizePathByGridCopy(pathIndices, startIndex, endIndex) {
    console.log('开始通过复制网格单元可视化路径...');
    console.log('路径索引序列:', pathIndices.map(p => `(${p.x},${p.y},${p.z})`).join(' -> '));
    console.log(`路径总长度: ${pathIndices.length}个栅格索引`);

    // 清除之前的路径
    this.clearPath();

    let copiedCount = 0;
    let missingCount = 0;

    for (let i = 0; i < pathIndices.length; i++) {
      const gridIndex = pathIndices[i];

      // 从3D数组中获取对应的网格单元
      const cell = this.grid3D[gridIndex.x][gridIndex.y][gridIndex.z];

      if (cell.exists && cell.entity) {
        // 获取原始网格实体的信息
        const sourceEntity = cell.entity;
        const sourcePosition = sourceEntity.position.getValue(this.viewer.clock.currentTime);
        const sourceDimensions = sourceEntity.box.dimensions.getValue();

        // 获取原始索引（用于显示和命名）
        const originalIndex = cell.originalIndex || {
          x: gridIndex.x + this.indexOffset.x,
          y: gridIndex.y + this.indexOffset.y,
          z: gridIndex.z + this.indexOffset.z
        };

        // 判断是否为起点或终点
        const isStartPoint = (gridIndex.x === startIndex.x &&
                             gridIndex.y === startIndex.y &&
                             gridIndex.z === startIndex.z);
        const isEndPoint = (gridIndex.x === endIndex.x &&
                           gridIndex.y === endIndex.y &&
                           gridIndex.z === endIndex.z);

        // 根据点的类型选择颜色和名称
        let entityColor, outlineColor, entityName, pointType;

        if (isStartPoint) {
          entityColor = this.config.startPointColor;
          outlineColor = this.config.startEndOutlineColor;
          entityName = `StartPoint_${originalIndex.x}_${originalIndex.y}_${originalIndex.z}`;
          pointType = '起点';
        } else if (isEndPoint) {
          entityColor = this.config.endPointColor;
          outlineColor = this.config.startEndOutlineColor;
          entityName = `EndPoint_${originalIndex.x}_${originalIndex.y}_${originalIndex.z}`;
          pointType = '终点';
        } else {
          entityColor = this.config.pathColor;
          outlineColor = this.config.pathOutlineColor;
          entityName = `PathGrid_${originalIndex.x}_${originalIndex.y}_${originalIndex.z}`;
          pointType = '路径';
        }

        // 复制网格单元，修改颜色
        const pathEntity = this.viewer.entities.add({
          name: entityName,
          position: sourcePosition,
          box: {
            dimensions: sourceDimensions,
            material: entityColor,
            outline: true,
            outlineColor: outlineColor,
            outlineWidth: this.config.outlineWidth
          },
          properties: {
            isPathGrid: true,
            gridIndex: originalIndex, // 使用原始索引
            arrayIndex: { x: gridIndex.x, y: gridIndex.y, z: gridIndex.z }, // 保存数组索引
            sourceEntityId: sourceEntity.id,
            pathStep: copiedCount,
            pointType: pointType
          }
        });

        this.pathEntities.push(pathEntity);
        copiedCount++;

        console.log(`复制网格单元 数组索引[${gridIndex.x}, ${gridIndex.y}, ${gridIndex.z}] 原始索引[${originalIndex.x}, ${originalIndex.y}, ${originalIndex.z}] -> ${pointType}网格`);

      } else {
        missingCount++;
        console.warn(`网格索引 [${gridIndex.x}, ${gridIndex.y}, ${gridIndex.z}] 对应的网格单元不存在`);
      }
    }

    // 设置路径状态
    this.hasActivePath = copiedCount > 0;

    // 如果启用了只显示路径模式，隐藏其他网格
    if (this.showOnlyPath && this.hasActivePath) {
      this.updateGridVisibility();
    }

    console.log(`路径可视化完成:`);
    console.log(`- 成功复制网格: ${copiedCount}个`);
    console.log(`- 缺失网格: ${missingCount}个`);

    // 显示起点和终点的原始索引
    const startOriginalIndex = this.grid3D[startIndex.x][startIndex.y][startIndex.z].originalIndex ||
      { x: startIndex.x + this.indexOffset.x, y: startIndex.y + this.indexOffset.y, z: startIndex.z + this.indexOffset.z };
    const endOriginalIndex = this.grid3D[endIndex.x][endIndex.y][endIndex.z].originalIndex ||
      { x: endIndex.x + this.indexOffset.x, y: endIndex.y + this.indexOffset.y, z: endIndex.z + this.indexOffset.z };

    console.log(`- 起点: 数组索引(${startIndex.x}, ${startIndex.y}, ${startIndex.z}) 原始索引(${startOriginalIndex.x}, ${startOriginalIndex.y}, ${startOriginalIndex.z}) -> 蓝色`);
    console.log(`- 终点: 数组索引(${endIndex.x}, ${endIndex.y}, ${endIndex.z}) 原始索引(${endOriginalIndex.x}, ${endOriginalIndex.y}, ${endOriginalIndex.z}) -> 蓝色`);

    if (missingCount > 0) {
      console.warn(`⚠️ 有${missingCount}个路径索引对应的网格单元缺失`);
    }

    // 统计路径的3D特征
    this.analyzePathCharacteristics(pathIndices);
  }

  // ================================
  // 分析路径特征
  // ================================
  analyzePathCharacteristics(pathIndices) {
    if (!pathIndices || pathIndices.length === 0) {
      return;
    }

    console.log('=== 路径特征分析 ===');

    // 计算路径的空间范围
    const xValues = pathIndices.map(p => p.x);
    const yValues = pathIndices.map(p => p.y);
    const zValues = pathIndices.map(p => p.z);

    const xRange = [Math.min(...xValues), Math.max(...xValues)];
    const yRange = [Math.min(...yValues), Math.max(...yValues)];
    const zRange = [Math.min(...zValues), Math.max(...zValues)];

    console.log(`X轴范围: [${xRange[0]}, ${xRange[1]}], 跨度: ${xRange[1] - xRange[0]}`);
    console.log(`Y轴范围: [${yRange[0]}, ${yRange[1]}], 跨度: ${yRange[1] - yRange[0]}`);
    console.log(`Z轴范围: [${zRange[0]}, ${zRange[1]}], 跨度: ${zRange[1] - zRange[0]}`);

    // 统计移动类型
    let verticalMoves = 0;
    let horizontalMoves = 0;
    let diagonalMoves = 0;

    for (let i = 1; i < pathIndices.length; i++) {
      const prev = pathIndices[i - 1];
      const curr = pathIndices[i];

      const dx = Math.abs(curr.x - prev.x);
      const dy = Math.abs(curr.y - prev.y);
      const dz = Math.abs(curr.z - prev.z);

      if (dz > 0) verticalMoves++;
      if (dx > 0 || dy > 0) horizontalMoves++;
      if ((dx && dy) || (dx && dz) || (dy && dz)) diagonalMoves++;
    }

    const totalMoves = pathIndices.length - 1;
    console.log(`总移动步数: ${totalMoves}`);
    console.log(`垂直移动: ${verticalMoves} (${(verticalMoves/totalMoves*100).toFixed(1)}%)`);
    console.log(`水平移动: ${horizontalMoves} (${(horizontalMoves/totalMoves*100).toFixed(1)}%)`);
    console.log(`对角移动: ${diagonalMoves} (${(diagonalMoves/totalMoves*100).toFixed(1)}%)`);

    // 检查3D路径特征
    const hasVerticalVariation = (zRange[1] - zRange[0]) > 0;
    if (hasVerticalVariation) {
      console.log('✅ 路径包含3D垂直变化，成功进行了立体路径规划');
    } else {
      console.log('ℹ️ 路径没有垂直变化，为平面路径');
    }

    console.log('=== 分析完成 ===');
  }

  // ================================
  // 清除路径
  // ================================
  clearPath() {
    // 从场景中移除路径网格实体
    for (const pathEntity of this.pathEntities) {
      if (this.viewer.entities.contains(pathEntity)) {
        this.viewer.entities.remove(pathEntity);
      }
    }

    this.pathEntities = [];
    this.hasActivePath = false; // 重置路径状态

    // 恢复所有网格的显示
    // this.updateGridVisibility();

    console.log('路径已清除，所有路径网格（包括起点、终点）已从场景中移除');
  }

  // ================================
  // 更新路径信息UI
  // ================================
  updatePathInfo(pathIndices) {
    const pathLengthElement = document.getElementById('pathLength');
    const pathStatusElement = document.getElementById('pathStatus');

    if (pathLengthElement) {
      pathLengthElement.textContent = pathIndices.length;
    }

    if (pathStatusElement) {
      pathStatusElement.textContent = '规划完成';
    }
  }

  // ================================
  // 设置只显示路径模式
  // ================================
  setShowOnlyPath(showOnly) {
    this.showOnlyPath = showOnly;
    this.updateGridVisibility();
    console.log(`只显示路径模式: ${showOnly ? '启用' : '禁用'}`);
  }

  // ================================
  // 更新网格显示状态
  // ================================
  updateGridVisibility() {
    if (!this.occupancyGrid.gridEntities.length) {
      return;
    }

    // 控制原始占用网格的显示
    for (const entity of this.occupancyGrid.gridEntities) {
      if (entity.show !== undefined) {
        if (this.showOnlyPath && this.hasActivePath) {
          // 只显示路径模式：隐藏原始网格
          entity.show = false;
        } else {
          // 正常显示模式：显示所有原始网格
          entity.show = true;
        }
      }
    }

    // 控制路径网格的显示（黄色路径网格始终显示，除非被清除）
    for (const pathEntity of this.pathEntities) {
      if (pathEntity.show !== undefined) {
        pathEntity.show = true; // 路径网格始终显示
      }
    }

    const pathGridCount = this.pathEntities.length;
    const originalGridCount = this.occupancyGrid.gridEntities.length;

    console.log(`网格显示状态已更新: ${this.showOnlyPath ? '只显示路径' : '显示全部'}`);
    console.log(`当前显示: 原始网格 ${this.showOnlyPath && this.hasActivePath ? 0 : originalGridCount} 个, 路径网格 ${pathGridCount} 个`);
  }

  // ================================
  // 验证栅格索引是否有效
  // ================================
  isValidGridIndex(index) {
    return index &&
           typeof index.x === 'number' &&
           typeof index.y === 'number' &&
           typeof index.z === 'number' &&
           index.x >= 0 && index.x < this.gridDimensions.width &&
           index.y >= 0 && index.y < this.gridDimensions.height &&
           index.z >= 0 && index.z < this.gridDimensions.depth;
  }

  // ================================
  // 首先显示起点和终点（蓝色）
  // ================================
  visualizeStartEndPoints(startIndex, endIndex) {
    console.log('开始显示起点和终点...');

    // 清除之前的路径（如果有）
    this.clearPath();

    // 显示起点（蓝色）
    this.createPointVisualization(startIndex, 'start');

    // 显示终点（蓝色）
    this.createPointVisualization(endIndex, 'end');

    console.log(`起点和终点已显示为蓝色:`);
    console.log(`- 起点: (${startIndex.x}, ${startIndex.y}, ${startIndex.z})`);
    console.log(`- 终点: (${endIndex.x}, ${endIndex.y}, ${endIndex.z})`);
  }

  // ================================
  // 创建单个点的可视化（起点或终点） - 使用properties数据
  // ================================
  createPointVisualization(gridIndex, pointType) {
    // 从3D数组中获取对应的网格单元
    const cell = this.grid3D[gridIndex.x][gridIndex.y][gridIndex.z];

    if (cell.exists && cell.entity) {
      // 获取原始网格实体的信息
      const sourceEntity = cell.entity;
      const sourcePosition = sourceEntity.position.getValue(this.viewer.clock.currentTime);
      const sourceDimensions = sourceEntity.box.dimensions.getValue();

      // 获取原始索引（用于显示和命名）
      const originalIndex = cell.originalIndex || {
        x: gridIndex.x + this.indexOffset.x,
        y: gridIndex.y + this.indexOffset.y,
        z: gridIndex.z + this.indexOffset.z
      };

      // 根据点类型设置属性
      let entityColor, outlineColor, entityName, pointTypeChinese;

      if (pointType === 'start') {
        entityColor = this.config.startPointColor;
        outlineColor = this.config.startEndOutlineColor;
        entityName = `StartPoint_${originalIndex.x}_${originalIndex.y}_${originalIndex.z}`;
        pointTypeChinese = '起点';
      } else if (pointType === 'end') {
        entityColor = this.config.endPointColor;
        outlineColor = this.config.startEndOutlineColor;
        entityName = `EndPoint_${originalIndex.x}_${originalIndex.y}_${originalIndex.z}`;
        pointTypeChinese = '终点';
      }

      // 创建点的可视化实体
      const pointEntity = this.viewer.entities.add({
        name: entityName,
        position: sourcePosition,
        box: {
          dimensions: sourceDimensions,
          material: entityColor,
          outline: true,
          outlineColor: outlineColor,
          outlineWidth: this.config.outlineWidth
        },
        properties: {
          isPathGrid: true,
          gridIndex: originalIndex, // 使用原始索引
          arrayIndex: { x: gridIndex.x, y: gridIndex.y, z: gridIndex.z }, // 保存数组索引
          sourceEntityId: sourceEntity.id,
          pointType: pointTypeChinese
        }
      });

      this.pathEntities.push(pointEntity);
      console.log(`创建${pointTypeChinese}可视化: 数组索引[${gridIndex.x}, ${gridIndex.y}, ${gridIndex.z}] 原始索引[${originalIndex.x}, ${originalIndex.y}, ${originalIndex.z}]`);

    } else {
      console.warn(`网格索引 [${gridIndex.x}, ${gridIndex.y}, ${gridIndex.z}] 对应的网格单元不存在，无法创建${pointType}可视化`);
    }
  }

  // ================================
  // 设置路径实体透明度
  // ================================
  setPathAlpha(alpha) {
    if (alpha < 0.1 || alpha > 1.0) {
      console.warn('透明度值应该在0.1到1.0之间');
      return;
    }

    console.log(`设置路径透明度为: ${alpha}`);

    // 更新配置中的颜色透明度
    this.config.pathColor = Cesium.Color.YELLOW.withAlpha(alpha);
    this.config.startPointColor = Cesium.Color.BLUE.withAlpha(alpha);
    this.config.endPointColor = Cesium.Color.BLUE.withAlpha(alpha);

    // 更新所有现有的路径实体透明度
    let updatedCount = 0;

    for (const pathEntity of this.pathEntities) {
      if (pathEntity.box && pathEntity.box.material) {
        try {
          // 获取实体的类型
          const pointType = pathEntity.properties && pathEntity.properties.pointType ?
            pathEntity.properties.pointType.getValue() : '路径';

          // 根据实体类型设置对应的颜色
          let newColor;
          if (pointType === '起点' || pointType === '终点') {
            newColor = Cesium.Color.BLUE.withAlpha(alpha);
          } else {
            newColor = Cesium.Color.YELLOW.withAlpha(alpha);
          }

          // 更新实体的材质颜色
          pathEntity.box.material = newColor;
          updatedCount++;

        } catch (error) {
          console.warn('更新路径实体透明度时出错:', error);
        }
      }
    }

    console.log(`成功更新${updatedCount}个路径实体的透明度`);
  }

  // ================================
  // 获取当前路径透明度
  // ================================
  getPathAlpha() {
    // 从配置中获取当前透明度
    if (this.config.pathColor && this.config.pathColor.alpha !== undefined) {
      return this.config.pathColor.alpha;
    }
    return 0.8; // 默认透明度
  }
}

// ================================
// 导出模块（如果使用模块系统）
// ================================
if (typeof module !== 'undefined' && module.exports) {
  module.exports = PathPlanning;
}


export default PathPlanning