package com.algo.service;

import com.algo.model.CTUPhysicalConfig;
import com.algo.model.PathCode;
import com.algo.model.PlannedPath;
import com.algo.util.JsonUtils;

import java.util.*;

/**
 * A*路径规划器实现
 * 使用3D A*算法进行CTU时空路径规划，支持物理约束
 */
public class AStarPathPlanner implements PathPlanner {

    /**
     * 路径映射表
     */
    private final Map<String, Map<String, Integer>> pathMapping;

    /**
     * 邻接表
     */
    private final Map<String, List<Map<String, String>>> adjacencyList;

    /**
     * 时间精度（毫秒）
     */
    private final long timeResolution = 1000;

    /**
     * 最大搜索时间（毫秒）
     */
    private final long maxSearchTime = 30000; // 30秒

    /**
     * 最大搜索深度
     */
    private final int maxSearchDepth = 15000;

    /**
     * 距离缓存 - 优化：缓存距离计算结果
     */
    private final Map<String, Double> distanceCache = new HashMap<>();

    /**
     * 快速路径缓存
     */
    private final Map<String, List<String>> fastPathCache = new HashMap<>();

    /**
     * 构造函数
     *
     * @param pathMapping 路径映射表
     */
    public AStarPathPlanner(Map<String, Map<String, Integer>> pathMapping) {
        this.pathMapping = pathMapping;
        this.adjacencyList = new HashMap<>();

        // 直接构建邻接表
        buildAdjacencyList();

        // 预计算常用距离
        precomputeCommonDistances();

        System.out.println("A*路径规划器初始化完成，邻接表包含 " + adjacencyList.size() + " 个节点");
    }

    @Override
    public PlannedPath planPath(String startCode, String endCode, List<double[]> constraints) {
        // 先尝试快速路径规划，失败后再使用完整的时空规划
        PlannedPath fastPath = tryFastPathPlanning(startCode, endCode, constraints);
        if (fastPath != null) {
            return fastPath;
        }
        return planSpaceTimePath(startCode, endCode, constraints, null, null);
    }

    @Override
    public PlannedPath planPath(String startCode, String endCode) {
        return planPath(startCode, endCode, null);
    }

    /**
     * 规划时空路径
     *
     * @param startCode             起始路径点ID
     * @param endCode               目标路径点ID
     * @param constraints           静态约束条件
     * @param spaceTimeOccupancyMap 时空占用表
     * @param physicalConfig        CTU物理配置
     * @return 规划的路径
     */
    public PlannedPath planSpaceTimePath(String startCode, String endCode,
                                         List<double[]> constraints,
                                         Map<String, String> spaceTimeOccupancyMap,
                                         CTUPhysicalConfig physicalConfig) {
        // 验证输入
        if (!isValidPathPoint(startCode) || !isValidPathPoint(endCode)) {
            System.out.println("无效的路径点: " + startCode + " 或 " + endCode);
            return null;
        }

        // 起点和终点相同
        if (startCode.equals(endCode)) {
            List<PathCode> codeList = new ArrayList<>();
            codeList.add(new PathCode(startCode, "90"));
            return new PlannedPath("", "", codeList);
        }

        // 使用默认物理配置
        if (physicalConfig == null) {
            physicalConfig = createDefaultPhysicalConfig();
        }

        // 获取起点和终点坐标
        int[] startCoord = JsonUtils.getCoordinate(startCode, pathMapping);
        int[] endCoord = JsonUtils.getCoordinate(endCode, pathMapping);

        if (startCoord == null || endCoord == null) {
            System.out.println("无法获取坐标: " + startCode + " 或 " + endCode);
            return null;
        }

        System.out.println("开始时空A*搜索: " + startCode + " -> " + endCode);

        // 时空A*算法实现
        PlannedPath result = spaceTimeAStarSearch(
                startCode, endCode, startCoord, endCoord,
                constraints, spaceTimeOccupancyMap, physicalConfig
        );

        if (result != null) {
            System.out.println("时空路径规划成功，路径长度: " + result.getCodeList().size());
        } else {
            System.out.println("时空路径规划失败");
        }

        return result;
    }

