/**
* \class CBiquadFilter
*
* \brief Declaration of CBiquadFilter class
* \date	June 2023
*
* \authors 3DI-DIANA Research Group (University of Malaga), in alphabetical order: M. Cuevas-Rodriguez, D. Gonzalez-Toledo, L. Molina-Tanco, F. Morales-Benitez ||
* Coordinated by , A. Reyes-Lecuona (University of Malaga)||
* \b Contact: areyes@uma.es
*
* \b Copyright: University of Malaga
* 
* \b Contributions: (additional authors/contributors can be added here)
*
* \b Project: 3D Tune-In (https://www.3dtunein.eu) and SONICOM (https://www.sonicom.eu/) ||
*
* \b Acknowledgement: This project has received funding from the European Union's Horizon 2020 research and innovation programme under grant agreements no. 644051 and no. 101017743
* 
* This class is part of the Binaural Rendering Toolbox (BRT), coordinated by A. Reyes-Lecuona (areyes@uma.es) and L. Picinali (l.picinali@imperial.ac.uk)
* Code based in the 3DTI Toolkit library (https://github.com/3DTune-In/3dti_AudioToolkit).
* 
* \b Licence: This program is free software, you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
*/

#ifndef _BIQUAD_FILTER_HPP_
#define _BIQUAD_FILTER_HPP_

#include <Common/Buffer.hpp>
#include <Common/CommonDefinitions.hpp>
#include <Common/GlobalParameters.hpp>

#include <cmath>
#include <iomanip>

#define _USE_MATH_DEFINES // TODO: Test in windows! Might also be problematic for other platforms??

namespace BRTProcessing {

	/** \brief Type definition for specifying the type of filter
	*/
	enum TBiquadType {
		LOWPASS_BUTTERWORTH = 0,	///< Low pass filter
		HIGHPASS_BUTTERWORTH = 1,	///< High pass filter
		BANDPASS_BUTTERWORTH = 2,	///< Band pass filter
		LOWSHELF = 3,	///< Low shelf filter
		HIGHSHELF = 4,	///< High shelf filter
		PEAKNOTCH = 5	///< Peak Notch filter
	};

	/** \brief Type definition for a vector of filter coefficients for one biquad
	*	\details Order: b0, b1, b2, a1, a2  
	*/
	typedef std::vector<float> TBiquadCoefficients; 


	/** \details This class implements a biquad filter (two poles and two zeros).
	\n Useful diagrams can be found in:	https://en.wikipedia.org/wiki/Digital_biquad_filter */
	class CBiquadFilter
	{
	public:
		///////////////////
		// PUBLIC METHODS
		//////////////////

		/** \brief Default constructor.
		*	\details By default, sets sampling frequency to 44100Hz.
		*   \eh Nothing is reported to the error handler.
		*/
		CBiquadFilter() : generalGain {1.0f}, crossfadingEnabled {false}, firstBuffer{true}
		{
			// error handler: Trust in SetSamplingFreq for result			
			InitFilterCoefficients();			
		}


		/** \brief Set up the filter
		*	\param [in] samplingRate sampling frequency, in Hertzs
		*	\param [in] b0 coefficient b0
		*	\param [in] b1 coefficient b1
		*	\param [in] b2 coefficient b2
		*	\param [in] a1 coefficient a1
		*	\param [in] a2 coefficient a2
		*	\param [in] _crossfadingEnabled true when cross fading must be applied between frames
		*   \eh On error, an error code is reported to the error handler.
		*/		
		void Setup(float b0, float b1, float b2, float a1, float a2, bool _crossfadingEnabled = true)
		{		
			crossfadingEnabled = _crossfadingEnabled;
			SetCoefficients(b0, b1, b2, a1, a2);
		}

		/**
		 * @brief Set up coefficients of the filter
		 * @param coefficients coefficients coefficients vector. Order: b0, b1, a1, a2 
		 * @param _crossfadingEnabled true when cross fading must be applied between frames
		 */
		void Setup(const TBiquadCoefficients& coefficients, bool _crossfadingEnabled = true)
		{
			crossfadingEnabled = _crossfadingEnabled;
			SetCoefficients(coefficients);
		}

