from math import *
import numpy as np
import pymap3d.ecef

EARTH_ECC = 0.01671022
EARTH_R = 6385607.4105288265  # equator km


def LLA_to_ECEF(lat, lon, h):
    deg2rad = pi / 180
    lat = lat * deg2rad
    lon = lon * deg2rad
    print((EARTH_R + h), cos(lat) ,cos(lon), (EARTH_R + h) * cos(lat) * cos(lon))
    X = (EARTH_R + h) * cos(lat) * cos(lon)
    Y = (EARTH_R + h) * cos(lat) * sin(lon)
    Z = (EARTH_R * (1 - EARTH_ECC**2) + h) * sin(lat)
    print(X, Y, Z)
    return (X, Y, Z)


def local_to_enu(aer):
    A, E, R = aer
    deg2rad = pi / 180
    A = A * deg2rad
    E = E * deg2rad
    e = R * cos(E) * sin(A)
    n = R * cos(E) * cos(A)
    u = R * sin(E)
    return (e, n, u)


def enu_to_ECEF(p0_LLA, enu):
    lat, lon, h = p0_LLA
    e, n, u = enu
    deg2rad = pi / 180
    lat = lat * deg2rad
    lon = lon * deg2rad
    mat = [[-sin(lon), -sin(lat) * cos(lon), cos(lat) * cos(lon)],
           [cos(lon),  -sin(lat) * sin(lon), cos(lat) * sin(lon)], 
           [0,         cos(lat),             sin(lat)]]
    mat = np.array(mat)
    # print(mat, enu)
    delta = mat @ np.array([e, n, u]).reshape(-1, 1)
    p0_xyz = np.array([*LLA_to_ECEF(*p0_LLA)]).reshape(-1, 1)
    # print("--------", delta, p0_xyz)
    return p0_xyz + delta


def observe_to_ECEF(station, aer):
    enu = local_to_enu(aer)
    r = enu_to_ECEF(station, enu)
    return r


if __name__ == "__main__":
    # lat, lon, h
    # obs = (13.0344804066577, 77.5111003779841, 0.839009756775573)
    # # aer = (132.67, 32.44, 16945.450)
    # aer = (123.08, 50.06, 37350.340)
    # # obs = (0, 0, 1000)
    # # aer = (0, 90, 1000)
    # ans = observe_to_ECEF(obs, aer)
    # print(ans)
    LLA_to_ECEF(36.2295894, 93.0661482, 746.049)
    print(pymap3d.ecef.geodetic2ecef(36.2295894, 93.0661482, 746.049))