;; biquad notch filter
(bind-func static notchbq_c
(lambda ()
(let* ((y1 0.0)
(y2 0.0)
(x1 0.0)
(x2 0.0)
(b0 0.0)
(b1 0.0)
(b2 0.0)
(a0 0.0)
(a1 0.0)
(a2 0.0)
(two:SAMPLE 2.0)
(oldfreq 0.0)
(oldbw 0.0))
;; bandwidth in octaves
(lambda (x freq bandwidth)
;; if frequency or bandwidth change
;; recalculate coefficients
(if (or (<> freq oldfreq)
(<> bandwidth oldbw))
(let* ((omega (* STWOPI (/ freq SAMPLERATE)))
(sino (sin omega))
(coso (cos omega))
(alpha (* sino (sinh (* (/ (log2 two) two)
bandwidth
(/ omega sino))))))
(set! oldfreq freq)
(set! oldbw bandwidth)
(set! b0 1.0)
(set! b1 (* -2.0 coso))
(set! b2 b0)
(set! a0 (+ 1.0 alpha))
(set! a1 b1)
(set! a2 (- 1.0 alpha))))
(let ((y (- (+ (* (/ b0 a0) x)
(* (/ b1 a0) x1)
(* (/ b2 a0) x2))
(* (/ a1 a0) y1)
(* (/ a2 a0) y2))))
(set! y2 y1)
(set! y1 y)
(set! x2 x1)
(set! x1 x)
y)))))