		void Setup(float frequency, float Q, TBiquadType filterType, double commandGain, bool _crossfadingEnabled = true) {			
			crossfadingEnabled = _crossfadingEnabled;
			SetCoefficients(frequency, Q, filterType, commandGain);
		}

		/** \brief Set up the filter
		*	\param [in] samplingRate sampling frequency, in Hertzs
		*	\param [in] frequency relevant frequency (cutoff or band center)
		*	\param [in] Q Q factor
		*	\param [in] filterType type of filter
		*   \eh On error, an error code is reported to the error handler.
		*/
		/*void Setup(float frequency, float Q, T_filterType filterType, bool _crossfadingEnabled = true)
		{			
			crossfadingEnabled = _crossfadingEnabled;
			SetCoefficients(frequency, Q, filterType);
		}*/
		

		/** \brief Set the sampling frequency at which audio samples were acquired
		*	\param [in] _samplingFreq sampling frequency, in Hertzs
		*   \eh On success, RESULT_OK is reported to the error handler.
		*       On error, an error code is reported to the error handler.
		*/
		/*void SetSamplingFreq(float _samplingFreq)		
		{
			if (_samplingFreq < 0.1)
			{
				SET_RESULT(RESULT_ERROR_INVALID_PARAM, "Sampling frequency for biquad filter is invalid");
				return;
			}

			SET_RESULT(RESULT_OK, "Sampling frequency for biquad filter succesfully set");
			samplingFreq = _samplingFreq;
		}*/

		/** \brief Filter the input data according to the filter setup.
		*	\param [in] inBuffer input buffer
		*	\param [out] outBuffer output buffer
		*	\param [in] addResult when true, samples resulting from the	filtering process are added to the current value of the output buffer.
		*	\pre Input and output buffers must have the same size, which should be greater than 0.
		*   \eh On error, an error code is reported to the error handler.
		*/
		void Process(const CMonoBuffer<float> & inBuffer, CMonoBuffer<float> & outBuffer, bool addResult = false) {
			ProcessInternal(inBuffer, &outBuffer, addResult);
		}

		/**
		 * @brief Filter the input data according to the filter setup.
		 * @param buffer input and output buffer
		 */
		void Process(CMonoBuffer<float> & buffer) {
			ProcessInternal(buffer, &buffer, false);
		}
		//void Process(const CMonoBuffer<float> &inBuffer, CMonoBuffer<float> & outBuffer, bool addResult = false)		
		//{
		//	int size = inBuffer.size();

		//	if (size <= 0)
		//	{
		//		//SET_RESULT(RESULT_ERROR_INVALID_PARAM, "The input buffer is empty");
		//		SET_RESULT(RESULT_ERROR_BADSIZE, "Attempt to process a biquad filter with an empty input buffer");
		//		return;
		//	}
		//	else if (size != outBuffer.size())
		//	{
		//		//SET_RESULT( RESULT_ERROR_INVALID_PARAM, "Input and output buffers size must agree" );
		//		SET_RESULT(RESULT_ERROR_BADSIZE, "Attempt to process a biquad filter with different sizes for input and output buffers");
		//		return;
		//	}

		//	//SET_RESULT(RESULT_OK, "");

		//	// This is expression of the biquad filter but the implementation follows a more efficient
		//	// approach in which only 2 delays cells are used.
		//	//   See schemes in: https://en.wikipedia.org/wiki/Digital_biquad_filter
		//	//   y(n) = b0.x(n) + b1.x(n-1) + b2.x(n-2) + a1.y(n-1) + a2.y(n-2) 	

		//	if (crossfadingEnabled && size > 0)  // size > 1 to avoid division by zero if size were 1 while calculating alpha
		//	{
		//		if (firstBuffer) {
		//			for (int c = 0; c < size; c++) {
		//				double res = ProcessSample(inBuffer[c], new_a1, new_a2, new_b0, new_b1, new_b2, new_z1, new_z2);
		//				outBuffer[c] = addResult ? outBuffer[c] + res : res;
		//			}
		//			firstBuffer = false;
		//		}
		//		else {
		//			for (int c = 0; c < size; c++)
		//			{
		//				// To ensure alpha is in [0,1] we use -2 because the buffer is stereo
		//				double alpha = ((double)c) / ((double)(size - 1));

