package com.algo.service;

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

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;

/**
 * 剩余路径处理器
 * 负责处理CTU未完成路径的续接逻辑，确保新路径与剩余路径的平滑连接
 */
public class RemainingPathProcessor {

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

    /**
     * 系统当前时间基准（毫秒）
     */
    private long systemBaseTime;

    public RemainingPathProcessor(Map<String, Map<String, Integer>> pathMapping) {
        this.pathMapping = pathMapping;
        this.systemBaseTime = System.currentTimeMillis();
    }

    /**
     * 处理所有CTU的剩余路径，构建时空占用表
     *
     * @param agvStatusList CTU状态列表
     * @return 时空占用表，key为"x,y,timeSlot"，value为CTU编号
     */
    public Map<String, String> buildSpaceTimeOccupancyMap(List<AGVStatus> agvStatusList) {
        Map<String, String> occupancyMap = new HashMap<>();

        for (AGVStatus agv : agvStatusList) {
            if (agv.hasRemainingPath()) {
                processRemainingPathOccupancy(agv, occupancyMap);
            }
        }

        return occupancyMap;
    }

    /**
     * 处理单个CTU的剩余路径时空占用
     *
     * @param agv          CTU状态
     * @param occupancyMap 时空占用表
     */
    private void processRemainingPathOccupancy(AGVStatus agv, Map<String, String> occupancyMap) {
        PlannedPath remainingPath = agv.getRemainingPath();
        List<PathCode> codeList = remainingPath.getCodeList();
        CTUPhysicalConfig config = agv.getPhysicalConfig();

        // 从当前位置开始计算时空占用
        long currentTime = agv.getNextPointArrivalTime();
        int startIndex = agv.getCurrentPathIndex();

        for (int i = startIndex; i < codeList.size(); i++) {
            PathCode pathCode = codeList.get(i);
            String position = pathCode.getCode();

            // 获取位置坐标
            int[] coord = JsonUtils.getCoordinate(position, pathMapping);
            if (coord == null) continue;

            // 计算在此位置的停留时间
            double stayDuration = calculateStayDuration(agv, i, codeList, config);

            // 将停留时间转换为时间片
            long startTimeSlot = currentTime / 1000; // 转换为秒
            long endTimeSlot = (long) (currentTime / 1000 + stayDuration);

            // 占用时空
            for (long timeSlot = startTimeSlot; timeSlot <= endTimeSlot; timeSlot++) {
                String spaceTimeKey = coord[0] + "," + coord[1] + "," + timeSlot;
                occupancyMap.put(spaceTimeKey, agv.getAgvId());

                // 考虑CTU的物理尺寸，占用周围的格子
                occupyAdjacentSpaces(coord, timeSlot, agv.getAgvId(), occupancyMap, config);
            }

            // 更新到下一个位置的时间
            currentTime += (long) (stayDuration * 1000);
            if (i < codeList.size() - 1) {
                PathCode nextPathCode = codeList.get(i + 1);
                double travelTime = calculateTravelTime(pathCode, nextPathCode, config);
                currentTime += (long) (travelTime * 1000);
            }
        }
    }

    /**
     * 占用相邻空间（考虑CTU物理尺寸）
     *
     * @param centerCoord  中心坐标
     * @param timeSlot     时间片
     * @param agvId        CTU编号
     * @param occupancyMap 占用表
     * @param config       物理配置
     */
    private void occupyAdjacentSpaces(int[] centerCoord, long timeSlot, String agvId,
                                      Map<String, String> occupancyMap, CTUPhysicalConfig config) {
        // 根据CTU的长度和宽度计算需要占用的格子数
        int lengthGrids = (int) Math.ceil(config.getCtuLength() / config.getStandardPointDistance());
        int widthGrids = (int) Math.ceil(config.getCtuWidth() / config.getStandardPointDistance());

        // 占用周围的格子
        for (int dx = -lengthGrids / 2; dx <= lengthGrids / 2; dx++) {
            for (int dy = -widthGrids / 2; dy <= widthGrids / 2; dy++) {
                if (dx == 0 && dy == 0) continue; // 中心点已经占用

                int adjX = centerCoord[0] + dx;
                int adjY = centerCoord[1] + dy;
                String spaceTimeKey = adjX + "," + adjY + "," + timeSlot;

                // 只有当该位置未被占用时才占用
                if (!occupancyMap.containsKey(spaceTimeKey)) {
                    occupancyMap.put(spaceTimeKey, agvId);
                }
            }
        }
    }

