package com.algo.service;

import com.algo.model.AGVStatus;
import com.algo.model.Conflict;
import com.algo.model.ExecutingTask;
import com.algo.model.PathCode;
import com.algo.model.PlannedPath;
import com.algo.model.CTUPhysicalConfig;
import com.algo.util.JsonUtils;
import com.algo.util.PathTimeCalculator;

import java.util.*;

/**
 * 碰撞解决器
 * 用于解决AGV路径之间的冲突
 */
public class CollisionResolver {

    /**
     * 碰撞检测器
     */
    private final CollisionDetector collisionDetector;
    
    /**
     * 路径时间计算器
     */
    private PathTimeCalculator timeCalculator;
    
    /**
     * 路径规划器
     */
    private PathPlanner pathPlanner;
    
    /**
     * 路径映射表（用于坐标转换）
     */
    private Map<String, Map<String, Integer>> pathMapping;

    /**
     * 构造函数
     *
     * @param collisionDetector 碰撞检测器
     */
    public CollisionResolver(CollisionDetector collisionDetector) {
        this.collisionDetector = collisionDetector;
    }
    
    /**
     * 设置时间计算器
     */
    public void setTimeCalculator(PathTimeCalculator timeCalculator) {
        this.timeCalculator = timeCalculator;
    }
    
    /**
     * 设置路径规划器
     */
    public void setPathPlanner(PathPlanner pathPlanner) {
        this.pathPlanner = pathPlanner;
    }
    
    /**
     * 设置路径映射表
     */
    public void setPathMapping(Map<String, Map<String, Integer>> pathMapping) {
        this.pathMapping = pathMapping;
    }

    /**
     * 解决路径冲突
     *
     * @param plannedPaths   规划路径列表
     * @param conflicts      冲突列表
     * @param executingTasks 执行中任务列表
     * @param agvStatusList  AGV状态列表（用于判断剩余路径）
     * @return 解决冲突后的路径列表
     */
    /**
     * 解决路径冲突
     */
    public List<PlannedPath> resolveConflicts(List<PlannedPath> plannedPaths,
                                              List<Conflict> conflicts,
                                              List<ExecutingTask> executingTasks,
                                              List<AGVStatus> agvStatusList,
                                              long unifiedTimestamp) {
        if (conflicts == null || conflicts.isEmpty()) {
            return plannedPaths;
        }

        System.out.println("开始解决 " + conflicts.size() + " 个路径冲突");

        // 构建路径字典便于快速访问
        Map<String, PlannedPath> pathsMap = new HashMap<>();
        for (PlannedPath path : plannedPaths) {
            pathsMap.put(path.getAgvId(), path);
        }
        
        // 构建AGV状态字典
        Map<String, AGVStatus> agvStatusMap = new HashMap<>();
        if (agvStatusList != null) {
            for (AGVStatus agv : agvStatusList) {
                if (agv.getAgvId() != null) {
                    agvStatusMap.put(agv.getAgvId(), agv);
                }
            }
        }
        
        // 构建所有已规划路径的占用位置集合（用于查找等待位置时避开）
        Set<String> allOccupiedPositions = new HashSet<>();
        for (PlannedPath path : plannedPaths) {
            if (path.getCodeList() != null) {
                for (PathCode code : path.getCodeList()) {
                    if (code.getCode() != null) {
                        allOccupiedPositions.add(code.getCode());
                    }
                }
            }
        }

        // 按时间步排序处理冲突
        List<Conflict> sortedConflicts = new ArrayList<>(conflicts);
        sortedConflicts.sort(Comparator.comparingInt(Conflict::getTimeStep));

        // 逐个解决冲突
        for (Conflict conflict : sortedConflicts) {
            // 对于每个冲突，计算两车路径重合的位置
            Set<String> overlappingPositions = findOverlappingPositions(pathsMap, conflict);
            resolveSingleConflict(pathsMap, conflict, executingTasks, agvStatusMap, overlappingPositions, unifiedTimestamp);
        }

        List<PlannedPath> resolvedPaths = new ArrayList<>(pathsMap.values());

        // 验证解决结果
        List<Conflict> remainingConflicts = collisionDetector.detectConflicts(resolvedPaths);
        System.out.println("冲突解决完成，剩余冲突数量: " + remainingConflicts.size());

        return resolvedPaths;
    }
    
    /**
     * 解决路径冲突
     *
     * @param plannedPaths   规划路径列表
     * @param conflicts      冲突列表
     * @param executingTasks 执行中任务列表
     * @return 解决冲突后的路径列表
     */
    public List<PlannedPath> resolveConflicts(List<PlannedPath> plannedPaths,
                                              List<Conflict> conflicts,
                                              List<ExecutingTask> executingTasks) {
        return resolveConflicts(plannedPaths, conflicts, executingTasks, null, System.currentTimeMillis() / 1000);
    }