    /**
     * 时空A*搜索算法实现
     *
     * @param startCode      起始路径点ID
     * @param endCode        目标路径点ID
     * @param startCoord     起始坐标
     * @param endCoord       目标坐标
     * @param constraints    约束条件
     * @param occupancyMap   时空占用表
     * @param physicalConfig 物理配置
     * @return 规划的路径
     */
    private PlannedPath spaceTimeAStarSearch(String startCode, String endCode,
                                             int[] startCoord, int[] endCoord,
                                             List<double[]> constraints,
                                             Map<String, String> occupancyMap,
                                             CTUPhysicalConfig physicalConfig) {

        // 使用优先队列实现开放列表
        PriorityQueue<SpaceTimeAStarNode> openSet = new PriorityQueue<>(
                Comparator.comparingDouble(n -> n.fScore)
        );
        Set<String> closedSet = new HashSet<>();
        Map<String, Double> gScores = new HashMap<>();
        Map<String, SpaceTimeAStarNode> cameFrom = new HashMap<>();

        // 起始时间
        long startTime = System.currentTimeMillis();

        // 初始化起始节点
        SpaceTimeAStarNode startNode = new SpaceTimeAStarNode(
                startCode, startCoord, startTime, 0,
                spaceTimeHeuristic(startCoord, endCoord, startTime, physicalConfig)
        );

        openSet.offer(startNode);
        String startKey = createSpaceTimeKey(startCode, startTime);
        gScores.put(startKey, 0.0);

        // 构建约束检查器
        EnhancedConstraintChecker constraintChecker = new EnhancedConstraintChecker(
                constraints, occupancyMap, pathMapping, physicalConfig
        );

        int searchDepth = 0;
        long searchStartTime = System.currentTimeMillis();

        while (!openSet.isEmpty()) {
            // 检查搜索时间和深度限制
            if (System.currentTimeMillis() - searchStartTime > maxSearchTime ||
                    searchDepth > maxSearchDepth) {
                System.out.println("搜索超时或达到最大深度，返回null");
                break;
            }

            SpaceTimeAStarNode current = openSet.poll();
            String currentKey = createSpaceTimeKey(current.code, current.timePoint);

            if (closedSet.contains(currentKey)) {
                continue;
            }

            closedSet.add(currentKey);
            searchDepth++;

            // 到达目标
            if (current.code.equals(endCode)) {
                System.out.println("找到目标，重建路径，搜索深度: " + searchDepth);
                return reconstructSpaceTimePath(cameFrom, startNode, current, physicalConfig);
            }

            // 扩展邻居节点
            expandNeighbors(current, endCoord, openSet, closedSet, gScores, cameFrom,
                    constraintChecker, physicalConfig);
        }

        System.out.println("未找到有效路径，搜索深度: " + searchDepth);
        return null; // 无法找到路径
    }

    /**
     * 扩展邻居节点
     */
    private void expandNeighbors(SpaceTimeAStarNode current, int[] endCoord,
                                 PriorityQueue<SpaceTimeAStarNode> openSet,
                                 Set<String> closedSet, Map<String, Double> gScores,
                                 Map<String, SpaceTimeAStarNode> cameFrom,
                                 EnhancedConstraintChecker constraintChecker,
                                 CTUPhysicalConfig physicalConfig) {

        // 获取空间邻居
        List<Map<String, String>> neighbors = getNeighbors(current.code);

        for (Map<String, String> neighbor : neighbors) {
            String neighborCode = neighbor.get("code");
            String direction = neighbor.get("direction");

            // 计算移动到邻居的时间成本
            long travelTime = calculateTravelTime(current.code, neighborCode, direction, physicalConfig);
            long arrivalTime = current.timePoint + travelTime;

            // 时间对齐到分辨率
            arrivalTime = alignToTimeResolution(arrivalTime);

            String neighborKey = createSpaceTimeKey(neighborCode, arrivalTime);

            if (closedSet.contains(neighborKey)) {
                continue;
            }

            // 约束检查
            if (!constraintChecker.isValidMove(current.code, neighborCode,
                    current.timePoint, arrivalTime)) {
                continue;
            }

            // 计算G值
            double gScore = gScores.get(createSpaceTimeKey(current.code, current.timePoint)) +
                    travelTime / 1000.0; // 转换为秒

            // 更新最佳路径
            if (!gScores.containsKey(neighborKey) || gScore < gScores.get(neighborKey)) {
                gScores.put(neighborKey, gScore);

                // 获取邻居坐标
                int[] neighborCoord = JsonUtils.getCoordinate(neighborCode, pathMapping);
                if (neighborCoord != null) {
                    // 创建邻居节点
                    SpaceTimeAStarNode neighborNode = new SpaceTimeAStarNode(
                            neighborCode, neighborCoord, arrivalTime, gScore,
                            gScore + spaceTimeHeuristic(neighborCoord, endCoord, arrivalTime, physicalConfig)
                    );

                    cameFrom.put(neighborKey, current);
                    openSet.offer(neighborNode);
                }
            }
        }

        // 减少等待选项 - 只在必要时等待（20%概率）
        if (Math.random() < 0.2) {
            long waitTime = timeResolution;
            long waitUntilTime = current.timePoint + waitTime;
            String waitKey = createSpaceTimeKey(current.code, waitUntilTime);

            if (!closedSet.contains(waitKey) &&
                    constraintChecker.isValidWait(current.code, current.timePoint, waitUntilTime)) {

                double waitGScore = gScores.get(createSpaceTimeKey(current.code, current.timePoint)) +
                        (waitTime / 1000.0);

                if (!gScores.containsKey(waitKey) || waitGScore < gScores.get(waitKey)) {
                    gScores.put(waitKey, waitGScore);

                    SpaceTimeAStarNode waitNode = new SpaceTimeAStarNode(
                            current.code, current.coordinates, waitUntilTime, waitGScore,
                            waitGScore + spaceTimeHeuristic(current.coordinates, endCoord, waitUntilTime, physicalConfig)
                    );

                    cameFrom.put(waitKey, current);
                    openSet.offer(waitNode);
                }
            }
        }
    }