    /**
     * 计算CTU在指定路径点的停留时间
     *
     * @param agv       CTU状态
     * @param pathIndex 路径点索引
     * @param codeList  完整路径代码列表
     * @param config    物理配置
     * @return 停留时间（秒）
     */
    private double calculateStayDuration(AGVStatus agv, int pathIndex,
                                         List<PathCode> codeList, CTUPhysicalConfig config) {
        PathCode currentCode = codeList.get(pathIndex);

        // 基础停留时间
        double stayTime = 0.5; // 默认0.5秒

        // 如果需要转向，增加转向时间
        if (pathIndex < codeList.size() - 1) {
            PathCode nextCode = codeList.get(pathIndex + 1);
            String currentDirection = currentCode.getDirection();
            String nextDirection = nextCode.getDirection();

            if (!currentDirection.equals(nextDirection)) {
                stayTime += config.getTurnTime(currentDirection, nextDirection);
            }
        }

        // 如果是动作点（取货、放货等），增加操作时间
        if (currentCode.getActionType() != null) {
            switch (currentCode.getActionType()) {
                case "1": // 取货
                    stayTime += 3.0; // 取货需要3秒
                    break;
                case "2": // 放货
                    stayTime += 2.0; // 放货需要2秒
                    break;
                case "3": // 充电
                    stayTime += 10.0; // 充电停靠需要10秒
                    break;
                default:
                    stayTime += 1.0; // 其他动作需要1秒
                    break;
            }
        }

        return stayTime;
    }

    /**
     * 计算两个路径点之间的移动时间
     *
     * @param fromCode 起始路径点
     * @param toCode   目标路径点
     * @param config   物理配置
     * @return 移动时间（秒）
     */
    private double calculateTravelTime(PathCode fromCode, PathCode toCode, CTUPhysicalConfig config) {
        // 获取坐标
        int[] fromCoord = JsonUtils.getCoordinate(fromCode.getCode(), pathMapping);
        int[] toCoord = JsonUtils.getCoordinate(toCode.getCode(), pathMapping);

        if (fromCoord == null || toCoord == null) {
            return config.getStandardPointDistance() / config.getNormalSpeed();
        }

        // 计算距离
        double distance = Math.sqrt(Math.pow(toCoord[0] - fromCoord[0], 2) +
                Math.pow(toCoord[1] - fromCoord[1], 2)) *
                config.getStandardPointDistance();

        // 计算移动时间
        return config.getTravelTime(distance, config.getNormalSpeed());
    }

    /**
     * 为新路径查找合适的起始时间，避免与现有路径冲突
     *
     * @param newPath      新路径
     * @param occupancyMap 现有时空占用表
     * @param config       物理配置
     * @return 建议的起始时间（毫秒）
     */
    public long findSafeStartTime(PlannedPath newPath, Map<String, String> occupancyMap,
                                  CTUPhysicalConfig config) {
        if (newPath.getCodeList() == null || newPath.getCodeList().isEmpty()) {
            return systemBaseTime;
        }

        // 从当前时间开始，逐步向后搜索安全的起始时间
        long currentTime = systemBaseTime;
        long timeStep = 1000; // 1秒步长
        long maxSearchTime = currentTime + 300000; // 最多搜索5分钟

        while (currentTime < maxSearchTime) {
            if (isPathTimeSafe(newPath, currentTime, occupancyMap, config)) {
                return currentTime;
            }
            currentTime += timeStep;
        }

        // 如果找不到安全时间，返回较远的未来时间
        return maxSearchTime;
    }

    /**
     * 检查指定时间开始的路径是否安全
     *
     * @param path         路径
     * @param startTime    起始时间
     * @param occupancyMap 占用表
     * @param config       物理配置
     * @return true如果安全
     */
    private boolean isPathTimeSafe(PlannedPath path, long startTime,
                                   Map<String, String> occupancyMap, CTUPhysicalConfig config) {
        List<PathCode> codeList = path.getCodeList();
        long currentTime = startTime;

        for (int i = 0; i < codeList.size(); i++) {
            PathCode pathCode = codeList.get(i);
            int[] coord = JsonUtils.getCoordinate(pathCode.getCode(), pathMapping);
            if (coord == null) continue;

            // 计算在此位置的停留时间
            double stayDuration = 2.0; // 简化计算，使用固定停留时间
            long startTimeSlot = currentTime / 1000;
            long endTimeSlot = (long) (currentTime / 1000 + stayDuration);

            // 检查时空冲突
            for (long timeSlot = startTimeSlot; timeSlot <= endTimeSlot; timeSlot++) {
                String spaceTimeKey = coord[0] + "," + coord[1] + "," + timeSlot;
                if (occupancyMap.containsKey(spaceTimeKey)) {
                    return false; // 发现冲突
                }

                // 检查安全距离内的冲突
                if (!isSafeFromOtherCTUs(coord, timeSlot, occupancyMap, config)) {
                    return false;
                }
            }

            // 更新到下一个位置的时间
            currentTime += (long) (stayDuration * 1000);
            if (i < codeList.size() - 1) {
                currentTime += (long) (config.getStandardPointDistance() / config.getNormalSpeed() * 1000);
            }
        }

        return true;
    }