    /**
     * 解决单个冲突
     */
    private void resolveSingleConflict(Map<String, PlannedPath> pathsMap,
                                       Conflict conflict,
                                       List<ExecutingTask> executingTasks,
                                       Map<String, AGVStatus> agvStatusMap,
                                       Set<String> overlappingPositions,
                                       long unifiedTimestamp) {
        String conflictType = conflict.getType();

        switch (conflictType) {
            case "vertex":
                resolveVertexConflict(pathsMap, conflict, executingTasks);
                break;
            case "edge":
                resolveEdgeConflict(pathsMap, conflict, executingTasks);
                break;
            case "follow":
                resolveFollowingConflict(pathsMap, conflict, executingTasks);
                break;
            case "time_gap_insufficient":
                resolveTimeGapConflict(pathsMap, conflict, executingTasks, agvStatusMap, overlappingPositions, unifiedTimestamp);
                break;
            default:
                System.out.println("未知冲突类型: " + conflictType);
        }
    }

    /**
     * 解决顶点冲突
     *
     * @param pathsMap       路径映射
     * @param conflict       冲突
     * @param executingTasks 执行中任务列表
     */
    private void resolveVertexConflict(Map<String, PlannedPath> pathsMap,
                                       Conflict conflict,
                                       List<ExecutingTask> executingTasks) {
        String agv1 = conflict.getAgv1();
        String agv2 = conflict.getAgv2();
        int timeStep = conflict.getTimeStep();

        // 评估AGV优先级
        AGVPriority priority1 = evaluateAgvPriority(agv1, pathsMap.get(agv1), executingTasks);
        AGVPriority priority2 = evaluateAgvPriority(agv2, pathsMap.get(agv2), executingTasks);

        // 优先级低的AGV进行延迟
        if (priority1.priorityScore >= priority2.priorityScore) {
            // agv2优先级低，延迟agv2
            addDelayToPath(pathsMap.get(agv2), timeStep, 1);
            System.out.println("为解决顶点冲突，AGV " + agv2 + " 在时间步 " + timeStep + " 延迟1步");
        } else {
            // agv1优先级低，延迟agv1
            addDelayToPath(pathsMap.get(agv1), timeStep, 1);
            System.out.println("为解决顶点冲突，AGV " + agv1 + " 在时间步 " + timeStep + " 延迟1步");
        }
    }

    /**
     * 解决边冲突
     *
     * @param pathsMap       路径映射
     * @param conflict       冲突
     * @param executingTasks 执行中任务列表
     */
    private void resolveEdgeConflict(Map<String, PlannedPath> pathsMap,
                                     Conflict conflict,
                                     List<ExecutingTask> executingTasks) {
        String agv1 = conflict.getAgv1();
        String agv2 = conflict.getAgv2();
        int timeStep = conflict.getTimeStep();

        // 评估AGV优先级
        AGVPriority priority1 = evaluateAgvPriority(agv1, pathsMap.get(agv1), executingTasks);
        AGVPriority priority2 = evaluateAgvPriority(agv2, pathsMap.get(agv2), executingTasks);

        // 优先级低的AGV进行延迟
        if (priority1.priorityScore >= priority2.priorityScore) {
            // agv2优先级低，延迟agv2
            addDelayToPath(pathsMap.get(agv2), timeStep, 2);
            System.out.println("为解决边冲突，AGV " + agv2 + " 在时间步 " + timeStep + " 延迟2步");
        } else {
            // agv1优先级低，延迟agv1
            addDelayToPath(pathsMap.get(agv1), timeStep, 2);
            System.out.println("为解决边冲突，AGV " + agv1 + " 在时间步 " + timeStep + " 延迟2步");
        }
    }

    /**
     * 解决跟随冲突
     *
     * @param pathsMap       路径映射
     * @param conflict       冲突
     * @param executingTasks 执行中任务列表
     */
    private void resolveFollowingConflict(Map<String, PlannedPath> pathsMap,
                                         Conflict conflict,
                                         List<ExecutingTask> executingTasks) {
        String agv1 = conflict.getAgv1();
        String agv2 = conflict.getAgv2();
        int timeStep = conflict.getTimeStep();

        // 评估AGV优先级
        AGVPriority priority1 = evaluateAgvPriority(agv1, pathsMap.get(agv1), executingTasks);
        AGVPriority priority2 = evaluateAgvPriority(agv2, pathsMap.get(agv2), executingTasks);

        // 优先级低的AGV进行延迟
        if (priority1.priorityScore >= priority2.priorityScore) {
            // agv2优先级低，延迟agv2
            addDelayToPath(pathsMap.get(agv2), timeStep, 1);
            System.out.println("为解决跟随冲突，AGV " + agv2 + " 在时间步 " + timeStep + " 延迟1步");
        } else {
            // agv1优先级低，延迟agv1
            addDelayToPath(pathsMap.get(agv1), timeStep, 1);
            System.out.println("为解决跟随冲突，AGV " + agv1 + " 在时间步 " + timeStep + " 延迟1步");
        }
    }

