package lujing;

import org.locationtech.jts.algorithm.Angle;
import org.locationtech.jts.geom.*;
import org.locationtech.jts.operation.distance.DistanceOp;
import org.locationtech.jts.operation.union.CascadedPolygonUnion;

import java.util.*;
import java.util.stream.Collectors;

/**
 * 障碍物路径规划器
 * * 优化方案：
 * 1. 预规划：先不考虑障碍物，规划出覆盖全图的平行线路径（弓字形）。
 * 2. 几何切割：根据障碍物的外扩（安全距离）几何体，将路径切断，移除落在障碍物内的部分。
 * 3. 区域重组：将剩余的路径段按连通性聚类为若干个“连续区域”，每个区域内部通过弓字形连接。
 * 4. 全局连接：使用避障算法（A*可视图）将这些孤立的区域串联起来。
 * * * 修复问题：
 * 1. 修复路径穿越障碍物的问题（通过更严格的 Interior Intersection 检查）。
 * 2. 修复路径超出地块边界的问题（通过加入 Boundary Covers 约束）。
 * 3. [本次修复] 修复 IntersectionMatrix 类型报错，使用正确的矩阵单元格检查替代不存在的方法。
 * * * 二次优化（针对用户反馈）：
 * 1. 引入 Tolerance Buffer (内缩) 用于相交检测，解决“接触即相交”导致的合法边缘路径被误判问题。
 * 2. 增强 isLineSafe 的边界约束，确保路径严格在 Boundary 内部。
 * 3. 优化 findSafePath，增加点位校验与吸附（Snap），防止因浮点误差导致的寻路失败而回退到直线。
 */
public class ObstaclePathPlanner {
    private final List<Coordinate> polygon;           // 割草区域边界点集
    private final double width;                      // 割草宽度
    private final String mode;                       // 模式
    private final List<List<Coordinate>> obstacles;   // 障碍物列表
    private final double safetyDistance;             // 安全距离
    private final GeometryFactory gf = new GeometryFactory();
    
    // 保存地块的几何形状，用于边界约束检查
    private Geometry boundaryGeom;
    // 用于检测的障碍物几何体（可能经过微调）
    private Geometry checkObstacleGeom;

    public ObstaclePathPlanner(List<Coordinate> polygon, double width, String mode,
                               List<List<Coordinate>> obstacles, double safetyDistance) {
        this.polygon = polygon;
        this.width = width;
        this.mode = mode;
        this.obstacles = obstacles != null ? obstacles : new ArrayList<>();
        this.safetyDistance = safetyDistance;
        
        initBoundaryGeom();
    }
    
    /**
     * 初始化边界几何体，用于后续的约束检查
     */
    private void initBoundaryGeom() {
        if (polygon == null || polygon.size() < 3) {
            this.boundaryGeom = gf.createPolygon();
            return;
        }
        List<Coordinate> closed = new ArrayList<>(polygon);
        if (!closed.get(0).equals2D(closed.get(closed.size() - 1))) {
            closed.add(new Coordinate(closed.get(0)));
        }
        LinearRing ring = gf.createLinearRing(closed.toArray(new Coordinate[0]));
        Polygon poly = gf.createPolygon(ring);
        // 确保几何有效性
        this.boundaryGeom = poly.isValid() ? poly : poly.buffer(0);
    }

    /**
     * 生成路径的主入口
     */
    public List<Lunjingguihua.PathSegment> generate() {
        // 1. 生成初始的完整平行线路径（忽略障碍物）
        List<Lunjingguihua.PathSegment> fullPath = generateFullPathWithoutObstacles();
        if (fullPath.isEmpty()) return fullPath;

        // 2. 构建障碍物的外扩安全区域 (Buffer)
        Geometry obstacleBuffer = createObstacleBuffer();
        
        // 初始化用于检测的几何体（内缩一点点，允许路径贴边）
        if (!obstacleBuffer.isEmpty()) {
            this.checkObstacleGeom = obstacleBuffer.buffer(-0.01); // 内缩1cm，容忍浮点误差
        } else {
            this.checkObstacleGeom = obstacleBuffer;
        }

        if (obstacleBuffer.isEmpty()) return fullPath;

        // 3. 切割路径：移除与障碍物重叠的部分
        List<Lunjingguihua.PathSegment> clippedSegments = clipPathWithObstacles(fullPath, obstacleBuffer);
        if (clippedSegments.isEmpty()) return new ArrayList<>();

        // 4. 重新规划：将碎片化的线段重组成连续的弓字形路径，并进行避障连接
        List<Lunjingguihua.PathSegment> finalPath = reorganizeAndConnectPath(clippedSegments, obstacleBuffer);

        // 5. 后处理（标记起终点等）
        postProcessPath(finalPath);

        return finalPath;
    }

