package com.xiaopeng.pushMessage.utils;

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

import java.time.Instant;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.concurrent.*;
import java.util.concurrent.atomic.AtomicInteger;

public class TrajectoryDensifier {

    private static final int THREAD_NUM = 5;

    /**
     * 轨迹补全计算线程池（类级别单例复用）
     * 修复：原代码每次调用 densify() 都 new ThreadPoolExecutor 并在方法结束时 shutdown，
     * 频繁创建/销毁线程开销大，且异常路径（latch.await 被中断）会跳过 shutdown 导致线程泄漏。
     * 改为单例线程池复用，避免上述问题。
     */
    private static final ThreadPoolExecutor DENSIFY_EXECUTOR = new ThreadPoolExecutor(
            5, 5, 60L, TimeUnit.SECONDS,
            new LinkedBlockingQueue<>(256),
            new ThreadFactory() {
                private final AtomicInteger counter = new AtomicInteger(0);
                @Override
                public Thread newThread(Runnable r) {
                    Thread t = new Thread(r, "trajectory-densify-" + counter.incrementAndGet());
                    t.setDaemon(true);
                    return t;
                }
            },
            new ThreadPoolExecutor.CallerRunsPolicy()
    );

    static {
        // 允许核心线程超时回收，空闲时不占用资源
        DENSIFY_EXECUTOR.allowCoreThreadTimeOut(true);
    }


    public static List<LonLatDto> densify(List<LonLatDto> originalPoints, int distance) {
        if (originalPoints == null || originalPoints.size() <= 1) {
            return new ArrayList<>(originalPoints);
        }
        int num = originalPoints.size() / THREAD_NUM;
        if(num == 0){
            num = 1;
        }
        CountDownLatch latch = new CountDownLatch(num);
        Map<Integer, List<LonLatDto>> map = new ConcurrentHashMap<>();
        for (int i = 0; i < num; i++) {
            List<LonLatDto> trackPoints = originalPoints.subList(i * THREAD_NUM, i == num - 1 ? originalPoints.size() : (i + 1) * THREAD_NUM);
            int finalI = i;
            DENSIFY_EXECUTOR.execute(()->{
                try {
                    List<LonLatDto> new_polyline = new ArrayList<>();
                    for (int j = 0; j < trackPoints.size(); j++) {
                        LonLatDto trackPoint = trackPoints.get(j);
                        if(j == trackPoints.size() - 1){
                            new_polyline.add(new LonLatDto(trackPoint.getLongitude(), trackPoint.getLatitude()));
                            break;
                        }
                        LonLatDto trackPoint1 = trackPoints.get(j+1);

                        //对两个点位进行坐标补全
                        List<TrackPoint> track = new ArrayList<>();
                        track.add(new TrackPoint(trackPoint.getLatitude(), trackPoint.getLongitude(), null));
                        track.add(new TrackPoint(trackPoint1.getLatitude(), trackPoint1.getLongitude(), null));
                        //10米补全一个点位
                        List<TrackPoint> densified = densifyByDistance(track, distance);
                        for (TrackPoint point : densified) {
                            new_polyline.add(new LonLatDto(point.getLongitude(), point.getLatitude()));
                        }
                    }
                    if (!new_polyline.isEmpty()) {
                        new_polyline.remove(new_polyline.size() - 1);
                    }
                    map.put(finalI, new_polyline);
                } finally {
                    latch.countDown();
                }
            });
        }
        try {
            latch.await();
        } catch (InterruptedException e) {
            Thread.currentThread().interrupt();
            throw new RuntimeException(e);
        }
        List<LonLatDto> result = new ArrayList<>();
        for (int i = 0; i < num; i++) {
            List<LonLatDto> chunk = map.get(i);
            if (null != chunk) {
                result.addAll(chunk);
            }
        }
        //添加最后一个点
        result.add(originalPoints.get(originalPoints.size() - 1));
        return result;
    }



