import math

def lla2dx(lon,lat,alt):
    # 定义一些常量
    RADIUS = 6378137.0  # 地球半径，单位为米
    MAX_LATITUDE = 85.0511287798

    # 将经纬度转换为弧度
    lon_rad = math.radians(lon)
    lat_rad = math.radians(lat)

    # 确保纬度在有效范围内[-85.0511287798, 85.0511287798]
    lat_rad = max(min(MAX_LATITUDE, lat_rad), -MAX_LATITUDE)

    # 计算x和y
    x = RADIUS * lon_rad
    y = RADIUS * math.log(math.tan((math.pi / 4) + (lat_rad / 2)))

    # 高度z在两个坐标系之间是不变的
    z = alt
    return x,y,z

def dx2lla(x,y,z):
    """ 距离坐标转普通坐标 """
    RADIUS = 6378137.0  # 地球半径，单位为米

    # 计算经度和纬度
    lon = math.degrees(x / RADIUS)
    lat = math.degrees(2 * math.atan(math.exp(y / RADIUS)) - math.pi / 2)

    # 高度z在两个坐标系之间是不变的
    alt = z
    return lon,lat,alt


def lla2dbe(lon1, lat1, alt1, lon2, lat2, alt2):
    """
    输入两个点的坐标经纬度
    :param lon1:
    :param lat1:
    :param alt1:
    :param lon2:
    :param lat2:
    :param alt2:
    :return:
    """
    print("输入的经纬度",lon1,lat1,lon2,lat2)

    x1, y1, z1 = lla2dx(lon1, lat1, alt1)
    x2, y2, z2 = lla2dx(lon2, lat2, alt2)

    dx = x2 - x1
    dy = y2 - y1
    dz = alt2 - alt1

    distance = math.sqrt(dx ** 2 + dy ** 2 + dz ** 2)

    # 计算方位角
    angle_with_x = math.degrees(math.atan2(dy, dx))
    azimuth = 90 - angle_with_x

    if azimuth < 0:
        azimuth += 360

    # 计算俯仰角
    angle_rad = math.asin(dz / distance)  # 结果为弧度
    elevation = math.degrees(angle_rad)  # 将弧度转换为度

    print("距离方位仰角",distance, azimuth, elevation)
    return distance, azimuth, elevation