package com.algo.service;

import com.algo.config.EnvDataConfig;
import com.algo.model.*;
import com.algo.util.JsonUtils;
import com.algo.util.PathTimeCalculator;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.stereotype.Service;

import javax.annotation.PostConstruct;
import java.util.*;
import java.util.concurrent.*;

/**
 * 路径规划服务
 */
@Service
public class PathPlanningService {

    @Autowired
    private EnvDataConfig envDataConfig;

    /**
     * 执行中任务提取
     */
    private ExecutingTaskExtractor taskExtractor;

    /**
     * 路径规划
     */
    private PathPlanner pathPlanner;

    /**
     * 碰撞检测
     */
    private CollisionDetector collisionDetector;

    /**
     * 碰撞解决
     */
    private CollisionResolver collisionResolver;

    /**
     * 剩余路径处理
     */
    private RemainingPathProcessor remainingPathProcessor;
    
    /**
     * 路径时间计算器
     */
    private PathTimeCalculator timeCalculator;


    /**
     * 线程池
     */
    private final ExecutorService executorService = Executors.newFixedThreadPool(Math.max(4, Runtime.getRuntime().availableProcessors()));

    /**
     * CTU批处理大小
     */
    private final int batchSize = 10;


    /**
     * 初始化各个组件
     */
    @PostConstruct
    public void initializeComponents() {

        // 初始化路径规划器
        this.pathPlanner = new AStarPathPlanner(envDataConfig.getPathMapping());

        // 初始化碰撞检测器
        this.collisionDetector = new CollisionDetector(envDataConfig.getPathMapping());

        // 初始化碰撞解决器
        this.collisionResolver = new CollisionResolver(collisionDetector);

        // 初始化剩余路径处理器
        this.remainingPathProcessor = new RemainingPathProcessor(envDataConfig.getPathMapping());
        
        // 初始化时间计算器
        Map<String, double[]> realCoordinateMapping = JsonUtils.loadRealCoordinateMapping("man_code.json");
        this.timeCalculator = new PathTimeCalculator(envDataConfig.getPathMapping(), realCoordinateMapping);
        
        // 为碰撞解决器设置时间计算器、路径规划器和路径映射表
        this.collisionResolver.setTimeCalculator(timeCalculator);
        this.collisionResolver.setPathPlanner((PathPlanner) pathPlanner);
        this.collisionResolver.setPathMapping(envDataConfig.getPathMapping());
    }

