package com.xiaopeng.automaticPilot.util;

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

/**
 * GPS路径生成工具
 */
public class GpsPathGenerator {

    /**
     * 地球半径（米）
     */
    private static final double EARTH_RADIUS = 6371000;

    /**
     * 简单GPS坐标点
     */
    public static class GpsPoint {
        public final double longitude;
        public final double latitude;

        public GpsPoint(double longitude, double latitude) {
            this.longitude = longitude;
            this.latitude = latitude;
        }
    }

    /**
     * 生成从起点到终点的GPS路径
     *
     * @param startLongitude  起点经度
     * @param startLatitude   起点纬度
     * @param endLongitude    终点经度
     * @param endLatitude     终点纬度
     * @param intervalSeconds 间隔秒数（每隔多少秒一个点）
     * @param speed           速度（米/秒），默认10.5米/秒
     * @return GPS坐标点列表
     */
    public static List<GpsPoint> generatePath(double startLongitude, double startLatitude,
                                               double endLongitude, double endLatitude,
                                               int intervalSeconds, double speed) {
        List<GpsPoint> pathPoints = new ArrayList<>();

        // 计算总距离（米）
        double totalDistance = calculateDistance(startLatitude, startLongitude, endLatitude, endLongitude);

        // 计算每个间隔移动的距离
        double distancePerInterval = speed * intervalSeconds;

        // 计算需要的点数
        int pointCount = (int) Math.ceil(totalDistance / distancePerInterval) + 1;

        // 生成路径点
        for (int i = 0; i < pointCount; i++) {
            double ratio = i * distancePerInterval / totalDistance;
            if (ratio > 1.0) {
                ratio = 1.0;
            }

            // 线性插值计算中间点
            double longitude = startLongitude + (endLongitude - startLongitude) * ratio;
            double latitude = startLatitude + (endLatitude - startLatitude) * ratio;

            pathPoints.add(new GpsPoint(longitude, latitude));
        }

        return pathPoints;
    }


    /**
     * 使用Haversine公式计算两个GPS坐标之间的距离（米）
     *
     * @param lat1 起点纬度
     * @param lon1 起点经度
     * @param lat2 终点纬度
     * @param lon2 终点经度
     * @return 距离（米）
     */
    public static double calculateDistance(double lat1, double lon1, double lat2, double lon2) {
        double lat1Rad = Math.toRadians(lat1);
        double lat2Rad = Math.toRadians(lat2);
        double deltaLat = Math.toRadians(lat2 - lat1);
        double deltaLon = Math.toRadians(lon2 - lon1);

        double a = Math.sin(deltaLat / 2) * Math.sin(deltaLat / 2) +
                Math.cos(lat1Rad) * Math.cos(lat2Rad) *
                        Math.sin(deltaLon / 2) * Math.sin(deltaLon / 2);
        double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1 - a));
        return EARTH_RADIUS * c;
    }

    /**
     * 计算两个点之间的方位角（度）
     *
     * @param lat1 起点纬度
     * @param lon1 起点经度
     * @param lat2 终点纬度
     * @param lon2 终点经度
     * @return 方位角（0-360度）
     */
    public static int calculateCourse(double lat1, double lon1, double lat2, double lon2) {
        double lat1Rad = Math.toRadians(lat1);
        double lat2Rad = Math.toRadians(lat2);
        double deltaLon = Math.toRadians(lon2 - lon1);

        double y = Math.sin(deltaLon) * Math.cos(lat2Rad);
        double x = Math.cos(lat1Rad) * Math.sin(lat2Rad) -
                Math.sin(lat1Rad) * Math.cos(lat2Rad) * Math.cos(deltaLon);

        double bearing = Math.toDegrees(Math.atan2(y, x));

        // 转换为0-360度
        return (int) ((bearing + 360) % 360);
    }
}