    /**
     * 时空启发式函数
     *
     * @param coord1         当前坐标
     * @param coord2         目标坐标
     * @param currentTime    当前时间
     * @param physicalConfig 物理配置
     * @return 启发式值
     */
    private double spaceTimeHeuristic(int[] coord1, int[] coord2, long currentTime, CTUPhysicalConfig physicalConfig) {
        // 空间距离
        double spatialDistance = Math.abs(coord1[0] - coord2[0]) + Math.abs(coord1[1] - coord2[1]);

        // 时间成本估计
        double timeEstimate = spatialDistance * physicalConfig.getStandardPointDistance() / physicalConfig.getNormalSpeed();

        // 考虑转向成本
        double turnPenalty = spatialDistance > 1 ? physicalConfig.getTurnTime90() : 0;

        return timeEstimate + turnPenalty;
    }

    /**
     * 计算移动时间
     *
     * @param fromCode       起始代码
     * @param toCode         目标代码
     * @param direction      移动方向
     * @param physicalConfig 物理配置
     * @return 移动时间（毫秒）
     */
    private long calculateTravelTime(String fromCode, String toCode, String direction, CTUPhysicalConfig physicalConfig) {
        // 基本移动时间
        double distance = physicalConfig.getStandardPointDistance();
        double speed = physicalConfig.getNormalSpeed();
        long moveTime = (long) ((distance / speed) * 1000);

        // 转向时间
        long turnTime = (long) (physicalConfig.getTurnTime90() * 1000 / 4); // 假设平均转向时间

        return moveTime + turnTime;
    }

    /**
     * 重建时空路径
     *
     * @param cameFrom       路径来源映射
     * @param startNode      起始节点
     * @param endNode        结束节点
     * @param physicalConfig 物理配置
     * @return 重建的路径
     */
    private PlannedPath reconstructSpaceTimePath(Map<String, SpaceTimeAStarNode> cameFrom,
                                                 SpaceTimeAStarNode startNode,
                                                 SpaceTimeAStarNode endNode,
                                                 CTUPhysicalConfig physicalConfig) {
        List<SpaceTimeAStarNode> pathNodes = new ArrayList<>();
        SpaceTimeAStarNode current = endNode;

        // 反向重建路径
        while (current != null) {
            pathNodes.add(0, current);
            String currentKey = createSpaceTimeKey(current.code, current.timePoint);
            current = cameFrom.get(currentKey);
        }

        // 转换为PathCode列表
        List<PathCode> codeList = new ArrayList<>();
        for (int i = 0; i < pathNodes.size(); i++) {
            SpaceTimeAStarNode node = pathNodes.get(i);
            String direction = "90"; // 默认方向

            // 计算方向
            if (i < pathNodes.size() - 1) {
                SpaceTimeAStarNode nextNode = pathNodes.get(i + 1);
                direction = calculateDirection(node.code, nextNode.code);
            }

            PathCode pathCode = new PathCode(node.code, direction);

            // 添加时间信息
            // pathCode.setArrivalTime(node.timePoint);

            codeList.add(pathCode);
        }

        return new PlannedPath("", "", codeList);
    }