    /**
     * 为所有CTU规划路径
     *
     * @param agvStatusList  CTU状态列表
     * @param includeIdleAgv 是否包含空闲CTU
     * @param constraints    路径约束条件
     * @return 路径规划结果
     */
    public PathPlanningResult planAllAgvPaths(List<TaskData> taskList, List<AGVStatus> agvStatusList,
                                              boolean includeIdleAgv,
                                              List<double[]> constraints) {
        // 初始化任务提取器
        this.taskExtractor = new ExecutingTaskExtractor(envDataConfig.getPathMapping(), taskList);

        long startTime = System.currentTimeMillis();
        long unifiedTimestamp = startTime / 1000;

        System.out.println("开始为 " + agvStatusList.size() + " 个CTU规划");
        System.out.println("时间基准: " + unifiedTimestamp + "秒 (" + new Date(startTime) + ")");

        // 统计故障车数量
        long faultyCount = agvStatusList.stream().filter(agv -> agv.getError() == 1).count();
        if (faultyCount > 0) {
            System.out.println("检测到 " + faultyCount + " 个故障车(error)，将在时空表中占据位置");
        }

        // 1. 构建现有剩余路径的时空占用表
        System.out.println("步骤1: 构建时空占用表");
        Map<String, String> spaceTimeOccupancyMap = remainingPathProcessor.buildSpaceTimeOccupancyMap(agvStatusList, unifiedTimestamp);
        System.out.println("时空占用表构建完成，包含 " + spaceTimeOccupancyMap.size() + " 个占用点");

        // 2. 初始化规划结果集合
        List<PlannedPath> plannedPaths = new ArrayList<>();
        List<ExecutingTask> executingTasks = new ArrayList<>();
        Map<String, String> plannedAgvIds = new HashMap<>(); // 提前初始化标记故障车

        // 3. 分类处理CTU：有剩余路径的和需要新路径的
        List<AGVStatus> ctuWithRemainingPaths = new ArrayList<>();
        List<AGVStatus> ctuNeedingNewPaths = new ArrayList<>();

        for (AGVStatus agv : agvStatusList) {
            // 故障车已在时空占用表中处理，标记为已处理但不规划路径
            if (agv.getError() == 1) {
                plannedAgvIds.put(agv.getAgvId(), "FAULTY");
                continue;
            }
            
            if (agv.hasRemainingPath()) {
                // 有剩余路径的AGV，先处理剩余路径
                ctuWithRemainingPaths.add(agv);
            } else {
                // 只要不是故障车且没有剩余路径，参与路径规划
                ctuNeedingNewPaths.add(agv);
            }
        }

        System.out.println("CTU分类完成: 有剩余路径=" + ctuWithRemainingPaths.size() +
                ", 需要新路径=" + ctuNeedingNewPaths.size());
        
        // 记录需要新路径的AGV信息（debug）
        if (!ctuNeedingNewPaths.isEmpty()) {
            System.out.println("需要新路径的AGV详情:");
            for (AGVStatus agv : ctuNeedingNewPaths) {
                System.out.println("  - AGV " + agv.getAgvId() + 
                    ": error=" + agv.getError() + 
                    ", status=" + agv.getStatus() +
                    ", hasRemainingPath=" + agv.hasRemainingPath() + 
                    ", isAvailable=" + agv.isAvailable() +
                    ", position=" + agv.getPosition());
            }
        }

        // 4. 处理有剩余路径的CTU

        System.out.println("步骤2: 处理有剩余路径的CTU");
        for (AGVStatus agv : ctuWithRemainingPaths) {
            PlannedPath remainingPath = processRemainingPath(agv, unifiedTimestamp);
            if (remainingPath != null) {
                plannedPaths.add(remainingPath);
                plannedAgvIds.put(agv.getAgvId(), "REMAINING_PATH");

                // 创建对应的执行任务
                ExecutingTask task = createExecutingTaskFromRemainingPath(agv);
                if (task != null) {
                    executingTasks.add(task);
                }

                System.out.println("CTU " + agv.getAgvId() + " 剩余路径处理完成，路径长度: " +
                        remainingPath.getCodeList().size());
            }
        }

        // 4.5. 检查空闲AGV是否需要让行
        System.out.println("检查空闲AGV是否需要让行");
        List<AGVStatus> yieldingAgvs = identifyYieldingAgvs(agvStatusList, plannedPaths);
        if (!yieldingAgvs.isEmpty()) {
            System.out.println("  发现 " + yieldingAgvs.size() + " 个需要让行的空闲AGV");
            for (AGVStatus yieldAgv : yieldingAgvs) {
                PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
                if (yieldPath != null) {
                    plannedPaths.add(yieldPath);
                    plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
                    System.out.println("  AGV " + yieldAgv.getAgvId() + " 让行路径规划成功，从 " + 
                        yieldAgv.getPosition() + " 移开");
                }
            }
        } else {
            System.out.println("  无需让行的AGV");
        }

        // 5. 为需要新路径的CTU提取执行中任务
        System.out.println("步骤3: 提取需要新路径的CTU任务");
        List<ExecutingTask> newTasks = taskExtractor.extractExecutingTasks(ctuNeedingNewPaths);
        System.out.println("提取到 " + newTasks.size() + " 个新任务");

        // 6. 为新任务规划路径（考虑时空约束）
        System.out.println("步骤4: 为新任务规划路径");
        for (ExecutingTask task : newTasks) {
            String agvId = task.getAgvId();
            String currentPos = task.getCurrentPosition();
            String targetPos = task.getTargetPosition();

            // 避免重复规划
            if (plannedAgvIds.containsKey(agvId)) {
                System.out.println("跳过CTU " + agvId + "：已规划路径");
                continue;
            }

            // 验证位置信息
            if (currentPos == null || targetPos == null) {
                System.out.println("CTU " + agvId + " 位置信息无效，跳过");
                continue;
            }

            // 获取CTU状态信息
            AGVStatus agvStatus = findAgvStatus(agvId, ctuNeedingNewPaths);
            if (agvStatus == null) {
                System.out.println("CTU " + agvId + " 状态信息未找到，跳过");
                continue;
            }

            // 规划时空安全的路径
            PlannedPath plannedPath = planPathWithSpaceTimeConstraints(
                    currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus, unifiedTimestamp
            );

            if (plannedPath != null) {
                // 设置CTU信息
                plannedPath.setAgvId(agvId);
                plannedPath.setSegId(generateSegId(task.getTaskId(), agvId, task.getTaskType()));

                enhancePathCodes(plannedPath, task);

                plannedPaths.add(plannedPath);
                plannedAgvIds.put(agvId, targetPos);
                executingTasks.add(task);

                // 更新时空占用表
                updateSpaceTimeOccupancyMap(plannedPath, spaceTimeOccupancyMap, agvStatus);

                System.out.println("CTU " + agvId + " 路径规划成功：" + currentPos + " -> " + targetPos +
                        "，路径长度: " + plannedPath.getCodeList().size());
            } else {
                System.out.println("CTU " + agvId + " 路径规划失败：" + currentPos + " -> " + targetPos);
            }
        }

        // 7. 处理空闲CTU
        if (includeIdleAgv) {
            System.out.println("步骤5: 处理空闲CTU");
            
            // 7.1. 检查空闲AGV是否占用已规划路径，如果占用则生成避让路径
            List<AGVStatus> yieldingIdleAgvs = identifyYieldingAgvs(agvStatusList, plannedPaths);

            List<AGVStatus> yieldingAgvsToHandle = new ArrayList<>();
            for (AGVStatus yieldAgv : yieldingIdleAgvs) {
                if (!plannedAgvIds.containsKey(yieldAgv.getAgvId())) {
                    yieldingAgvsToHandle.add(yieldAgv);
                }
            }
            
            if (!yieldingAgvsToHandle.isEmpty()) {
                System.out.println( yieldingAgvsToHandle.size() + " 个需要避让的空闲AGV");
                for (AGVStatus yieldAgv : yieldingAgvsToHandle) {
                    PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
                    if (yieldPath != null) {
                        plannedPaths.add(yieldPath);
                        plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
                    }
                }
            } else {
                System.out.println("  无需避让的空闲AGV");
            }
            
            // 7.2. 然后为剩余的空闲AGV规划待机/充电路径
            List<PlannedPath> idlePaths = planIdleAgvPathsWithConstraints(
                    ctuNeedingNewPaths, plannedAgvIds, constraints, spaceTimeOccupancyMap, unifiedTimestamp
            );
            plannedPaths.addAll(idlePaths);
            System.out.println("  空闲CTU路径规划，数量: " + idlePaths.size());
        }

        // 7.5. 最终避让检查
        performFinalYieldingCheck(agvStatusList, plannedPaths, plannedAgvIds, spaceTimeOccupancyMap, constraints, unifiedTimestamp);

        // 7.6. 重新计算所有路径的时间戳
        recalculateAllPathTimings(plannedPaths, agvStatusList, unifiedTimestamp);

        // 8. 最终碰撞检测和解决
        System.out.println("步骤6: 最终碰撞检测");
        List<Conflict> conflicts = collisionDetector.detectConflicts(plannedPaths);

        if (!conflicts.isEmpty()) {
            System.out.println("发现 " + conflicts.size() + " 个碰撞，开始解决");
            plannedPaths = collisionResolver.resolveConflicts(plannedPaths, conflicts, executingTasks, agvStatusList, unifiedTimestamp);

            // 重新检测冲突
            List<Conflict> remainingConflicts = collisionDetector.detectConflicts(plannedPaths);
            conflicts = remainingConflicts;
        }

        long endTime = System.currentTimeMillis();
        double totalTime = (endTime - startTime) / 1000.0;

        // 构建结果
        PathPlanningResult result = new PathPlanningResult(
                agvStatusList.size(),
                executingTasks.size(),
                plannedPaths.size(),
                totalTime,
                conflicts.size(),
                conflicts.isEmpty(),
                "A*_WITH_SPACETIME_CONSTRAINTS",
                plannedPaths,
                executingTasks
        );

        System.out.println("路径规划完成 - 总CTU: " + result.getTotalAgvs() +
                ", 执行任务: " + result.getExecutingTasksCount() +
                ", 规划路径: " + result.getPlannedPathsCount() +
                ", 耗时: " + result.getTotalPlanningTime() + "s" +
                ", 无冲突: " + result.isCollisionFree());

        return result;
    }