    /**
     * 找到两车路径重合的位置
     */
    private Set<String> findOverlappingPositions(Map<String, PlannedPath> pathsMap, Conflict conflict) {
        Set<String> overlappingPositions = new HashSet<>();
        
        PlannedPath path1 = pathsMap.get(conflict.getAgv1());
        PlannedPath path2 = pathsMap.get(conflict.getAgv2());
        
        if (path1 == null || path2 == null || 
            path1.getCodeList() == null || path2.getCodeList() == null) {
            return overlappingPositions;
        }
        
        // 收集path1的所有位置
        Set<String> positions1 = new HashSet<>();
        for (PathCode code : path1.getCodeList()) {
            if (code.getCode() != null) {
                positions1.add(code.getCode());
            }
        }
        
        // 收集path2的所有位置
        Set<String> positions2 = new HashSet<>();
        for (PathCode code : path2.getCodeList()) {
            if (code.getCode() != null) {
                positions2.add(code.getCode());
            }
        }
        
        // 找到重合的位置
        for (String pos : positions1) {
            if (positions2.contains(pos)) {
                overlappingPositions.add(pos);
            }
        }
        
        System.out.println("[调试] 两车路径重合位置: " + overlappingPositions);
        return overlappingPositions;
    }
    
    /**
     * 解决时间间隔不足冲突
     * 策略：有剩余路径的AGV继续执行，新路径的AGV移动到安全等待位置
     */
    private void resolveTimeGapConflict(Map<String, PlannedPath> pathsMap,
                                        Conflict conflict,
                                        List<ExecutingTask> executingTasks,
                                        Map<String, AGVStatus> agvStatusMap,
                                        Set<String> overlappingPositions,
                                        long unifiedTimestamp) {
        String firstAgv = conflict.getAgv1();  // 先经过的AGV
        String secondAgv = conflict.getAgv2(); // 后经过的AGV
        String conflictPosition = conflict.getPosition1(); // 冲突位置
        
        PlannedPath path1 = pathsMap.get(firstAgv);
        PlannedPath path2 = pathsMap.get(secondAgv);
        
        if (path1 == null || path2 == null) {
            System.out.println("警告: 无法解决冲突，路径不存在");
            return;
        }
        
        // 打印原始路径信息（调试用）
        System.out.println("[冲突解决debug] 冲突位置: " + conflictPosition);
        System.out.println("[冲突解决debug] AGV " + firstAgv + " 原始路径: " + 
                         (path1.getCodeList() != null && !path1.getCodeList().isEmpty() ? 
                          path1.getCodeList().get(0).getCode() + " -> " + 
                          path1.getCodeList().get(path1.getCodeList().size() - 1).getCode() + 
                          " (共" + path1.getCodeList().size() + "点)" : "空路径"));
        System.out.println("[冲突解决debug] AGV " + secondAgv + " 原始路径: " + 
                         (path2.getCodeList() != null && !path2.getCodeList().isEmpty() ? 
                          path2.getCodeList().get(0).getCode() + " -> " + 
                          path2.getCodeList().get(path2.getCodeList().size() - 1).getCode() + 
                          " (共" + path2.getCodeList().size() + "点)" : "空路径"));
        
        // 检查原始路径是否经过冲突位置
        if (path1.getCodeList() != null) {
            boolean path1HasConflictPos = path1.getCodeList().stream()
                    .anyMatch(code -> conflictPosition.equals(code.getCode()));
            System.out.println("[冲突解决debug] AGV " + firstAgv + " 路径是否经过冲突位置 " + conflictPosition + ": " + path1HasConflictPos);
        }
        if (path2.getCodeList() != null) {
            boolean path2HasConflictPos = path2.getCodeList().stream()
                    .anyMatch(code -> conflictPosition.equals(code.getCode()));
            System.out.println("[冲突解决debug] AGV " + secondAgv + " 路径是否经过冲突位置 " + conflictPosition + ": " + path2HasConflictPos);
        }
        
        // 判断哪个AGV有剩余路径
        AGVStatus agv1Status = agvStatusMap != null ? agvStatusMap.get(firstAgv) : null;
        AGVStatus agv2Status = agvStatusMap != null ? agvStatusMap.get(secondAgv) : null;
        
        boolean agv1HasRemainingPath = agv1Status != null && agv1Status.hasRemainingPath();
        boolean agv2HasRemainingPath = agv2Status != null && agv2Status.hasRemainingPath();
        
        // 确定需要等待的AGV
        String waitingAgv;
        PlannedPath waitingPath;
        AGVStatus waitingAgvStatus;
        
        if (agv1HasRemainingPath && !agv2HasRemainingPath) {
            // AGV1有剩余路径，AGV2是新路径 -> AGV2等待
            waitingAgv = secondAgv;
            waitingPath = path2;
            waitingAgvStatus = agv2Status;
            System.out.println("冲突解决: AGV " + firstAgv + " 有剩余路径继续执行，AGV " + secondAgv + " 新路径需要等待");
        } else if (!agv1HasRemainingPath && agv2HasRemainingPath) {
            // AGV1是新路径，AGV2有剩余路径 -> AGV1等待
            waitingAgv = firstAgv;
            waitingPath = path1;
            waitingAgvStatus = agv1Status;
            System.out.println("冲突解决: AGV " + secondAgv + " 有剩余路径继续执行，AGV " + firstAgv + " 新路径需要等待");
        } else if (agv1HasRemainingPath && agv2HasRemainingPath) {
            // 两个都有剩余路径(最好不出现这个情况)
            AGVPriority priority1 = evaluateAgvPriority(firstAgv, path1, executingTasks);
            AGVPriority priority2 = evaluateAgvPriority(secondAgv, path2, executingTasks);
            
            if (priority1.priorityScore >= priority2.priorityScore) {
                // AGV1优先级高，AGV2等待
                waitingAgv = secondAgv;
                waitingPath = path2;
                waitingAgvStatus = agv2Status;
                System.out.println("冲突解决: 两个都有剩余路径，AGV " + firstAgv + " 优先级高继续执行，AGV " + secondAgv + " 等待");
            } else {
                // AGV2优先级高，AGV1等待
                waitingAgv = firstAgv;
                waitingPath = path1;
                waitingAgvStatus = agv1Status;
                System.out.println("冲突解决: 两个都有剩余路径，AGV " + secondAgv + " 优先级高继续执行，AGV " + firstAgv + " 等待");
            }
        } else {
            // 两个都是新路径 -> 根据优先级决定
            AGVPriority priority1 = evaluateAgvPriority(firstAgv, path1, executingTasks);
            AGVPriority priority2 = evaluateAgvPriority(secondAgv, path2, executingTasks);
            
            if (priority1.priorityScore >= priority2.priorityScore) {
                waitingAgv = secondAgv;
                waitingPath = path2;
                waitingAgvStatus = agv2Status;
                System.out.println("冲突解决: 两个都是新路径，AGV " + firstAgv + " 优先级高继续执行，AGV " + secondAgv + " 等待");
            } else {
                waitingAgv = firstAgv;
                waitingPath = path1;
                waitingAgvStatus = agv1Status;
                System.out.println("冲突解决: 两个都是新路径，AGV " + secondAgv + " 优先级高继续执行，AGV " + firstAgv + " 等待");
            }
        }
        
        // 找到等待AGV的当前位置（路径起点）
        String currentPosition = null;
        if (waitingPath != null && !waitingPath.getCodeList().isEmpty()) {
            currentPosition = waitingPath.getCodeList().get(0).getCode();
        }
        if (currentPosition == null && waitingAgvStatus != null) {
            currentPosition = waitingAgvStatus.getPosition();
        }
        
        if (currentPosition == null || conflictPosition == null) {
            System.out.println("警告: 无法找到当前位置或冲突位置，跳过冲突解决");
            return;
        }
        
        // 检查等待AGV的路径是否经过冲突位置
        boolean waitingPathHasConflictPos = false;
        if (waitingPath != null && waitingPath.getCodeList() != null) {
            waitingPathHasConflictPos = waitingPath.getCodeList().stream()
                    .anyMatch(code -> conflictPosition.equals(code.getCode()));
        }
        
        if (!waitingPathHasConflictPos) {
            System.out.println("AGV " + waitingAgv + " 的路径不经过冲突位置 " + conflictPosition + "，跳过冲突解决");
            return;
        }
        
        String waitPosition = findWaitPositionOnOriginalPath(waitingPath, conflictPosition, waitingAgvStatus, overlappingPositions);
        boolean waitPositionOnOriginalPath = (waitPosition != null);

        if (waitPosition == null) {
            System.out.println("原始路径上没有合适的等待位置，尝试寻找其他位置");
            waitPosition = findSafeWaitingPosition(currentPosition, conflictPosition, waitingAgvStatus, overlappingPositions);
        }
        
        if (waitPosition == null) {
            System.out.println("警告: 无法找到安全等待位置，AGV " + waitingAgv + " 保持当前位置");
            // 如果找不到等待位置，至少截断路径，只保留到当前位置
            truncatePathToCurrentPosition(waitingPath, currentPosition);
            return;
        }

        if (waitPositionOnOriginalPath && waitingPath != null) {
            truncatePathToWaitPosition(waitingPath, waitPosition);
            System.out.println("AGV " + waitingAgv + " 路径已截断到等待位置 " + waitPosition + "（在原始路径上，距离冲突点 " + conflictPosition + " 至少5步）");
            return;
        }
        
        String originalSegId = waitingPath != null ? waitingPath.getSegId() : null;
        ExecutingTask waitingTask = null;
        if (executingTasks != null) {
            for (ExecutingTask task : executingTasks) {
                if (task.getAgvId().equals(waitingAgv)) {
                    waitingTask = task;
                    break;
                }
            }
        }
        
        // 规划到等待位置的路径
        PlannedPath newWaitPath = planPathToWaitPosition(waitingAgv, currentPosition, waitPosition, 
                                                         waitingAgvStatus, originalSegId, waitingTask, unifiedTimestamp);
        
        if (newWaitPath != null) {
            pathsMap.put(waitingAgv, newWaitPath);
            System.out.println("AGV " + waitingAgv + " 已规划到等待位置 " + waitPosition + "（距离冲突点 " + conflictPosition + " 至少5步）");
        } else {
            System.out.println("警告: 无法规划到等待位置的路径，AGV " + waitingAgv + " 保持当前位置");
            truncatePathToCurrentPosition(waitingPath, currentPosition);
        }
    }
    
