package com.ltgk.boat.utils;


import lombok.extern.slf4j.Slf4j;
import org.jgrapht.GraphPath;
import org.jgrapht.alg.connectivity.ConnectivityInspector;
import org.jgrapht.alg.shortestpath.AStarShortestPath;
import org.jgrapht.graph.DefaultWeightedEdge;
import org.jgrapht.graph.SimpleWeightedGraph;
import org.locationtech.jts.geom.Coordinate;
import org.locationtech.jts.geom.Envelope;
import org.locationtech.jts.geom.GeometryFactory;
import org.locationtech.jts.geom.Point;
import org.locationtech.jts.index.quadtree.Quadtree;

import java.io.FileNotFoundException;
import java.io.PrintWriter;
import java.util.Collections;
import java.util.List;
import java.util.Set;

/**
 * A*路径规划算法
 *
 * @author Qi ChengBin
 * @date 2025/05/27
 */
@Slf4j
public class AStarPathfinder {
    private final GeoJsonLandChecker geoJsonLandChecker;
    private final GeometryFactory geometryFactory = new GeometryFactory();
    // 核心参数配置
    private static final double RESOLUTION = 0.0004; // 邻接节点间距（约44米）
    private static final double COORDINATE_EPSILON = 1e-5; // 坐标匹配精度（约1米精度）
    private static final int MAX_DEPTH = 20; // 防止无限递归
    private static final double BRIDGE_SEARCH_RADIUS = 0.001; // 约111米搜索半径

    private Point end;

    public AStarPathfinder(GeoJsonLandChecker geoJsonLandChecker) {
        this.geoJsonLandChecker = geoJsonLandChecker;
    }

    public List<Point> findPath(Point start, Point end) {
        SimpleWeightedGraph<Point, DefaultWeightedEdge> graph = new SimpleWeightedGraph<>(DefaultWeightedEdge.class);
        Quadtree quadtree = new Quadtree();

        this.end = end;

        // 递归构建路径网络
        addNodeWithDepthCheck(graph, quadtree, start, 0);
        addNodeWithDepthCheck(graph, quadtree, end, 0);

        // 调试信息输出
        log.info("Graph contains start: {}", graph.containsVertex(start));
        log.info("Graph contains end: {}", graph.containsVertex(end));
        log.info("Total vertices: {}", graph.vertexSet().size());

        exportGraphToCSV(graph);
        // 在findPath方法末尾添加连通性诊断
        ConnectivityInspector<Point, DefaultWeightedEdge> inspector = new ConnectivityInspector<>(graph);
        if (!inspector.pathExists(start, end)) {
            log.error("起点终点不在同一连通分量");

            // 输出每个连通分量的详细信息
            List<Set<Point>> components = inspector.connectedSets();
            for (int i = 0; i < components.size(); i++) {
                Set<Point> component = components.get(i);
                log.info("连通分量[{}]: 大小={} 包含起点={} 包含终点={}",
                        i, component.size(), component.contains(start), component.contains(end));
            }
        }

        AStarShortestPath<Point, DefaultWeightedEdge> astar = new AStarShortestPath<>(graph, this::heuristic);
        GraphPath<Point, DefaultWeightedEdge> path = astar.getPath(start, end);
        // 在方法末尾添加路径诊断
        if (path == null) {
            log.error("路径不存在，可能原因：");
            log.error("1. 起点{}是否在水域: {}", start, !isLand(start));
            log.error("2. 终点{}是否在水域: {}", end, !isLand(end));
            log.error("3. 图结构顶点数量: {}", graph.vertexSet().size());
            log.error("4. 起点连接边数: {}", graph.edgesOf(start).size());
            log.error("5. 终点连接边数: {}", graph.edgesOf(end).size());
        }
        return path != null ? path.getVertexList() : Collections.emptyList();
    }

    private void addNodeWithDepthCheck(SimpleWeightedGraph<Point, DefaultWeightedEdge> graph,
                                       Quadtree quadtree, Point point, int depth) {
        if (depth > MAX_DEPTH) {
//            log.warn("Max recursion depth reached at {}", point);
            return;
        }
        addNodeToGraph(graph, quadtree, point, depth);
    }

    private void addNodeToGraph(SimpleWeightedGraph<Point, DefaultWeightedEdge> graph,
                                Quadtree quadtree, Point point, int depth) {
        // 扩展查询范围
//        Envelope searchEnv = point.getEnvelopeInternal();
//        searchEnv.expandBy(COORDINATE_EPSILON * 3);

        // 修改后（精确坐标存储）
        Envelope preciseEnv = new Envelope(
                point.getX() - COORDINATE_EPSILON/2,
                point.getX() + COORDINATE_EPSILON/2,
                point.getY() - COORDINATE_EPSILON/2,
                point.getY() + COORDINATE_EPSILON/2
        );

        // 精确坐标比对
        boolean exists = graph.containsVertex(point);

        if (!exists && !isLand(point)) {
            graph.addVertex(point);
            quadtree.insert(preciseEnv, point); // 存储扩展后的envelope
            log.debug("Added node: ({}, {})", String.format("%.6f", point.getX()), String.format("%.6f", point.getY()));

            // 优先向终点方向扩展
            if (depth % 5 == 0) {
                double dx = (end.getX() - point.getX()) * 0.1;
                double dy = (end.getY() - point.getY()) * 0.1;
                generateNeighbors(graph, quadtree, createNeighbor(point, dx, dy), depth+1);
            }

            // 生成8邻域连接
            generateNeighbors(graph, quadtree, point, depth);
        }
        if (!graph.containsVertex(point)) {
            log.warn("节点添加失败：{} | 陆地状态：{} | 已存在：{}",
                    point, isLand(point), exists);
        }
    }

