def filter_VE(data):
    f=np.linspace(0,1,50000)
    a = np.ones(50000)
    
    #Switching power supply frequency (~290 KHz)
    a[2850:2950] =0
    
    #1.49 MHz
    a[14850:14950]=0
    
    #AM 1.37 MHz
    a[13650:13750] = 0
    
    #80 m Ham band (3.97 MHz)
    a[39650:39750] = 0
    
    #80 m Ham band (4 MHz)
    a[39950:40050]= 0
    
    a[-1]=0
    
    b=sigs.firwin2(3000,f,a)
    [h, fpoints] = sigs.freqz(b, 1, 50000,10E6)
    
    #Run the FIR filter
    vec = sigs.lfilter(b,1,data)
    return vec

def lab6_ex2():

    # set parameters of system
    fn = 1.0
    nt = 50 #number of taps
    freq = array([0,0.1,0.2,0.5,1.0]) #frequency sampling points
    gain = array([1.0,1.0,0.01,0.001,0])
    b = firwin2(nt,freq,gain,nyq=fn) #calc FIR coefficients with given frequency response
    a = array([1.0,0]) #(no feedback (FIR), poles at origin reduced to 1 for simplification)  

    # calc frequency response of filter and plot
    w,h = freqz(b,a)
    plot(w/pi, 20*log10(abs(h)),'b-')
    title('FIR of given frequency response')
    xlabel('normalized frequency (1 = fn)')
    ylabel('magnitude (dB scale)')
    grid()
    show()

    print('\nResponses in dB scale (all frequencies are relative to fn):\nf = 0, gain = 0 dB\nf = 0.1, gain = -1.05 dB\nf = 0.2, gain = -18.25 dB\nf = 0.5, gain = -56.4 dB\nf = 1.0, gain =~ -92 dB (filter tries to approach -inf and goes off the scale around f = 0.999)')
    
    print('\nThe filter has 49 zeros (the same as number of taps - 1)')
    print('The filter should also have 49 poles (all at the origin) so that it is causal. However, in this implementation (firwin2) there are no poles (except those added by the user)')
    
    # calc and show zeros and poles on zplane
    z,p,k = tf2zpk(b,a)
    zplane(z,p)
    title('zplane of FIR of given frequency response (50 taps)')
    show()

    print('\nFrom the zplane diagram, we can see pairs of conjugate pairs of zeros that follow along the unit circle. For each pair of conjugate pairs, one conjugate pair is inside of the unit circle and one is on the outside. As the frequency increases to fn, each pair of zeros generally gets closer and closer to the unit circle, thus attenuating the signal even further, up to a gain of 0 @ f = fn)')

def fir2FiltAlt(f1, f2, f3, f4, filterType, snd, fs, filterOrder):
    fs = float(fs)
    f1 = (f1 * 2) / fs
    f2 = (f2 * 2) / fs
    f3 = (f3 * 2) / fs
    f4 = (f4 * 2) / fs

    n = filterOrder

    if filterType == 'lowpass':
        f = [0, f3, f4, 1]
        m = [1, 1, 0.00003, 0]
    elif filterType == 'highpass':
        f = [0, f1, f2, 0.999999, 1] #high pass
        m = [0, 0.00003, 1, 1, 0]
    elif filterType == 'bandpass':
        f = [0, f1, f2, ((f2+f3)/2), f3, f4, 1]
        m = [0, 0.00003, 1, 1, 1, 0.00003, 0]
    elif filterType == 'bandstop':
        f = [0, f1, f2, ((f2+f3)/2), f3, f4, 0.999999, 1] #band stop
        m = [1, 1, 0.00003, 0, 0.00003, 1, 1, 0] 
   

    b = firwin2 (n,f,m);
    x = copy.copy(snd)
    x = convolve(snd, b, 1)
    #x[:, 1] = convolve(snd[:,1], b, 1)
    
    return x