    /**
     * 创建时空键
     *
     * @param code      位置代码
     * @param timePoint 时间点
     * @return 时空键
     */
    private String createSpaceTimeKey(String code, long timePoint) {
        return code + "_T" + timePoint;
    }

    /**
     * 时间对齐到分辨率
     *
     * @param timePoint 时间点
     * @return 对齐后的时间
     */
    private long alignToTimeResolution(long timePoint) {
        return (timePoint / timeResolution) * timeResolution;
    }

    /**
     * 创建默认物理配置
     *
     * @return 默认配置
     */
    private CTUPhysicalConfig createDefaultPhysicalConfig() {
        CTUPhysicalConfig config = new CTUPhysicalConfig();
        config.setMaxSpeed(2.0);
        config.setNormalSpeed(1.5);
        config.setMaxAcceleration(1.0);
        config.setMaxDeceleration(1.5);
        config.setTurnTime90(2.0);
        config.setTurnTime180(4.0);
        config.setMinSafetyDistance(3.0);
        config.setMinFollowingDistance(2.0);
        config.setCtuLength(1.2);
        config.setCtuWidth(0.8);
        config.setStartupTime(1.0);
        config.setStopTime(0.5);
        config.setStandardPointDistance(1.0);
        return config;
    }

    /**
     * 计算移动方向
     *
     * @param fromCode 起始路径点
     * @param toCode   目标路径点
     * @return 方向角度字符串
     */
    private String calculateDirection(String fromCode, String toCode) {
        int[] fromCoord = JsonUtils.getCoordinate(fromCode, pathMapping);
        int[] toCoord = JsonUtils.getCoordinate(toCode, pathMapping);

        if (fromCoord == null || toCoord == null) {
            return "90";
        }

        int dx = toCoord[0] - fromCoord[0];
        int dy = toCoord[1] - fromCoord[1];

        // 确定方向
        if (dx == 0 && dy == -1) return "270"; // 上
        if (dx == 1 && dy == 0) return "0";    // 右
        if (dx == 0 && dy == 1) return "90";   // 下
        if (dx == -1 && dy == 0) return "180"; // 左

        return "90"; // 默认方向
    }

    @Override
    public boolean isValidPathPoint(String pathCode) {
        return pathCode != null && pathMapping.containsKey(pathCode);
    }

    @Override
    public double calculateDistance(String startCode, String endCode) {
        int[] startCoord = JsonUtils.getCoordinate(startCode, pathMapping);
        int[] endCoord = JsonUtils.getCoordinate(endCode, pathMapping);

        if (startCoord == null || endCoord == null) {
            return -1;
        }

        return JsonUtils.calculateManhattanDistance(startCoord, endCoord);
    }

    @Override
    public Map<String, Map<String, Integer>> getPathMapping() {
        return pathMapping;
    }

    @Override
    public List<Map<String, String>> getNeighbors(String pathCode) {
        return adjacencyList.getOrDefault(pathCode, new ArrayList<>());
    }

    /**
     * 构建邻接表
     */
    private void buildAdjacencyList() {
        // 创建坐标到编号的临时映射
        Map<String, String> tempCoordToCode = new HashMap<>();
        for (Map.Entry<String, Map<String, Integer>> entry : pathMapping.entrySet()) {
            String code = entry.getKey();
            Map<String, Integer> coordMap = entry.getValue();

            if (coordMap.containsKey("x") && coordMap.containsKey("y")) {
                String coordKey = coordMap.get("x") + "," + coordMap.get("y");
                tempCoordToCode.put(coordKey, code);
            }
        }

        // 四个方向：上、右、下、左
        int[][] directions = {{0, -1}, {1, 0}, {0, 1}, {-1, 0}};
        String[] directionAngles = {"270", "0", "90", "180"};

        for (Map.Entry<String, Map<String, Integer>> entry : pathMapping.entrySet()) {
            String code = entry.getKey();
            Map<String, Integer> coordMap = entry.getValue();

            if (!coordMap.containsKey("x") || !coordMap.containsKey("y")) {
                continue;
            }

            int x = coordMap.get("x");
            int y = coordMap.get("y");

            List<Map<String, String>> neighbors = new ArrayList<>();

            // 检查四个方向的邻居
            for (int i = 0; i < directions.length; i++) {
                int newX = x + directions[i][0];
                int newY = y + directions[i][1];
                String coordKey = newX + "," + newY;

                String neighborCode = tempCoordToCode.get(coordKey);
                if (neighborCode != null) {
                    Map<String, String> neighbor = new HashMap<>();
                    neighbor.put("code", neighborCode);
                    neighbor.put("direction", directionAngles[i]);
                    neighbors.add(neighbor);
                }
            }

            adjacencyList.put(code, neighbors);
        }
    }

