package com.xiaopeng.pushMessage.utils;

import com.xiaopeng.api.dto.operation.LonLatDto;
import lombok.extern.slf4j.Slf4j;

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

@Slf4j
public class MapMatchingUtil {

    /**
     * 轨迹裁剪（线段版本）
     * @param polyline
     * @param currentPoint
     * @return
     */
    public static List<LonLatDto> clipBySegmentDistance(List<LonLatDto> polyline,LonLatDto currentPoint) {

        if (polyline == null || polyline.size() < 2) {
            return Collections.emptyList();
        }

        double minDistance = Double.MAX_VALUE;
        int minSegmentIndex = 0;

        int increaseCount = 0;
        final int threshold = 3;
        final double tolerance = 1.0;

        for (int i = 0; i < polyline.size() - 1; i++) {

            double d = pointToSegmentDistance(
                    currentPoint,
                    polyline.get(i),
                    polyline.get(i + 1)
            );

            if (d < minDistance) {
                minDistance = d;
                minSegmentIndex = i;
                increaseCount = 0;
            } else if (d > minDistance + tolerance) {
                increaseCount++;
            }

            if (increaseCount >= threshold) {
                break;
            }
        }

        // 从最优线段的后一个点开始截取
        return new ArrayList<>(
                polyline.subList(minSegmentIndex + 1, polyline.size())
        );
    }


    /**
     * 点到线段距离
     * @param p
     * @param a
     * @param b
     * @return
     */
    public static double pointToSegmentDistance(
            LonLatDto p,
            LonLatDto a,
            LonLatDto b) {

        double refLat = p.getLatitude();

        double[] pXY = toXY(p.getLatitude(), p.getLongitude(), refLat);
        double[] aXY = toXY(a.getLatitude(), a.getLongitude(), refLat);
        double[] bXY = toXY(b.getLatitude(), b.getLongitude(), refLat);

        double x = pXY[0];
        double y = pXY[1];

        double x1 = aXY[0];
        double y1 = aXY[1];

        double x2 = bXY[0];
        double y2 = bXY[1];

        double dx = x2 - x1;
        double dy = y2 - y1;

        if (dx == 0 && dy == 0) {
            return Math.hypot(x - x1, y - y1);
        }

        double t = ((x - x1) * dx + (y - y1) * dy) / (dx * dx + dy * dy);

        if (t < 0) {
            return Math.hypot(x - x1, y - y1);
        } else if (t > 1) {
            return Math.hypot(x - x2, y - y2);
        }

        double projX = x1 + t * dx;
        double projY = y1 + t * dy;

        return Math.hypot(x - projX, y - projY);
    }

    /**
     * 经纬度转平面坐标（局部近似）
     * @param lat
     * @param lon
     * @param refLat
     * @return
     */
    private static double[] toXY(double lat, double lon, double refLat) {
        double R = 6378137; // 地球半径
        double x = Math.toRadians(lon) * R * Math.cos(Math.toRadians(refLat));
        double y = Math.toRadians(lat) * R;
        return new double[]{x, y};
    }


}