import numpy as np
# from scipy import signal

def nlms(x,d,M,w_cut,alpha,beta):
    w = np.zeros(M)
    sound_length = len(x)
    N = sound_length
    xn = x[0:M]
    nor_xn = np.matmul(xn.T,xn)
    dn = d[0:M]
    nor_dn = np.matmul(dn.T,dn)
    en = dn
    nor_en = np.matmul(en.T,en)
    e = np.zeros(N)
    e[0:M] = dn
    y = np.zeros(N)
    mis=np.zeros(N)
    ERLE= np.zeros(N)
    for n in range(M,sound_length):
        xn = x[n:n-M:-1]
        y[n]=np.matmul(xn.T,w)
        e[n]=d[n]-y[n]
        miu = alpha / (beta + nor_xn)
        w = w + miu * xn * e[n]
        mis[n]=np.power(np.linalg.norm(w-w_cut),2)/np.power(np.linalg.norm(w_cut),2)
        if nor_en-np.spacing(1) >0:
            max0 = nor_en
        else:
            max0 = np.spacing(1)
        tea = nor_dn/(max0+beta)
        ERLE[n] = 10 * np.log10(tea+beta)
        nor_xn = nor_xn + x[n] * x[n] - x[n - M] * x[n - M]
        nor_dn = nor_dn + d[n] * d[n] - d[n - M] * d[n - M]
        nor_en = nor_en + e[n] * e[n] - e[n - M] * e[n - M]
    return e,w,mis,ERLE

def FLMS(x,d,w_cut,alpha,lambda1):
    """
        input
        :param x: 输入信号
        :param d: 期望信号
        :param w_cut: 截止频率
        :param alpha: 步长
        :param lambda1: 遗忘因子
        return
        :param e: 滤波器误差
        :param w: 滤波器系数
        :param mis: 失调系数
        :param ERLE：信号增益
        """
    #初始化自适应滤波器的系数为0，估计输入信号的功率为0
    estimated_power = 0.01
    filter_length = len(w_cut)  #滤波器长度
    FILTER_COEFF = np.zeros(filter_length*2)   #滤波器系数
    input_length = len(x)  #输入信号长度

    #np.floor（）函数用于向下取整至最接近的整数。
    #block_length为整个信号能够被滤波器长度整除的最大整数倍数的长度。这个长度将用于对输入信号进行分块处理。
    block_length = np.floor(input_length / filter_length) * filter_length  #分块长度
    x = x[0:int(block_length)]  #信号x（包含待滤波信号和干扰信号）
    d = d[0:int(block_length)]
    x = x[:]
    d = d[:]
    e = d
    Blocks = int(block_length / filter_length)  #分块数量
    mis = np.zeros(Blocks)
    ERLE = np.zeros(Blocks)
    for k in range(int(Blocks - 1)):

        #对第k个数据块，计算当前输入xt，进行FFT变换以得到频域信号xn
        xt = x[k * filter_length:(k + 2) * filter_length]
        xn = np.fft.fft(xt, 2 * filter_length)

        #将 xn 与滤波器系数 FILTER_COEFF 进行乘法得到输出信号 output，进行 IFFT 反变换得到时域信号 output1
        output = np.fft.ifft(np.multiply(xn, FILTER_COEFF))
        output1 = output[filter_length:2 * filter_length].real

        #取期望信号 d 的下一个数据块 desired_vec，计算误差信号 e，
        desired_vec = d[(k + 1) * filter_length:(k + 2) * filter_length]  # 2048
        AAAA = desired_vec * desired_vec
        e[(k + 1) * filter_length:(k + 2) * filter_length] = desired_vec - output1

        # 根据 e 做 FFT 并更新估计功率 estimated_power
        e_VEC = np.fft.fft(
            np.concatenate((np.zeros(filter_length), e[(k + 1) * filter_length:(k + 2) * filter_length]), axis=0),
            2 * filter_length)
        estimated_power = lambda1 * estimated_power + (1 - lambda1) * np.power(np.abs(xn), 2)

        # 以此计算出滤波器权值的目标 DESIRED_VEC
        DESIRED_VEC = np.ones(estimated_power.shape[0]) / (1 + estimated_power)

        #根据目标权值和误差向量 e_VEC，通过 FFT 得到频域下的领域权值 phivec，
        # 再通过 IFFT 计算得到 phivec 的时域权值，
        # 并将其与 FILTER_COEFF 做加权运算得到新的滤波器系数 FILTER_COEFF
        phivec = np.fft.ifft(np.multiply(np.multiply(DESIRED_VEC, xn.conjugate()), e_VEC), 2 * filter_length)
        phivec = phivec[0:filter_length]
        FILTER_COEFF = FILTER_COEFF + alpha * np.fft.fft(np.concatenate((phivec, np.zeros(filter_length)), axis=0),
                                                         2 * filter_length)
        #根据新的滤波器系数 FILTER_COEFF 更新实数域的滤波器权值 w，并计算失调系数 mis 和信号增益 ERLE
        e = np.real(e[:])
        w = np.fft.ifft(FILTER_COEFF)
        w = np.real(w[0:int(len(FILTER_COEFF) / 2)])
        mis[k] = np.power(np.linalg.norm(w - w_cut), 2) / np.power(np.linalg.norm(w_cut), 2)
        a1 = np.multiply(e[(k + 1) * filter_length:(k + 2) * filter_length],
                         e[(k + 1) * filter_length:(k + 2) * filter_length])
        MM = np.sum(np.multiply(e[(k + 1) * filter_length:(k + 2) * filter_length],
                                e[(k + 1) * filter_length:(k + 2) * filter_length]))
        #np.spacing(1)：产生一个无穷小的随机数
        #该代码是为了确保 max0 不会小于一个极小值，同时尽可能地设定为 MM
        if MM - np.spacing(1) > 0:
            max0 = MM
        else:
            max0 = np.spacing(1)
        tea = np.sum(AAAA) / (max0+0.00001)
        ERLE[k] = 10 * np.log10(tea+0.00001)
    return e,w,mis,ERLE