    /**
     * 时空A*节点类
     */
    private static class SpaceTimeAStarNode {
        final String code;
        final int[] coordinates;
        final long timePoint;
        final double gScore;
        final double fScore;

        public SpaceTimeAStarNode(String code, int[] coordinates, long timePoint,
                                  double gScore, double fScore) {
            this.code = code;
            this.coordinates = coordinates;
            this.timePoint = timePoint;
            this.gScore = gScore;
            this.fScore = fScore;
        }
    }

    /**
     * 约束检查器
     */
    private static class EnhancedConstraintChecker {
        private final List<double[]> staticConstraints;
        private final Map<String, String> spaceTimeOccupancyMap;
        private final Map<String, Map<String, Integer>> pathMapping;
        private final CTUPhysicalConfig physicalConfig;

        public EnhancedConstraintChecker(List<double[]> staticConstraints,
                                         Map<String, String> spaceTimeOccupancyMap,
                                         Map<String, Map<String, Integer>> pathMapping,
                                         CTUPhysicalConfig physicalConfig) {
            this.staticConstraints = staticConstraints;
            this.spaceTimeOccupancyMap = spaceTimeOccupancyMap != null ? spaceTimeOccupancyMap : new HashMap<>();
            this.pathMapping = pathMapping;
            this.physicalConfig = physicalConfig;
        }

        /**
         * 检查移动是否有效
         */
        public boolean isValidMove(String fromCode, String toCode, long fromTime, long toTime) {
            // 检查静态约束
            if (!isValidStaticConstraint(toCode)) {
                return false;
            }

            // 检查时空占用
            if (!isValidSpaceTimeConstraint(toCode, toTime)) {
                return false;
            }

            // 检查物理约束
            return isValidPhysicalConstraint(fromCode, toCode, fromTime, toTime);
        }

        /**
         * 检查等待是否有效
         */
        public boolean isValidWait(String code, long fromTime, long toTime) {
            // 检查时空占用期间是否有冲突
            for (long t = fromTime; t <= toTime; t += 100) { // 100ms检查间隔
                if (!isValidSpaceTimeConstraint(code, t)) {
                    return false;
                }
            }
            return true;
        }

        private boolean isValidStaticConstraint(String pathCode) {
            if (staticConstraints == null || staticConstraints.isEmpty()) {
                return true;
            }

            int[] coord = JsonUtils.getCoordinate(pathCode, pathMapping);
            if (coord == null) {
                return false;
            }

            for (double[] constraint : staticConstraints) {
                double cx = constraint[0];
                double cy = constraint[1];
                double radius = constraint[2];

                double distance = Math.sqrt(Math.pow(coord[0] - cx, 2) + Math.pow(coord[1] - cy, 2));
                if (distance <= radius) {
                    return false;
                }
            }

            return true;
        }

        private boolean isValidSpaceTimeConstraint(String pathCode, long timePoint) {
            int[] coord = JsonUtils.getCoordinate(pathCode, pathMapping);
            if (coord == null) {
                return false;
            }

            // 检查精确的时空占用
            String spaceTimeKey = coord[0] + "," + coord[1] + "," + (timePoint / 1000);
            if (spaceTimeOccupancyMap.containsKey(spaceTimeKey)) {
                return false;
            }

            // 检查周围时间段的占用（考虑安全间隔）
            long safetyMargin = (long) (physicalConfig.getMinSafetyDistance() * 1000 / physicalConfig.getNormalSpeed());
            for (long t = timePoint - safetyMargin; t <= timePoint + safetyMargin; t += 100) {
                String key = coord[0] + "," + coord[1] + "," + (t / 1000);
                if (spaceTimeOccupancyMap.containsKey(key)) {
                    return false;
                }
            }

            return true;
        }

