import numpy as np
from scipy import signal


class AoAConverter:
    def __init__(self):
        self.p = [1e8, 1e8, 1e8]

    def to_cartesian(self, theta_rad, phi_rad):
        # theta_rad = np.radians(theta)
        # phi_rad = np.radians(phi)
        # 注意！程序输入的是弧度单位
        """将球坐标转换为直角坐标"""
        x = np.sin(theta_rad) * np.cos(phi_rad)
        y = np.sin(theta_rad) * np.sin(phi_rad)
        z = np.cos(theta_rad)
        pc =np.array([x,y,z])
        return  pc

    def calc_error(self, pc, mc):
        # 计算预测坐标与实际观测坐标之间的差的平方
        mc = np.expand_dims(mc, axis=1)
        diff_squared = (pc - mc) ** 2
        # 对差值的平方求和，得到误差的平方
        error_squared = np.sum(diff_squared, axis=0)
        # 开平方根得到误差
        return np.sqrt(error_squared)

    import numpy as np

    def find_best_r(self, theta, phi, mc, r_range):
        """在给定范围内搜索最优的 r 值"""
        # 将 r_range 转换为 NumPy 数组，以便进行矢量化操作
        r_values = np.array(r_range)
        # 计算所有可能的直角坐标
        pc = self.to_cartesian(theta, phi)
        # 进行维度扩充以进行矩阵乘法
        r_values = np.expand_dims(r_values, axis=0)
        pc = np.expand_dims(pc, axis=1)
        # 计算所有 r 值对应的误差
        # print([pc.shape,r_values.shape])
        D = np.dot(pc, r_values)
        errors = self.calc_error(D, mc)
        r_values = np.squeeze(r_values)

        # 找到最小误差及其对应的 r 值
        min_error = np.min(errors)
        best_r = r_values[np.argmin(errors)] #因为多加了一维，所以这里要反求0

        return [best_r,min_error]

    def projected_measure(self,theta, phi, r,p0):
        pc = self.to_cartesian(theta, phi)
        neo_p = r*pc + p0
        return np.array(neo_p)

converter = AoAConverter()


def calculate_euclidean_distances(A, BX):
    # 计算A和B之间的欧式距离
    B = BX['data_matrix']
    N = B.shape[0]
    r_range = np.linspace(-5, 5, 100)
    if BX.get('AOA_pos'):
        # 若是来自AOA的数据，则进行替换
        sensor_pos = BX.get('AOA_pos')
        ob_pos = A - sensor_pos
        r0 = np.linalg.norm(ob_pos)
        B_new = []
        for i in range(N):
            theta = B[i,0]
            phi = B[i,1]
            [best_r,min_error] = converter.find_best_r(theta, phi,ob_pos, r0+r_range)
            print(min_error)
            B_new.append(converter.projected_measure(theta, phi,best_r,sensor_pos))
        B_new = np.array(B_new)
    else:
        B_new = B


    distances = np.linalg.norm(A - B_new, axis=1)
    # 找到最小距离及其索引
    min_distance_index = np.argmin(distances)
    min_distance = distances[min_distance_index]
    return [min_distance, min_distance_index, B_new]

def are_lists_equal(listA, listB):
    # 对两个列表中的子列表进行排序
    if len(listA) == 0:
        return False
    sorted_listA = sorted(listA, key=lambda x: (x[0], x[1]))
    sorted_listB = sorted(listB, key=lambda x: (x[0], x[1]))
    # 比较排序后的列表是否相等
    return sorted_listA == sorted_listB

def sigmoid(x, a=10, b=0.1):
    # 调整Sigmoid函数使其在x=4时值为0.5
    # a和b是调整参数，用于控制函数的形状
    return 1 / (1 + np.exp(-a * (x - 1))) + b


def calculate_correlation(A, B):
    """
    计算两个数组矩阵所有列的相关系数的最大值。

    参数:
    A -- 第一个NumPy数组
    B -- 第二个NumPy数组
    """
    A = np.exp(-1j*A/50)
    B = np.exp(1j*B/50)
    corr_res = []
    for col in range(3):
        a = A[:, col]
        b = B[:, col]
        convolution = signal.convolve(a, b[::-1])
        corr_res.append(convolution)
    max_corr = np.sum(np.abs(np.array(corr_res)),0)
    max_corr = np.max(max_corr)/3

    return max_corr


def calculate_history_distances(target, b):
    # 使用前后向的形式进行计算
    A = target.m_history
    v = target.v
    # 计算每一行与向量b的差的L2范数（欧氏距离）
    if A.shape[0] < 10:
        return np.inf
    local_time =  np.linspace(0, 10, 20)
    local_time =  np.expand_dims(local_time, axis=1)
    v = np.expand_dims(v, axis=1)
    A_pre = A[-10:,0:3]
    A_post = np.dot(local_time,v.T)
    A_all = np.vstack((A_pre, A_post))
    distances = np.linalg.norm(A_all - b, axis=1)
    # 找到最小距离
    min_distance = np.min(distances)

    return min_distance