def AP(x,d,K,P,w_cut,mu,epsilon):
    # 定义函数AP，输入参数为x、d、K、P、w_cut、mu和epsilon
    L = len(x)
    # 计算输入x的长度L
    w = np.zeros((L, P))
    # 初始化滤波器系数矩阵w，大小为L*P
    e = np.zeros(L)
    y = np.zeros(L)
    xvec = np.zeros(P)
    X = np.zeros((K, P))
    dvec = np.zeros(K)
    y = np.zeros(L)
    mis = np.zeros(L)
    ERLE = np.zeros(L)
    # 初始化其他变量（e、y、xvec、X、dvec、y、mis和ERLE），大小分别为L、L、P、K*P、K、L、L和L
    for i in range(L-2):
        # 循环遍历输入x的每个元素，从0到L-3
        xvec = np.append(x[i],xvec[0:P-1])
        # 将输入x的前P个元  放入向量xvec，将其余元素用0填充
        c = []
        c.append([xvec])
        c.append([X[0,:]])
        X = np.concatenate(c)
        # 更新矩阵X，将向量xvec和矩阵X的第一行合并，得到新的矩阵X
        y[i] = np.matmul(w[i,:] , xvec)
        # 计算输出y的i-th元素，使用上一步中计算的滤波器系数w和输入向量xvec
        e[i] = d[i] - y[i]
        # 计算误差信号e的i-th元素，使用目标信号d和上一步中计算的输出信号y
        dvec = np.append(d[i],dvec[0:K-1])
        evec = dvec - np.matmul(X , w[i,:])
        # 计算向量dvec和误差向量evec，使用当前的目标信号d、矩阵X、以及当前的滤波器系数w
        upd1 = np.matmul(mu * X.T,np.linalg.inv(epsilon * np.eye(K)+np.matmul(X,X.T)))
        upd = np.matmul(upd1,evec)
        # 计算更新量upd，使用前面计算的向量evec和矩阵X
        w[i + 1,:] = w[i,:] + upd.T
        # 更新滤波器系数w，使用前面计算的更新量upd和当前的滤波器系数w
        mis[i] = np.power(np.linalg.norm(w[i,:] - w_cut),2)/np.power(np.linalg.norm(w_cut), 2)
        # 计算方差压缩误差MIS（Misadjustment），使用当前的滤波器系数w、以及真实滤波器系数w_cut
        MM = np.sum(np.multiply(evec, evec))
        if MM - np.spacing(1) > 0:
            max0 = MM
        else:
            max0 = np.spacing(1)
        # 计算最大误差max0，避免出现除以0的情况
        ERLE[i] = 10 * np.log10(np.sum(np.multiply(dvec, dvec))/ (max0+0.00001)+0.00001)
        # 计算剩余误差增益（ERLE），使用当前的目标信号d和最大误差max0
    return e,w,mis,ERLE
    # 返回结果，包括误差信号e、滤波器系数w、MIS（Misadjustment）和ERLE（剩余误差增益）


def zfft(x, f0, fs, N):
    D = 32
    x = x[0:N-1]
    x1 = np.zeros(N)
    for n in range(N-1):  # 复调制
        x1[n] = x[n] *np.exp(-1j*n*2*np.pi*f0/fs)
    bw = fs/(2*D)
    b = signal.firwin(32, 2*bw/fs/np.pi)  # FIR
    y = signal.lfilter(b, 1, x1)  # 低通滤波
    Nd = 16
    yd = np.zeros(Nd)
    for i in range (len(yd)-1):
        yd[i] = y[i*D]

    ydd = np.zeros(Nd+N-32)  # 重采样
    for i in range(Nd):
        ydd[i] = yd[i]
    for i in range(N-32):
        ydd[i+Nd] = 0
    Yd = np.fft.fft(yd, N)  # FFT

    Ymag = np.abs(Yd)
    Ymag = list(Ymag)
    index = Ymag.index(max(Ymag))
    freq = (index - 1) * fs / (N * D) + f0
    return freq

def cal_howl_feature(fftf,type):
    global len, ratio
    fftframe = np.abs(fftf)
    threshold = -5
    nebor = 12
    harmonic = 1.2
    len = len(fftframe) / 2
    ratios = np.zeros(len, 1)
    for i in range(len - 1):
        if type == 'PHPR':
            if i < (len/harmonic):
                ratio = 20 * np.log10(fftframe(i)/fftframe(np.floor(i*harmonic)))
            else:
                ratio = [ ]
        if type == 'PNPR':
            if i < nebor:
                ratio = 20 * np.log10(fftframe(i)/fftframe(i + nebor - 1))
            else:
                if i > len - nebor:
                    ratio = 20 * np.log10(fftframe(i) / fftframe(i - nebor + 1))
                else:
                    PNPRu = 20 * np.log10(fftframe(i)/fftframe(i + nebor - 1))
                    PNPRl = 20 * np.log10(fftframe(i) / fftframe(i - nebor + 1))
                    ratio = (PNPRl+PNPRu)/2
        if ratio > threshold:
            ratios[i] = ratio

    return ratios