    /**
     * 步骤1：调用核心库生成无障碍的平行线路径
     */
    private List<Lunjingguihua.PathSegment> generateFullPathWithoutObstacles() {
        Lunjingguihua.PlannerCore tempPlanner = new Lunjingguihua.PlannerCore(
                polygon, width, mode, new ArrayList<>());
        return tempPlanner.generateParallelPath();
    }

    /**
     * 步骤2：创建所有障碍物的并集 + 安全距离外扩
     */
    private Geometry createObstacleBuffer() {
        if (obstacles.isEmpty()) return gf.createPolygon();

        List<Geometry> geoms = new ArrayList<>();
        for (List<Coordinate> obs : obstacles) {
            if (obs == null || obs.size() < 3) continue;
            List<Coordinate> closed = new ArrayList<>(obs);
            if (!closed.get(0).equals2D(closed.get(closed.size() - 1))) {
                closed.add(new Coordinate(closed.get(0)));
            }
            LinearRing ring = gf.createLinearRing(closed.toArray(new Coordinate[0]));
            Polygon poly = gf.createPolygon(ring);
            if (!poly.isValid()) poly = (Polygon) poly.buffer(0);
            geoms.add(poly);
        }

        if (geoms.isEmpty()) return gf.createPolygon();

        Geometry union = CascadedPolygonUnion.union(geoms);
        // 对合并后的障碍物进行外扩（Buffer）
        Geometry buffered = union.buffer(safetyDistance, 8); 
        return buffered.isValid() ? buffered : buffered.buffer(0);
    }

    /**
     * 步骤3：用障碍物几何体切割路径
     */
    private List<Lunjingguihua.PathSegment> clipPathWithObstacles(List<Lunjingguihua.PathSegment> fullPath, Geometry obstacleBuffer) {
        List<Lunjingguihua.PathSegment> validSegments = new ArrayList<>();

        for (Lunjingguihua.PathSegment seg : fullPath) {
            if (!seg.isMowing) continue;

            LineString line = gf.createLineString(new Coordinate[]{seg.start, seg.end});
            Geometry diff;
            try {
                diff = line.difference(obstacleBuffer);
            } catch (Exception e) {
                continue; 
            }

            if (diff.isEmpty()) continue;

            for (int i = 0; i < diff.getNumGeometries(); i++) {
                Geometry g = diff.getGeometryN(i);
                if (g instanceof LineString) {
                    Coordinate[] coords = g.getCoordinates();
                    for (int k = 0; k < coords.length - 1; k++) {
                        Coordinate p1 = coords[k];
                        Coordinate p2 = coords[k+1];
                        if (p1.distance(p2) > 0.1) {
                            validSegments.add(new Lunjingguihua.PathSegment(p1, p2, true));
                        }
                    }
                }
            }
        }
        return validSegments;
    }

