''' Digital Biquadratic filter implementation '''

class BiquadFilter:
    ''' Implementation of a digital second-order biquadratic filter. '''
    def __init__(self, omega, q, dt, ftype='lowpass'):
        ''' Builds a biquad filter from the given parameters. '''
        self.InitFilter(0)
        self.ComputeContinuousTFBiquad(omega, q, dt, ftype)
        self.ConvertContinuousToDiscrete()

    def SetContinuousTF(self, b0, b1, b2, a0, a1, a2, dt):
        ''' Sets the continuous-time coefficients of the second order transfer function. '''
        self.dt  = dt
        self.b0c = b0
        self.b1c = b1
        self.b2c = b2
        self.a0c = a0
        self.a1c = a1
        self.a2c = a2

    def ComputeContinuousTFBiquad(self, omega, q, dt, ftype='lowpass'):
        ''' Computes the continuous time transfer function from the given parameters.
            b0 + b1*s + b2*s^2
            ------------------
            a0 + a1*s + a2*s^2
        '''
        self.omega = omega
        self.q     = q
        self.dt    = dt
        if ftype == 'highpass':
            self.b0c = 0
            self.b1c = 0
            self.b2c = 1
            self.a0c = omega**2
            self.a1c = omega/q
            self.a2c = 1
        elif ftype == 'bandpass':
            self.b0c = 0
            self.b1c = omega
            self.b2c = 0
            self.a0c = q*omega**2
            self.a1c = omega
            self.a2c = q
        elif ftype == 'notch':
            self.b0c = omega**2
            self.b1c = 0
            self.b2c = 1
            self.a0c = omega**2
            self.a1c = omega/q
            self.a2c = 1
        else:
            # lowpass filter
            self.b0c = omega**2
            self.b1c = 0
            self.b2c = 0
            self.a0c = omega**2
            self.a1c = omega/q
            self.a2c = 1

    def ConvertContinuousToDiscrete(self):
        ''' Converts the continuous coefficients to discrete coefficients using the bilinear transform. '''
        self.b0d = 4*self.b2c - 2*self.b1c*self.dt + self.b0c*self.dt**2
        self.b1d = - 8*self.b2c + 2*self.b0c*self.dt**2
        self.b2d = 4*self.b2c + 2*self.b1c*self.dt + self.b0c*self.dt**2
        self.a0d = 4*self.a2c - 2*self.a1c*self.dt + self.a0c*self.dt**2
        self.a1d = - 8*self.a2c + 2*self.a0c*self.dt**2
        self.a2d = 4*self.a2c + 2*self.a1c*self.dt + self.a0c*self.dt**2

    def PrintContinuousTF(self):
        ''' Prints the continuous-time transfer function coefficients. '''
        print('Continuous-time transfer function :')
        print('%f + %f s + %f s**2' % (self.b0c, self.b1c, self.b2c))
        print('----------------------------------------')
        print('%f + %f s + %f s**2' % (self.a0c, self.a1c, self.a2c))

    def PrintDiscreteTF(self):
        ''' Prints the discrete-time transfer function coefficients. '''
        print('Discrete-time transfer function :')
        print('%f + %f z + %f z**2' % (self.b0d, self.b1d, self.b2d))
        print('----------------------------------------')
        print('%f + %f z + %f z**2' % (self.a0d, self.a1d, self.a2d))
        print('dt = %f' % self.dt)

    def PrintAllTF(self):
        self.PrintContinuousTF()
        self.PrintDiscreteTF()

    def InitFilter(self, v):
        ''' Initializes the filter with the value v. '''
        self.xn_0 = v
        self.xn_1 = v
        self.xn_2 = v
        self.yn_0 = v
        self.yn_1 = v
        self.yn_2 = v

    def Filter(self, xn_0):
        ''' Applies the filter to the sample xn_0. '''
        self.xn_0 = xn_0
        self.yn_0 = (self.b2d*self.xn_0 + self.b1d*self.xn_1 + self.b0d*self.xn_2 - self.a1d*self.yn_1 - self.a0d*self.yn_2)/self.a2d
        self.xn_2 = self.xn_1
        self.xn_1 = self.xn_0
        self.yn_2 = self.yn_1
        self.yn_1 = self.yn_0
        return self.yn_0