package lujing;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;

/**
 * 无障碍物凸形草地路径规划类 (终极优化版)
 * 特性：
 * 1. 最小投影宽度方向选择 (效率最高，转弯最少)
 * 2. 边缘轮廓优先切割 (无死角覆盖)
 * 3. 支持外部传入已包含重叠率的宽度参数
 */
public class AoxinglujingNoObstacle {

    // 引入极小值用于浮点数比较，处理几何精度误差
    private static final double EPSILON = 1e-6;

    /**
     * 路径段类
     */
    public static class PathSegment {
        public Point start;
        public Point end;
        public boolean isMowing; // true为割草工作段，false为过渡段

        public PathSegment(Point start, Point end, boolean isMowing) {
            this.start = start;
            this.end = end;
            this.isMowing = isMowing;
        }

        @Override
        public String toString() {
            return String.format("[%s -> %s, isMowing=%b]", start, end, isMowing);
        }
    }

    /**
     * 坐标点类
     */
    public static class Point {
        double x, y;

        public Point(double x, double y) {
            this.x = x;
            this.y = y;
        }

        @Override
        public String toString() {
            return String.format("(%.4f, %.4f)", x, y);
        }
    }

    /**
     * 对外公开的静态调用方法
     *
     * @param boundaryCoordsStr 地块边界坐标字符串 "x1,y1;x2,y2;..."
     * @param mowingWidthStr    有效割草宽度字符串 (已包含重叠率)，如 "0.30"
     * @param safetyMarginStr   安全边距字符串，如 "0.2"
     * @return 路径段列表
     */
    public static List<PathSegment> planPath(String boundaryCoordsStr, String mowingWidthStr, String safetyMarginStr) {
        // 1. 解析参数
        List<Point> originalPolygon = parseCoords(boundaryCoordsStr);
        double mowingWidth;
        double safetyMargin;

        try {
            mowingWidth = Double.parseDouble(mowingWidthStr);
            safetyMargin = Double.parseDouble(safetyMarginStr);
        } catch (NumberFormatException e) {
            throw new IllegalArgumentException("割草宽度或安全边距格式错误");
        }

        // 2. 调用核心算法
        return planPathCore(originalPolygon, mowingWidth, safetyMargin);
    }

    /**
     * 核心算法逻辑
     */
    private static List<PathSegment> planPathCore(List<Point> originalPolygon, double width, double safetyMargin) {
        if (originalPolygon == null || originalPolygon.size() < 3) {
            return new ArrayList<>(); // 或抛出异常，视业务需求而定
        }

        // 确保多边形是逆时针方向
        ensureCCW(originalPolygon);

        // 1. 根据安全边距内缩，得到实际作业区域
        List<Point> workAreaPolygon = shrinkPolygon(originalPolygon, safetyMargin);

        // 如果内缩后区域失效（如地块太小），返回空路径
        if (workAreaPolygon.size() < 3) {
            return new ArrayList<>();
        }

        List<PathSegment> finalPath = new ArrayList<>();

        // 2. [优化] 优先生成轮廓路径 (Contour Pass)
        // 沿作业边界走一圈，确保边缘整齐且无遗漏
        addContourPath(workAreaPolygon, finalPath);

        // 3. [优化] 计算最佳扫描角度
        // 寻找让多边形投影高度最小的角度，从而最小化转弯次数
        double bestAngle = findOptimalScanAngle(workAreaPolygon);

        // 4. 生成内部弓字形路径
        // 直接使用传入的 width (已包含重叠率)
        List<PathSegment> zigZagPaths = generateClippedMowingLines(workAreaPolygon, bestAngle, width);

        // 5. 连接轮廓路径和弓字形路径
        if (!finalPath.isEmpty() && !zigZagPaths.isEmpty()) {
            Point contourEnd = finalPath.get(finalPath.size() - 1).end;
            Point zigzagStart = zigZagPaths.get(0).start;
            
            // 如果轮廓终点与弓字形起点不重合，添加过渡段
            if (distanceSq(contourEnd, zigzagStart) > EPSILON) {
                finalPath.add(new PathSegment(contourEnd, zigzagStart, false));
            }
        }

        // 6. 合并弓字形路径
        finalPath.addAll(connectPathSegments(zigZagPaths));

        return finalPath;
    }