    /**
     * 检查指定位置和时间是否与其他CTU保持安全距离
     *
     * @param coord        位置坐标
     * @param timeSlot     时间片
     * @param occupancyMap 占用表
     * @param config       物理配置
     * @return true如果安全
     */
    private boolean isSafeFromOtherCTUs(int[] coord, long timeSlot,
                                        Map<String, String> occupancyMap, CTUPhysicalConfig config) {
        int safetyRadius = (int) Math.ceil(config.getMinSafetyDistance() / config.getStandardPointDistance());

        // 检查安全半径内是否有其他CTU
        for (int dx = -safetyRadius; dx <= safetyRadius; dx++) {
            for (int dy = -safetyRadius; dy <= safetyRadius; dy++) {
                if (dx == 0 && dy == 0) continue;

                String checkKey = (coord[0] + dx) + "," + (coord[1] + dy) + "," + timeSlot;
                if (occupancyMap.containsKey(checkKey)) {
                    // 计算实际距离
                    double distance = Math.sqrt(dx * dx + dy * dy) * config.getStandardPointDistance();
                    if (distance < config.getMinSafetyDistance()) {
                        return false; // 距离太近
                    }
                }
            }
        }

        return true;
    }

    /**
     * 连接剩余路径和新路径
     *
     * @param agv     CTU状态
     * @param newPath 新规划的路径
     * @return 连接后的完整路径
     */
    public PlannedPath connectRemainingAndNewPath(AGVStatus agv, PlannedPath newPath) {
        if (!agv.hasRemainingPath()) {
            // 没有剩余路径，直接返回新路径
            return newPath;
        }

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

        // 添加剩余路径（从当前位置开始）
        List<PathCode> originalCodes = remainingPath.getCodeList();
        for (int i = agv.getCurrentPathIndex(); i < originalCodes.size(); i++) {
            remainingCodes.add(originalCodes.get(i));
        }

        // 连接新路径
        if (newPath != null && newPath.getCodeList() != null) {
            // 检查连接点是否一致，避免重复
            if (!remainingCodes.isEmpty() && !newPath.getCodeList().isEmpty()) {
                PathCode lastRemaining = remainingCodes.get(remainingCodes.size() - 1);
                PathCode firstNew = newPath.getCodeList().get(0);

                if (lastRemaining.getCode().equals(firstNew.getCode())) {
                    // 跳过重复的连接点
                    for (int i = 1; i < newPath.getCodeList().size(); i++) {
                        remainingCodes.add(newPath.getCodeList().get(i));
                    }
                } else {
                    // 直接连接
                    remainingCodes.addAll(newPath.getCodeList());
                }
            } else {
                remainingCodes.addAll(newPath.getCodeList());
            }
        }

        // 创建连接后的路径
        PlannedPath connectedPath = new PlannedPath();
        connectedPath.setAgvId(agv.getAgvId());
        connectedPath.setCodeList(remainingCodes);

        // 生成新的段落ID
        String segId = agv.getAgvId() + "_CONNECTED_" + System.currentTimeMillis();
        if (newPath != null && newPath.getSegId() != null) {
            segId = newPath.getSegId() + "_EXTENDED";
        }
        connectedPath.setSegId(segId);

        return connectedPath;
    }

    /**
     * 更新系统基准时间
     *
     * @param baseTime 新的基准时间
     */
    public void updateSystemBaseTime(long baseTime) {
        this.systemBaseTime = baseTime;
    }

    /**
     * 获取CTU预计完成剩余路径的时间
     *
     * @param agv CTU状态
     * @return 预计完成时间（毫秒）
     */
    public long getEstimatedCompletionTime(AGVStatus agv) {
        if (!agv.hasRemainingPath()) {
            return System.currentTimeMillis();
        }

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

        long currentTime = agv.getNextPointArrivalTime();

        // 计算完成剩余路径所需的时间
        for (int i = agv.getCurrentPathIndex(); i < codeList.size(); i++) {
            double stayDuration = calculateStayDuration(agv, i, codeList, config);
            currentTime += (long) (stayDuration * 1000);

            if (i < codeList.size() - 1) {
                PathCode currentCode = codeList.get(i);
                PathCode nextCode = codeList.get(i + 1);
                double travelTime = calculateTravelTime(currentCode, nextCode, config);
                currentTime += (long) (travelTime * 1000);
            }
        }

        return currentTime;
    }
}