		//				double     sample = ProcessSample(inBuffer[c], a1, a2, b0, b1, b2, z1, z2);
		//				double new_sample = ProcessSample(inBuffer[c], new_a1, new_a2, new_b0, new_b1, new_b2, new_z1, new_z2);

		//				double res = sample * (1.0 - alpha) + new_sample * alpha;

		//				outBuffer[c] = addResult ? outBuffer[c] + res : res;
		//			}
		//		}
		//		UpdateAttributesAfterCrossfading();
		//	}
		//	else
		//	{
		//		for (int c = 0; c < size; c++)
		//		{
		//			double res = ProcessSample(inBuffer[c], a1, a2, b0, b1, b2, z1, z2);
		//			outBuffer[c] = addResult ? outBuffer[c] + res : res;
		//		}
		//	}
		//	CheckCoeficientsNanValues();
		//}


		///**
		//\overload
		//*/
		//void Process(CMonoBuffer<float> &buffer)		
		//{
		//	int size = buffer.size();

		//	if (size <= 0)
		//	{
		//		SET_RESULT(RESULT_ERROR_BADSIZE, "Attempt to process a biquad filter with an empty input buffer");
		//		return;
		//	}

		//	//SET_RESULT(RESULT_OK, "Biquad filter process succesfull");
		//	
		//	if (crossfadingEnabled)
		//	{
		//		if (firstBuffer) {
		//			for (int c = 0; c < size; c++) {
		//				buffer[c] = ProcessSample(buffer[c], new_a1, new_a2, new_b0, new_b1, new_b2, new_z1, new_z2);
		//			}
		//			firstBuffer = false;
		//		}
		//		else {
		//			for (int c = 0; c < size; c++)
		//			{
		//				double alpha = ((double)c) / ((double)(size - 1));

		//				double     sample = ProcessSample(buffer[c], a1, a2, b0, b1, b2, z1, z2);
		//				double new_sample = ProcessSample(buffer[c], new_a1, new_a2, new_b0, new_b1, new_b2, new_z1, new_z2);

		//				buffer[c] = sample * (1.0 - alpha) + new_sample * alpha;
		//			}
		//		}
		//		UpdateAttributesAfterCrossfading();
		//	}
		//	else
		//	{
		//		for (int c = 0; c < size; c++)
		//			buffer[c] = ProcessSample(buffer[c], a1, a2, b0, b1, b2, z1, z2);
		//	}

		//	CheckCoeficientsNanValues();
		//}

		/** \brief Set the gain of the filter 
		*	\param [in] _gain filter gain 
		*   \eh Nothing is reported to the error handler.
		*/
		void SetGeneralGain(float _gain)		
		{
			generalGain = _gain;
		}

		/** \brief Get the gain of the filter
		*	\retval gain filter gain
		*   \eh Nothing is reported to the error handler.
		*/
		float GetGeneralGain()
		{
			return generalGain;
		}
		
		void ResetBuffers() {
			z1 = 0;
			z2 = 0;
			new_z1 = 0;
			new_z2 = 0;
			firstBuffer = true;
		}

		void ResetFilter() {
			InitFilterCoefficients();
			generalGain = 1.0f;
			crossfadingEnabled = false; 
			firstBuffer = true;
		}

	private:
		
		/// METHODS

