import numpy as np
from numpy.linalg import LinAlgError
from numpy import seterr
seterr(all='raise')


def get_toa_gdop(bs_list, xyz):
    """
    compute the gdop of the toa
    :param bs_list: the base station coordinate
    :param xyz: the location coordinate of the tag
    :return: the gdop value
    """
    bs_n = len(bs_list)
    if bs_n < 3:
        return None
    h1 = np.mat(np.zeros((bs_n, 3)))

    for i in range(0, len(bs_list)):
        ri = np.sqrt((bs_list[i][0] - xyz[0]) ** 2 +
                     (bs_list[i][1] - xyz[1]) ** 2)
        if ri == 0:
            h1[i, 0] = 0
            h1[i, 1] = 0
            h1[i, 2] = 1
            # h1[i, 3] = 1

        else:
            h1[i, 0] = (bs_list[i][0] - xyz[0]) / ri
            h1[i, 1] = (bs_list[i][1] - xyz[1]) / ri
            h1[i, 2] = 1
            # h1[i, 3] = 1

    m_tmp = h1.T * h1
    gdop_value = None

    try:
        g = m_tmp.I
        gdop_value = np.sqrt(g[0, 0] + g[1, 1])
        # print(gdop_value)
    finally:
        return gdop_value


# 计算GDOP 不考虑时钟误差版本即DOP
def calculate_gdop(uavs, estimate_value):
    """
    # :param point: 待定位点真实坐标
    :param uavs: 无人机坐标集
    :param estimate_value: 解算出的坐标
    :return: gdop
    """
    # estimate_value.append(0)
    estimate_value=[estimate_value[0],estimate_value[1],0]
    var1 = np.array(uavs) - estimate_value
    # 计算距离列向量
    d_estimate = (np.sum(var1**2, axis=1)**0.5).reshape(-1, 1)
    # 计算visible MATRIX
    H = var1 / d_estimate
    GDOP = float('inf')
    try:
        GDOP = np.trace(np.linalg.inv(np.dot(np.transpose(H), H))) ** 0.5
    except:
        GDOP=float('inf')
    finally:
        return GDOP



if __name__ == '__main__':
    bs_list=[[1,1,1],[2,3,4],[6,6,5]]
    bs_list2 = [[1, 3, 3], [3, 1, 3], [5, 4, 3]]
    xyz=[3,3.167,0]
    print(calculate_gdop(bs_list,xyz))
    print(get_toa_gdop(bs_list2,xyz))