    /**
     * 步骤4：核心重组逻辑
     */
    private List<Lunjingguihua.PathSegment> reorganizeAndConnectPath(List<Lunjingguihua.PathSegment> segments, Geometry obstacleBuffer) {
        if (segments.isEmpty()) return new ArrayList<>();

        // --- A. 分析扫描线方向 ---
        Lunjingguihua.PathSegment firstSeg = segments.get(0);
        double angle = Math.atan2(firstSeg.end.y - firstSeg.start.y, firstSeg.end.x - firstSeg.start.x);
        double sin = Math.sin(angle);
        double cos = Math.cos(angle);

        // --- B. 给每个线段分配扫描线索引 (Grid Index) ---
        double gridStep = width; 
        
        class IndexedSegment {
            final Lunjingguihua.PathSegment segment;
            final int gridIndex;
            final double projectVal;
            
            IndexedSegment(Lunjingguihua.PathSegment s) {
                this.segment = s;
                double cx = (s.start.x + s.end.x) / 2;
                double cy = (s.start.y + s.end.y) / 2;
                double perpDist = -cx * sin + cy * cos;
                this.gridIndex = (int) Math.floor(perpDist / gridStep);
                this.projectVal = cx * cos + cy * sin;
            }
        }

        List<IndexedSegment> indexedSegments = segments.stream()
                .map(IndexedSegment::new)
                .sorted(Comparator.comparingInt((IndexedSegment s) -> s.gridIndex)
                        .thenComparingDouble(s -> s.projectVal))
                .collect(Collectors.toList());

        // --- C. 构建“区域链” (Zones) ---
        List<List<Lunjingguihua.PathSegment>> zones = new ArrayList<>();
        Set<IndexedSegment> visited = new HashSet<>();
        
        while (visited.size() < indexedSegments.size()) {
            IndexedSegment startNode = null;
            for (IndexedSegment is : indexedSegments) {
                if (!visited.contains(is)) {
                    startNode = is;
                    break;
                }
            }
            if (startNode == null) break;

            List<Lunjingguihua.PathSegment> zone = new ArrayList<>();
            zone.add(startNode.segment);
            visited.add(startNode);
            
            IndexedSegment current = startNode;
            boolean lookingForNext = true;

            while (lookingForNext) {
                IndexedSegment bestNext = null;
                double minDistance = Double.MAX_VALUE;

                // 搜索最佳后续线段
                for (int i = 0; i < indexedSegments.size(); i++) {
                    IndexedSegment candidate = indexedSegments.get(i);
                    if (visited.contains(candidate)) continue;
                    
                    if (Math.abs(candidate.gridIndex - current.gridIndex) > 1) continue;
                    
                    double d = current.segment.end.distance(candidate.segment.start);
                    
                    if (d > width * 3.0) continue; 
                    
                    if (d < minDistance) {
                        // 使用 checkObstacleGeom 进行检测
                        if (isLineSafe(current.segment.end, candidate.segment.start)) {
                            minDistance = d;
                            bestNext = candidate;
                        }
                    }
                }

                if (bestNext != null) {
                    zone.add(bestNext.segment);
                    visited.add(bestNext);
                    current = bestNext;
                } else {
                    lookingForNext = false; 
                }
            }
            zones.add(zone);
        }

        // --- D. 连接所有 Zones ---
        List<Lunjingguihua.PathSegment> resultPath = new ArrayList<>();
        
        List<List<Lunjingguihua.PathSegment>> remainingZones = new ArrayList<>(zones);
        List<Lunjingguihua.PathSegment> currentProcessingZone = remainingZones.remove(0);
        
        addZoneToPath(resultPath, currentProcessingZone, obstacleBuffer, false);

        while (!remainingZones.isEmpty()) {
            Lunjingguihua.PathSegment lastSeg = resultPath.get(resultPath.size() - 1);
            Coordinate currentPos = lastSeg.end;

            int bestZoneIdx = -1;
            double minDist = Double.MAX_VALUE;

            for (int i = 0; i < remainingZones.size(); i++) {
                List<Lunjingguihua.PathSegment> z = remainingZones.get(i);
                if (z.isEmpty()) continue;
                double d = currentPos.distance(z.get(0).start);
                if (d < minDist) {
                    minDist = d;
                    bestZoneIdx = i;
                }
            }

            if (bestZoneIdx != -1) {
                List<Lunjingguihua.PathSegment> nextZone = remainingZones.remove(bestZoneIdx);
                addZoneToPath(resultPath, nextZone, obstacleBuffer, true);
            } else {
                break;
            }
        }

        return resultPath;
    }