		/**
		 * @brief Filter the input data according to the filter setup.
		 * @param inBuffer 
		 * @param outBuffer 
		 * @param addResult 
		 */
		void ProcessInternal(const CMonoBuffer<float> & inBuffer, CMonoBuffer<float> * outBuffer, bool addResult) {
			const int size = inBuffer.size();

			if (size <= 0) {
				SET_RESULT(RESULT_ERROR_BADSIZE, "Attempt to process a biquad filter with an empty input buffer");
				return;
			}
			if (!outBuffer || size != outBuffer->size()) {
				SET_RESULT(RESULT_ERROR_BADSIZE, "Input and output buffers size must agree");
				return;
			}

			auto writeSample = [&](int i, double v) {
				// addResult only applies when output != input and the caller asks for it
				(*outBuffer)[i] = addResult ? (*outBuffer)[i] + static_cast<float>(v)
											: static_cast<float>(v);
			};

			if (crossfadingEnabled) {
				if (firstBuffer) {
					for (int c = 0; c < size; ++c) {
						const double r = ProcessSample(inBuffer[c], new_a1, new_a2, new_b0, new_b1, new_b2, new_z1, new_z2);
						writeSample(c, r);
					}
					firstBuffer = false;
				} else {
					for (int c = 0; c < size; ++c) {
						const double alpha = static_cast<double>(c) / static_cast<double>(size - 1);
						const double s0 = ProcessSample(inBuffer[c], a1, a2, b0, b1, b2, z1, z2);
						const double s1 = ProcessSample(inBuffer[c], new_a1, new_a2, new_b0, new_b1, new_b2, new_z1, new_z2);
						writeSample(c, s0 * (1.0 - alpha) + s1 * alpha);
					}
				}
				UpdateAttributesAfterCrossfading();
			} else {
				for (int c = 0; c < size; ++c) {
					const double r = ProcessSample(inBuffer[c], a1, a2, b0, b1, b2, z1, z2);
					writeSample(c, r);
				}
			}
			CheckCoeficientsNanValues();
		}		
		
		/**
		 * @brief 
		 */
		void InitFilterCoefficients() {
			z1 = 0;
			z2 = 0;			

			b0 = 1;
			b1 = 0;
			b2 = 0;
			a1 = 0;
			a2 = 0;

			new_b0 = 1;
			new_b1 = 0;
			new_b2 = 0;
			new_a1 = 0;
			new_a2 = 0;

			new_z1 = 0;
			new_z2 = 0;		
			SetCoefficients(1, 0, 0, 0, 0);
		}

		/** \brief Set up coefficients of the filter
		*	\param [in] b0 coefficient b0
		*	\param [in] b1 coefficient b1
		*	\param [in] b2 coefficient b2
		*	\param [in] a1 coefficient a1
		*	\param [in] a2 coefficient a2
		*   \eh Nothing is reported to the error handler.
		*/
		void SetCoefficients(float _b0, float _b1, float _b2, float _a1, float _a2)
		{
			if (crossfadingEnabled) {			
				new_b0 = _b0;
				new_b1 = _b1;
				new_b2 = _b2;
				new_a1 = _a1;
				new_a2 = _a2;

				new_z1 = 0;
				new_z2 = 0;				
			}
			else {
				b0 = _b0;
				b1 = _b1;
				b2 = _b2;
				a1 = _a1;
				a2 = _a2;
			}
		}

		/** \brief Set up coefficients of the filter
		*	\param [in] coefficients coefficients array. Order: b0, b1, a1, a2
		*   \eh Nothing is reported to the error handler. */
		void SetCoefficients(float* coefficients)
		{
			//SET_RESULT(RESULT_OK, "");
			SetCoefficients(coefficients[0], coefficients[1], coefficients[2], coefficients[3], coefficients[4]);
		}

		/** \brief Set up coefficients of the filter
		*	\param [in] coefficients coefficients vector. Order: b0, b1, a1, a2
		*   \eh Nothing is reported to the error handler. */
		void SetCoefficients(const TBiquadCoefficients& coefficients)
		{
			if (coefficients.size() == 6) {
				float a0 = coefficients[3];
				SetCoefficients(coefficients[0] / a0, coefficients[1] / a0, coefficients[2] / a0, coefficients[4] / a0, coefficients[5] / a0);
			}
			else if (coefficients.size() == 5) {
				SetCoefficients(coefficients[0], coefficients[1], coefficients[2], coefficients[3], coefficients[4]);
			}
			else {
				SET_RESULT(RESULT_ERROR_INVALID_PARAM, "A vector with 5 or 6 coefficients was expected in BiquadFilter definition.");
			}
		}

