'''
通用算法
'''
import numpy as np
import math

def FindInfomationFromJson(HeaderFile_dom_json,node_path_list):
    '''
    在Json文件中，按照指定路径解析出制定节点
    '''
    result_node=HeaderFile_dom_json
    for nodename in node_path_list:
        result_node=result_node[nodename]
    return result_node

def GetVectorNorm(Vecter):
    '''
    得到向量的模
    '''
    Vecter=Vecter.reshape(-1,1)
    Vecter_Norm_pow=np.matmul(Vecter.T,Vecter)
    return np.sqrt(Vecter_Norm_pow) 
    
def appendInfomationForCsv(appendtext,FilePath="./GridBlock"):
    '''在文件末尾追加信息
    Args:
        appendtext: 需要附加的字符串信息
        FilePath: 文件路径
    return:
        state：文件状态
    '''
    with open(FilePath,'a',encoding='utf-8') as fp:
        fp.write("{}\n".format(appendtext))
        return True
    return False
    
def LLA_to_XYZ(self,latitude, longitude, altitude):
    ''' 经纬度转大地坐标系
    args:
        latitude:纬度
        longitude:经纬
        altitude:海拔高程
    refrence: https://blog.csdn.net/dulingwen/article/details/96868530
    '''
    # 经纬度的余弦值
    cosLat = math.cos(latitude * math.pi / 180)
    sinLat = math.sin(latitude * math.pi / 180)
    cosLon = math.cos(longitude * math.pi / 180)
    sinLon = math.sin(longitude * math.pi / 180)
    
    # WGS84坐标系的参数
    rad = 6378137.0    #地球赤道平均半径（椭球长半轴：a）
    f = 1.0 / 298.257224   # WGS84椭球扁率 :f = (a-b)/a
    C = 1.0 / math.sqrt(cosLat * cosLat + (1-f) * (1-f) * sinLat * sinLat)
    S = (1-f) * (1-f) * C
    h = altitude
    
    # 计算XYZ坐标
    X = (rad * C + h) * cosLat * cosLon
    Y = (rad * C + h) * cosLat * sinLon
    Z = (rad * S + h) * sinLat
    
    return np.array([X, Y, Z]).reshape(1,3)
   



""" def XYZ_to_LLA(X, Y, Z):
    ''' 大地坐标系转经纬度
    适用于WGS84坐标系
    args:
        x,y,z
    return:
        lat,long,altitude
    '''
    # WGS84坐标系的参数
    a = 6378137.0        # 椭球长半轴
    b = 6356752.314245   # 椭球短半轴
    ea = np.sqrt((a ** 2 - b ** 2) / a ** 2)
    eb = np.sqrt((a ** 2 - b ** 2) / b ** 2)
    p = np.sqrt(X ** 2 + Y ** 2)
    theta = np.arctan2(Z * a, p * b)
    
    # 计算经纬度及海拔
    longitude = np.arctan2(Y, X)
    latitude = np.arctan2(Z + eb ** 2 * b * np.sin(theta) ** 3, p - ea ** 2 * a * np.cos(theta) ** 3)
    N = a / np.sqrt(1 - ea ** 2 * np.sin(latitude) ** 2)
    altitude = p / np.cos(latitude) - N
    
    return np.array([np.degrees(latitude), np.degrees(longitude), altitude]) """
 

def XYZ_to_LLA(x, y,z):
    epsilon = 0.000000000000001
    pi = 3.14159265358979323846
    d2r = pi / 180
    r2d = 180 / pi

    a = 6378137.0
    f_inverse = 298.257223563  
    b = a - a / f_inverse

    e = np.sqrt(a * a - b * b) / a


    tmpX =  x
    temY = y 
    temZ = z

    curB = 0
    N = 0; 
    calB = math.atan2(temZ, math.sqrt(tmpX * tmpX + temY * temY)); 

    counter = 0
    while (abs(curB - calB) * r2d > epsilon  and counter < 25):
        curB = calB
        N = a / math.sqrt(1 - e * e * math.sin(curB) * math.sin(curB));
        calB = math.atan2(temZ + N * e * e * math.sin(curB), math.sqrt(tmpX * tmpX + temY * temY));
        counter=counter+1

    x = math.atan2(temY, tmpX) * r2d
    y = curB * r2d
    z = temZ / math.sin(curB) - N * (1 - e * e); 
    result= np.array([x,y, z])
    return result