From a87ee25f66a69d4e8d78b206ce77064c2cdd5fb3 Mon Sep 17 00:00:00 2001
From: Ahmet Inan <inan@aicodix.de>
Date: Fri, 22 Feb 2019 19:32:09 +0100
Subject: [PATCH] added biquad notch filter

---
 README.md |  1 +
 biquad.hh | 16 ++++++++++++++++
 2 files changed, 17 insertions(+)

diff --git a/README.md b/README.md
index 2bbc0f2..38f0025 100644
--- a/README.md
+++ b/README.md
@@ -39,6 +39,7 @@ The following [infinite impulse response](https://en.wikipedia.org/wiki/Infinite
 
 * [second-order Butterworth low or high pass filter](https://en.wikipedia.org/wiki/Butterworth_filter)
 * [2n-order Butterworth cascade of second-order low or high pass filters](https://en.wikipedia.org/wiki/Butterworth_filter)
+* [second-order notch filter](https://en.wikipedia.org/wiki/Band-stop_filter)
 
 ### [phasor.hh](phasor.hh)
 
diff --git a/biquad.hh b/biquad.hh
index c928b1e..d273acb 100644
--- a/biquad.hh
+++ b/biquad.hh
@@ -53,6 +53,22 @@ public:
 		a1a0 = a1 / a0;
 		a2a0 = a2 / a0;
 	}
+	void notch(int n, int N, VALUE Q)
+	{
+		VALUE alpha = UnitCircle<VALUE>::sin(n, N) / (VALUE(2) * Q),
+			cn = UnitCircle<VALUE>::cos(n, N),
+			b0 = VALUE(1),
+			b1 = -VALUE(2) * cn,
+			b2 = VALUE(1),
+			a0 = VALUE(1) + alpha,
+			a1 = -VALUE(2) * cn,
+			a2 = VALUE(1) - alpha;
+		b0a0 = b0 / a0;
+		b1a0 = b1 / a0;
+		b2a0 = b2 / a0;
+		a1a0 = a1 / a0;
+		a2a0 = a2 / a0;
+	}
 	TYPE operator()(TYPE x0)
 	{
 		TYPE y0 = b0a0*x0 + b1a0*x1 + b2a0*x2