		/** \brief Set up coefficients of the filter
		*	\details Lowpass, Highpass and Bandpass filters are Butterworth design
		*	\param [in] frequency relevant frequency (cutoff or band center)
		*	\param [in] Q Q factor
		*	\param [in] filterType type of filter
		*   \eh On error, an error code is reported to the error handler.
		*/
		void SetCoefficients(float & frequency, float & Q, TBiquadType & filterType, double & commandGain)
		{
			if (filterType == LOWPASS_BUTTERWORTH) {
				SetCoefsFor_LPF(frequency, Q);
			} else if (filterType == HIGHPASS_BUTTERWORTH) {
				SetCoefsFor_HPF(frequency, Q);
			} else if (filterType == BANDPASS_BUTTERWORTH) {
				SetCoefsFor_BandPassFilter(frequency, Q);
			} else if (filterType == LOWSHELF) {
				SetCoefsFor_LowShelf(frequency, Q, commandGain);
			} else if (filterType == HIGHSHELF) {
				SetCoefsFor_HighShelf(frequency, Q, commandGain);
			} else if (filterType == PEAKNOTCH) {
				SetCoefsFor_PeakNotch(frequency, Q, commandGain);
			} else {
				SET_RESULT(RESULT_ERROR_INVALID_PARAM, "Invalid filter type");
			}
		}		

		/**
		 * @brief Prevent the filter from ending up in unstable states
		*/
		void CheckCoeficientsNanValues()		
		{
			// FIXME: IIRs filters can eventually end up in a non stable state that can lead the filter output
			//      to +/-Inf. To prevent this situation we reset the delay cells of the filter when this happens.
			//    A known scenario in whinch this happens is this: In the binaural test app when two sound sources
			//   are played at the same time using anechoic an reverb and one of the sourcers is moved beyond the 
			//   far distances threshold, the LPF of the distances can end up with this unstable state.

			if (std::isnan(z1)) z1 = 0;
			if (std::isnan(z2)) z2 = 0;			

			if (std::isnan(new_z1)) new_z1 = 0;
			if (std::isnan(new_z2)) new_z2 = 0;			
		}				

		/**
		 * @brief Calculates the coefficients of a biquad band-pass filter.
		 * @param centerFreqHz 
		 * @param Q 
		 * @return 
		*/
		bool SetCoefsFor_BandPassFilter(double centerFreqHz, double Q)		
		{
			float samplingFreq = globalParameters.GetSampleRate();
			if (samplingFreq < 0.1 || Q < 0.0000001 || centerFreqHz > samplingFreq / 2.0) // To prevent aliasing problems
			{
				SET_RESULT(RESULT_ERROR_INVALID_PARAM, "Cutoff frequency of biquad (bandpass) filter is higher than Nyquist frequency");
				return false;
			}

			try // -> To handle division by 0
			{
				double K = std::tan(M_PI * centerFreqHz / samplingFreq);

				double norm = 1 / (1 + K / Q + K * K);
				double _b0 = K / Q * norm;
				double _b1 = 0;
				double _b2 = -_b0;
				double _a1 = 2 * (K * K - 1) * norm;
				double _a2 = (1 - K / Q + K * K) * norm;

				SetCoefficients(_b0, _b1, _b2, _a1, _a2);

				SET_RESULT(RESULT_OK, "Bandpass filter coefficients of biquad filter succesfully set");

				return true;
			}
			catch (std::exception e)
			{
				//SET_RESULT(RESULT_ERROR_INVALID_PARAM, "");
				SET_RESULT(RESULT_ERROR_DIVBYZERO, "Division by zero setting coefficients for bandpass biquad filter");
				return false;
			}
		}