    /**
     * 处理CTU的剩余路径
     *
     * @param agv CTU状态
     * @param unifiedTimestamp 统一时间戳（秒）
     * @return 处理后的路径
     */
    private PlannedPath processRemainingPath(AGVStatus agv, long unifiedTimestamp) {
        if (!agv.hasRemainingPath()) {
            return null;
        }

        PlannedPath remainingPath = agv.getRemainingPath();
        List<PathCode> remainingCodes = new ArrayList<>();

        // 获取剩余路径
        List<PathCode> originalCodes = remainingPath.getCodeList();
        for (int i = agv.getCurrentPathIndex(); i < originalCodes.size(); i++) {
            PathCode originalCode = originalCodes.get(i);
            PathCode newCode = new PathCode(originalCode.getCode(), originalCode.getDirection());
            newCode.setActionType(originalCode.getActionType());
            newCode.setTaskId(originalCode.getTaskId());
            newCode.setPosType(originalCode.getPosType());
            newCode.setLev(originalCode.getLev());
            newCode.setTargetPoint(originalCode.isTargetPoint());
            remainingCodes.add(newCode);
        }

        PlannedPath processedPath = new PlannedPath();
        processedPath.setAgvId(agv.getAgvId());
        processedPath.setCodeList(remainingCodes);
        
        // 使用输入中的segId，如果没有则生成新的
        String segId = remainingPath.getSegId();
        if (segId == null || segId.trim().isEmpty()) {
            segId = agv.getAgvId() + "_REMAINING_" + System.currentTimeMillis();
        }
        processedPath.setSegId(segId);

        if (timeCalculator != null && !remainingCodes.isEmpty()) {
            // 使用统一时间戳作为起始时间（转换为毫秒）
            long startTime = unifiedTimestamp * 1000;
            
            CTUPhysicalConfig config = agv.getPhysicalConfig();
            
            // 估算当前速度（AGV移动中为正常速度；静止为0）
            double initialSpeed = agv.hasRemainingPath() && agv.getRemainingPathLength() > 0 
                                ? config.getNormalSpeed() : 0.0;
            
            // 计算时间信息
            // arrivalTime, departureTime, cumulativeTime
            timeCalculator.calculatePathTiming(
                processedPath, 
                startTime, 
                config, 
                initialSpeed
            ); 
        } else {
            System.out.println("  未能为剩余路径设置时间信息 - AGV: " + agv.getAgvId());
        }

        return processedPath;
    }

    /**
     * 从剩余路径创建执行任务
     *
     * @param agv CTU状态
     * @return 执行任务
     */
    private ExecutingTask createExecutingTaskFromRemainingPath(AGVStatus agv) {
        if (!agv.hasRemainingPath()) {
            return null;
        }

        PlannedPath remainingPath = agv.getRemainingPath();
        List<PathCode> codeList = remainingPath.getCodeList();

        if (codeList.isEmpty() || agv.getCurrentPathIndex() >= codeList.size()) {
            return null;
        }

        // 获取当前位置和目标位置
        PathCode currentCode = codeList.get(agv.getCurrentPathIndex());
        PathCode targetCode = codeList.get(codeList.size() - 1);

        ExecutingTask task = new ExecutingTask();
        task.setAgvId(agv.getAgvId());
        task.setTaskId(remainingPath.getSegId());
        task.setCurrentPosition(currentCode.getCode());
        task.setTargetPosition(targetCode.getCode());
        task.setTaskType("remaining");
        task.setLoaded(false); // 假设值
        task.setBackpackIndex(0);
        task.setPriority(1);

        return task;
    }

