package com.algo.service;

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

import java.util.*;

/**
 * 碰撞检测器
 */
public class CollisionDetector {

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

    /**
     * 默认最小安全距离
     */
    private final double defaultMinDistance;

    /**
     * 时间精度（毫秒）
     * 提高时间精度以更准确地检测碰撞
     */
    private final long timeResolution = 50; 

    /**
     * 空间分辨率（米）
     */
    private final double spaceResolution = 0.1; 

    /**
     * 构造函数
     *
     * @param pathMapping        路径映射表
     * @param defaultMinDistance 默认最小安全距离
     */
    public CollisionDetector(Map<String, Map<String, Integer>> pathMapping,
                             double defaultMinDistance) {
        this.pathMapping = pathMapping;
        this.defaultMinDistance = defaultMinDistance;
    }

    /**
     * 构造函数
     *
     * @param pathMapping 路径映射表
     */
    public CollisionDetector(Map<String, Map<String, Integer>> pathMapping) {
        this(pathMapping, 3.0);
    }

    /**
     * 检测路径冲突
     *
     * @param plannedPaths 规划路径列表
     * @return 冲突列表
     */
    public List<Conflict> detectConflicts(List<PlannedPath> plannedPaths) {
        List<Conflict> conflicts = new ArrayList<>();

        if (plannedPaths == null || plannedPaths.size() < 2) {
            return conflicts;
        }

        System.out.println("开始碰撞检测，路径数量: " + plannedPaths.size());

        // 构建时空表
        Map<Long, List<SpaceTimeNode>> spaceTimeTable = buildSpaceTimeTable(plannedPaths);

        // 检测顶点冲突（同一时间同一位置）
        List<Conflict> vertexConflicts = detectVertexConflicts(spaceTimeTable);
        conflicts.addAll(vertexConflicts);
        System.out.println("顶点冲突数量: " + vertexConflicts.size());

        // 检测边冲突（CTU交换位置）
        List<Conflict> edgeConflicts = detectEdgeConflicts(spaceTimeTable);
        conflicts.addAll(edgeConflicts);
        System.out.println("边冲突数量: " + edgeConflicts.size());

        // 检测跟随冲突
        List<Conflict> followingConflicts = detectFollowingConflicts(spaceTimeTable);
        conflicts.addAll(followingConflicts);
        System.out.println("跟随冲突数量: " + followingConflicts.size());

        // 检测物理尺寸冲突
        List<Conflict> physicalConflicts = detectPhysicalSizeConflicts(spaceTimeTable);
        conflicts.addAll(physicalConflicts);
        System.out.println("物理尺寸冲突数量: " + physicalConflicts.size());

        System.out.println("碰撞检测完成，总冲突数量: " + conflicts.size());
        return conflicts;
    }

    /**
     * 构建时空表
     *
     * @param plannedPaths 规划路径列表
     * @return 时空表
     */
    private Map<Long, List<SpaceTimeNode>> buildSpaceTimeTable(List<PlannedPath> plannedPaths) {
        Map<Long, List<SpaceTimeNode>> spaceTimeTable = new HashMap<>();

        for (PlannedPath path : plannedPaths) {
            String agvId = path.getAgvId();
            List<PathCode> codeList = path.getCodeList();

            if (codeList == null || codeList.isEmpty()) {
                continue;
            }

            // 获取CTU的物理配置
            CTUPhysicalConfig physicalConfig = getPhysicalConfigForCTU(agvId);


            for (PathCode pathCode : codeList) {
                String position = pathCode.getCode();
                Long arrivalTime = pathCode.getArrivalTime();
                Long departureTime = pathCode.getDepartureTime();

                // 如果时间戳为空，跳过
                if (arrivalTime == null || departureTime == null) {
                    System.out.println("警告: AGV " + agvId + " 在位置 " + position + " 的时间戳为空，跳过");
                    continue;
                }

                int[] coordinates = JsonUtils.getCoordinate(position, pathMapping);
                if (coordinates != null) {
                    // 创建时间段内的多个时间点（使用实际时间戳）
                    for (long timePoint = arrivalTime; timePoint <= departureTime; timePoint += timeResolution) {
                        SpaceTimeNode node = new SpaceTimeNode(
                                agvId, position, coordinates, timePoint, arrivalTime, departureTime, physicalConfig
                        );

                        spaceTimeTable.computeIfAbsent(timePoint, k -> new ArrayList<>()).add(node);
                    }
                }
            }
        }

        return spaceTimeTable;
    }