		/// Calculate the coefficients of a biquad low-pass filter.
		bool SetCoefsFor_LPF(double cutoffFreq, double Q)		
		{
			float samplingFreq = globalParameters.GetSampleRate();

			if (samplingFreq < 0.1 || cutoffFreq > samplingFreq / 2.0) // To prevent aliasing problems
			{
				SET_RESULT(RESULT_ERROR_INVALID_PARAM, "Cutoff frequency of biquad (LPF) filter is higher than Nyquist frequency");
				return false;
			}

			try // -> To handle division by 0
			{
				double K = std::tan(M_PI * cutoffFreq / samplingFreq);

				double norm = 1 / (1 + K / Q + K * K);
				double _b0 = K * K * norm;
				double _b1 = 2 * _b0;
				double _b2 = _b0;
				double _a1 = 2 * (K * K - 1) * norm;
				double _a2 = (1 - K / Q + K * K) * norm;

				SetCoefficients(_b0, _b1, _b2, _a1, _a2);

				//SET_RESULT(RESULT_OK, "LPF filter coefficients of biquad filter succesfully set");

				return true;
			}
			catch (std::exception e)
			{
				//SET_RESULT(RESULT_ERROR_INVALID_PARAM, "");
				SET_RESULT(RESULT_ERROR_DIVBYZERO, "Division by zero setting coefficients for LPF biquad filter");
				return false;
			}
		}
		
		/// Calculates the coefficients of a biquad high-pass filter.     		
		bool SetCoefsFor_HPF(double cutoffFreq, double Q)
		{
			float samplingFreq = globalParameters.GetSampleRate();
			if (samplingFreq < 0.1 || cutoffFreq > samplingFreq / 2.0) // To prevent aliasing problems
			{
				SET_RESULT(RESULT_ERROR_INVALID_PARAM, "Cutoff frequency of biquad (HPF) filter is higher than Nyquist frequency");
				return false;
			}

			try // -> To handle division by 0
			{
				double K = std::tan(M_PI * cutoffFreq / samplingFreq);

				double norm = 1 / (1 + K / Q + K * K);
				double _b0 = 1 * norm;
				double _b1 = -2 * _b0;
				double _b2 = _b0;
				double _a1 = 2 * (K * K - 1) * norm;
				double _a2 = (1 - K / Q + K * K) * norm;

				SetCoefficients(_b0, _b1, _b2, _a1, _a2);

				SET_RESULT(RESULT_OK, "HPF filter coefficients of biquad filter succesfully set");

				return true;
			}
			catch (std::exception e)
			{
				//SET_RESULT(RESULT_ERROR_INVALID_PARAM, "");
				SET_RESULT(RESULT_ERROR_DIVBYZERO, "Division by zero setting coefficients for HPF biquad filter");
				return false;
			}
		}

		/**
		 * @brief Calculates the coefficients of a biquad peak/notch filter.
		 * @param cutoffFreq 
		 * @param Q 
		 * @param gain 
		 * @return 
		 */
		bool SetCoefsFor_LowShelf(double cutoffFreq, double Q, double gain) {
			float samplingFreq = globalParameters.GetSampleRate();
			if (cutoffFreq > samplingFreq / 2.0) // To prevent aliasing problems
			{
				SET_RESULT(RESULT_ERROR_INVALID_PARAM, "Cutoff frequency of biquad (LOWSHELF) filter is higher than Nyquist frequency");
				return false;
			}

			try // -> To handle division by 0
			{
				// Low shelf implementation from Välimäki, V., Reiss, J. D., "All About Audio Equalization: Solutions and Frontiers", MDPI, 2016
				//      https://www.mdpi.com/2076-3417/6/5/129
				double sqrtGain = std::sqrt(gain);
				double fourthrtGain = std::sqrt(sqrtGain);
				double wc = 2 * M_PI * cutoffFreq / samplingFreq;
				double sigma = std::tan(wc / 2);
				double sigma2 = sigma * sigma;

				double _b0 = sqrtGain * (sqrtGain * sigma2 + std::sqrt(2.0) * sigma * fourthrtGain + 1);
				double _b1 = sqrtGain * 2 * (sqrtGain * sigma2 - 1);
				double _b2 = sqrtGain * (sqrtGain * sigma2 - std::sqrt(2.0) * sigma * fourthrtGain + 1);
				double _a0 = sqrtGain + std::sqrt(2.0) * sigma * fourthrtGain + sigma2;
				double _a1 = +2 * (sigma2 - sqrtGain);
				double _a2 = sqrtGain - std::sqrt(2.0) * sigma * fourthrtGain + sigma2;

				_b2 = _b2 / _a0;
				_b1 = _b1 / _a0;
				_b0 = _b0 / _a0;
				_a2 = _a2 / _a0;
				_a1 = _a1 / _a0;

				SetCoefficients(_b0, _b1, _b2, _a1, _a2);

				SET_RESULT(RESULT_OK, "LOWSHELF filter coefficients of biquad filter succesfully set");

				return true;
			} catch (std::exception e) {
				SET_RESULT(RESULT_ERROR_DIVBYZERO, "Division by zero setting coefficients for LOWSHELF biquad filter");
				return false;
			}
		}