    /**
     * 规划带时空约束的路径
     *
     * @param startPos     起始位置
     * @param endPos       目标位置
     * @param constraints  约束条件
     * @param occupancyMap 时空占用表
     * @param agvStatus    CTU状态
     * @param unifiedTimestamp 统一时间戳（秒）
     * @return 规划的路径
     */
    private PlannedPath planPathWithSpaceTimeConstraints(String startPos, String endPos,
                                                         List<double[]> constraints,
                                                         Map<String, String> occupancyMap,
                                                         AGVStatus agvStatus,
                                                         long unifiedTimestamp) {
        // 带时空占用表的路径规划
        long startTimeMs = unifiedTimestamp * 1000; 
        PlannedPath spacetimePath = ((AStarPathPlanner)pathPlanner).planSpaceTimePath(
                startPos, 
                endPos, 
                constraints, 
                occupancyMap, 
                agvStatus.getPhysicalConfig(),
                startTimeMs 
        );
        
        if (spacetimePath == null) {
            return null;
        }

        timeCalculator.calculatePathTiming(spacetimePath, startTimeMs, agvStatus.getPhysicalConfig(), 0.0);

        return spacetimePath;
    }

    /**
     * 路径的时间信息
     *
     * @param path      路径
     * @param startTime 起始时间
     * @param config    物理配置
     */
    private void enhancePathWithTimeInfo(PlannedPath path, long startTime, CTUPhysicalConfig config) {
        List<PathCode> codeList = path.getCodeList();
        if (codeList == null || codeList.isEmpty()) {
            return;
        }

        long currentTime = startTime;

        for (int i = 0; i < codeList.size(); i++) {
            PathCode pathCode = codeList.get(i);

            // 设置预计到达时间

            // 计算到下一个点的时间
            if (i < codeList.size() - 1) {
                double travelTime = config.getStandardPointDistance() / config.getNormalSpeed();
                currentTime += (long) (travelTime * 1000);

                // 如果有方向变化，增加转向时间
                PathCode nextCode = codeList.get(i + 1);
                // 添加null检查
                if (pathCode.getDirection() != null && nextCode.getDirection() != null) {
                    if (!pathCode.getDirection().equals(nextCode.getDirection())) {
                        double turnTime = config.getTurnTime(pathCode.getDirection(), nextCode.getDirection());
                        currentTime += (long) (turnTime * 1000);
                    }
                }
            }
        }
    }

    /**
     * 更新时空占用表
     *
     * @param path         新路径
     * @param occupancyMap 占用表
     * @param agvStatus    CTU状态
     */
    private void updateSpaceTimeOccupancyMap(PlannedPath path, Map<String, String> occupancyMap,
                                             AGVStatus agvStatus) {
        // 将新路径添加到占用表中
        List<PathCode> codeList = path.getCodeList();
        CTUPhysicalConfig config = agvStatus.getPhysicalConfig();

        long currentTime = System.currentTimeMillis() / 1000; // 转换为秒

        for (PathCode pathCode : codeList) {
            int[] coord = JsonUtils.getCoordinate(pathCode.getCode(), envDataConfig.getPathMapping());
            if (coord != null) {
                String spaceTimeKey = coord[0] + "," + coord[1] + "," + currentTime;
                occupancyMap.put(spaceTimeKey, agvStatus.getAgvId());
                currentTime += 2; // 假设每个点停留2秒
            }
        }
    }

    /**
     * 查找CTU状态
     *
     * @param agvId   CTU编号
     * @param agvList CTU列表
     * @return CTU状态
     */
    private AGVStatus findAgvStatus(String agvId, List<AGVStatus> agvList) {
        for (AGVStatus agv : agvList) {
            if (agv.getAgvId().equals(agvId)) {
                return agv;
            }
        }
        return null;
    }

    /**
     * 规划带约束的空闲CTU路径
     *
     * @param agvStatusList CTU状态列表
     * @param plannedAgvIds 已规划CTU映射
     * @param constraints   约束条件
     * @param occupancyMap  时空占用表
     * @param unifiedTimestamp 统一时间戳（秒）
     * @return 空闲CTU路径列表
     */
    private List<PlannedPath> planIdleAgvPathsWithConstraints(List<AGVStatus> agvStatusList,
                                                              Map<String, String> plannedAgvIds,
                                                              List<double[]> constraints,
                                                              Map<String, String> occupancyMap,
                                                              long unifiedTimestamp) {
        List<PlannedPath> idlePaths = new ArrayList<>();

        for (AGVStatus agvStatus : agvStatusList) {
            String agvId = agvStatus.getAgvId();

            // 跳过已规划的CTU
            if (plannedAgvIds.containsKey(agvId)) {
                continue;
            }

            // 检查CTU是否空闲
            if (isAgvIdle(agvStatus)) {
                String currentPos = agvStatus.getPosition();
                String targetPos = getIdleAgvTarget(agvId, currentPos);

                if (targetPos != null && !targetPos.equals(currentPos)) {
                    PlannedPath idlePath = planPathWithSpaceTimeConstraints(
                            currentPos, targetPos, constraints, occupancyMap, agvStatus, unifiedTimestamp
                    );

                    if (idlePath != null) {
                        idlePath.setAgvId(agvId);
                        idlePath.setSegId(generateSegId("IDLE", agvId, "idle"));

                        // 设置空闲路径的代码信息
                        enhanceIdlePathCodes(idlePath, agvStatus);

                        idlePaths.add(idlePath);
                        plannedAgvIds.put(agvId, targetPos);

                        // 更新占用表
                        updateSpaceTimeOccupancyMap(idlePath, occupancyMap, agvStatus);

                        System.out.println("空闲CTU " + agvId + " 路径规划成功：" + currentPos + " -> " + targetPos);
                    }
                }
            }
        }

        return idlePaths;
    }