    /**
     * 获取CTU的物理配置
     *
     * @param agvId CTU编号
     * @return 物理配置
     */
    private CTUPhysicalConfig getPhysicalConfigForCTU(String agvId) {
        // 从AGV状态中获取
        CTUPhysicalConfig config = new CTUPhysicalConfig();
        // 标准配置
        // maxSpeed=1.0, normalSpeed=1.0, maxAcceleration=0.4, maxDeceleration=0.4
        // turnTime90=4.0, turnTime180=8.0, minSafetyDistance=3.0, minFollowingDistance=3.0
        // ctuLength=1.5, ctuWidth=1.0, startupTime=1.0, stopTime=1.5, standardPointDistance=1.0
        return config; // 使用默认构造函数的标准配置
    }

    /**
     * 计算到达时间
     *
     * @param startTime 起始时间
     * @param pathIndex 路径索引
     * @param codeList  路径代码列表
     * @param config    物理配置
     * @return 到达时间
     */
    private long calculateArrivalTime(long startTime, int pathIndex, List<PathCode> codeList, CTUPhysicalConfig config) {
        if (pathIndex == 0) {
            return startTime;
        }

        long travelTime = 0;

        for (int i = 1; i <= pathIndex; i++) {
            PathCode currentCode = codeList.get(i);
            PathCode previousCode = codeList.get(i - 1);

            // 计算移动时间
            double distance = config.getStandardPointDistance();
            double speed = config.getNormalSpeed();
            travelTime += (long) ((distance / speed) * 1000); // 转换为毫秒

            // 如果有方向变化，增加转向时间
            // 添加null检查
            if (currentCode.getDirection() != null && previousCode.getDirection() != null) {
                if (!currentCode.getDirection().equals(previousCode.getDirection())) {
                    double turnTime = calculateTurnTime(previousCode.getDirection(), currentCode.getDirection(), config);
                    travelTime += (long) (turnTime * 1000);
                }
            }

            // 考虑加速和减速时间
            if (i == 1) {
                travelTime += (long) (config.getStartupTime() * 1000);
            }
        }

        return startTime + travelTime;
    }

    /**
     * 计算离开时间
     *
     * @param arrivalTime 到达时间
     * @param pathCode    路径代码
     * @param config      物理配置
     * @return 离开时间
     */
    private long calculateDepartureTime(long arrivalTime, PathCode pathCode, CTUPhysicalConfig config) {
        long stayTime = 500; // 默认停留时间500毫秒

        // 根据路径代码类型确定停留时间
        if (pathCode.isTargetPoint()) {
            if ("1".equals(pathCode.getPosType())) { // 取货
                stayTime = 3000; // 取货需要3秒
            } else if ("2".equals(pathCode.getPosType())) { // 放货
                stayTime = 2000; // 放货需要2秒
            }
        }

        return arrivalTime + stayTime;
    }

    /**
     * 计算转向时间
     *
     * @param fromDirection 起始方向
     * @param toDirection   目标方向
     * @param config        物理配置
     * @return 转向时间（秒）
     */
    private double calculateTurnTime(String fromDirection, String toDirection, CTUPhysicalConfig config) {
        if (fromDirection.equals(toDirection)) {
            return 0.0;
        }

        // 方向转换计算
        int angleDiff = Math.abs(Integer.parseInt(toDirection) - Integer.parseInt(fromDirection));
        angleDiff = Math.min(angleDiff, 4 - angleDiff); // 考虑环形

        switch (angleDiff) {
            case 1:
                return config.getTurnTime90();
            case 2:
                return config.getTurnTime180();
            default:
                return config.getTurnTime90() * angleDiff;
        }
    }

