package com.xiaopeng.pushMessage.utils;

import com.xiaopeng.api.dto.operation.LonLatDto;
import lombok.extern.slf4j.Slf4j;
import org.gavaghan.geodesy.Ellipsoid;
import org.gavaghan.geodesy.GeodeticCalculator;
import org.gavaghan.geodesy.GeodeticCurve;
import org.gavaghan.geodesy.GlobalCoordinates;

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


@Slf4j
public class MapMatchingUtil {




    /**
     * 获取剩余导航数据
     * @param polyline
     * @param longitude
     * @param latitude
     * @return
     */
    public static List<LonLatDto> getRemainingNavigation(List<LonLatDto> polyline, Double longitude, Double latitude){
        //导航数据只有一个点的情况
        if(polyline.size() == 1){
            LonLatDto lonLatDto = polyline.get(0);
            GlobalCoordinates source1 = new GlobalCoordinates(lonLatDto.getLatitude(), lonLatDto.getLongitude());
            GlobalCoordinates target1 = new GlobalCoordinates(latitude, longitude);
            GeodeticCurve geoCurve1 = new GeodeticCalculator().calculateGeodeticCurve(Ellipsoid.WGS84, source1, target1);
            double distance =geoCurve1.getEllipsoidalDistance();
            //判断距离小于5米，则可以返回空数据
            if(5 >= distance){
                return Collections.emptyList();
            }
            //大于5米的情况，将车子当前位置添加进结果中
            lonLatDto = new LonLatDto(longitude, latitude);
            polyline.add(0, lonLatDto);
            return polyline;
        }
        //多个点位的情况，需要找出当前点位处于那两个点位之间，然后截取掉超出的部分轨迹
        return getRecentlyPoint(polyline, longitude, latitude);
    }


    /**
     * 判断点位在轨迹线路中的点位之间
     * 返回截取后的剩余轨迹
     * @param polyline
     * @param longitude
     * @param latitude
     * @return
     */
    public static List<LonLatDto> getRecentlyPoint(List<LonLatDto> polyline, Double longitude, Double latitude){
        for (int i = 0; i < polyline.size(); i++) {
            LonLatDto lonLatDto1 = polyline.get(i);

            GlobalCoordinates source1 = new GlobalCoordinates(lonLatDto1.getLatitude(), lonLatDto1.getLongitude());
            GlobalCoordinates target1 = new GlobalCoordinates(latitude, longitude);
            GeodeticCurve geoCurve1 = new GeodeticCalculator().calculateGeodeticCurve(Ellipsoid.WGS84, source1, target1);
            double distance1 =geoCurve1.getEllipsoidalDistance();

            LonLatDto lonLatDto2 = polyline.get(i + 1);
            GlobalCoordinates source2 = new GlobalCoordinates(lonLatDto2.getLatitude(), lonLatDto2.getLongitude());
            GlobalCoordinates target2 = new GlobalCoordinates(latitude, longitude);
            GeodeticCurve geoCurve2 = new GeodeticCalculator().calculateGeodeticCurve(Ellipsoid.WGS84, source2, target2);
            double distance2 =geoCurve2.getEllipsoidalDistance();


            GlobalCoordinates source3 = new GlobalCoordinates(lonLatDto1.getLatitude(), lonLatDto1.getLongitude());
            GlobalCoordinates target3 = new GlobalCoordinates(lonLatDto2.getLatitude(), lonLatDto2.getLongitude());
            GeodeticCurve geoCurve3 = new GeodeticCalculator().calculateGeodeticCurve(Ellipsoid.WGS84, source3, target3);
            double distance3 =geoCurve3.getEllipsoidalDistance();

            //判断当前位置是否处于两个点位之间
            if((distance1 + distance2) == distance3 || (distance1 + distance2) <= distance3 + 5){
                //取距离最近的点位数据
                if(distance1 < distance2){
                    return polyline.subList(i, polyline.size() - 1);
                }
                return polyline.subList(i + 1, polyline.size() - 1);
            }
        }
        return Collections.emptyList();
    }







    /**
     * 轨迹裁剪（线段版本）
     * @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};
    }


}