# test of biquad filter
import numpy as np
from BiquadFilter import BiquadFilter
from RateLimiter import RateLimiter
import matplotlib.pyplot as plt

omega = 10
q = 2
dt = 0.02

tflow   = BiquadFilter(omega, q, dt, 'lowpass')
tfhigh  = BiquadFilter(omega, q, dt, 'highpass')
tfband  = BiquadFilter(omega, q, dt, 'bandpass')
tfnotch = BiquadFilter(omega, q, dt, 'notch')

tfint   = BiquadFilter(omega, q, dt)
tfint.SetContinuousTF(1, 0, 0, 0, 1, 0, dt)     # 1/s : simple integrator
tfint.ConvertContinuousToDiscrete()

tfRateLim = RateLimiter(-0.3, 0.5, dt)

tflow.PrintAllTF(); print('')
tfhigh.PrintAllTF(); print('')
tfband.PrintAllTF(); print('')
tfnotch.PrintAllTF(); print('')
tfint.PrintAllTF(); print('')

# simulate filter response
tend = 10
Npts = int(np.ceil(tend/dt))
Yl = np.zeros(Npts)
Yh = np.zeros(Npts)
Yband = np.zeros(Npts)
Ynotch = np.zeros(Npts)
Yint = np.zeros(Npts)
Yratelim = np.zeros(Npts)
T = np.zeros(Npts)

for i in range(Npts):
    t = i*dt
    if t >= 1 and t < 6:
        x = 1
    else:
        x = 0

    T[i]        = t
    Yl[i]       = tflow.Filter(x)
    Yh[i]       = tfhigh.Filter(x)
    Yband[i]    = tfband.Filter(x)
    Ynotch[i]   = tfnotch.Filter(x)
    Yint[i]     = tfint.Filter(x)
    Yratelim[i] = tfRateLim.Filter(x)

plt.plot(T, Yl, T, Yh, T, Yband, T, Ynotch, T, Yint, T, Yratelim)
plt.grid(True)
plt.show()