    /**
     * 将一个 Zone 添加到结果路径中
     */
    private void addZoneToPath(List<Lunjingguihua.PathSegment> path, 
                               List<Lunjingguihua.PathSegment> zone, 
                               Geometry obstacleBuffer,
                               boolean needConnectToZoneStart) {
        if (zone.isEmpty()) return;

        // 1. 连接到 Zone 的起点
        if (needConnectToZoneStart && !path.isEmpty()) {
            Coordinate from = path.get(path.size() - 1).end;
            Coordinate to = zone.get(0).start;
            List<Coordinate> travel = findSafePath(from, to, obstacleBuffer);
            if (travel.size() > 1) {
                for (int i = 0; i < travel.size() - 1; i++) {
                    path.add(new Lunjingguihua.PathSegment(travel.get(i), travel.get(i+1), false));
                }
            }
        }

        // 2. 处理 Zone 内部
        for (int i = 0; i < zone.size(); i++) {
            Lunjingguihua.PathSegment seg = zone.get(i);
            
            if (i > 0) {
                Coordinate prevEnd = zone.get(i-1).end;
                Coordinate currStart = seg.start;
                if (!prevEnd.equals2D(currStart)) {
                    if (isLineSafe(prevEnd, currStart)) {
                         path.add(new Lunjingguihua.PathSegment(prevEnd, currStart, false));
                    } else {
                        List<Coordinate> detour = findSafePath(prevEnd, currStart, obstacleBuffer);
                        if (detour.size() > 1) {
                            for (int k = 0; k < detour.size() - 1; k++) {
                                path.add(new Lunjingguihua.PathSegment(detour.get(k), detour.get(k+1), false));
                            }
                        }
                    }
                }
            }
            path.add(seg);
        }
    }

    /**
     * 检查两点连线是否安全
     * 修改点：
     * 1. 严格检查 Boundary (Covers)
     * 2. 使用 checkObstacleGeom (内缩版) 检查障碍物，允许贴边
     * 3. [Fix] 使用 matrix.get() 代替不存在的 isIntersects(int)
     */
    private boolean isLineSafe(Coordinate p1, Coordinate p2) {
        if (p1.equals2D(p2)) return true;
        LineString line = gf.createLineString(new Coordinate[]{p1, p2});
        
        // 1. 边界约束：线段必须完全在地块内部
        if (boundaryGeom != null && !boundaryGeom.covers(line)) {
            return false;
        }

        // 2. 避障约束：使用内缩后的 buffer 检查
        // 如果 checkObstacleGeom 为空（无障碍），则安全
        if (checkObstacleGeom == null || checkObstacleGeom.isEmpty()) return true;

        IntersectionMatrix matrix = line.relate(checkObstacleGeom);
        
        // 我们要检查：线段的任何部分（Interior）是否穿过障碍物内部（Interior）
        // 或者 线段的端点（Boundary）是否在障碍物内部（Interior）
        // 如果两者都是 Dimension.FALSE (-1)，则说明没有穿过内部
        
        boolean interiorIntersects = matrix.get(Location.INTERIOR, Location.INTERIOR) != Dimension.FALSE;
        boolean boundaryIntersects = matrix.get(Location.BOUNDARY, Location.INTERIOR) != Dimension.FALSE;

        return !interiorIntersects && !boundaryIntersects;
    }

    /**
     * 寻找两点间的安全路径
     * 修改点：
     * 1. 增加点位校验与吸附（Snap），确保起点终点有效
     * 2. 移除直线强制回退，若寻路失败则返回空（或保留起点），避免穿墙
     */
    private List<Coordinate> findSafePath(Coordinate start, Coordinate end, Geometry obstacleBuffer) {
        // 0. 数据清洗：吸附起点终点到合法区域
        Coordinate safeStart = snapPointToValid(start);
        Coordinate safeEnd = snapPointToValid(end);

        List<Coordinate> path = new ArrayList<>();
        
        // 1. 尝试直连
        if (isLineSafe(safeStart, safeEnd)) {
            path.add(safeStart);
            path.add(safeEnd);
            return path;
        }

        // 2. 构建可视图
        Set<Coordinate> nodes = new HashSet<>();
        nodes.add(safeStart);
        nodes.add(safeEnd);
        
        // 提取障碍物顶点
        addPolygonVertices(obstacleBuffer, nodes);
        // 提取边界顶点（关键：处理凹形边界）
        addPolygonVertices(boundaryGeom, nodes);

        List<Coordinate> nodeList = new ArrayList<>(nodes);
        
        // 构建邻接表
        Map<Coordinate, List<Coordinate>> graph = new HashMap<>();
        for (Coordinate c1 : nodeList) {
            for (Coordinate c2 : nodeList) {
                if (c1 == c2) continue;
                if (isLineSafe(c1, c2)) {
                    graph.computeIfAbsent(c1, k -> new ArrayList<>()).add(c2);
                }
            }
        }

        // Dijkstra 寻路
        Map<Coordinate, Double> dist = new HashMap<>();
        Map<Coordinate, Coordinate> prev = new HashMap<>();
        PriorityQueue<Coordinate> pq = new PriorityQueue<>(Comparator.comparingDouble(dist::get));

        for (Coordinate n : nodeList) dist.put(n, Double.MAX_VALUE);
        dist.put(safeStart, 0.0);
        pq.add(safeStart);

        while (!pq.isEmpty()) {
            Coordinate u = pq.poll();
            if (u.equals2D(safeEnd)) break;
            if (dist.get(u) == Double.MAX_VALUE) break;

            if (graph.containsKey(u)) {
                for (Coordinate v : graph.get(u)) {
                    double alt = dist.get(u) + u.distance(v);
                    if (alt < dist.get(v)) {
                        dist.put(v, alt);
                        prev.put(v, u);
                        pq.add(v); 
                    }
                }
            }
        }

        // 重构路径
        if (prev.containsKey(safeEnd)) {
            LinkedList<Coordinate> p = new LinkedList<>();
            Coordinate curr = safeEnd;
            while (curr != null) {
                p.addFirst(curr);
                curr = prev.get(curr);
            }
            return p;
        }

        // 寻路失败，返回空（避免错误的直线穿越）
        return path;
    }