    /**
     * 找到距离冲突点安全距离的安全等待位置
     */
    private String findSafeWaitingPosition(String currentPosition, String conflictPosition, 
                                          AGVStatus agvStatus, Set<String> overlappingPositions) {
        if (pathPlanner == null) {
            System.out.println("警告: 路径规划器未设置，无法查找等待位置");
            return null;
        }

        Map<String, Map<String, Integer>> pathMappingToUse = pathMapping;
        if (pathMappingToUse == null && pathPlanner != null) {
            pathMappingToUse = pathPlanner.getPathMapping();
        }
        
        if (pathMappingToUse == null) {
            System.out.println("警告: 路径映射表未设置，无法查找等待位置");
            return null;
        }

        CTUPhysicalConfig config = agvStatus != null && agvStatus.getPhysicalConfig() != null 
                ? agvStatus.getPhysicalConfig() 
                : new CTUPhysicalConfig();
        
        // 安全距离要求
        int minTargetDistanceSteps = 5;

        int[] conflictCoord = JsonUtils.getCoordinate(conflictPosition, pathMappingToUse);
        if (conflictCoord == null) {
            System.out.println("警告: 无法获取冲突位置的坐标");
            return null;
        }
        
        // 使用BFS搜索满足距离要求的位置
        Queue<String> queue = new LinkedList<>();
        Map<String, Integer> visited = new HashMap<>(); 
        Set<String> blockedPositions = new HashSet<>();
        blockedPositions.add(conflictPosition); 

        if (overlappingPositions != null) {
            blockedPositions.addAll(overlappingPositions);
        }

        int[] currentCoord = JsonUtils.getCoordinate(currentPosition, pathMappingToUse);
        if (currentCoord == null) {
            System.out.println("警告: 无法获取当前位置的坐标");
            return null;
        }
        
        queue.offer(currentPosition);
        visited.put(currentPosition, 0);
        
        final int MAX_SEARCH_DEPTH = 20;

        List<CandidatePosition> candidates = new ArrayList<>();
        
        while (!queue.isEmpty()) {
            String pos = queue.poll();
            int depth = visited.get(pos);
            
            if (depth >= MAX_SEARCH_DEPTH) {
                break;
            }

            int[] posCoord = JsonUtils.getCoordinate(pos, pathMappingToUse);
            if (posCoord == null) {
                continue;
            }

            int distanceToConflict = Math.abs(posCoord[0] - conflictCoord[0]) + 
                                    Math.abs(posCoord[1] - conflictCoord[1]);

            int distanceToCurrent = Math.abs(posCoord[0] - currentCoord[0]) + 
                                   Math.abs(posCoord[1] - currentCoord[1]);

            if (distanceToConflict >= minTargetDistanceSteps) {
                if (!blockedPositions.contains(pos)) {
                    candidates.add(new CandidatePosition(pos, distanceToConflict, distanceToCurrent));
                }
            }

            List<Map<String, String>> neighbors = pathPlanner.getNeighbors(pos);
            if (neighbors != null) {
                for (Map<String, String> neighbor : neighbors) {
                    String neighborPos = neighbor.get("code");
                    if (neighborPos == null || visited.containsKey(neighborPos)) {
                        continue;
                    }
                    visited.put(neighborPos, depth + 1);
                    queue.offer(neighborPos);
                }
            }
        }

        if (!candidates.isEmpty()) {
            candidates.sort((c1, c2) -> {
                int compare = Integer.compare(c1.distanceToCurrent, c2.distanceToCurrent);
                if (compare != 0) {
                    return compare;
                }
                // 如果距离当前位置相同，选择距离冲突点更近的
                return Integer.compare(c1.distanceToConflict, c2.distanceToConflict);
            });
            
            CandidatePosition best = candidates.get(0);
            System.out.println("找到安全等待位置: " + best.position + 
                             "，距离冲突点 " + best.distanceToConflict + " 步" +
                             "，距离当前位置 " + best.distanceToCurrent + " 步");
            return best.position;
        }
        
        System.out.println("警告: 无法找到满足距离要求的安全等待位置");
        return null;
    }
    