    // ================= 核心逻辑辅助方法 =================

    /**
     * 添加轮廓路径 (围着多边形走一圈)
     */
    private static void addContourPath(List<Point> polygon, List<PathSegment> path) {
        int n = polygon.size();
        for (int i = 0; i < n; i++) {
            Point p1 = polygon.get(i);
            Point p2 = polygon.get((i + 1) % n);
            path.add(new PathSegment(p1, p2, true));
        }
    }

    /**
     * 寻找最优扫描角度 (最小投影高度法)
     */
    private static double findOptimalScanAngle(List<Point> polygon) {
        double minHeight = Double.MAX_VALUE;
        double bestAngle = 0;
        int n = polygon.size();

        // 遍历每一条边，计算以该边为“底”时，多边形的高度
        for (int i = 0; i < n; i++) {
            Point p1 = polygon.get(i);
            Point p2 = polygon.get((i + 1) % n);

            // 当前边的角度
            double currentAngle = Math.atan2(p2.y - p1.y, p2.x - p1.x);

            // 计算在这个角度下的投影高度
            double height = calculatePolygonHeightAtAngle(polygon, currentAngle);

            if (height < minHeight) {
                minHeight = height;
                bestAngle = currentAngle;
            }
        }
        return bestAngle;
    }

    /**
     * 计算多边形在特定旋转角度下的Y轴投影高度
     */
    private static double calculatePolygonHeightAtAngle(List<Point> poly, double angle) {
        double minY = Double.MAX_VALUE;
        double maxY = -Double.MAX_VALUE;

        double cos = Math.cos(-angle);
        double sin = Math.sin(-angle);

        for (Point p : poly) {
            // 只需计算旋转后的Y坐标
            double rotatedY = p.x * sin + p.y * cos;
            if (rotatedY < minY) minY = rotatedY;
            if (rotatedY > maxY) maxY = rotatedY;
        }
        return maxY - minY;
    }

    private static List<PathSegment> generateClippedMowingLines(List<Point> polygon, double angle, double width) {
        List<PathSegment> segments = new ArrayList<>();

        // 旋转多边形至水平
        List<Point> rotatedPoly = rotatePolygon(polygon, -angle);

        double minY = Double.MAX_VALUE;
        double maxY = -Double.MAX_VALUE;
        for (Point p : rotatedPoly) {
            if (p.y < minY) minY = p.y;
            if (p.y > maxY) maxY = p.y;
        }

        // 起始扫描线位置：
        // 从 minY + width/2 开始，因为之前已经走了轮廓线(Contour Pass)。
        // 轮廓线负责清理边缘区域，内部填充线保持 width 的间距即可。
        // 加上 EPSILON 防止浮点数刚好落在边界上导致的判断误差
        double currentY = minY + width / 2.0 + EPSILON;

        while (currentY < maxY) {
            List<Double> xIntersections = new ArrayList<>();
            int n = rotatedPoly.size();

            for (int i = 0; i < n; i++) {
                Point p1 = rotatedPoly.get(i);
                Point p2 = rotatedPoly.get((i + 1) % n);

                // 忽略水平线段
                if (Math.abs(p1.y - p2.y) < EPSILON) continue;

                double minP = Math.min(p1.y, p2.y);
                double maxP = Math.max(p1.y, p2.y);

                if (currentY >= minP && currentY < maxP) {
                    double x = p1.x + (currentY - p1.y) * (p2.x - p1.x) / (p2.y - p1.y);
                    xIntersections.add(x);
                }
            }

            Collections.sort(xIntersections);

            if (xIntersections.size() >= 2) {
                // 取最左和最右交点
                double xStart = xIntersections.get(0);
                double xEnd = xIntersections.get(xIntersections.size() - 1);

                if (xEnd - xStart > EPSILON) {
                    // 反向旋转回原坐标系
                    Point rStart = rotatePoint(new Point(xStart, currentY), angle);
                    Point rEnd = rotatePoint(new Point(xEnd, currentY), angle);
                    segments.add(new PathSegment(rStart, rEnd, true));
                }
            }
            // 步进
            currentY += width;
        }

        return segments;
    }