    /**
     * 路径代码信息
     *
     * @param plannedPath 规划路径
     * @param task        执行任务
     */
    private void enhancePathCodes(PlannedPath plannedPath, ExecutingTask task) {
        List<PathCode> codeList = plannedPath.getCodeList();
        if (codeList == null || codeList.isEmpty()) {
            return;
        }

        String taskId = task.getTaskId();
        int backpackLevel = task.getBackpackIndex();

        for (int i = 0; i < codeList.size(); i++) {
            PathCode pathCode = codeList.get(i);

            // 设置基本信息
            pathCode.setTaskId(taskId);
            pathCode.setLev(backpackLevel);

            // 设置目标点信息
            if (i == codeList.size() - 1) { // 最后一个点
                pathCode.setTargetPoint(true);
                // 根据任务类型设置actionType和posType（
                if (task.isLoaded()) {
                    pathCode.setActionType("2"); // 放货
                    pathCode.setPosType("2"); // 放货
                } else {
                    pathCode.setActionType("1"); // 取货
                    pathCode.setPosType("1"); // 取货
                }
            } else {
                pathCode.setTargetPoint(false);
                pathCode.setActionType(null); 
                pathCode.setPosType(null);
            }
        }
    }

    /**
     * 空闲路径代码信息
     *
     * @param plannedPath 规划路径
     * @param agvStatus   CTU状态
     */
    private void enhanceIdlePathCodes(PlannedPath plannedPath, AGVStatus agvStatus) {
        List<PathCode> codeList = plannedPath.getCodeList();
        if (codeList == null || codeList.isEmpty()) {
            return;
        }

        String actionType = agvStatus.needsCharging() ? "3" : "4"; // 充电或待机

        for (PathCode pathCode : codeList) {
            pathCode.setActionType(actionType);
            pathCode.setTaskId(null);
            pathCode.setPosType(null);
            pathCode.setLev(0);
            pathCode.setTargetPoint(false);
        }
    }

    /**
     * 判断CTU是否空闲
     *
     * @param agvStatus CTU状态
     * @return true如果CTU空闲
     */
    private boolean isAgvIdle(AGVStatus agvStatus) {
        if (!agvStatus.isAvailable()) {
            return false;
        }

        // 检查背篓是否有执行中的任务
        List<BackpackData> backpack = agvStatus.getBackpack();
        if (backpack != null) {
            for (BackpackData bp : backpack) {
                if (bp.getTaskId() != null && !bp.getTaskId().trim().isEmpty()) {
                    return false;
                }
            }
        }

        return true;
    }

    /**
     * 获取空闲CTU的目标位置
     *
     * @param agvId           CTU编号
     * @param currentPosition 当前位置
     * @return 目标位置
     */
    private String getIdleAgvTarget(String agvId, String currentPosition) {
        // 根据CTU ID的哈希值选择位置
        int hashValue = Math.abs(agvId.hashCode() % 1000);

        // 优先选择充电位置
        List<String> chargingPositions = taskExtractor.getChargingPositions();
        if (!chargingPositions.isEmpty()) {
            for (String pos : chargingPositions) {
                if (!pos.equals(currentPosition)) {
                    return pos;
                }
            }
        }

        // 其次选择待机位置
        List<String> standbyPositions = taskExtractor.getStandbyPositions();
        if (!standbyPositions.isEmpty()) {
            return standbyPositions.get(hashValue % standbyPositions.size());
        }

        return null;
    }

    /**
     * 生成段落ID
     *
     * @param taskId   任务ID
     * @param agvId    CTU编号
     * @param taskType 任务类型
     * @return 段落ID
     */
    private String generateSegId(String taskId, String agvId, String taskType) {
        return taskId + "_" + agvId + "_" + taskType;
    }

    /**
     * 迭代检查所有空闲AGV是否占用已规划路径
     */
    private void performFinalYieldingCheck(List<AGVStatus> agvStatusList, 
                                           List<PlannedPath> plannedPaths,
                                           Map<String, String> plannedAgvIds,
                                           Map<String, String> spaceTimeOccupancyMap,
                                           List<double[]> constraints,
                                           long unifiedTimestamp) {
        final int MAX_ITERATIONS = 5; // 最大迭代次数
        int iteration = 0;
        int totalYieldingCount = 0;
        
        while (iteration < MAX_ITERATIONS) {
            iteration++;
            
            // 识别需要避让的空闲AGV
            List<AGVStatus> yieldingAgvs = identifyYieldingAgvs(agvStatusList, plannedPaths);
            
            if (yieldingAgvs.isEmpty()) {
                if (iteration == 1) {
                    System.out.println("  无需避让的AGV");
                } else {
                    System.out.println("  第" + iteration + "轮检查：无新的避让需求");
                }
                break;
            }
            
            System.out.println("  第" + iteration + "轮检查：" + yieldingAgvs.size() + " 个需要避让的AGV");
            
            // 规划避让路径
            int successCount = 0;
            for (AGVStatus yieldAgv : yieldingAgvs) {
                if (plannedAgvIds.containsKey(yieldAgv.getAgvId())) {
                    continue;
                }
                
                PlannedPath yieldPath = planYieldPath(yieldAgv, plannedPaths, spaceTimeOccupancyMap, constraints, unifiedTimestamp);
                if (yieldPath != null) {
                    plannedPaths.add(yieldPath);
                    plannedAgvIds.put(yieldAgv.getAgvId(), "AVOIDING");
                    successCount++;
                    totalYieldingCount++;
                }
            }
            
            if (successCount == 0) {
                System.out.println("  第" + iteration + "轮检查：无法为需要避让的AGV规划路径，停止迭代");
                break;
            }
        }
        
        if (iteration >= MAX_ITERATIONS) {
            System.out.println("  达到最大迭代次数(" + MAX_ITERATIONS + ")，可能存在复杂的避让场景");
        }
        
        if (totalYieldingCount > 0) {
            System.out.println("  避让检查完成：共处理 " + totalYieldingCount + " 个避让AGV，迭代 " + iteration + " 轮");
        }
    }