    /**
     * 在原始路径上找一个距离冲突点足够远的位置
     */
    private String findWaitPositionOnOriginalPath(PlannedPath originalPath, String conflictPosition,
                                                 AGVStatus agvStatus, Set<String> overlappingPositions) {
        if (originalPath == null || originalPath.getCodeList() == null || originalPath.getCodeList().isEmpty()) {
            return null;
        }

        Map<String, Map<String, Integer>> pathMappingToUse = pathMapping;
        if (pathMappingToUse == null && pathPlanner != null) {
            pathMappingToUse = pathPlanner.getPathMapping();
        }
        if (pathMappingToUse == null) {
            return null;
        }

        int[] conflictCoord = JsonUtils.getCoordinate(conflictPosition, pathMappingToUse);
        if (conflictCoord == null) {
            return null;
        }
        
        // 安全距离要求：距离冲突点至少5步
        int minTargetDistanceSteps = 5;
        
        List<PathCode> codeList = originalPath.getCodeList();
        List<CandidatePosition> candidates = new ArrayList<>();

        int conflictIndex = -1;
        for (int i = 0; i < codeList.size(); i++) {
            if (conflictPosition.equals(codeList.get(i).getCode())) {
                conflictIndex = i;
                break;
            }
        }

        if (conflictIndex < 0) {
            System.out.println("[调试] 原始路径中未找到冲突位置 " + conflictPosition);
            return null;
        }

        int checkedCount = 0;
        int blockedCount = 0;
        int tooCloseCount = 0;
        for (int i = 0; i < conflictIndex; i++) {
            String pos = codeList.get(i).getCode();
            checkedCount++;
            
            if (overlappingPositions != null && overlappingPositions.contains(pos)) {
                blockedCount++;
                continue;
            }
            
            int[] posCoord = JsonUtils.getCoordinate(pos, pathMappingToUse);
            if (posCoord == null) {
                continue;
            }
            
            int distanceToConflict = Math.abs(posCoord[0] - conflictCoord[0]) + 
                                    Math.abs(posCoord[1] - conflictCoord[1]);
            
            int distanceToStart = i;
            
            if (distanceToConflict >= minTargetDistanceSteps) {
                candidates.add(new CandidatePosition(pos, distanceToConflict, distanceToStart));
            } else {
                tooCloseCount++;
            }
        }
        
        System.out.println("[调试] 原始路径检查: 冲突位置=" + conflictPosition + 
                         ", 索引=" + conflictIndex + 
                         ", 检查了" + checkedCount + "个点, " +
                         "重合位置=" + blockedCount + ", " +
                         "距离太近=" + tooCloseCount + ", " +
                         "候选位置=" + candidates.size());
        
        if (!candidates.isEmpty()) {
            candidates.sort((c1, c2) -> {
                int compare = Integer.compare(c1.distanceToConflict, c2.distanceToConflict);
                if (compare != 0) {
                    return compare;
                }
                return Integer.compare(c2.distanceToCurrent, c1.distanceToCurrent);
            });
            
            CandidatePosition best = candidates.get(0);
            System.out.println("在原始路径上找到等待位置: " + best.position + 
                             "，距离冲突点 " + best.distanceToConflict + " 步" +
                             "，在路径中的位置: 第" + (best.distanceToCurrent + 1) + "个点");
            return best.position;
        }
        
        System.out.println("[调试] 原始路径上没有找到合适的等待位置（距离冲突点至少5步）");
        return null;
    }
    