    /**
     * 检测顶点冲突
     *
     * @param spaceTimeTable 时空表
     * @return 顶点冲突列表
     */
    private List<Conflict> detectVertexConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) {
        List<Conflict> conflicts = new ArrayList<>();

        for (Map.Entry<Long, List<SpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
            long timePoint = entry.getKey();
            List<SpaceTimeNode> nodes = entry.getValue();

            // 按位置分组
            Map<String, List<SpaceTimeNode>> positionGroups = new HashMap<>();
            for (SpaceTimeNode node : nodes) {
                positionGroups.computeIfAbsent(node.position, k -> new ArrayList<>()).add(node);
            }

            // 检查每个位置是否有多个CTU
            for (Map.Entry<String, List<SpaceTimeNode>> posEntry : positionGroups.entrySet()) {
                String position = posEntry.getKey();
                List<SpaceTimeNode> ctuNodes = posEntry.getValue();

                if (ctuNodes.size() > 1) {
                    // 进一步检查时间重叠
                    for (int i = 0; i < ctuNodes.size(); i++) {
                        for (int j = i + 1; j < ctuNodes.size(); j++) {
                            SpaceTimeNode node1 = ctuNodes.get(i);
                            SpaceTimeNode node2 = ctuNodes.get(j);

                            // 检查时间段是否重叠
                            if (timeRangeOverlap(node1.arrivalTime, node1.departureTime,
                                    node2.arrivalTime, node2.departureTime)) {
                                Conflict conflict = new Conflict(
                                        "vertex",
                                        node1.agvId,
                                        node2.agvId,
                                        (int) (timePoint / 1000), // 转换为秒用于显示
                                        position,
                                        position,
                                        String.format("CTU %s 和 %s 在时间 %d 占用同一位置 %s（时间重叠）",
                                                node1.agvId, node2.agvId, timePoint / 1000, position)
                                );
                                conflicts.add(conflict);
                            }
                        }
                    }
                }
            }
        }

        return conflicts;
    }

    /**
     * 检测边冲突
     *
     * @param spaceTimeTable 时空表
     * @return 边冲突列表
     */
    private List<Conflict> detectEdgeConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) {
        List<Conflict> conflicts = new ArrayList<>();

        if (spaceTimeTable.isEmpty()) {
            return conflicts;
        }

        List<Long> sortedTimes = new ArrayList<>(spaceTimeTable.keySet());
        Collections.sort(sortedTimes);

        for (int i = 0; i < sortedTimes.size() - 1; i++) {
            long currentTime = sortedTimes.get(i);
            long nextTime = sortedTimes.get(i + 1);

            // 只检查连续的时间点
            if (nextTime - currentTime <= timeResolution * 2) {
                List<SpaceTimeNode> currentNodes = spaceTimeTable.get(currentTime);
                List<SpaceTimeNode> nextNodes = spaceTimeTable.get(nextTime);

                conflicts.addAll(detectPositionSwapping(currentNodes, nextNodes, currentTime, nextTime));
            }
        }

        return conflicts;
    }

    /**
     * 检测位置交换
     *
     * @param currentNodes 当前时间的节点
     * @param nextNodes    下一时间的节点
     * @param currentTime  当前时间
     * @param nextTime     下一时间
     * @return 冲突列表
     */
    private List<Conflict> detectPositionSwapping(List<SpaceTimeNode> currentNodes,
                                                  List<SpaceTimeNode> nextNodes,
                                                  long currentTime, long nextTime) {
        List<Conflict> conflicts = new ArrayList<>();

        Map<String, String> currentPositions = new HashMap<>();
        Map<String, String> nextPositions = new HashMap<>();

        for (SpaceTimeNode node : currentNodes) {
            currentPositions.put(node.agvId, node.position);
        }

        for (SpaceTimeNode node : nextNodes) {
            nextPositions.put(node.agvId, node.position);
        }

        for (Map.Entry<String, String> entry1 : currentPositions.entrySet()) {
            String ctu1 = entry1.getKey();
            String pos1Current = entry1.getValue();

            for (Map.Entry<String, String> entry2 : currentPositions.entrySet()) {
                String ctu2 = entry2.getKey();
                String pos2Current = entry2.getValue();

                if (ctu1.compareTo(ctu2) >= 0) {
                    continue;
                }

                String pos1Next = nextPositions.get(ctu1);
                String pos2Next = nextPositions.get(ctu2);

                if (pos1Next != null && pos2Next != null &&
                        pos1Current.equals(pos2Next) && pos2Current.equals(pos1Next)) {

                    Conflict conflict = new Conflict(
                            "edge",
                            ctu1,
                            ctu2,
                            (int) (currentTime / 1000),
                            pos1Current,
                            pos2Current,
                            String.format("CTU %s 和 %s 在时间 %d-%d 交换位置 %s<->%s",
                                    ctu1, ctu2, currentTime / 1000, nextTime / 1000, pos1Current, pos2Current)
                    );
                    conflicts.add(conflict);
                }
            }
        }

        return conflicts;
    }

    /**
     * 检测跟随冲突
     *
     * @param spaceTimeTable 时空表
     * @return 跟随冲突列表
     */
    private List<Conflict> detectFollowingConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) {
        List<Conflict> conflicts = new ArrayList<>();

        // 第一部分：检查同一时间点的节点
        for (Map.Entry<Long, List<SpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
            long timePoint = entry.getKey();
            List<SpaceTimeNode> nodes = entry.getValue();

            for (int i = 0; i < nodes.size(); i++) {
                for (int j = i + 1; j < nodes.size(); j++) {
                    SpaceTimeNode node1 = nodes.get(i);
                    SpaceTimeNode node2 = nodes.get(j);

                    double distance = calculateDistance(node1.coordinates, node2.coordinates);
                    double minSafeDistance = Math.max(
                            node1.physicalConfig.getMinFollowingDistance(),
                            node2.physicalConfig.getMinFollowingDistance()
                    );

                    if (distance > 0 && distance < minSafeDistance) {
                        Conflict conflict = new Conflict(
                                "follow",
                                node1.agvId,
                                node2.agvId,
                                (int) (timePoint / 1000),
                                node1.position,
                                node2.position,
                                String.format("CTU %s 和 %s 在时间 %d 距离过近 (%.2f < %.2f)",
                                        node1.agvId, node2.agvId, timePoint / 1000, distance, minSafeDistance)
                        );
                        conflicts.add(conflict);
                    }

                    // 检查同一时间点的时间范围重叠（同时占用）
                    if (distance <= minSafeDistance && timeRangeOverlap(
                            node1.arrivalTime, node1.departureTime,
                            node2.arrivalTime, node2.departureTime)) {
                        double normalSpeed = Math.min(
                                node1.physicalConfig.getNormalSpeed(),
                                node2.physicalConfig.getNormalSpeed()
                        );
                        long minTimeGap = (long) ((minSafeDistance / normalSpeed + 10.0) * 1000);
                        
                        long timeGap = Math.abs(node1.arrivalTime - node2.arrivalTime);
                        if (timeGap < minTimeGap) {
                            Conflict conflict = new Conflict(
                                    "time_gap_insufficient",
                                    node1.agvId,
                                    node2.agvId,
                                    (int) (timePoint / 1000),
                                    node1.position,
                                    node2.position,
                                    String.format("CTU %s 和 %s 在位置 %s/%s 同时占用且时间间隔不足 (%.2f秒 < %.2f秒)",
                                            node1.agvId, node2.agvId, node1.position, node2.position,
                                            timeGap / 1000.0, minTimeGap / 1000.0)
                            );
                            conflicts.add(conflict);
                        }
                    }
                }
            }
        }

        // 第二部分：检查同一位置连续经过的时间间隔
        Map<String, Map<String, SpaceTimeNode>> positionAgvMap = new HashMap<>();
        for (List<SpaceTimeNode> nodes : spaceTimeTable.values()) {
            for (SpaceTimeNode node : nodes) {
                positionAgvMap.computeIfAbsent(node.position, k -> new HashMap<>())
                        .compute(node.agvId, (k, existing) -> {
                            if (existing == null) {
                                return node;
                            } else {
                                // 合并时间范围
                                long minArrival = Math.min(existing.arrivalTime, node.arrivalTime);
                                long maxDeparture = Math.max(existing.departureTime, node.departureTime);
                                return new SpaceTimeNode(
                                        node.agvId, node.position, node.coordinates,
                                        minArrival, minArrival, maxDeparture, node.physicalConfig
                                );
                            }
                        });
            }
        }

        // 对于每个位置，检查所有经过该位置的AGV的时间间隔
        Set<String> processedPairs = new HashSet<>(); 
        for (Map.Entry<String, Map<String, SpaceTimeNode>> positionEntry : positionAgvMap.entrySet()) {
            String position = positionEntry.getKey();
            Map<String, SpaceTimeNode> agvNodes = positionEntry.getValue();

            List<SpaceTimeNode> nodes = new ArrayList<>(agvNodes.values());
            nodes.sort((n1, n2) -> Long.compare(n1.arrivalTime, n2.arrivalTime));

            for (int i = 0; i < nodes.size(); i++) {
                for (int j = i + 1; j < nodes.size(); j++) {
                    SpaceTimeNode node1 = nodes.get(i);
                    SpaceTimeNode node2 = nodes.get(j);

                    if (node1.agvId.equals(node2.agvId)) {
                        continue;
                    }

                    String pairKey1 = node1.agvId + "_" + node2.agvId + "_" + position;
                    String pairKey2 = node2.agvId + "_" + node1.agvId + "_" + position;
                    if (processedPairs.contains(pairKey1) || processedPairs.contains(pairKey2)) {
                        continue; 
                    }

                    // 计算时间间隔：node1离开后，node2到达的时间间隔
                    long timeGap;
                    if (node1.departureTime <= node2.arrivalTime) {
                        timeGap = node2.arrivalTime - node1.departureTime;
                    } else if (node2.departureTime <= node1.arrivalTime) {
                        timeGap = node1.arrivalTime - node2.departureTime;
                    } else {
                        continue;
                    }

                    // 计算最小安全时间间隔
                    double minSafeDistance = Math.max(
                            node1.physicalConfig.getMinSafetyDistance(),
                            node2.physicalConfig.getMinSafetyDistance()
                    );
                    double normalSpeed = Math.min(
                            node1.physicalConfig.getNormalSpeed(),
                            node2.physicalConfig.getNormalSpeed()
                    );
                    // 最小时间间隔 = 安全距离/速度 + 额外安全缓冲(7秒)
                    long minTimeGap = (long) ((minSafeDistance / normalSpeed + 7.0) * 1000);

                    // 检查时间间隔是否足够
                    if (timeGap < minTimeGap) {
                        String firstAgv = node1.arrivalTime < node2.arrivalTime ? node1.agvId : node2.agvId;
                        String secondAgv = node1.arrivalTime < node2.arrivalTime ? node2.agvId : node1.agvId;
                        long secondArrival = node1.arrivalTime < node2.arrivalTime ? node2.arrivalTime : node1.arrivalTime;

                        Conflict conflict = new Conflict(
                                "time_gap_insufficient",
                                firstAgv,
                                secondAgv,
                                (int) (secondArrival / 1000),
                                position,
                                position,
                                String.format("CTU %s 和 %s 在位置 %s 连续经过时间间隔不足 (%.2f秒 < %.2f秒, %s离开后%.2f秒%s到达)",
                                        firstAgv, secondAgv, position,
                                        timeGap / 1000.0, minTimeGap / 1000.0,
                                        firstAgv, timeGap / 1000.0, secondAgv)
                        );
                        conflicts.add(conflict);
                        processedPairs.add(pairKey1); 
                    }
                }
            }
        }

        return conflicts;
    }

    /**
     * 检测物理尺寸冲突
     *
     * @param spaceTimeTable 时空表
     * @return 物理冲突列表
     */
    private List<Conflict> detectPhysicalSizeConflicts(Map<Long, List<SpaceTimeNode>> spaceTimeTable) {
        List<Conflict> conflicts = new ArrayList<>();

        for (Map.Entry<Long, List<SpaceTimeNode>> entry : spaceTimeTable.entrySet()) {
            long timePoint = entry.getKey();
            List<SpaceTimeNode> nodes = entry.getValue();

            for (int i = 0; i < nodes.size(); i++) {
                for (int j = i + 1; j < nodes.size(); j++) {
                    SpaceTimeNode node1 = nodes.get(i);
                    SpaceTimeNode node2 = nodes.get(j);

                    if (checkPhysicalCollision(node1, node2)) {
                        Conflict conflict = new Conflict(
                                "physical_size",
                                node1.agvId,
                                node2.agvId,
                                (int) (timePoint / 1000),
                                node1.position,
                                node2.position,
                                String.format("CTU %s 和 %s 在时间 %d 发生物理尺寸冲突",
                                        node1.agvId, node2.agvId, timePoint / 1000)
                        );
                        conflicts.add(conflict);
                    }
                }
            }
        }

        return conflicts;
    }

    /**
     * 检查两个CTU是否发生物理碰撞
     *
     * @param node1 CTU节点1
     * @param node2 CTU节点2
     * @return 是否碰撞
     */
    private boolean checkPhysicalCollision(SpaceTimeNode node1, SpaceTimeNode node2) {
        double distance = calculateDistance(node1.coordinates, node2.coordinates);

        // 考虑CTU的物理尺寸
        double ctu1Radius = Math.max(node1.physicalConfig.getCtuLength(), node1.physicalConfig.getCtuWidth()) / 2;
        double ctu2Radius = Math.max(node2.physicalConfig.getCtuLength(), node2.physicalConfig.getCtuWidth()) / 2;

        double minClearance = ctu1Radius + ctu2Radius + spaceResolution;

        return distance > 0 && distance < minClearance;
    }

    /**
     * 检查时间范围是否重叠
     *
     * @param start1 开始时间1
     * @param end1   结束时间1
     * @param start2 开始时间2
     * @param end2   结束时间2
     * @return 是否重叠
     */
    private boolean timeRangeOverlap(long start1, long end1, long start2, long end2) {
        return Math.max(start1, start2) < Math.min(end1, end2);
    }

    /**
     * 计算两个坐标之间的距离
     *
     * @param coord1 坐标1
     * @param coord2 坐标2
     * @return 距离值
     */
    private double calculateDistance(int[] coord1, int[] coord2) {
        if (coord1 == null || coord2 == null || coord1.length != 2 || coord2.length != 2) {
            return -1;
        }

        return Math.sqrt(Math.pow(coord1[0] - coord2[0], 2) + Math.pow(coord1[1] - coord2[1], 2));
    }

    /**
     * 时空节点内部类
     */
    private static class SpaceTimeNode {
        final String agvId;
        final String position;
        final int[] coordinates;
        final long timePoint;
        final long arrivalTime;
        final long departureTime;
        final CTUPhysicalConfig physicalConfig;

        public SpaceTimeNode(String agvId, String position, int[] coordinates,
                             long timePoint, long arrivalTime, long departureTime,
                             CTUPhysicalConfig physicalConfig) {
            this.agvId = agvId;
            this.position = position;
            this.coordinates = coordinates;
            this.timePoint = timePoint;
            this.arrivalTime = arrivalTime;
            this.departureTime = departureTime;
            this.physicalConfig = physicalConfig;
        }
    }
}