    private void recalculateAllPathTimings(List<PlannedPath> plannedPaths, 
                                           List<AGVStatus> agvStatusList, 
                                           long unifiedTimestamp) {
        if (plannedPaths == null || plannedPaths.isEmpty() || timeCalculator == null) {
            return;
        }

        Map<String, AGVStatus> agvStatusMap = new HashMap<>();
        for (AGVStatus agv : agvStatusList) {
            if (agv.getAgvId() != null) {
                agvStatusMap.put(agv.getAgvId(), agv);
            }
        }

        long startTimeMs = unifiedTimestamp * 1000;
        int recalculatedCount = 0;

        // 遍历所有路径，重新计算时间戳
        for (PlannedPath path : plannedPaths) {
            if (path == null || path.getCodeList() == null || path.getCodeList().isEmpty()) {
                continue;
            }

            String agvId = path.getAgvId();
            if (agvId == null) {
                continue;
            }

            // 获取AGV的物理配置
            AGVStatus agvStatus = agvStatusMap.get(agvId);
            CTUPhysicalConfig config = (agvStatus != null && agvStatus.getPhysicalConfig() != null) 
                    ? agvStatus.getPhysicalConfig() 
                    : new CTUPhysicalConfig(); 

            double initialSpeed = 0.0;

            // 重新计算路径时间戳
            timeCalculator.calculatePathTiming(path, startTimeMs, config, initialSpeed);
            recalculatedCount++;
        }

        System.out.println("  已重新计算 " + recalculatedCount + " 条路径的时间戳（统一时间基准: " + unifiedTimestamp + "秒）");
    }

    /**
     * 识别需要让行的AGV
     * 检查空闲AGV是否占用了其他AGV剩余路径上的位置
     *
     * @param agvStatusList 所有AGV状态列表
     * @param plannedPaths  已规划的路径列表（包含剩余路径）
     * @return 需要让行的AGV列表
     */
    private List<AGVStatus> identifyYieldingAgvs(List<AGVStatus> agvStatusList, List<PlannedPath> plannedPaths) {
        List<AGVStatus> yieldingAgvs = new ArrayList<>();
        
        // 收集所有已规划路径上的位置
        Set<String> occupiedPositions = new HashSet<>();
        for (PlannedPath path : plannedPaths) {
            if (path.getCodeList() != null) {
                for (PathCode code : path.getCodeList()) {
                    if (code.getCode() != null) {
                        occupiedPositions.add(code.getCode());
                    }
                }
            }
        }
        
        // 检查每个AGV
        for (AGVStatus agv : agvStatusList) {
            // 如果这个AGV已经有剩余路径，跳过（已在任务）
            if (agv.hasRemainingPath()) {
                continue;
            }
            
            // 检查AGV是否有位置信息
            if (agv.getPosition() == null || agv.getPosition().trim().isEmpty()) {
                continue;
            }
            
            // 检查AGV是否是空闲状态（status=0）
            // 只要没有剩余路径且有位置，就检查是否需要让行
            int status = agv.getStatus();
            if (status != 0) {
                // status=1或2（问题或忙碌）等其他状态跳过
                continue;
            }
            
            // 检查该AGV的位置是否在其他AGV的路径上
            String currentPos = agv.getPosition();
            if (occupiedPositions.contains(currentPos)) {
                System.out.println(" CTU " + agv.getAgvId() + 
                    " (status=" + status + ") 在位置 " + currentPos + " 占用了其他CTU路径，需要让行");
                yieldingAgvs.add(agv);
            }
        }
        
        return yieldingAgvs;
    }

    /**
     * 为让行AGV规划避让路径
     *
     * @param yieldAgv         需要让行的AGV
     * @param existingPaths    已存在的路径列表
     * @param occupancyMap     时空占用表
     * @param constraints      路径约束
     * @return 让行路径
     */
    private PlannedPath planYieldPath(AGVStatus yieldAgv, List<PlannedPath> existingPaths,
                                     Map<String, String> occupancyMap, List<double[]> constraints,
                                     long unifiedTimestamp) {
        String currentPos = yieldAgv.getPosition();
        String agvId = yieldAgv.getAgvId();
        
        // 1. 找到安全的目标位置
        Set<String> blockedPositions = new HashSet<>();
        for (PlannedPath path : existingPaths) {
            if (path.getCodeList() != null) {
                for (PathCode code : path.getCodeList()) {
                    if (code.getCode() != null) {
                        blockedPositions.add(code.getCode());
                    }
                }
            }
        }
        
        // 2. 寻找合适让行目标位置
        String targetPos = findYieldTargetPosition(currentPos, blockedPositions, yieldAgv);
        
        if (targetPos == null || targetPos.equals(currentPos)) {
            System.out.println("    AGV " + agvId + " 无法找到合适的让行位置");
            return null;
        }
        
        // 3. 规划避让路径
        PlannedPath yieldPath = planPathWithSpaceTimeConstraints(
            currentPos, targetPos, constraints, occupancyMap, yieldAgv, unifiedTimestamp
        );
        
        if (yieldPath != null) {
            yieldPath.setAgvId(agvId);
            yieldPath.setSegId(generateSegId("AVOID", agvId, "avoiding"));
            
            // 设置路径代码信息
            enhancePathCodesForYielding(yieldPath);
            
            // 更新占用表
            updateSpaceTimeOccupancyMap(yieldPath, occupancyMap, yieldAgv);
        }
        
        return yieldPath;
    }