    /**
     * 候选位置内部类
     */
    private static class CandidatePosition {
        final String position;
        final int distanceToConflict;
        final int distanceToCurrent;
        
        CandidatePosition(String position, int distanceToConflict, int distanceToCurrent) {
            this.position = position;
            this.distanceToConflict = distanceToConflict;
            this.distanceToCurrent = distanceToCurrent;
        }
    }
    
    /**
     * 规划到等待位置的路径
     */
    private PlannedPath planPathToWaitPosition(String agvId, String currentPosition, 
                                               String waitPosition, AGVStatus agvStatus,
                                               String originalSegId, ExecutingTask task,
                                               long unifiedTimestamp) {
        if (pathPlanner == null) {
            return null;
        }

        CTUPhysicalConfig config = agvStatus != null && agvStatus.getPhysicalConfig() != null 
                ? agvStatus.getPhysicalConfig() 
                : new CTUPhysicalConfig();

        try {
            PlannedPath path = pathPlanner.planPath(currentPosition, waitPosition);
            if (path != null) {
                path.setAgvId(agvId);

                String segId;
                if (originalSegId != null && !originalSegId.isEmpty()) {
                    segId = originalSegId;
                } else if (task != null && task.getTaskId() != null) {
                    segId = task.getTaskId() + "_" + agvId + "_" + (task.getTaskType() != null ? task.getTaskType() : "waiting");
                } else {
                    segId = "UNKNOWN_" + agvId + "_waiting";
                    System.out.println("警告: AGV " + agvId + " 无法获取任务信息，使用默认segId: " + segId);
                }
                path.setSegId(segId);

                // 使用统一时间戳（转换为毫秒）
                long startTimeMs = unifiedTimestamp * 1000;
                if (timeCalculator != null && path.getCodeList() != null) {
                    timeCalculator.calculatePathTiming(path, startTimeMs, config, 0.0);
                }

                if (!path.getCodeList().isEmpty()) {
                    PathCode lastCode = path.getCodeList().get(path.getCodeList().size() - 1);
                    lastCode.setTargetPoint(true);
                    lastCode.setActionType(null);
                    lastCode.setPosType(null);
                }
                
                return path;
            }
        } catch (Exception e) {
            System.out.println("规划到等待位置的路径失败: " + e.getMessage());
        }
        
        return null;
    }
    