    private static List<PathSegment> connectPathSegments(List<PathSegment> lines) {
        List<PathSegment> result = new ArrayList<>();
        if (lines.isEmpty()) return result;

        for (int i = 0; i < lines.size(); i++) {
            PathSegment currentLine = lines.get(i);
            Point actualStart, actualEnd;

            // 弓字形规划：偶数行正向，奇数行反向
            if (i % 2 == 0) {
                actualStart = currentLine.start;
                actualEnd = currentLine.end;
            } else {
                actualStart = currentLine.end;
                actualEnd = currentLine.start;
            }

            // 添加过渡段
            if (i > 0) {
                Point prevEnd = result.get(result.size() - 1).end;
                if (distanceSq(prevEnd, actualStart) > EPSILON) {
                    result.add(new PathSegment(prevEnd, actualStart, false));
                }
            }

            result.add(new PathSegment(actualStart, actualEnd, true));
        }
        return result;
    }

    // ================= 基础几何工具 =================

    private static List<Point> parseCoords(String s) {
        List<Point> list = new ArrayList<>();
        if (s == null || s.trim().isEmpty()) return list;

        String[] parts = s.split(";");
        for (String part : parts) {
            String[] xy = part.split(",");
            if (xy.length >= 2) {
                try {
                    double x = Double.parseDouble(xy[0].trim());
                    double y = Double.parseDouble(xy[1].trim());
                    list.add(new Point(x, y));
                } catch (NumberFormatException e) {
                    // 忽略格式错误
                }
            }
        }
        return list;
    }

    private static void ensureCCW(List<Point> polygon) {
        double sum = 0;
        for (int i = 0; i < polygon.size(); i++) {
            Point p1 = polygon.get(i);
            Point p2 = polygon.get((i + 1) % polygon.size());
            sum += (p2.x - p1.x) * (p2.y + p1.y);
        }
        if (sum > 0) {
            Collections.reverse(polygon);
        }
    }

    private static List<Point> shrinkPolygon(List<Point> polygon, double margin) {
        List<Point> newPoints = new ArrayList<>();
        int n = polygon.size();

        for (int i = 0; i < n; i++) {
            Point p1 = polygon.get(i);
            Point p2 = polygon.get((i + 1) % n);
            Point p0 = polygon.get((i - 1 + n) % n);

            Line line1 = offsetLine(p1, p2, margin);
            Line line0 = offsetLine(p0, p1, margin);

            Point intersection = getIntersection(line0, line1);
            if (intersection != null) {
                newPoints.add(intersection);
            }
        }
        return newPoints;
    }

    private static class Line {
        double a, b, c;
        public Line(double a, double b, double c) { this.a = a; this.b = b; this.c = c; }
    }

    private static Line offsetLine(Point p1, Point p2, double dist) {
        double dx = p2.x - p1.x;
        double dy = p2.y - p1.y;
        double len = Math.sqrt(dx * dx + dy * dy);

        if (len < EPSILON) return new Line(0, 0, 0);

        double nx = -dy / len;
        double ny = dx / len;

        // 向左侧平移（假设逆时针）
        double newX = p1.x + nx * dist;
        double newY = p1.y + ny * dist;

        double a = -dy;
        double b = dx;
        double c = -a * newX - b * newY;
        return new Line(a, b, c);
    }

    private static Point getIntersection(Line l1, Line l2) {
        double det = l1.a * l2.b - l2.a * l1.b;
        if (Math.abs(det) < EPSILON) return null;
        double x = (l1.b * l2.c - l2.b * l1.c) / det;
        double y = (l2.a * l1.c - l1.a * l2.c) / det;
        return new Point(x, y);
    }

    private static Point rotatePoint(Point p, double angle) {
        double cos = Math.cos(angle);
        double sin = Math.sin(angle);
        return new Point(p.x * cos - p.y * sin, p.x * sin + p.y * cos);
    }

    private static List<Point> rotatePolygon(List<Point> poly, double angle) {
        List<Point> res = new ArrayList<>();
        for (Point p : poly) {
            res.add(rotatePoint(p, angle));
        }
        return res;
    }

    private static double distanceSq(Point p1, Point p2) {
        return (p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y);
    }
}