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.*;

/**
 * 碰撞检测器（增强版）
 * 用于检测CTU路径之间的时空冲突，支持物理参数和高精度时间检测
 */
public class CollisionDetector {

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

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

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

    /**
     * 空间分辨率（米）
     */
    private final double spaceResolution = 0.1; // 10厘米的空间精度

    /**
     * 构造函数
     *
     * @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);
    }

    /**
     * 检测路径冲突（增强版，支持CTU物理参数）
     *
     * @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<EnhancedSpaceTimeNode>> spaceTimeTable = buildEnhancedSpaceTimeTable(plannedPaths);

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

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

        // 检测跟随冲突（CTU距离过近，考虑物理参数）
        List<Conflict> followingConflicts = detectEnhancedFollowingConflicts(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<EnhancedSpaceTimeNode>> buildEnhancedSpaceTimeTable(List<PlannedPath> plannedPaths) {
        Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable = new HashMap<>();

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

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

            // 获取CTU的物理配置（这里需要从某个地方获取，可能需要扩展PlannedPath类）
            CTUPhysicalConfig physicalConfig = getPhysicalConfigForCTU(agvId);

            long currentTime = System.currentTimeMillis(); // 基准时间

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

                int[] coordinates = JsonUtils.getCoordinate(position, pathMapping);
                if (coordinates != null) {
                    // 计算精确的到达时间
                    long arrivalTime = calculateArrivalTime(currentTime, i, codeList, physicalConfig);

                    // 计算停留时间
                    long departureTime = calculateDepartureTime(arrivalTime, pathCode, physicalConfig);

                    // 创建时间段内的多个时间点
                    for (long timePoint = arrivalTime; timePoint <= departureTime; timePoint += timeResolution) {
                        EnhancedSpaceTimeNode node = new EnhancedSpaceTimeNode(
                                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) {
        // 这里应该从配置管理器或数据库获取实际的CTU物理参数
        // 暂时返回默认配置
        CTUPhysicalConfig config = new CTUPhysicalConfig();
        config.setMaxSpeed(2.0);
        config.setNormalSpeed(1.5);
        config.setMaxAcceleration(1.0);
        config.setMaxDeceleration(1.5);
        config.setTurnTime90(2.0);
        config.setTurnTime180(4.0);
        config.setMinSafetyDistance(defaultMinDistance);
        config.setMinFollowingDistance(2.0);
        config.setCtuLength(1.2);
        config.setCtuWidth(0.8);
        config.setStartupTime(1.0);
        config.setStopTime(0.5);
        config.setStandardPointDistance(1.0);
        return config;
    }

    /**
     * 计算到达时间
     *
     * @param 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); // 转换为毫秒

            // 如果有方向变化，增加转向时间
            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> detectEnhancedVertexConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
        List<Conflict> conflicts = new ArrayList<>();

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

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

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

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

                            // 检查时间段是否重叠
                            if (timeRangeOverlap(node1.arrivalTime, node1.departureTime,
                                    node2.arrivalTime, node2.departureTime)) {
                                Conflict conflict = new Conflict(
                                        "enhanced_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> detectEnhancedEdgeConflicts(Map<Long, List<EnhancedSpaceTimeNode>> 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<EnhancedSpaceTimeNode> currentNodes = spaceTimeTable.get(currentTime);
                List<EnhancedSpaceTimeNode> 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<EnhancedSpaceTimeNode> currentNodes,
                                                  List<EnhancedSpaceTimeNode> nextNodes,
                                                  long currentTime, long nextTime) {
        List<Conflict> conflicts = new ArrayList<>();

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

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

        for (EnhancedSpaceTimeNode 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(
                            "enhanced_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;
    }

    /**
     * 检测增强的跟随冲突（考虑CTU物理参数）
     *
     * @param spaceTimeTable 时空表
     * @return 跟随冲突列表
     */
    private List<Conflict> detectEnhancedFollowingConflicts(Map<Long, List<EnhancedSpaceTimeNode>> spaceTimeTable) {
        List<Conflict> conflicts = new ArrayList<>();

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

            for (int i = 0; i < nodes.size(); i++) {
                for (int j = i + 1; j < nodes.size(); j++) {
                    EnhancedSpaceTimeNode node1 = nodes.get(i);
                    EnhancedSpaceTimeNode 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(
                                "enhanced_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);
                    }
                }
            }
        }

        return conflicts;
    }

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

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

            for (int i = 0; i < nodes.size(); i++) {
                for (int j = i + 1; j < nodes.size(); j++) {
                    EnhancedSpaceTimeNode node1 = nodes.get(i);
                    EnhancedSpaceTimeNode 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(EnhancedSpaceTimeNode node1, EnhancedSpaceTimeNode 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 EnhancedSpaceTimeNode {
        final String agvId;
        final String position;
        final int[] coordinates;
        final long timePoint;
        final long arrivalTime;
        final long departureTime;
        final CTUPhysicalConfig physicalConfig;

        public EnhancedSpaceTimeNode(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;
        }
    }
}