    /**
     * 截断路径到等待位置
     *
     * @param path 路径
     * @param waitPosition 等待位置
     */
    private void truncatePathToWaitPosition(PlannedPath path, String waitPosition) {
        if (path == null || path.getCodeList() == null || path.getCodeList().isEmpty()) {
            return;
        }
        
        List<PathCode> codeList = path.getCodeList();
        List<PathCode> truncatedList = new ArrayList<>();

        int waitIndex = -1;
        for (int i = 0; i < codeList.size(); i++) {
            if (codeList.get(i).getCode().equals(waitPosition)) {
                waitIndex = i;
                break;
            }
        }
        
        if (waitIndex >= 0) {
            truncatedList.addAll(codeList.subList(0, waitIndex + 1));

            if (!truncatedList.isEmpty()) {
                PathCode lastCode = truncatedList.get(truncatedList.size() - 1);
                lastCode.setTargetPoint(true);
                lastCode.setActionType(null);
                lastCode.setPosType(null);
            }
            
            path.setCodeList(truncatedList);
            System.out.println("路径已截断到等待位置 " + waitPosition);
        }
    }
    
    /**
     * 截断路径到当前位置（
     */
    private void truncatePathToCurrentPosition(PlannedPath path, String currentPosition) {
        if (path == null || path.getCodeList() == null || path.getCodeList().isEmpty()) {
            return;
        }
        
        List<PathCode> codeList = path.getCodeList();
        List<PathCode> truncatedList = new ArrayList<>();

        int currentIndex = -1;
        for (int i = 0; i < codeList.size(); i++) {
            if (codeList.get(i).getCode().equals(currentPosition)) {
                currentIndex = i;
                break;
            }
        }
        
        if (currentIndex >= 0) {
            truncatedList.addAll(codeList.subList(0, currentIndex + 1));

            if (!truncatedList.isEmpty()) {
                PathCode lastCode = truncatedList.get(truncatedList.size() - 1);
                lastCode.setTargetPoint(true);
                lastCode.setActionType(null);
                lastCode.setPosType(null);
            }
            
            path.setCodeList(truncatedList);
            System.out.println("路径已截断到当前位置 " + currentPosition);
        }
    }

    /**
     * 评估AGV优先级
     *
     * @param agvId          AGV编号
     * @param path           路径
     * @param executingTasks 执行中任务列表
     * @return AGV优先级信息
     */
    private AGVPriority evaluateAgvPriority(String agvId, PlannedPath path, List<ExecutingTask> executingTasks) {
        // 查找对应的执行任务
        ExecutingTask task = null;
        for (ExecutingTask et : executingTasks) {
            if (et.getAgvId().equals(agvId)) {
                task = et;
                break;
            }
        }

        double priorityScore = 0.0;
        String explanation = "默认优先级";

        if (task != null) {
            String taskStatus = task.getTaskType();
            int taskPriority = task.getPriority();

            // 基于任务类型的优先级评分
            switch (taskStatus) {
                case "delivery":
                    priorityScore += 50; // 送货任务优先级高
                    explanation = "送货任务 - 高优先级";
                    break;
                case "pickup":
                    priorityScore += 30; // 取货任务中等优先级
                    explanation = "取货任务 - 中等优先级";
                    break;
                case "charging":
                    priorityScore += 20; // 充电任务低优先级
                    explanation = "充电任务 - 低优先级";
                    break;
                default:
                    priorityScore += 10; // 其他任务最低优先级
                    explanation = "其他任务 - 最低优先级";
            }

            // 任务优先级加成
            priorityScore += taskPriority * 10;

            // 路径长度因子（路径越短优先级越高）
            if (path != null && path.getCodeList() != null) {
                double pathLengthFactor = Math.max(0, 50 - path.getCodeList().size());
                priorityScore += pathLengthFactor;
            }
        }

        return new AGVPriority(agvId, priorityScore, explanation);
    }