        private boolean isValidPhysicalConstraint(String fromCode, String toCode, long fromTime, long toTime) {
            // 物理约束检查
            long timeDiff = toTime - fromTime;
            double minTimeRequired = physicalConfig.getStandardPointDistance() / physicalConfig.getMaxSpeed() * 1000;

            // 时间不能小于物理最小时间
            return timeDiff >= minTimeRequired;
        }
    }

    /**
     * 快速路径规划 - 先尝试简化空间路径规划
     * 对于近距离路径，直接返回结果避免复杂的时空计算
     */
    private PlannedPath tryFastPathPlanning(String startCode, String endCode, List<double[]> constraints) {
        // 检查缓存
        String cacheKey = startCode + "->" + endCode;
        if (fastPathCache.containsKey(cacheKey)) {
            List<String> cachedPath = fastPathCache.get(cacheKey);
            return convertToPlannedPath(cachedPath);
        }

        // 计算距离，如果距离较近则使用快速算法
        double distance = getCachedDistance(startCode, endCode);
        if (distance > 0 && distance <= 40) { // 40个单位以内使用快速算法
            List<String> fastPath = simpleAStarSearch(startCode, endCode, constraints);
            if (fastPath != null && !fastPath.isEmpty()) {
                fastPathCache.put(cacheKey, fastPath); // 缓存结果
                return convertToPlannedPath(fastPath);
            }
        }

        return null; // 快速路径失败，需要使用完整算法
    }

    /**
     * 简化A*搜索
     */
    private List<String> simpleAStarSearch(String startCode, String endCode, List<double[]> constraints) {
        int[] startCoord = JsonUtils.getCoordinate(startCode, pathMapping);
        int[] endCoord = JsonUtils.getCoordinate(endCode, pathMapping);

        if (startCoord == null || endCoord == null) {
            return null;
        }

        PriorityQueue<SimpleAStarNode> openSet = new PriorityQueue<>(
                Comparator.comparingDouble(n -> n.fScore)
        );
        Set<String> closedSet = new HashSet<>();
        Map<String, Double> gScores = new HashMap<>();
        Map<String, SimpleAStarNode> cameFrom = new HashMap<>();

        SimpleAStarNode startNode = new SimpleAStarNode(
                startCode, startCoord, 0, spatialHeuristic(startCoord, endCoord)
        );

        openSet.offer(startNode);
        gScores.put(startCode, 0.0);

        // 简化的约束检查器
        FastConstraintChecker constraintChecker = new FastConstraintChecker(constraints);

        int searchDepth = 0;
        long searchStartTime = System.currentTimeMillis();

        while (!openSet.isEmpty() && searchDepth < 3000) {
            if (System.currentTimeMillis() - searchStartTime > 5000) { // 5秒超时
                break;
            }

            SimpleAStarNode current = openSet.poll();

            if (closedSet.contains(current.code)) {
                continue;
            }

            closedSet.add(current.code);
            searchDepth++;

            if (current.code.equals(endCode)) {
                return reconstructSimplePath(cameFrom, startNode, current);
            }

            // 扩展邻居
            expandSimpleNeighbors(current, endCoord, openSet, closedSet, gScores, cameFrom, constraintChecker);
        }

        return null;
    }

    /**
     * 预计算常用距离
     */
    private void precomputeCommonDistances() {
        System.out.println("距离缓存初始化完成");
    }

    /**
     * 获取缓存的距离
     */
    private double getCachedDistance(String from, String to) {
        String key = from + "-" + to;
        return distanceCache.computeIfAbsent(key, k -> {
            int[] coord1 = JsonUtils.getCoordinate(from, pathMapping);
            int[] coord2 = JsonUtils.getCoordinate(to, pathMapping);
            if (coord1 != null && coord2 != null) {
                return JsonUtils.calculateManhattanDistance(coord1, coord2);
            }
            return -1.0;
        });
    }

    /**
     * 空间启发式函数
     */
    private double spatialHeuristic(int[] coord1, int[] coord2) {
        return Math.abs(coord1[0] - coord2[0]) + Math.abs(coord1[1] - coord2[1]);
    }