    // 辅助：验证并吸附点到合法区域
    private Coordinate snapPointToValid(Coordinate p) {
        Point point = gf.createPoint(p);
        boolean inBoundary = (boundaryGeom == null) || boundaryGeom.covers(point);
        boolean outObstacle = (checkObstacleGeom == null) || !checkObstacleGeom.contains(point); // 使用 contains 而不是 intersects interior，稍微严格点

        if (inBoundary && outObstacle) return p;

        // 如果点无效，尝试找最近的有效点（边界或障碍物边缘）
        // 这里简化处理：如果在障碍物内，吸附到障碍物边界；如果在边界外，吸附到边界
        // 实际上 JTS DistanceOp.nearestPoints 可以做这个
        
        Geometry target = boundaryGeom;
        if (!outObstacle && checkObstacleGeom != null) {
            // 在障碍物内，优先吸附出障碍物
             Coordinate[] nearest = DistanceOp.nearestPoints(point, checkObstacleGeom.getBoundary());
             return nearest[1];
        }
        
        if (!inBoundary && boundaryGeom != null) {
             Coordinate[] nearest = DistanceOp.nearestPoints(point, boundaryGeom);
             return nearest[1];
        }
        
        return p;
    }
    
    private void addPolygonVertices(Geometry geom, Set<Coordinate> nodes) {
        if (geom == null) return;
        if (geom instanceof Polygon) {
            Collections.addAll(nodes, ((Polygon) geom).getExteriorRing().getCoordinates());
            for(int i=0; i<((Polygon)geom).getNumInteriorRing(); i++) {
                Collections.addAll(nodes, ((Polygon)geom).getInteriorRingN(i).getCoordinates());
            }
        } else if (geom instanceof MultiPolygon) {
            MultiPolygon mp = (MultiPolygon) geom;
            for (int i = 0; i < mp.getNumGeometries(); i++) {
                addPolygonVertices(mp.getGeometryN(i), nodes);
            }
        } else if (geom instanceof GeometryCollection) {
             GeometryCollection gc = (GeometryCollection) geom;
             for (int i = 0; i < gc.getNumGeometries(); i++) {
                 addPolygonVertices(gc.getGeometryN(i), nodes);
             }
        }
    }

    /**
     * 后处理：移除短线段，标记起终点
     */
    private void postProcessPath(List<Lunjingguihua.PathSegment> path) {
        if (path.isEmpty()) return;
        path.removeIf(seg -> seg.start.distance(seg.end) < 0.05);
        for (Lunjingguihua.PathSegment seg : path) {
            seg.isStartPoint = false;
            seg.isEndPoint = false;
        }
        boolean startFound = false;
        for (Lunjingguihua.PathSegment seg : path) {
            if (seg.isMowing) {
                seg.setAsStartPoint();
                startFound = true;
                break;
            }
        }
        for (int i = path.size() - 1; i >= 0; i--) {
            Lunjingguihua.PathSegment seg = path.get(i);
            if (seg.isMowing) {
                seg.setAsEndPoint();
                break;
            }
        }
    }
}