package com.hivekion.common;

/**
 * [类的简要说明]
 * <p>
 * [详细描述，可选]
 * <p>
 *
 * @author LiDongYU
 * @since 2025/7/22
 */
public class GeoPosition {
  // 地球半径（单位：米）
  private static final double EARTH_RADIUS = 6371000.0;

  // 角度转弧度
  public static double toRadians(double degree) {
    return degree * Math.PI / 180.0;
  }

  // 弧度转角度
  public static double toDegrees(double radian) {
    return radian * 180.0 / Math.PI;
  }

  // 球面距离（haversine公式）
  public static double haversine(double lat1, double lon1, double lat2, double lon2) {
    double dLat = toRadians(lat2 - lat1);
    double dLon = toRadians(lon2 - lon1);
    lat1 = toRadians(lat1);
    lat2 = toRadians(lat2);

    double a = Math.sin(dLat/2) * Math.sin(dLat/2) +
        Math.cos(lat1) * Math.cos(lat2) *
            Math.sin(dLon/2) * Math.sin(dLon/2);
    double c = 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
    return EARTH_RADIUS * c;
  }

  // 球面线性插值（Slerp，大圆航线插值）
  public static double[] interpolate(double lat1, double lon1, double lat2, double lon2, double f) {
    // 转为弧度
    lat1 = toRadians(lat1);
    lon1 = toRadians(lon1);
    lat2 = toRadians(lat2);
    lon2 = toRadians(lon2);

    double sinLat1 = Math.sin(lat1), cosLat1 = Math.cos(lat1);
    double sinLat2 = Math.sin(lat2), cosLat2 = Math.cos(lat2);
    double deltaLon = lon2 - lon1;

    double a = Math.sin((1-f) * haversineAngle(lat1, lon1, lat2, lon2)) / Math.sin(haversineAngle(lat1, lon1, lat2, lon2));
    double b = Math.sin(f * haversineAngle(lat1, lon1, lat2, lon2)) / Math.sin(haversineAngle(lat1, lon1, lat2, lon2));

    // 球面坐标
    double x = a * cosLat1 * Math.cos(lon1) + b * cosLat2 * Math.cos(lon2);
    double y = a * cosLat1 * Math.sin(lon1) + b * cosLat2 * Math.sin(lon2);
    double z = a * sinLat1 + b * sinLat2;

    double lat = Math.atan2(z, Math.sqrt(x*x + y*y));
    double lon = Math.atan2(y, x);

    return new double[] { toDegrees(lat), toDegrees(lon) };
  }

  // 计算球面距离的角度（弧度）
  public static double haversineAngle(double lat1, double lon1, double lat2, double lon2) {
    double dLat = lat2 - lat1;
    double dLon = lon2 - lon1;
    double a = Math.sin(dLat/2) * Math.sin(dLat/2)
        + Math.cos(lat1) * Math.cos(lat2)
        * Math.sin(dLon/2) * Math.sin(dLon/2);
    return 2 * Math.atan2(Math.sqrt(a), Math.sqrt(1-a));
  }

  public static void main(String[] args) {
    // 起点
    double lon1 = 121.148362;
    double lat1 = 25.007064;
    // 终点
    double lon2 = 121.147192;
    double lat2 = 25.007612;
    // 速度（米/秒）
    double speed = 5.0;
    // 已行驶时间（秒）
    double seconds = 30.0;

    // 计算总距离
    double totalDist = haversine(lat1, lon1, lat2, lon2);
    // 已行驶距离
    double movedDist = speed * seconds;
    // 占比
    double f = Math.min(movedDist / totalDist, 1.0);

    // 球面插值
    double[] pos = interpolate(lat1, lon1, lat2, lon2, f);

    System.out.printf("当前位置：经度 %.6f, 纬度 %.6f\n", pos[1], pos[0]);
  }
}