NotchFilter push initialization/reset into filter

- this simplifies the reset by allowing a notch filter to reset as
needed
 - improves cascade initialization, on reset each filter will reset
properly from the previous

src/lib/mathlib/math/filter/NotchFilter.hpp

@@ -66,26 +66,24 @@ public:
 	 */
 	inline T apply(const T &sample)
 	{
-		// Direct Form I implementation
-		T output = _b0 * sample + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 - _a2 *
-			   _delay_element_output_2;
+		if (!_initialized) {
+			reset(sample);
+			_initialized = true;
+		}
 
-		// shift inputs
-		_delay_element_2 = _delay_element_1;
-		_delay_element_1 = sample;
-
-		// shift outputs
-		_delay_element_output_2 = _delay_element_output_1;
-		_delay_element_output_1 = output;
-
-		return output;
+		return applyInternal(sample);
 	}
 
 	// Filter array of samples in place using the direct form I
 	inline void applyArray(T samples[], int num_samples)
 	{
+		if (!_initialized) {
+			reset(samples[0]);
+			_initialized = true;
+		}
+
 		for (int n = 0; n < num_samples; n++) {
-			samples[n] = apply(samples[n]);
+			samples[n] = applyInternal(samples[n]);
 		}
 	}
 
@@ -130,6 +128,8 @@ public:
 		_b2 = b[2];
 	}
 
+	void reset() { _initialized = false; }
+
 	void reset(const T &sample)
 	{
 		const T input = isFinite(sample) ? sample : T{};
@@ -140,6 +140,8 @@ public:
 		if (!isFinite(_delay_element_1) || !isFinite(_delay_element_2)) {
 			_delay_element_output_1 = _delay_element_output_2 = {};
 		}
+
+		_initialized = true;
 	}
 
 	void disable()
@@ -160,9 +162,34 @@ public:
 
 		_a1 = 0.f;
 		_a2 = 0.f;
+
+		_initialized = false;
 	}
 
 protected:
+
+	/**
+	 * Add a new raw value to the filter using the Direct Form I
+	 *
+	 * @return retrieve the filtered result
+	 */
+	inline T applyInternal(const T &sample)
+	{
+		// Direct Form I implementation
+		T output = _b0 * sample + _b1 * _delay_element_1 + _b2 * _delay_element_2 - _a1 * _delay_element_output_1 - _a2 *
+			   _delay_element_output_2;
+
+		// shift inputs
+		_delay_element_2 = _delay_element_1;
+		_delay_element_1 = sample;
+
+		// shift outputs
+		_delay_element_output_2 = _delay_element_output_1;
+		_delay_element_output_1 = output;
+
+		return output;
+	}
+
 	T _delay_element_1{};
 	T _delay_element_2{};
 	T _delay_element_output_1{};
@@ -179,6 +206,8 @@ protected:
 	float _notch_freq{};
 	float _bandwidth{};
 	float _sample_freq{};
+
+	bool _initialized{false};
 };
 
 /**
@@ -242,7 +271,7 @@ void NotchFilter<T>::setParameters(float sample_freq, float notch_freq, float ba
 	_a1 = _b1;
 	_a2 = (1.f - alpha) * a0_inv;
 
-	if (!isFinite(_b0) || !isFinite(_b1) || !isFinite(_b2) || !isFinite(_a1) || !isFinite(_a2)) {
+	if (!isFinite(_b0) || !isFinite(_b1) || !isFinite(_b2) || !isFinite(_a2)) {
 		disable();
 	}
 }