    /**
     * 为路径添加延迟
     *
     * @param path       路径
     * @param timeStep   延迟开始的时间步
     * @param delaySteps 延迟步数
     */
    private void addDelayToPath(PlannedPath path, int timeStep, int delaySteps) {
        if (path == null || path.getCodeList() == null || path.getCodeList().isEmpty()) {
            return;
        }

        List<PathCode> codeList = path.getCodeList();

        // 确保时间步在有效范围内
        if (timeStep < 0 || timeStep >= codeList.size()) {
            return;
        }

        // 获取延迟位置的路径代码
        PathCode delayCode = codeList.get(timeStep);
        
        // 计算延迟时间（每个延迟步骤1秒）
        long delayDuration = delaySteps * 1000L;

        // 创建延迟步骤
        List<PathCode> delaySteps_list = new ArrayList<>();
        for (int i = 0; i < delaySteps; i++) {
            PathCode waitCode = new PathCode(delayCode.getCode(), delayCode.getDirection());
            waitCode.setActionType(delayCode.getActionType());
            waitCode.setTaskId(delayCode.getTaskId());
            waitCode.setPosType(delayCode.getPosType());
            waitCode.setLev(delayCode.getLev());
            waitCode.setTargetPoint(false); // 等待步骤不是目标点
            
            // 设置等待步骤的时间信息
            if (delayCode.getArrivalTime() != null) {
                long waitStartTime = delayCode.getArrivalTime() + (i * 1000L);
                waitCode.setArrivalTime(waitStartTime);
                waitCode.setDepartureTime(waitStartTime + 1000L);
            }
            
            delaySteps_list.add(waitCode);
        }

        // 插入延迟步骤
        codeList.addAll(timeStep, delaySteps_list);

        // 更新原路径点的时间（向后推迟）
        if (delayCode.getArrivalTime() != null) {
            delayCode.setArrivalTime(delayCode.getArrivalTime() + delayDuration);
            if (delayCode.getDepartureTime() != null) {
                delayCode.setDepartureTime(delayCode.getDepartureTime() + delayDuration);
            }
        }

        // 更新路径
        path.setCodeList(codeList);
        
        // 🔧 关键修复：重新计算后续路径点的时间窗
        if (timeCalculator != null) {
            CTUPhysicalConfig defaultConfig = createDefaultPhysicalConfig();
            timeCalculator.recalculatePathTimingFromIndex(path, timeStep + delaySteps, defaultConfig);
        } else {
            // 备用方案：手动更新后续时间
            updateSubsequentPathTiming(path, timeStep + delaySteps, delayDuration);
        }
    }
    
    /**
     * 备用方案：手动更新后续路径点的时间
     */
    private void updateSubsequentPathTiming(PlannedPath path, int fromIndex, long timeOffset) {
        List<PathCode> codeList = path.getCodeList();
        
        for (int i = fromIndex; i < codeList.size(); i++) {
            PathCode pathCode = codeList.get(i);
            
            if (pathCode.getArrivalTime() != null) {
                pathCode.setArrivalTime(pathCode.getArrivalTime() + timeOffset);
            }
            
            if (pathCode.getDepartureTime() != null) {
                pathCode.setDepartureTime(pathCode.getDepartureTime() + timeOffset);
            }
            
            if (pathCode.getCumulativeTime() != null) {
                pathCode.setCumulativeTime(pathCode.getCumulativeTime() + timeOffset);
            }
        }
    }
    
    /**
     * 创建默认物理配置
     */
    private CTUPhysicalConfig createDefaultPhysicalConfig() {
        return new CTUPhysicalConfig(); // 使用默认构造函数
    }

    /**
     * AGV优先级评估结果内部类 - 优化版本，只保留必要字段
     */
    private static class AGVPriority {
        final String agvId;
        final double priorityScore;
        final String explanation;

        public AGVPriority(String agvId, double priorityScore, String explanation) {
            this.agvId = agvId;
            this.priorityScore = priorityScore;
            this.explanation = explanation;
        }

        @Override
        public String toString() {
            return String.format("AGV %s: 优先级=%.1f, 说明=%s",
                    agvId, priorityScore, explanation);
        }
    }
}