    /**
     * 轨迹补密主方法
     * @param originalPoints 原始轨迹点列表（必须按时间排序）
     * @param distance 补密的间隔距离，例如每 5 米补一个点
     * @return 补密后的轨迹列表
     */
    public static List<TrackPoint> densifyByDistance(List<TrackPoint> originalPoints, int distance) {
        List<TrackPoint> result = new ArrayList<>();
        if (originalPoints == null || originalPoints.size() < 2) {
            return originalPoints;
        }

        // 添加第一个点
        result.add(originalPoints.get(0));

        for (int i = 0; i < originalPoints.size() - 1; i++) {
            TrackPoint start = originalPoints.get(i);
            TrackPoint end = originalPoints.get(i + 1);

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

            // 如果两点间时间差小于间隔，说明不需要补密，直接添加终点（在循环结束后添加）
            if (distance1 <= distance) {
                // 这里不做处理，等待下一轮或直接添加终点
            } else {
                // 计算需要插入多少个点
                long steps = (long) (distance1 / distance);

                for (long j = 1; j <= steps; j++) {

                    // 计算插值比例 t (0.0 ~ 1.0)
                    double fraction = (double) (j * distance) / distance1;

                    // 线性插值计算经纬度
                    double newLat = start.getLatitude() + fraction * (end.getLatitude() - start.getLatitude());
                    double newLon = start.getLongitude() + fraction * (end.getLongitude() - start.getLongitude());

                    result.add(new TrackPoint(newLat, newLon, null));
                }
            }

            // 确保每一段的终点被添加（除了最后一次循环，因为循环结束后会添加最后一个点）
            // 为了避免重复添加，我们只在循环最后添加最后一个点，或者在这里判断
        }

        // 添加最后一个点
//        result.add(originalPoints.get(originalPoints.size() - 1));

        return result;
    }



    /**
     * 轨迹补密主方法
     * @param originalPoints 原始轨迹点列表（必须按时间排序）
     * @param intervalSeconds 补密的时间间隔（秒），例如每 5 秒补一个点
     * @return 补密后的轨迹列表
     */
    public static List<TrackPoint> densifyByTime(List<TrackPoint> originalPoints, long intervalSeconds) {
        List<TrackPoint> result = new ArrayList<>();
        if (originalPoints == null || originalPoints.size() < 2) {
            return originalPoints;
        }

        // 添加第一个点
        result.add(originalPoints.get(0));

        for (int i = 0; i < originalPoints.size() - 1; i++) {
            TrackPoint start = originalPoints.get(i);
            TrackPoint end = originalPoints.get(i + 1);

            long timeDiffMillis = end.getTimestamp().toEpochMilli() - start.getTimestamp().toEpochMilli();
            long intervalMillis = intervalSeconds * 1000;

            // 如果两点间时间差小于间隔，说明不需要补密，直接添加终点（在循环结束后添加）
            if (timeDiffMillis <= intervalMillis) {
                // 这里不做处理，等待下一轮或直接添加终点
            } else {
                // 计算需要插入多少个点
                long steps = timeDiffMillis / intervalMillis;

                for (long j = 1; j <= steps; j++) {
                    // 计算当前插入点的时间
                    Instant newTime = start.getTimestamp().plusMillis(j * intervalMillis);

                    // 如果计算出的时间超过了终点时间，则停止（防止浮点数误差或边界问题）
                    if (newTime.isAfter(end.getTimestamp())) {
                        break;
                    }

                    // 计算插值比例 t (0.0 ~ 1.0)
                    double fraction = (double) (j * intervalMillis) / timeDiffMillis;

                    // 线性插值计算经纬度
                    double newLat = start.getLatitude() + fraction * (end.getLatitude() - start.getLatitude());
                    double newLon = start.getLongitude() + fraction * (end.getLongitude() - start.getLongitude());

                    result.add(new TrackPoint(newLat, newLon, newTime));
                }
            }

            // 确保每一段的终点被添加（除了最后一次循环，因为循环结束后会添加最后一个点）
            // 为了避免重复添加，我们只在循环最后添加最后一个点，或者在这里判断
        }

        // 添加最后一个点
        result.add(originalPoints.get(originalPoints.size() - 1));

        return result;
    }

    // 主函数测试
    public static void main(String[] args) {
//        List<TrackPoint> track = new ArrayList<>();
//        // 模拟数据：两个点相差 20 秒
//        track.add(new TrackPoint(39.9042, 116.4074, Instant.parse("2026-03-28T10:00:00Z"))); // 北京
//        track.add(new TrackPoint(39.9050, 116.4080, Instant.parse("2026-03-28T10:00:20Z")));
//
//        // 每 5 秒补一个点
//        List<TrackPoint> densified = densifyByDistance(track, 10);
//
//        System.out.println("原始点数: " + track.size());
//        System.out.println("补密后点数: " + densified.size());
//        System.out.println("轨迹详情:");
//        for (TrackPoint p : densified) {
//            System.out.println("[" + p.getLongitude() + ", " + p.getLatitude() + "]");
//        }

        List<LonLatDto> track = new ArrayList<>();
        track.add(new LonLatDto(113.386036, 23.15308));
        track.add(new LonLatDto(113.385671, 23.153252));
        track.add(new LonLatDto(113.385402, 23.153397));
        track.add(new LonLatDto(113.385211, 23.15352));

        List<LonLatDto> densify = densify(track, 10);
        for (LonLatDto p : densify) {
            System.out.println("[" + p.getLongitude() + ", " + p.getLatitude() + "]");
        }
    }

}