From 78436e706cbde3501cf94a3081704483694efaf7 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 3 Nov 2021 12:01:13 -0400
Subject: [PATCH] mathlib: NotchFilter add optimized parameter update if only
 notch frequency changes

---
 src/lib/mathlib/math/filter/NotchFilter.hpp | 34 +++++++++++++++++++--
 1 file changed, 32 insertions(+), 2 deletions(-)

diff --git a/src/lib/mathlib/math/filter/NotchFilter.hpp b/src/lib/mathlib/math/filter/NotchFilter.hpp
index 5bf99a7119..f656cc9e07 100644
--- a/src/lib/mathlib/math/filter/NotchFilter.hpp
+++ b/src/lib/mathlib/math/filter/NotchFilter.hpp
@@ -197,9 +197,39 @@ void NotchFilter<T>::setParameters(float sample_freq, float notch_freq, float ba
 		return;
 	}
 
-	_notch_freq = math::constrain(notch_freq, 5.f, sample_freq / 2); // TODO: min based on actual numerical limit
-	_bandwidth = math::constrain(bandwidth, 5.f, sample_freq / 2);
+	const float freq_min = sample_freq * 0.001f;
+
+	const float notch_freq_new = math::max(notch_freq, freq_min);
+	const float bandwidth_new = math::max(bandwidth, freq_min);
+
+	bool sample_freq_change = (fabsf(sample_freq - _sample_freq) > FLT_EPSILON);
+	bool bandwidth_change = (fabsf(bandwidth_new - _bandwidth) > FLT_EPSILON);
+
+	if (!sample_freq_change && !bandwidth_change) {
+		if (fabsf(notch_freq_new - _notch_freq) > FLT_EPSILON) {
+			// only notch frequency has changed
+			_notch_freq = notch_freq_new;
+
+			const float beta = -cosf(2.f * M_PI_F * _notch_freq / _sample_freq);
+
+			_b1 = 2.f * beta * _b0;
+			_a1 = _b1;
+
+			if (!isFinite(_b1)) {
+				disable();
+			}
+
+			return;
+
+		} else {
+			// no change, do nothing
+			return;
+		}
+	}
+
 	_sample_freq = sample_freq;
+	_notch_freq = notch_freq_new;
+	_bandwidth = bandwidth_new;
 
 	const float alpha = tanf(M_PI_F * _bandwidth / _sample_freq);
 	const float beta = -cosf(2.f * M_PI_F * _notch_freq / _sample_freq);