    /**
     * 扩展邻居节点
     */
    private void expandSimpleNeighbors(SimpleAStarNode current, int[] endCoord,
                                       PriorityQueue<SimpleAStarNode> openSet,
                                       Set<String> closedSet, Map<String, Double> gScores,
                                       Map<String, SimpleAStarNode> cameFrom,
                                       FastConstraintChecker constraintChecker) {

        List<Map<String, String>> neighbors = getNeighbors(current.code);

        for (Map<String, String> neighbor : neighbors) {
            String neighborCode = neighbor.get("code");

            if (closedSet.contains(neighborCode)) {
                continue;
            }

            if (!constraintChecker.isValidPosition(neighborCode)) {
                continue;
            }

            double edgeCost = getCachedDistance(current.code, neighborCode);
            if (edgeCost < 0) edgeCost = 1.0; // 默认边权重

            double gScore = gScores.get(current.code) + edgeCost;

            if (!gScores.containsKey(neighborCode) || gScore < gScores.get(neighborCode)) {
                gScores.put(neighborCode, gScore);

                int[] neighborCoord = JsonUtils.getCoordinate(neighborCode, pathMapping);
                if (neighborCoord != null) {
                    SimpleAStarNode neighborNode = new SimpleAStarNode(
                            neighborCode, neighborCoord, gScore,
                            gScore + spatialHeuristic(neighborCoord, endCoord)
                    );

                    cameFrom.put(neighborCode, current);
                    openSet.offer(neighborNode);
                }
            }
        }
    }

    /**
     * 重建简化路径
     */
    private List<String> reconstructSimplePath(Map<String, SimpleAStarNode> cameFrom,
                                               SimpleAStarNode startNode, SimpleAStarNode endNode) {
        List<String> path = new ArrayList<>();
        SimpleAStarNode current = endNode;

        while (current != null) {
            path.add(0, current.code);
            current = cameFrom.get(current.code);
        }

        return path;
    }

    /**
     * 将路径转换为PlannedPath
     */
    private PlannedPath convertToPlannedPath(List<String> pathCodes) {
        if (pathCodes == null || pathCodes.isEmpty()) {
            return null;
        }

        List<PathCode> codeList = new ArrayList<>();

        for (int i = 0; i < pathCodes.size(); i++) {
            String position = pathCodes.get(i);
            String direction = "90"; // 默认方向

            // 计算方向
            if (i < pathCodes.size() - 1) {
                direction = calculateDirection(position, pathCodes.get(i + 1));
            }

            PathCode pathCode = new PathCode(position, direction);
            codeList.add(pathCode);
        }

        return new PlannedPath("", "", codeList);
    }

    /**
     * A*节点类
     */
    private static class SimpleAStarNode {
        final String code;
        final int[] coordinates;
        final double gScore;
        final double fScore;

        public SimpleAStarNode(String code, int[] coordinates, double gScore, double fScore) {
            this.code = code;
            this.coordinates = coordinates;
            this.gScore = gScore;
            this.fScore = fScore;
        }
    }

    /**
     * 约束检查器
     */
    private class FastConstraintChecker {
        private final Set<String> blockedPositions;

        public FastConstraintChecker(List<double[]> constraints) {
            this.blockedPositions = new HashSet<>();
            precomputeBlockedPositions(constraints);
        }

        private void precomputeBlockedPositions(List<double[]> constraints) {
            if (constraints == null || constraints.isEmpty()) {
                return;
            }

            for (Map.Entry<String, Map<String, Integer>> entry : pathMapping.entrySet()) {
                String pathCode = entry.getKey();
                int[] coord = JsonUtils.getCoordinate(pathCode, pathMapping);

                if (coord != null && isBlocked(coord, constraints)) {
                    blockedPositions.add(pathCode);
                }
            }
        }

        private boolean isBlocked(int[] coord, List<double[]> constraints) {
            for (double[] constraint : constraints) {
                double cx = constraint[0];
                double cy = constraint[1];
                double radius = constraint[2];

                double distance = Math.sqrt(Math.pow(coord[0] - cx, 2) + Math.pow(coord[1] - cy, 2));
                if (distance <= radius) {
                    return true;
                }
            }
            return false;
        }

        public boolean isValidPosition(String pathCode) {
            return !blockedPositions.contains(pathCode);
        }
    }
}