    /**
     * 寻找让行的目标位置
     */
    private String findYieldTargetPosition(String currentPos, Set<String> blockedPositions, AGVStatus agv) {
        // 使用BFS搜索满足安全距离的空闲位置
        final int MAX_SEARCH_DEPTH = 15;
        
        // 获取安全距离参数（从物理配置中获取，默认3.0）
        CTUPhysicalConfig config = agv.getPhysicalConfig();
        double minSafetyDistance = config != null ? config.getMinSafetyDistance() : 3.0;
        
        // 将安全距离转换为网格步数（假设每个节点间距为1）
        int safetySteps = (int) Math.ceil(minSafetyDistance);
        
        Queue<String> queue = new LinkedList<>();
        Map<String, Integer> visited = new HashMap<>(); // 记录位置和距离
        
        queue.offer(currentPos);
        visited.put(currentPos, 0);
        
        while (!queue.isEmpty()) {
            String pos = queue.poll();
            int depth = visited.get(pos);

            if (depth >= MAX_SEARCH_DEPTH) {
                break;
            }

            List<Map<String, String>> neighbors = pathPlanner.getNeighbors(pos);
            for (Map<String, String> neighbor : neighbors) {
                String neighborPos = neighbor.get("code");
                
                if (neighborPos == null || visited.containsKey(neighborPos)) {
                    continue;
                }
                
                int distanceToPos = depth + 1;
                visited.put(neighborPos, distanceToPos);
                
                // 检查该位置是否满足要求
                if (!blockedPositions.contains(neighborPos) && 
                    isSafeDistanceFromBlockedPositions(neighborPos, blockedPositions, safetySteps)) {
                    System.out.println("  找到安全避让位置: " + neighborPos);
                    return neighborPos;
                }
                
                // 加入队列继续搜索
                queue.offer(neighborPos);
            }
        }
        
        System.out.println(" 未找到满足安全距离的避让位置，降低安全要求重试");
        
        // 如果找不到满足安全距离的位置，降低要求再次搜索（至少保证2步距离）
        return findYieldTargetPositionWithReducedSafety(currentPos, blockedPositions, 2);
    }
    
    /**
     * 检查候选位置与所有被占用位置是否满足安全距离
     */
    private boolean isSafeDistanceFromBlockedPositions(String candidatePos, 
                                                       Set<String> blockedPositions, 
                                                       int minSteps) {
        // 获取候选位置的坐标
        int[] candidateCoord = JsonUtils.getCoordinate(candidatePos, envDataConfig.getPathMapping());
        if (candidateCoord == null) {
            return false;
        }
        
        // 检查与每个被占用位置的距离
        for (String blockedPos : blockedPositions) {
            int[] blockedCoord = JsonUtils.getCoordinate(blockedPos, envDataConfig.getPathMapping());
            if (blockedCoord == null) {
                continue;
            }
            
            // 计算曼哈顿距离
            int distance = Math.abs(candidateCoord[0] - blockedCoord[0]) + 
                          Math.abs(candidateCoord[1] - blockedCoord[1]);
            
            if (distance < minSteps) {
                return false;
            }
        }
        
        return true;
    }
    
    /**
     * 使用降低的安全要求查找让行位置（备选）
     * 
     * @param currentPos 当前位置
     * @param blockedPositions 被占用的位置集合
     * @param minSteps 最小步数要求
     * @return 让行目标位置
     */
    private String findYieldTargetPositionWithReducedSafety(String currentPos, 
                                                            Set<String> blockedPositions, 
                                                            int minSteps) {
        final int MAX_SEARCH_DEPTH = 10;
        
        Queue<String> queue = new LinkedList<>();
        Map<String, Integer> visited = new HashMap<>();
        
        queue.offer(currentPos);
        visited.put(currentPos, 0);
        
        while (!queue.isEmpty()) {
            String pos = queue.poll();
            int depth = visited.get(pos);

            if (depth >= MAX_SEARCH_DEPTH) {
                break;
            }

            List<Map<String, String>> neighbors = pathPlanner.getNeighbors(pos);
            for (Map<String, String> neighbor : neighbors) {
                String neighborPos = neighbor.get("code");
                
                if (neighborPos == null || visited.containsKey(neighborPos)) {
                    continue;
                }
                
                visited.put(neighborPos, depth + 1);
                
                // 使用降低的安全要求
                if (!blockedPositions.contains(neighborPos) && 
                    isSafeDistanceFromBlockedPositions(neighborPos, blockedPositions, minSteps)) {
                    System.out.println("  找到降级安全避让位置: " + neighborPos + 
                                     " (距离=" + (depth + 1) + "步, 最小安全距离=" + minSteps + "步)");
                    return neighborPos;
                }
                
                queue.offer(neighborPos);
            }
        }
        
        System.out.println("  无法找到避让位置");
        return null;
    }

    private void enhancePathCodesForYielding(PlannedPath yieldPath) {
        if (yieldPath == null || yieldPath.getCodeList() == null) {
            return;
        }
        
        List<PathCode> codes = yieldPath.getCodeList();
        for (int i = 0; i < codes.size(); i++) {
            PathCode code = codes.get(i);
            code.setActionType("0"); 
            code.setPosType(null);
            code.setLev(0);
            code.setTargetPoint(i == codes.size() - 1); 
        }
    }

    /**
     * 路径规划结果类
     */
    public static class PathPlanningResult {
        private int totalAgvs;
        private int executingTasksCount;
        private int plannedPathsCount;
        private double totalPlanningTime;
        private int conflictsDetected;
        private boolean collisionFree;
        private String algorithm;
        private List<PlannedPath> plannedPaths;
        private List<ExecutingTask> executingTasks;

