//------------------------------------------------------------------------------
//	グラフィックイコライザで使う Biquad フィルタのパラメータ計算等
//
//		作成者：三上直樹，2022/02/25 作成，©三上直樹 2022
//------------------------------------------------------------------------------

using System.Numerics;

using static System.Math;
using static System.Numerics.Complex;

namespace Mikami.Application
{
	public class BiquadGrEq
	{
		public struct Coefs { public double a1, a2, b0, b1, b2; }
		// ピーキング，ローシェルビング，ハイシェルビングの区別用
		public enum Type { PEAK, LSHL, HSHL };	

		private readonly double w0_;	// 正規化中心角周波数
		private readonly double qVal_;	// Q値
		private readonly Type plh_;		// フィルタのタイプの別

		private Coefs coefs_;			// 係数

		// コンストラクタ
		public BiquadGrEq(double f0, double qVal, Type plh, double fs)
		{
			w0_ = 2*PI*f0/fs;
			if (plh == Type.LSHL) w0_ = w0_*1.414214;
			if (plh == Type.HSHL) w0_ = w0_/1.414214;
			qVal_ = qVal;
			plh_ = plh;
			SetCoefficients(0.0);		// 初期値の周波数特性はフラット
		}

		public void SetCoefficients(double gDb)
		{
			double gSqrt = Sqrt(Pow(10, gDb/20.0));

			if (plh_ == Type.PEAK)	// ピーキング･フィルタ
			{
				double alpha = Sin(w0_)/(2.0*qVal_);
				double beta = 1.0f + alpha/gSqrt;
				coefs_.a1 = 2.0*Cos(w0_)/beta;
				coefs_.a2 = -(1.0 - alpha/gSqrt)/beta;
				coefs_.b0 = (1.0f + alpha*gSqrt)/beta;
				coefs_.b1 = -coefs_.a1;
				coefs_.b2 = (1.0f - alpha*gSqrt)/beta;
			}
			else	// ロー/ハイ･シェルビング･フィルタ
			{
				double alpha = Sin(w0_)/1.414214; // Q = 1/sqrt(2)
				double g_a = Sqrt(gSqrt);
				double cosW0 = Cos(w0_);
				double gSqrtP1 = gSqrt+1.0f;
				double gSqrtM1 = gSqrt-1.0f;

				if (plh_ == Type.LSHL)	// ロー･シェルビング･フィルタ
				{
					double beta = gSqrtP1 + gSqrtM1*cosW0 + 2.0*g_a*alpha;
					coefs_.a1 = 2.0*(gSqrtM1 + gSqrtP1*cosW0)/beta;
					coefs_.a2 = -(gSqrtP1 + gSqrtM1*cosW0 - 2.0*g_a*alpha)/beta;
					coefs_.b0 = gSqrt*(gSqrtP1 - gSqrtM1*cosW0 + 2.0*g_a*alpha)/beta;
					coefs_.b1 = 2.0*gSqrt*(gSqrtM1 - gSqrtP1*cosW0)/beta;
					coefs_.b2 = gSqrt*(gSqrtP1 - gSqrtM1*cosW0 - 2.0*g_a*alpha)/beta;
				}
				else					// ハイ･シェルビング･フィルタ
				{
					double beta = gSqrtP1 - gSqrtM1*cosW0 + 2.0*g_a*alpha;
					coefs_.a1 = -2.0*(gSqrtM1 - gSqrtP1*cosW0)/beta;
					coefs_.a2 = -(gSqrtP1 - gSqrtM1*cosW0 - 2.0*g_a*alpha)/beta;
					coefs_.b0 = gSqrt*(gSqrtP1 + gSqrtM1*cosW0 + 2.0*g_a*alpha)/beta;
					coefs_.b1 = -2.0f*gSqrt*(gSqrtM1 + gSqrtP1*cosW0)/beta;
					coefs_.b2 = gSqrt*(gSqrtP1 + gSqrtM1*cosW0 - 2.0*g_a*alpha)/beta;
				}
			}
		}

		// 周波数応答を計算するための伝達関数の定義
		public Complex GetResponse(Complex u)
		{
			return (coefs_.b0 + (coefs_.b1 + coefs_.b2*u)*u)/
					(1.0 - (coefs_.a1 + coefs_.a2*u)*u);
		}
	}
}
