//------------------------------------------------------------------------------
//	双一次ｚ変換による IIR バタワースフィルタの設計
// 		偶数次の LPF, HPF のみ
//
//		作成者：三上直樹，2022/02/25 作成，©三上直樹 2022
//------------------------------------------------------------------------------

using System.Numerics;
using System.Windows.Forms;

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

namespace Mikami.Application
{
	public class BiliearDesign
	{
		public struct Coefs { public float a1, a2, b1; }
		public enum Type { LPF, HPF };	// LPF, HPF の区別用

		private Complex[] sP_;	// ｓ平面の極
		private Complex[] zP_;	// ｚ平面の極
		private Coefs[] ck_;	// 縦続形に対応する伝達関数の係数
		private float gain_;	// 縦続形の利得定数
		private int order_;		// 伝達関数の次数

		public void Design(int order, Type pb, float fs, float fc)
		{
			order_ = order;	// 次数（偶数のみ）
			if ( (fc < 0) || (fc>fs/2) )
			{
				MessageBox.Show("遮断周波数が不適切です", "入力エラー");
				return;
			}
	
			gain_ = 1.0f;
			sP_ = new Complex[order_/2];
			zP_ = new Complex[order_/2];
			ck_ = new Coefs[order_/2];

			Butterworth();
			Bilinear(fc, fs);
			ToCascade(pb);
			gain_ = GetGain(pb);
		}

		// 周波数応答を計算するための伝達関数の定義
		public Complex GetResponse(Complex u)
		{
			Complex hz = gain_;
			for (int k=0; k<order_/2; k++)
				hz = hz*(1.0 + (ck_[k].b1 + u)*u)/
						(1.0 - (ck_[k].a1 + ck_[k].a2*u)*u);
			return hz;
		}

		// 設計された係数の取得
		public void GetCoefficients(out float g0, out Coefs[] coefs)
		{
			g0 = gain_;
			coefs = ck_;
		}

		// 低域通過 Butterworth フィルタの極の計算（遮断角周波数 = 1）
		private void Butterworth()
		{
			for (int j=0; j<order_/2; j++)	// 極(虚部≧0 の極のみ)
			{
				double theta = (2.0*j + 1.0)*PI/(2.0*order_);
				sP_[j] = new Complex(-Cos(theta), Sin(theta));
			}
		}
		
		// 双一次ｚ変換
		private void Bilinear(float fc, float fs)
		{
			double wc = Tan(fc*PI/fs);
			for (int k=0; k<order_/2; k++)
				zP_[k] = (1.0 + wc*sP_[k])/(1.0 - wc*sP_[k]);
		}
		
		// 縦続形の係数に変換
		private void ToCascade(Type pb)
		{
			for (int j=0; j<order_/2; j++)
			{
				ck_[j].a1 = (float)(2.0*(zP_[j].Real));	// a1m
				double abs_zP = Abs(zP_[j]);
				ck_[j].a2 = (float)(-abs_zP*abs_zP);	// a2m
				if (pb == Type.LPF) ck_[j].b1 = 2.0f;	// b1m, LPF
				else				ck_[j].b1 = -2.0f;	// b1m, HPF
			}
		}

		// 利得定数の計算
		private float GetGain(Type pb)
		{
			if (pb == Type.LPF)
				return 1.0f/(float)(Abs(GetResponse(1)));
			else
				return 1.0f/(float)(Abs(GetResponse(-1)));
		}
	}
}