        public PathPlanningResult(int totalAgvs, int executingTasksCount, int plannedPathsCount,
                                  double totalPlanningTime, int conflictsDetected, boolean collisionFree,
                                  String algorithm, List<PlannedPath> plannedPaths, List<ExecutingTask> executingTasks) {
            this.totalAgvs = totalAgvs;
            this.executingTasksCount = executingTasksCount;
            this.plannedPathsCount = plannedPathsCount;
            this.totalPlanningTime = totalPlanningTime;
            this.conflictsDetected = conflictsDetected;
            this.collisionFree = collisionFree;
            this.algorithm = algorithm;
            this.plannedPaths = plannedPaths;
            this.executingTasks = executingTasks;
        }

        // Getter方法
        public int getTotalAgvs() {
            return totalAgvs;
        }

        public int getExecutingTasksCount() {
            return executingTasksCount;
        }

        public int getPlannedPathsCount() {
            return plannedPathsCount;
        }

        public double getTotalPlanningTime() {
            return totalPlanningTime;
        }

        public int getConflictsDetected() {
            return conflictsDetected;
        }

        public boolean isCollisionFree() {
            return collisionFree;
        }

        public String getAlgorithm() {
            return algorithm;
        }

        public List<PlannedPath> getPlannedPaths() {
            return plannedPaths;
        }

        public List<ExecutingTask> getExecutingTasks() {
            return executingTasks;
        }

        @Override
        public String toString() {
            return "PathPlanningResult{" +
                    "totalAgvs=" + totalAgvs +
                    ", executingTasksCount=" + executingTasksCount +
                    ", plannedPathsCount=" + plannedPathsCount +
                    ", executingTasksCount=" + executingTasksCount +
                    ", totalPlanningTime=" + totalPlanningTime +
                    ", conflictsDetected=" + conflictsDetected +
                    ", collisionFree=" + executingTasksCount +
                    ", algorithm=" + algorithm +
                    ", plannedPaths=" + plannedPaths +
                    ", executingTasks='" + executingTasks + '\'' +
                    '}';
        }
    }

    /**
     * 并行规划任务路径
     */
    private List<PlannedPath> planTasksInParallel(List<ExecutingTask> tasks,
                                                  List<double[]> constraints,
                                                  Map<String, String> spaceTimeOccupancyMap,
                                                  List<AGVStatus> agvStatusList,
                                                  long unifiedTimestamp) {

        List<PlannedPath> allPaths = Collections.synchronizedList(new ArrayList<>());

        // 按优先级排序任务
        List<ExecutingTask> sortedTasks = new ArrayList<>(tasks);
        sortedTasks.sort((t1, t2) -> Integer.compare(t2.getPriority(), t1.getPriority()));

        // 分批处理
        List<List<ExecutingTask>> batches = createBatches(sortedTasks, batchSize);

        for (int batchIndex = 0; batchIndex < batches.size(); batchIndex++) {
            List<ExecutingTask> batch = batches.get(batchIndex);

            System.out.println("处理批次 " + (batchIndex + 1) + "/" + batches.size() +
                    ", 任务数: " + batch.size());

            List<Future<PlannedPath>> futures = new ArrayList<>();

            for (ExecutingTask task : batch) {
                Future<PlannedPath> future = executorService.submit(() -> {
                    return planSingleTaskPath(task, constraints, spaceTimeOccupancyMap, agvStatusList, unifiedTimestamp);
                });
                futures.add(future);
            }

            for (Future<PlannedPath> future : futures) {
                try {
                    PlannedPath path = future.get(60, TimeUnit.SECONDS);
                    if (path != null) {
                        allPaths.add(path);
                        System.out.println("CTU " + path.getAgvId() + " 路径规划成功");
                    }
                } catch (TimeoutException e) {
                    System.out.println("路径规划超时，跳过");
                    future.cancel(true);
                } catch (Exception e) {
                    System.out.println("路径规划异常: " + e.getMessage());
                }
            }
        }

        return allPaths;
    }

    /**
     * 为单个任务规划路径
     */
    private PlannedPath planSingleTaskPath(ExecutingTask task,
                                           List<double[]> constraints,
                                           Map<String, String> spaceTimeOccupancyMap,
                                           List<AGVStatus> agvStatusList,
                                           long unifiedTimestamp) {

        String agvId = task.getAgvId();
        String currentPos = task.getCurrentPosition();
        String targetPos = task.getTargetPosition();

        if (currentPos == null || targetPos == null) {
            return null;
        }

        // 获取CTU状态信息
        AGVStatus agvStatus = findAgvStatus(agvId, agvStatusList);
        if (agvStatus == null) {
            return null;
        }

        // 规划时空安全的路径（使用统一时间戳）
        PlannedPath plannedPath = planPathWithSpaceTimeConstraints(
                currentPos, targetPos, constraints, spaceTimeOccupancyMap, agvStatus, unifiedTimestamp
        );

        if (plannedPath != null) {
            // 设置CTU信息
            plannedPath.setAgvId(agvId);
            plannedPath.setSegId(generateSegId(task.getTaskId(), agvId, task.getTaskType()));

            enhancePathCodes(plannedPath, task);
        }

        return plannedPath;
    }

    /**
     * 创建批次
     */
    private List<List<ExecutingTask>> createBatches(List<ExecutingTask> tasks, int batchSize) {
        List<List<ExecutingTask>> batches = new ArrayList<>();

        for (int i = 0; i < tasks.size(); i += batchSize) {
            int end = Math.min(i + batchSize, tasks.size());
            batches.add(new ArrayList<>(tasks.subList(i, end)));
        }

        return batches;
    }

    /**
     * 关闭服务
     */
    public void shutdown() {
        if (executorService != null && !executorService.isShutdown()) {
            executorService.shutdown();
            try {
                if (!executorService.awaitTermination(10, TimeUnit.SECONDS)) {
                    executorService.shutdownNow();
                }
            } catch (InterruptedException e) {
                executorService.shutdownNow();
                Thread.currentThread().interrupt();
            }
        }
    }
}