    private void generateNeighbors(SimpleWeightedGraph<Point, DefaultWeightedEdge> graph,
                                   Quadtree quadtree, Point basePoint, int depth) {

        // 新增终点方向计算
        double directionX = (end.getX() - basePoint.getX()) * 0.2;
        double directionY = (end.getY() - basePoint.getY()) * 0.2;

        // 扩展方向数组（增加终点方向偏移）
        double[] offsets = {-RESOLUTION, 0, RESOLUTION, directionX};
        double[] yOffsets = {-RESOLUTION, 0, RESOLUTION, directionY};

        // 增加斜向连接支持
//        double[] offsets = {-RESOLUTION*2, -RESOLUTION, 0, RESOLUTION, RESOLUTION*2};
//        for (double factor : new double[]{1.0}) {
            for (double dx : offsets) {
                for (double dy : yOffsets) {
                    if (dx == 0 && dy == 0) continue;

//                    double actualDx = dx * factor;
//                    double actualDy = dy * factor;
//                    if (Math.abs(actualDx) < COORDINATE_EPSILON &&
//                            Math.abs(actualDy) < COORDINATE_EPSILON) {
//                        continue;
//                    }

                    Point neighbor = createNeighbor(basePoint, dx, dy);

                    // 动态判断是否为潜在桥梁节点
                    boolean isBridgeCandidate = isLand(neighbor) &&
                            isNearWater(neighbor, quadtree, COORDINATE_EPSILON * 10);

                    if (!isLand(neighbor) || isBridgeCandidate) {
                        addNodeWithDepthCheck(graph, quadtree, neighbor, depth + 1);
                        // 再创建连接（确保节点已存在）
                        if (graph.containsVertex(basePoint) && graph.containsVertex(neighbor)) {
                            createEdge(graph, basePoint, neighbor);
                        }
                    }
                }
            }
//        }
    }

    // 新增方法：判断节点是否靠近水域
    private boolean isNearWater(Point point, Quadtree quadtree, double searchRadius) {
        Envelope searchEnv = new Envelope(
                point.getX() - searchRadius,
                point.getX() + searchRadius,
                point.getY() - searchRadius,
                point.getY() + searchRadius
        );

        return quadtree.query(searchEnv).stream()
                .anyMatch(p -> !isLand((Point) p));
    }

    private Point createNeighbor(Point base, double dx, double dy) {
        return geometryFactory.createPoint(
                new Coordinate(
                        precisionRound(base.getX() + dx),
                        precisionRound(base.getY() + dy)
                )
        );
    }

    private void createEdge(SimpleWeightedGraph<Point, DefaultWeightedEdge> graph,
                            Point source, Point target) {

        // 添加自环检测
        if (source.equals(target)) {
            log.debug("跳过自环边: {} -> {}", source, target);
            return;
        }

        if (graph.containsVertex(source) && graph.containsVertex(target)) {
            DefaultWeightedEdge edge = graph.addEdge(source, target);
            if (edge != null) {
                graph.setEdgeWeight(edge, heuristic(source, target));
            }

            // 添加反向边（使用相同顶点检查）
            DefaultWeightedEdge reverseEdge = graph.addEdge(target, source);
            if (reverseEdge != null) {
                graph.setEdgeWeight(reverseEdge, heuristic(target, source));
            }
        } else {
            log.warn("节点缺失无法创建边: {} -> {}", source, target);
        }
    }

    // 精度标准化方法
    private double precisionRound(double value) {
        // 增加最小变化量检测
//        double rounded = Math.round(value / COORDINATE_EPSILON) * COORDINATE_EPSILON;
//        if (Math.abs(rounded - value) < 1e-10) {
//            return rounded + COORDINATE_EPSILON; // 避免零变化
//        }
        return Math.round(value / COORDINATE_EPSILON) * COORDINATE_EPSILON;
    }

    // 精确坐标比对方法
    private boolean coordinateEquals(Point a, Point b) {
        return Math.abs(a.getX() - b.getX()) < COORDINATE_EPSILON &&
                Math.abs(a.getY() - b.getY()) < COORDINATE_EPSILON;
    }

    private boolean isLand(Point point) {
        return geoJsonLandChecker.isLand(point.getY(), point.getX());
    }

    //    // 快速距离估算（曼哈顿距离）
//    private double fastDistance(Point p1, Point p2) {
//        return Math.abs(p1.getX() - p2.getX()) + Math.abs(p1.getY() - p2.getY());
//    }
// 改用欧几里得距离
    private double heuristic(Point source, Point target) {
        return Math.hypot(
                source.getX() - target.getX(),
                source.getY() - target.getY()
        );
    }

    // 添加路径网络导出方法
    // 修改后的CSV导出方法
    public void exportGraphToCSV(SimpleWeightedGraph<Point, DefaultWeightedEdge> graph) {
        try (PrintWriter writer = new PrintWriter("path_network.csv")) {
            // 输出顶点（点图层）
            writer.println("type,lon,lat");
            graph.vertexSet().forEach(p ->
                    writer.printf("node,%.6f,%.6f%n", p.getX(), p.getY()));

            // 输出边（线图层）
            writer.println("\ntype,wkt"); // WKT几何格式
            graph.edgeSet().forEach(e -> {
                Point s = graph.getEdgeSource(e);
                Point t = graph.getEdgeTarget(e);
                String wkt = String.format("LINESTRING (%.6f %.6f, %.6f %.6f)",
                        s.getX(), s.getY(), t.getX(), t.getY());
                writer.printf("line,%s%n", wkt);
            });
        } catch (FileNotFoundException e) {
            throw new RuntimeException(e);
        }
    }
}
