#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""!
Author: J.A. de Jong - ASCEE V.O.F.

Description: Filter design implementation of common biquad filters that are
often used in parametric equalizers.

Major source is Audio EQ Cookbook: 
    https://archive.is/20121220231853/http://www.musicdsp.org/
    files/Audio-EQ-Cookbook.txt

The definition of the BiQuad filter coefficients as coming out of these
functions defines the filter as:

y[n] = 1/ba[3] * (  ba[0] * x[n]   + ba[1] * x[n-1] + ba[2] * x[n-2] +
                  + ba[4] * y[n-1] + ba[5] * y[n-2] 
                 )

*Note that all filters are normalized such that ba[3] is by definition equal to
1.0!*


"""
__all__ = ['peaking', 'biquadTF', 'notch', 'lowpass', 'highpass',
           'highshelf', 'lowshelf']

from numpy import array, cos, pi, sin, sqrt
from scipy.interpolate import interp1d
from scipy.signal import sosfreqz


def peaking(fs, f0, Q, gain):
    """
    Design of peaking biquad filter

    Args:
        fs: Sampling frequency [Hz]
        f0: Center frequency
        Q: Quality factor (~ inverse of bandwidth)
        gain: Increase in level at the center frequency [dB]
    """
    A = sqrt(10**(gain/20))
    omg0 = 2*pi*f0/fs
    alpha = sin(omg0)/Q/2
    b0 = 1+alpha*A
    b1 = -2*cos(omg0)
    b2 = 1-alpha*A
    a0 = 1 + alpha/A
    a1 = -2*cos(omg0)
    a2 = 1-alpha/A

    return array([b0/a0, b1/a0, b2/a0, a0/a0, a1/a0, a2/a0])


def notch(fs, f0, Q, gain=None):
    """
    Notch filter, parameter gain not used

    Args:
        fs: Sampling frequency [Hz]
        f0: Center frequency [Hz]
        Q: Quality factor (~ inverse of bandwidth)
    """
    omg0 = 2*pi*f0/fs
    alpha = sin(omg0)/Q/2
    b0 = 1
    b1 = -2*cos(omg0)
    b2 = 1
    a0 = 1 + alpha
    a1 = -2*cos(omg0)
    a2 = 1 - alpha
    return array([b0/a0, b1/a0, b2/a0, a0/a0, a1/a0, a2/a0])


def lowpass(fs, f0, Q, gain=None):
    """
    Second order low pass filter, parameter gain not used

    Args:
        fs: Sampling frequency [Hz]
        f0: Cut-off frequency [Hz]
        Q: Quality factor (~ inverse of bandwidth)
    """
    w0 = 2*pi*f0/fs
    alpha = sin(w0)/Q/2
    b0 = (1 - cos(w0))/2
    b1 = 1 - cos(w0)
    b2 = (1 - cos(w0))/2
    a0 = 1 + alpha
    a1 = -2*cos(w0)
    a2 = 1 - alpha
    return array([b0/a0, b1/a0, b2/a0, a0/a0, a1/a0, a2/a0])


def highpass(fs, f0, Q, gain=None):
    """
    Second order high pass filter, parameter gain not used

    Args:
        fs: Sampling frequency [Hz]
        f0: Cut-on frequency [Hz]
        Q: Quality factor (~ inverse of bandwidth)
    """
    w0 = 2*pi*f0/fs
    alpha = sin(w0)/Q/2

    b0 = (1 + cos(w0))/2
    b1 = -(1 + cos(w0))
    b2 = (1 + cos(w0))/2
    a0 = 1 + alpha
    a1 = -2*cos(w0)
    a2 = 1 - alpha
    return array([b0/a0, b1/a0, b2/a0, a0/a0, a1/a0, a2/a0])


def highshelf(fs, f0, Q, gain):
    """
    High shelving filter 

    Args:
        fs: Sampling frequency [Hz]
        f0: Cut-on frequency [Hz]
        Q: Quality factor (~ inverse of bandwidth)
        gain: Increase in level w.r.t. "wire" [dB]
    """
    w0 = 2*pi*f0/fs
    alpha = sin(w0)/Q/2
    A = 10**(gain/40)
    b0 = A*((A+1) + (A-1)*cos(w0) + 2*sqrt(A)*alpha)
    b1 = -2*A*((A-1) + (A+1)*cos(w0))
    b2 = A*((A+1) + (A-1)*cos(w0) - 2*sqrt(A)*alpha)
    a0 = (A+1) - (A-1)*cos(w0) + 2*sqrt(A)*alpha
    a1 = 2*((A-1) - (A+1)*cos(w0))
    a2 = (A+1) - (A-1)*cos(w0) - 2*sqrt(A)*alpha
    return array([b0/a0, b1/a0, b2/a0, a0/a0, a1/a0, a2/a0])


def lowshelf(fs, f0, Q, gain):
    """
    Low shelving filter 

    Args:
        fs: Sampling frequency [Hz]
        f0: Cut-on frequency [Hz]
        Q: Quality factor (~ inverse of bandwidth)
        gain: Increase in level w.r.t. "wire" [dB]
    """
    w0 = 2*pi*f0/fs
    alpha = sin(w0)/Q/2
    A = 10**(gain/40)
    b0 = A*((A+1) - (A-1)*cos(w0) + 2*sqrt(A)*alpha)
    b1 = 2*A*((A-1) - (A+1)*cos(w0))
    b2 = A*((A+1) - (A-1)*cos(w0) - 2*sqrt(A)*alpha)
    a0 = (A+1) + (A-1)*cos(w0) + 2*sqrt(A)*alpha
    a1 = -2*((A-1) + (A+1)*cos(w0))
    a2 = (A+1) + (A-1)*cos(w0) - 2*sqrt(A)*alpha
    return array([b0/a0, b1/a0, b2/a0, a0/a0, a1/a0, a2/a0])


def biquadTF(fs, freq, sos):
    """
    Computes the transfer function of the biquad.

    Interpolates the frequency response to `freq`

    Args:
        fs: Sampling frequency [Hz]
        freq: Frequency array to compute the 
        ba: Biquad filter coefficients in common form.

    TODO: This code is not yet tested
    """
    freq2, h = sosfreqz(sos, worN=freq.size, fs=fs)
    interpolator = interp1d(freq2, h, kind='quadratic')
    return interpolator(freq)