		/**
		 * @brief Calculates the coefficients of a biquad peak/notch filter.
		 * @param cutoffFreq 
		 * @param Q 
		 * @param gain 		 
		 * @return 
		 */
		bool SetCoefsFor_HighShelf(double cutoffFreq, double Q, double gain) {
			float samplingFreq = globalParameters.GetSampleRate();
			if (cutoffFreq > samplingFreq / 2.0) // To prevent aliasing problems
			{
				SET_RESULT(RESULT_ERROR_INVALID_PARAM, "Cutoff frequency of biquad (LOWSHELF) filter is higher than Nyquist frequency");
				return false;
			}

			try // -> To handle division by 0
			{
				// High shelf implementation from Välimäki, V., Reiss, J. D., "All About Audio Equalization: Solutions and Frontiers", MDPI, 2016
				//      https://www.mdpi.com/2076-3417/6/5/129
				double sqrtGain = std::sqrt(gain);
				double fourthrtGain = std::sqrt(sqrtGain);
				double wc = 2 * M_PI * cutoffFreq / samplingFreq;
				double sigma = std::tan(wc / 2);
				double sigma2 = sigma * sigma;

				double _b0 = sqrtGain * (sqrtGain + std::sqrt(2.0) * sigma * fourthrtGain + sigma2);
				double _b1 = sqrtGain * (-2) * (sqrtGain - sigma2);
				double _b2 = sqrtGain * (sqrtGain - std::sqrt(2.0) * sigma * fourthrtGain + sigma2);
				double _a0 = sqrtGain * sigma2 + std::sqrt(2.0) * sigma * fourthrtGain + 1;
				double _a1 = 2 * (sqrtGain * sigma2 - 1);
				double _a2 = sqrtGain * sigma2 - std::sqrt(2.0) * sigma * fourthrtGain + 1;

				_b2 = _b2 / _a0;
				_b1 = _b1 / _a0;
				_b0 = _b0 / _a0;
				_a2 = _a2 / _a0;
				_a1 = _a1 / _a0;

				SetCoefficients(_b0, _b1, _b2, _a1, _a2);

				SET_RESULT(RESULT_OK, "LOWSHELF filter coefficients of biquad filter succesfully set");

				return true;
			} catch (std::exception e) {
				SET_RESULT(RESULT_ERROR_DIVBYZERO, "Division by zero setting coefficients for LOWSHELF biquad filter");
				return false;
			}
		}