def get_channelizer_taps(M, n_taps=100):
    taps = signal.firwin2(100, [0, 1.0/M, 1.0/M+0.05, 1], [1, 1, 0, 0])
    # Divide by integral of absolute values to prevent the possibility
    # of overflow.
    chantaps = [taps[i::M] for i in range(M)]
    scaledtaps, tapscalefactor = scale_taps(chantaps)
    return scaledtaps, tapscalefactor

 def test04(self):
     """Test firwin2 when window=None."""
     ntaps = 5
     # Ideal lowpass: gain is 1 on [0,0.5], and 0 on [0.5, 1.0]
     freq = [0.0, 0.5, 0.5, 1.0]
     gain = [1.0, 1.0, 0.0, 0.0]
     taps = firwin2(ntaps, freq, gain, window=None, nfreqs=8193)
     alpha = 0.5 * (ntaps - 1)
     m = np.arange(0, ntaps) - alpha
     h = 0.5 * sinc(0.5 * m)
     assert_array_almost_equal(h, taps)

    def test05(self):
        """Test firwin2 for calculating Type IV filters"""
        ntaps = 1500

        freq = [0.0, 1.0]
        gain = [0.0, 1.0]
        taps = firwin2(ntaps, freq, gain, window=None, antisymmetric=True)
        assert_array_almost_equal(taps[: ntaps // 2], -taps[ntaps // 2:][::-1])

        freqs, response = freqz(taps, worN=2048)
        assert_array_almost_equal(abs(response), freqs / np.pi, decimal=4)

 def test03(self):
     width = 0.02
     ntaps, beta = kaiserord(120, width)
     # ntaps must be odd for positive gain at Nyquist.
     ntaps = int(ntaps) | 1
     freq = [0.0, 0.4, 0.4, 0.5, 0.5, 1.0]
     gain = [1.0, 1.0, 0.0, 0.0, 1.0, 1.0]
     taps = firwin2(ntaps, freq, gain, window=("kaiser", beta))
     freq_samples = np.array([0.0, 0.4 - width, 0.4 + width, 0.45, 0.5 - width, 0.5 + width, 0.75, 1.0])
     freqs, response = freqz(taps, worN=np.pi * freq_samples)
     assert_array_almost_equal(np.abs(response), [1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0], decimal=5)

def firf(x, f_range, fs = 1000, w = 7, tw = .15):
    """
    Filter signal with an FIR filter

    x : array-like, 1d
        Time series to filter
    f_range : (low, high), Hz
        Cutoff frequencies of bandpass filter
    fs : float, Hz
        Sampling rate
    w : float
        Length of the filter in terms of the number of cycles of the oscillation
        whose frequency is the center of the bandpass filter
    tw : float
        Transition width of the filter in normalized frequency space

    Returns
    -------
    x_filt : array-like, 1d
        Filtered time series
    """
    
    if w <= 0:
        raise ValueError('Number of cycles in a filter must be a positive number.')
        
    if np.logical_or(tw < 0, tw > 1):
        raise ValueError('Transition width must be between 0 and 1.')        
        
    nyq = fs/2
    if np.any(np.array(f_range) > nyq):
        raise ValueError('Filter frequencies must be below nyquist rate.')
        
    if np.any(np.array(f_range) < 0):
        raise ValueError('Filter frequencies must be positive.')
        
    cf = np.mean(f_range)
    Ntaps = np.floor(w * fs / cf)
    if len(x) < Ntaps:
        raise RuntimeError('Length of filter is loger than data. Provide more data or a shorter filter.')
        
    # Characterize desired filter
    f = [0, (1-tw)*f_range[0]/nyq, f_range[0]/nyq, f_range[1]/nyq, (1+tw)*f_range[1]/nyq, 1]
    m = [0,0,1,1,0,0]
    if any(np.diff(f)<0):
        raise RuntimeError('Invalid FIR filter parameters. Please decrease the transition width parameter.')
    
    # Perform filtering
    taps = firwin2(Ntaps, f, m)
    x_filt = filtfilt(taps,[1],x)
    
    if any(np.isnan(x_filt)):
        raise RuntimeError('Filtered signal contains nans. Adjust filter parameters.')
        
    return x_filt

 def test02(self):
     width = 0.04
     beta = 12.0
     # ntaps must be odd for positive gain at Nyquist.
     ntaps = 401
     # An ideal highpass filter.
     freq = [0.0, 0.5, 0.5, 1.0]
     gain = [0.0, 0.0, 1.0, 1.0]
     taps = firwin2(ntaps, freq, gain, window=("kaiser", beta))
     freq_samples = np.array([0.0, 0.25, 0.5 - width, 0.5 + width, 0.75, 1.0])
     freqs, response = freqz(taps, worN=np.pi * freq_samples)
     assert_array_almost_equal(np.abs(response), [0.0, 0.0, 0.0, 1.0, 1.0, 1.0], decimal=5)

 def test01(self):
     width = 0.04
     beta = 12.0
     ntaps = 400
     # Filter is 1 from w=0 to w=0.5, then decreases linearly from 1 to 0 as w
     # increases from w=0.5 to w=1  (w=1 is the Nyquist frequency).
     freq = [0.0, 0.5, 1.0]
     gain = [1.0, 1.0, 0.0]
     taps = firwin2(ntaps, freq, gain, window=("kaiser", beta))
     freq_samples = np.array([0.0, 0.25, 0.5 - width / 2, 0.5 + width / 2, 0.75, 1.0 - width / 2])
     freqs, response = freqz(taps, worN=np.pi * freq_samples)
     assert_array_almost_equal(np.abs(response), [1.0, 1.0, 1.0, 1.0 - width, 0.5, width], decimal=5)

    def test06(self):
        """Test firwin2 for calculating Type III filters"""
        ntaps = 1501

        freq = [0.0, 0.5, 0.55, 1.0]
        gain = [0.0, 0.5, 0.0, 0.0]
        taps = firwin2(ntaps, freq, gain, window=None, antisymmetric=True)
        assert_equal(taps[ntaps // 2], 0.0)
        assert_array_almost_equal(taps[: ntaps // 2], -taps[ntaps // 2 + 1:][::-1])

        freqs, response1 = freqz(taps, worN=2048)
        response2 = np.interp(freqs / np.pi, freq, gain)
        assert_array_almost_equal(abs(response1), response2, decimal=3)

def design_filter(filter_type, f_p, f_w, filter_dur, window):
    if filter_type == 'highpass':
        f_s = f_p - f_w
        freq = [0., f_s, f_p, sfreq / 2.]
        gain = [0., 0., 1., 1.]
    else:
        f_s = f_p + f_w
        freq = [0., f_p, f_s, sfreq / 2.]
        gain = [1., 1., 0., 0.]

    n = int(sfreq * filter_dur)
    n += ~(n % 2)  # Type II filter can't have 0 attenuation at nyq

    h = signal.firwin2(n, freq, gain, nyq=sfreq / 2., window=window)
    return h

def lock2(f0, fp, fc, fs, coeff_ratio=8.0, coeffs=None,
          window='blackman', print_response=True):
    """Create a gentle fir filter. Pass frequencies below fp, cutoff frequencies
    above fc, and gradually taper to 0 in between."""

    # Convert to digital frequencies, normalizing f_nyq to 1,
    # as requested by scipy.signal.firwin2
    nyq = fs / 2
    fp = fp / nyq
    fc = fc / nyq

    if coeffs is None:
        coeffs = int(round(coeff_ratio / fc, 0))

    # Force number of tukey coefficients odd
    alpha = (1-fp*1.0/fc)
    n = int(round(1000. / alpha) // 2)

    N = n * 2 + 1
    f = np.linspace(0, fc, n+1)

    fm = np.zeros(n + 2)
    mm = np.zeros(n + 2)
    fm[:-1] = f
    # Append fm = nyquist frequency by hand; needed by firwin2
    fm[-1] = 1.
    m = signal.tukey(N, alpha=alpha)
    # Only take the falling part of the tukey window,
    # not the part equal to zero
    mm[:-1] = m[n:]

    # Use approx. 8x more frequencies than total coefficients we need
    nfreqs = 2**(int(round(np.log2(coeffs)))+3)+1

    b = signal.firwin2(coeffs, fm, mm,
                       nfreqs=nfreqs,
                       window=window)

    # Force filter gain to 1 at DC; corrects for small rounding errors
    b = b / np.sum(b)

    w, rep = signal.freqz(b, worN=np.pi*np.array([0., fp/2, fp, fc, 2*fc,
                                                  0.5*f0/nyq, f0/nyq, 1.]))
    if print_response:
        print("Response:")
        _print_magnitude_data(w, rep, fs)

    return b

def computefir(fc, L: int, ofn, fs: int, method: str):
    """
    bandpass FIR design

    https://docs.scipy.org/doc/scipy/reference/generated/scipy.signal.firwin.html
    http://docs.scipy.org/doc/scipy/reference/generated/scipy.signal.remez.html

    L: number of taps

    output:
    b: FIR filter coefficients
    """

    assert len(fc) == 2, 'specify lower and upper bandpass filter corner frequencies in Hz.'

    if method == 'remez':
        b = signal.remez(numtaps=L,
                         bands=[0, 0.9*fc[0], fc[0], fc[1], 1.1*fc[1], 0.5*fs],
                         desired=[0, 1, 0],
                         Hz=fs)
    elif method == 'firwin':
        b = signal.firwin(L, [fc[0], fc[1]],
                          window='blackman',
                          pass_zero=False, nyq=fs//2)
    elif method == 'firwin2':
        b = signal.firwin2(L, [0, fc[0], fc[1], fs//2], [0, 1, 1, 0],
                           window='blackman',
                           nyq=fs//2,
                           # antisymmetric=True,
                           )
    else:
        raise ValueError(f'unknown filter design method {method}')

    if ofn:
        ofn = Path(ofn).expanduser()
        print(f'writing {ofn}')
# FIXME make binary
        with ofn.open('w') as h:
            h.write(f'{b.size}\n')  # first line is number of coefficients
            b.tofile(h, sep=" ")  # second line is space-delimited coefficents

    return b

def _matched_filters(ks, x_m, N_pts, dec=16, window='hann', n_pts_eval_fir=48000):
    ks = ks / dec
    N = N_pts // dec
    k = np.linspace(0, ks/2, n_pts_eval_fir)

    resp_ac = _j1filt(k * 2 * np.pi * x_m)

    fir_ac_dec = signal.firwin2(N, k, resp_ac, nyq=k[-1], window=window)
    fir_dc_dec = signal.firwin(N, jn_zeros(0, 1)[0] / (2*np.pi*x_m),
                               nyq=k[-1], window=window)

    # Manually force gain to 1 at DC; firwin2 rounding errors probable cause of
    # minor losses (< 1 percent)
    fir_ac_dec = fir_ac_dec / np.sum(fir_ac_dec)
    fir_dc_dec = fir_dc_dec / np.sum(fir_dc_dec)

    fir_ac = np.fft.irfft(np.fft.rfft(fir_ac_dec), fir_ac_dec.size * dec)
    fir_dc = np.fft.irfft(np.fft.rfft(fir_dc_dec), fir_dc_dec.size * dec)

    return fir_ac, fir_dc

def get_taps(N, R, M, ntaps=128, cutoff=0.45):
    """
    Find the coefficients of the half-band FIR filter that compensate the CIC filter from 0 to cutoff
    N : number of CIC stages
    R : decimation rate
    M : differential delay in the comb section stages of the filter
    """
    f = np.arange(2048) / 2047.
    cic_response = lambda f : abs( M/R * (np.sin((f*R)/2)) / (np.sin((f*M)/2.)) )**N if f !=0 else 1

    H = np.array(map(cic_response, f*np.pi))

    # Define frequency reponse of ideal compensation filter
    H = np.array(map(cic_response, f*np.pi / R))
    Hc = 1/H * (f < cutoff)

    beta = 8
    taps = signal.firwin2(ntaps, f, Hc, nfreqs = 1025, window=('kaiser', beta))
    taps /= np.sum(taps)
    return taps, cic_response

	def _built_in_filter_design(self,f_ch):
		"""
		Design basic shape-matching 127th order FIR filter.
		
		The filter will attempt to match the following gain envelope:
		    w = [0, s1, p1, p2, s1, 1]
		    h = [0,  0,  1,  1,  0, 0]
		where
		    s1 = f_ch - DIGITAL_CHANNEL_WIDTH*0.6
		    p1 = f_ch - DIGITAL_CHANNEL_WIDTH*0.4
		    p2 = f_ch + DIGITAL_CHANNEL_WIDTH*0.4
		    s2 = f_ch + DIGITAL_CHANNEL_WIDTH*0.6,
		h is normalized to a maximum of 1, and w is the angular frequency 
		normalized to pi.
		
		Parameters
		----------
		f_ch : float
		    Center frequency of bandpass filter.
		
		Returns
		-------
		B : ndarray
		    FIR filter coefficients.
		"""
		# filter channel should be at least more than digital bandwidth from sampled boundaries
		f_lower = self.DIGITAL_CHANNEL_WIDTH
		f_upper = self.ADC_SAMPLE_RATE/2-self.DIGITAL_CHANNEL_WIDTH
		if f_ch <= f_lower or f_ch >= f_upper:
			raise RuntimeError("Digital channel center frequency is {0:7.3f}MHz, but should be within ({1:7.3f},{2:7.3f}) MHz".format(f_ch/1e6,f_lower/1e6,f_upper/1e6))
		# construct envelope
		f_pass = f_ch + array([-1,1])*self.DIGITAL_CHANNEL_WIDTH*0.4
		f_stop = f_ch + array([-1,1])*self.DIGITAL_CHANNEL_WIDTH*0.6
		w_pass = f_pass/(self.ADC_SAMPLE_RATE/2)
		w_stop = f_stop/(self.ADC_SAMPLE_RATE/2)
		filt_gain = array([0,0,1,1,0,0])
		filt_freq = concatenate(([0],[w_stop[0]], w_pass, [w_pass[1]], [1.0]))
		B = firwin2(128,filt_freq,filt_gain,window='boxcar')
		# normalize to absolute maximum of 0.5
		B = 0.5*B/(abs(B).max())
		return B

    def _design_window(self):
        self._nyquist = self.sample_rate / 2

        # firwin2 requires that the freqs vector begin at 0 and end at nyquist.
        # therefore we will add those manually if they're not there.

        if len(self.freqs) != len(self.gains):
            raise ValueError("Lengths of freqs and gains should be the same.")

        self.freqs = [float(freq) for freq in self.freqs]
        self.gains = [float(gain) for gain in self.gains]

        if self.freqs[0] != 0:
            self.freqs.insert(0,0)

        if self.freqs[-1] != self._nyquist:
            self.freqs.append(self._nyquist)

        # Pad the filter appropriately.
        my_type = self.get_filter_type()
        print("Type of filter is ", my_type)

        if my_type == 2 or my_type == 3:
            if self.gains[-1] != 0:
                self.gains.append(0)
        if my_type == 3 or my_type == 4:
            if self.gains[0] != 0:
                self.gains.insert(0,0)

        while len(self.freqs) > len(self.gains):
            print("Had to pad so that len freqs = len gains...")
            self.gains.append(0)

        print("freqs vector became ", self.freqs)
        print("gains vector became ", self.gains)

        self.B = signal.firwin2(self.taps, self.freqs, self.gains,
                                window=self.window, nyq=self._nyquist,
                                antisymmetric=self.antisymmetric)

def build_filter():
    die_window = 3
    live_window = 4
    live_gain = 2 ** 0.5

    freq_full = [0, 3, 5,
            50-live_window, 50-die_window, 50+die_window, 50+live_window,
            100-live_window, 100-die_window, 100+die_window, 100+live_window,
            nyq]

    gain_full = [0, 0, 2 ** 0.5,
            live_gain, 0, 0, live_gain,
            live_gain, 0, 0, live_gain,
            1]

    freq_low = [0, 3, 5,
               40, 45, nyq]

    gain_low = [0, 0, live_gain,
               live_gain, 0, 0]

    b = signal.firwin2(sfreq, freq_low, gain_low, nyq=nyq, antisymmetric=False)
    return b

import numpy as np
import scipy.signal as signal
import matplotlib.pyplot as plt

b = signal.firwin2(150, [0.0, 0.3, 0.6, 1.0], [1.0, 2.0, 0.5, 0.0])
w, h = signal.freqz(b)

plt.title('Digital filter frequency response')
plt.plot(w, np.abs(h))
plt.title('Digital filter frequency response')
plt.ylabel('Amplitude Response')
plt.xlabel('Frequency (rad/sample)')
plt.grid()
plt.show()