		/**
		 * @brief Calculates the coefficients of a biquad peak/notch filter.
		 * @param centerFreqHz 
		 * @param Q 
		 * @param gain 
		 * @return 
		 */
		bool SetCoefsFor_PeakNotch(double centerFreqHz, double Q, double gain) {
			
			float samplingFreq = globalParameters.GetSampleRate();
			// Use Välimäki's method to calculate the coefficients of a peak-notch filter
			// See: Välimäki, V., Reiss, J. D., "All About Audio Equalization: Solutions and Frontiers", MDPI, 2016
			//      https://www.mdpi.com/2076-3417/6/5/129
			if (gain < 0) {
				SET_RESULT(RESULT_ERROR_INVALID_PARAM, "Gain of biquad (peak-notch) filter is negative");
				return false;
			}
			if (centerFreqHz > samplingFreq / 2.0) // To warn of aliasing problems
			{
				SET_RESULT(RESULT_WARNING, "Cutoff frequency of biquad (peak-notch) filter is higher than Nyquist frequency");
			}

			try { // -> To handle division by 0
				double A = std::sqrt(gain);
				double Wc = 2 * M_PI * centerFreqHz / samplingFreq;
				double Bw = Wc / Q;

				double _b0 = A + gain * std::tan(Bw / 2);
				double _b1 = -2 * A * std::cos(Wc);
				double _b2 = A - gain * std::tan(Bw / 2);
				double _a0 = A + std::tan(Bw / 2);
				double _a1 = -2 * A * std::cos(Wc);
				double _a2 = A - std::tan(Bw / 2);

				_b2 = _b2 / _a0;
				_b1 = _b1 / _a0;
				_b0 = _b0 / _a0;
				_a2 = _a2 / _a0;
				_a1 = _a1 / _a0;

				SetCoefficients(_b0, _b1, _b2, _a1, _a2);

				SET_RESULT(RESULT_OK, "Peak-Notch filter coefficients of biquad filter succesfully set");

				return true;
			} catch (std::exception & e) {
				SET_RESULT(RESULT_ERROR_DIVBYZERO, "Division by zero setting coefficients for peak-notch biquad filter");
				return false;
			}
		}
		
		/**
		 * @brief Processes a single sample through a biquad digital filter using the Direct Form II implementation.
		 * @param sample The input sample to be filtered.
		 * @param a1 The filter coefficient for y(n-1).
		 * @param a2 The filter coefficient for y(n-2).
		 * @param b0 The filter coefficient for x(n).
		 * @param b1 The filter coefficient for x(n-1).
		 * @param b2 The filter coefficient for x(n-2).
		 * @param z1 Reference to the first delay cell (state variable), updated in-place.
		 * @param z2 Reference to the second delay cell (state variable), updated in-place.
		 * @return The filtered output sample.
		 */
		double ProcessSample(const double& sample, const double& a1, const double& a2, const double& b0, const double& b1, const double& b2, double &z1, double &z2)		
		{
			// This is expression of the biquad filter but the implementation follows a more efficient
			// approach in which only 2 delays cells are used.
			//   See schemes in: https://en.wikipedia.org/wiki/Digital_biquad_filter
			//   y(n) = b0.x(n) + b1.x(n-1) + b2.x(n-2) + a1.y(n-1) + a2.y(n-2) 	

			double m_l = sample - a1 * z1 - a2 * z2;
			double res = generalGain * (float)(b0 * m_l + b1 * z1 + b2 * z2);
			z2 = z1;
			z1 = m_l;
			return res;
		}
				
		/**
		 * @brief Updates filter attributes after crossfading by assigning new values and disabling crossfading.
		 * @details Set current coefficients to new cofficients and updates the delay cells. Also update the crossfading Needed attribute.
		 */
		void UpdateAttributesAfterCrossfading()
		{
			crossfadingEnabled = false;
			z1 = new_z1;
			z2 = new_z2;			

			b0 = new_b0;
			b1 = new_b1;
			b2 = new_b2;
			a1 = new_a1;
			a2 = new_a2;
		}

		////////////////
		// ATTRIBUTES
		////////////////

		Common::CGlobalParameters globalParameters;			// Get access to BRT global audio parameters
		float generalGain;									// Gain applied to every sample obtained with Process

		//double samplingFreq;                              // Keep the sampling rate at which audio samples were taken
		double z1, z2;										// Keep last values to implement the delays of the filter (left and right channels)
		double b0, b1, b2, a1, a2;                          // Coeficients of the Butterworth filter

		double new_b0, new_b1, new_b2, new_a1, new_a2;      // New coefficients to implement cross fading
		double new_z1, new_z2;								// Keep last values to implement the delays of the filter (left and right channels)
		bool   crossfadingEnabled;                          // True when cross fading must be applied in the next frame
		bool   firstBuffer;									// Controls when the first buffer is to be processed 
	};
}
#endif
