#include <math.h>
#include "const.h"
#include "IIRFilter.h"

void IIRFilter::call_peaking_eq_with_lowfreq(double fc, double gain, double q, int fs
	, double* coef_b, double* coef_a)
{
	double A, H, k;
	double a, d;
	double f2, f1;
	double ftemp = 0.0;
	double bw;

	ftemp = sqrt(1 + 1 / (4 * q * q));
	f1 = fc * (ftemp - 0.5 / q);
	f2 = fc * (ftemp + 0.5 / q);
	if (f2 > 22000)
		f2 = 22000;

	bw = f2 - f1;
	if (bw < 0.1)
		bw = 0.1;


	A = pow(10, (gain * 0.05));
	k = tan(pi * bw / fs);

	if (A < 1.0)
		a = (k - A) / (k + A);
	else
		a = (k - 1) / (k + 1);

	H = (A - 1) / 2;
	d = -cos(2 * pi * fc / fs);

	coef_b[0] = 1 + (1 + a) * H;
	coef_b[1] = d * (1 - a);
	coef_b[2] = (-a - (coef_b[0] - 1));	//( - a - (1 + a) * H / 2);

	coef_a[0] = 1;
	coef_a[1] = coef_b[1];
	coef_a[2] = -a;

}

void IIRFilter::call_peaking_eq(double fc, double gain, double q, int fs, double* coef_b, double* coef_a)
{
	double bw;
	double deltaw;
	double g0, g, gb;
	double beta;
	double w0;
	double f2, f1;
	double ftemp;

	if (gain<0.1 && gain>-0.1) {
		coef_b[0] = coef_a[0] = 1;
		coef_b[1] = coef_b[2] = coef_a[1] = coef_a[2] = 0;
		return;
	}

	if (fc < 100) return call_peaking_eq_with_lowfreq(fc, gain, q, fs, coef_b, coef_a);
	//bw = fc / q;
	//if (bw >= fs / 2) bw = fs / 2 - 100;
	ftemp = sqrt(1 + 1 / (4 * q * q));
	f1 = fc * (ftemp - 0.5 / q);
	f2 = fc * (ftemp + 0.5 / q);
	if (f2 >= fs / 2.0f) f2 = fs / 2.0f - 100.0f;

	bw = f2 - f1;
	w0 = 2 * pi * fc / fs;
	deltaw = 2 * pi * bw / fs;

	g0 = 1.0;
	g = pow(10, (gain / 20)) * g0;

	if (fabs(gain) <= 6) {
		gb = pow(10, gain / 40);
	}
	else if (gain > 0) {
		gb = pow(10, (gain - 3) / 20); //·åÖµÒÔÏÂ-3dB´¦´ø¿í
	}
	else {
		gb = pow(10, (gain + 3) / 20);  //·åÖµÒÔÉÏ3dB´¦´ø¿í
	}

	beta = sqrt((gb * gb - g0 * g0) / (g * g - gb * gb)) * tan(deltaw / 2);

	coef_a[0] = 1;
	coef_a[1] = -2 * (cos(w0) / (1 + beta));
	coef_a[2] = (1 - beta) / (1 + beta);

	coef_b[0] = (g0 + g * beta) / (1 + beta);
	coef_b[1] = -2 * (g0 * cos(w0) / (1 + beta));
	coef_b[2] = (g0 - g * beta) / (1 + beta);
}

int IIRFilter::eq(IIRBANDType type, int f0, double g, double q, int fs, double(*sos)[6])
{
	double A = sqrt(pow(10, g * 0.05));
	double w0 = 2 * pi * f0 / fs;
	double alpha = sin(w0) / (2 * q);
	double* b = sos[0];
	double* a = sos[0] + 3;

	switch (type)
	{
	case lowpass:
		A = pow(10, g * 0.05);

		b[0] = A * (1 - cos(w0)) / 2;
		b[1] = A * (1 - cos(w0));
		b[2] = A * (1 - cos(w0)) / 2;

		a[0] = 1 + alpha;
		a[1] = -2 * cos(w0);
		a[2] = 1 - alpha;
		break;
	case highpass:
		A = pow(10, g * 0.05);
		b[0] = A * (1 + cos(w0)) / 2;
		b[1] = -A * (1 + cos(w0));
		b[2] = A * (1 + cos(w0)) / 2;

		a[0] = 1 + alpha;
		a[1] = -2 * cos(w0);
		a[2] = 1 - alpha;
		break;
	case bpf_pq:
		break;
	case bpf_zero:
		break;
	case notch: {
		double w0 = 2 * pi * f0 / fs;
		double alpha = sin(w0) / (2 * q);
		double A = pow(10.0, g / 40.0);  // gain/40 ¶ø²»ÊÇ gain/20

		// ·Ö×ÓϵÊý
		b[0] = 1.0 + alpha * A;
		b[1] = -2.0 * cos(w0);
		b[2] = 1.0 - alpha * A;

		// ·ÖĸϵÊý
		a[0] = 1.0 + alpha / A;
		a[1] = -2.0 * cos(w0);
		a[2] = 1.0 - alpha / A;
	}
		break;
	case apf:
		A = pow(10, g * 0.05);
		b[0] = A * (1 - alpha);
		b[1] = A * -2 * cos(w0);
		b[2] = A * (1 + alpha);

		a[0] = 1 + alpha;
		a[1] = -2 * cos(w0);
		a[2] = 1 - alpha;
		break;
	case peaking_eq:
		call_peaking_eq(f0, g, q, fs, b, a);
		break;
	case lowshelf: {
		double cosw, sqrta;

		A = pow(10, g * 0.025);
		cosw = cos(w0);
		sqrta = 2 * sqrt(A) * alpha;

		b[0] = A * ((A + 1) - (A - 1) * cosw + sqrta);
		b[1] = 2 * A * ((A - 1) - (A + 1) * cosw);
		b[2] = A * ((A + 1) - (A - 1) * cosw - sqrta);
		a[0] = (A + 1) + (A - 1) * cosw + sqrta;
		a[1] = -2 * ((A - 1) + (A + 1) * cosw);
		a[2] = (A + 1) + (A - 1) * cosw - sqrta;
	}
		break;
	case highshelf: {
		double cosw, sqrta;

		A = pow(10, g * 0.025);
		cosw = cos(w0);
		sqrta = 2 * sqrt(A) * alpha;

		b[0] = A * ((A + 1) + (A - 1) * cosw + sqrta);
		b[1] = -2 * A * ((A - 1) + (A + 1) * cosw);
		b[2] = A * ((A + 1) + (A - 1) * cosw - sqrta);
		a[0] = (A + 1) - (A - 1) * cosw + sqrta;
		a[1] = 2 * ((A - 1) - (A + 1) * cosw);
		a[2] = (A + 1) - (A - 1) * cosw - sqrta;
	}
		break;
	default:
		return ret_param_error;
		break;
	}

	return ret_success;
}
int IIRFilter::butters(IIRBANDType type, int n, int fc, int fs, double g, double(*sos)[6])
{
	double wc = tan(2 * pi * fc / fs / 2);
	double w;
	double re_p[8], im_p[8];
	int i, nsection;
	double a1;
	double re_m, re_k, im_m, im_k;

	nsection = (n + 1) / 2;

	for (i = 0; i < n; i++)
	{
		w = (pi * (n + 2 * i + 1) / (2 * n));
		re_p[i] = wc * cos(w);
		im_p[i] = wc * sin(w);
	}

	if (type == IIRBANDType::lowpass)
	{
		for (i = 0; i < nsection; i++)
		{
			if (i == n - i - 1)
			{
				a1 = 1 - re_p[i];
				sos[i][0] = wc / a1;
				sos[i][1] = wc / a1;
				sos[i][2] = 0;
				sos[i][3] = 1;
				sos[i][4] = (-re_p[i] - 1) / a1;
				sos[i][5] = 0;

			}
			else
			{
				re_m = re_p[i] * re_p[n - i - 1] - im_p[i] * im_p[n - i - 1];
				im_m = re_p[i] * im_p[n - i - 1] + im_p[i] * re_p[n - i - 1];

				re_k = re_p[i] + re_p[n - i - 1];
				im_k = im_p[i] + im_p[n - i - 1];

				a1 = 1 - re_k + re_m;

				sos[i][0] = wc * wc / a1;
				sos[i][1] = 2 * wc * wc / a1;
				sos[i][2] = wc * wc / a1;
				sos[i][3] = 1;
				sos[i][4] = (2 * re_m - 2) / a1;
				sos[i][5] = (re_m + re_k + 1) / a1;
			}
		}
	}
	else if (type == IIRBANDType::highpass)
	{
		for (i = 0; i < nsection; i++)
		{
			if (i == n - i - 1)
			{
				a1 = wc * wc - re_p[i];
				sos[i][0] = wc / a1;
				sos[i][1] = -wc / a1;
				sos[i][2] = 0;
				sos[i][3] = 1;
				sos[i][4] = (wc * wc + re_p[i]) / a1;
				sos[i][5] = 0;

			}
			else
			{
				re_m = re_p[i] * re_p[n - i - 1] - im_p[i] * im_p[n - i - 1];
				im_m = re_p[i] * im_p[n - i - 1] + im_p[i] * re_p[n - i - 1];

				re_k = re_p[i] + re_p[n - i - 1];
				im_k = im_p[i] + im_p[n - i - 1];

				a1 = wc * wc * wc * wc - re_k * wc * wc + re_m;

				sos[i][0] = wc * wc / a1;
				sos[i][1] = -2 * wc * wc / a1;
				sos[i][2] = wc * wc / a1;
				sos[i][3] = 1;
				sos[i][4] = (2 * wc * wc * wc * wc - 2 * re_m) / a1;
				sos[i][5] = (wc * wc * wc * wc + re_k * wc * wc + re_m) / a1;
			}
		}
	}

	g = pow(10.0, g / 20.0);
	sos[0][0] *= g;
	sos[0][1] *= g;
	sos[0][2] *= g;

	return nsection;
}

int IIRFilter::linkwitzs(IIRBANDType type, int n, int fc, int fs, double g, double(*sos)[6])
{
	double q[5];
	double wc = tan(2 * pi * fc / fs / 2);
	int i;
	int nsection = n / 2;
	double a1;

	switch (n)
	{
	case 2:
		q[0] = 1 / 0.5;
		break;
	case 4:
		q[0] = 1 / 0.71;
		q[1] = 1 / 0.71;
		break;
	case 6:
		q[0] = 1 / 0.5;
		q[1] = 1.0;
		q[2] = 1.0;
		break;
	case 8:
		q[0] = 1 / 0.54;
		q[1] = 1 / 1.34;
		q[2] = 1 / 0.54;
		q[3] = 1 / 1.34;
		break;
	case 10:
		q[0] = 1 / 0.5;
		q[1] = 1 / 0.62;
		q[2] = 1 / 1.62;
		q[3] = 1 / 0.62;
		q[4] = 1 / 1.62;
		break;
	}

	if (type == IIRBANDType::lowpass)
	{
		for (i = 0; i < nsection; i++)
		{
			a1 = 1 + q[i] * wc + wc * wc;
			sos[i][0] = wc * wc / a1;
			sos[i][1] = 2 * wc * wc / a1;
			sos[i][2] = wc * wc / a1;
			sos[i][3] = 1;
			sos[i][4] = (2 * wc * wc - 2) / a1;
			sos[i][5] = (wc * wc - q[i] * wc + 1) / a1;

		}
	}
	else if (type == IIRBANDType::highpass)
	{
		for (i = 0; i < nsection; i++)
		{
			a1 = 1 + q[i] * wc + wc * wc;
			sos[i][0] = 1 / a1;
			sos[i][1] = -2 / a1;
			sos[i][2] = 1 / a1;
			sos[i][3] = 1;
			sos[i][4] = (2 * wc * wc - 2) / a1;
			sos[i][5] = (wc * wc - q[i] * wc + 1) / a1;

		}
	}
	g = pow(10.0, g / 20.0);
	sos[0][0] *= g;
	sos[0][1] *= g;
	sos[0][2] *= g;

	return nsection;
}
int IIRFilter::bessels(IIRBANDType type, int n, int fc, int fs, double g, double(*sos)[6])
{
	int nsection = (n + 1) / 2;
	double wc = tan(2 * pi * fc / fs / 2);
	double re_p[8],im_p[8];
	int i;
	double a1;
	double constant[] = { 1.0, 1.272, 1.413, 1.533, 1.613, 1.735, 1.804, 1.956 };
	int k = 0;


	if (n == 1)
	{
		re_p[0] = -1; im_p[0] = 0;
	}
	else if (n == 2)
	{
		re_p[0] = -.8660254037844386467637229; im_p[0] = .4999999999999999999999996;
		re_p[1] = -.8660254037844386467637229; im_p[1] = -.4999999999999999999999996;
	}
	else if (n == 3)
	{
		re_p[0] = -.9416000265332067855971980; im_p[0] = 0;
		re_p[1] = -.7456403858480766441810907; im_p[1] = -.7113666249728352680992154;
		re_p[2] = -.7456403858480766441810907; im_p[2] = .7113666249728352680992154;
	}
	else if (n == 4)
	{
		re_p[0] = -.6572111716718829545787781; im_p[0] = -.8301614350048733772399715;
		re_p[1] = -.6572111716718829545787788; im_p[1] = .8301614350048733772399715;
		re_p[2] = -.9047587967882449459642637; im_p[2] = -.2709187330038746636700923;
		re_p[3] = -.9047587967882449459642624; im_p[3] = .2709187330038746636700926;
	}
	else if (n == 5)
	{
		re_p[0] = -.9264420773877602247196260; im_p[0] = 0;
		re_p[1] = -.8515536193688395541722677; im_p[1] = -.4427174639443327209850002;
		re_p[2] = -.8515536193688395541722677; im_p[2] = .4427174639443327209850002;
		re_p[3] = -.5905759446119191779319432; im_p[3] = -.9072067564574549539291747;
		re_p[4] = -.5905759446119191779319432; im_p[4] = .9072067564574549539291747;
	}
	else if (n == 6)
	{
		re_p[0] = -.9093906830472271808050953; im_p[0] = -.1856964396793046769246397;
		re_p[1] = -.9093906830472271808050953; im_p[1] = .1856964396793046769246397;
		re_p[2] = -.7996541858328288520243325; im_p[2] = -.5621717346937317988594118;
		re_p[3] = -.7996541858328288520243325; im_p[3] = .5621717346937317988594118;
		re_p[4] = -.5385526816693109683073792; im_p[4] = -.9616876881954277199245657;
		re_p[5] = -.5385526816693109683073792; im_p[5] = .9616876881954277199245657;
	}
	else if (n == 7)
	{
		re_p[0] = -.9194871556490290014311619; ; im_p[0] = 0;
		re_p[1] = -.8800029341523374639772340; im_p[1] = -.3216652762307739398381830;
		re_p[2] = -.8800029341523374639772340; im_p[2] = .3216652762307739398381830;
		re_p[3] = -.7527355434093214462291616; im_p[3] = -.6504696305522550699212995;
		re_p[4] = -.7527355434093214462291616; im_p[4] = .6504696305522550699212995;
		re_p[5] = -.4966917256672316755024763; im_p[5] = -1.002508508454420401230220;
		re_p[6] = -.4966917256672316755024763; im_p[6] = 1.002508508454420401230220;
	}
	else if (n == 8)
	{
		re_p[0] = -.9096831546652910216327629; im_p[0] = -.1412437976671422927888150;
		re_p[1] = -.9096831546652910216327629; im_p[1] = .1412437976671422927888150;
		re_p[2] = -.8473250802359334320103023; im_p[2] = -.4259017538272934994996429;
		re_p[3] = -.8473250802359334320103023; im_p[3] = .4259017538272934994996429;
		re_p[4] = -.7111381808485399250796172; im_p[4] = -.7186517314108401705762571;
		re_p[5] = -.7111381808485399250796172; im_p[5] = .7186517314108401705762571;
		re_p[6] = -.4621740412532122027072175; im_p[6] = -1.034388681126901058116589;
		re_p[7] = -.4621740412532122027072175; im_p[7] = 1.034388681126901058116589;
	}


	if (type == IIRBANDType::lowpass)
	{
		wc = wc / constant[n - 1];

		if ((n & 1) == 1)
		{
			a1 = wc - re_p[0];
			sos[k][0] = 1 / a1;
			sos[k][1] = -1 / a1;
			sos[k][2] = 0;

			sos[k][3] = 1;
			sos[k][4] = (re_p[0] + wc) / a1;
			sos[k][5] = 0;

			k = 1;
		}
		for (i = k; i < n; i += 2)
		{
			double re_s, re_m, im_s, im_m;

			re_m = re_p[i] * re_p[i + 1] - im_p[i] * im_p[i + 1];
			im_m = re_p[i] * im_p[i + 1] + im_p[i] * re_p[i + 1];

			re_s = re_p[i] + re_p[i + 1];
			im_s = im_p[i] + im_p[i + 1];

			a1 = wc * wc - re_s * wc + re_m;

			sos[k][0] = 1 / a1;
			sos[k][1] = -2 / a1;
			sos[k][2] = 1 / a1;

			sos[k][3] = 1;
			sos[k][4] = (2 * wc * wc - 2 * re_m) / a1;
			sos[k][5] = (wc * wc + re_s * wc + re_m) / a1;
			k = k + 1;
		}
	}
	else if (type == IIRBANDType::highpass)
	{
		wc = wc * constant[n - 1];

		if ((n & 1) == 1)
		{
			a1 = 1 - re_p[0] * wc;
			sos[k][0] = wc / a1;
			sos[k][1] = wc / a1;
			sos[k][2] = 0;

			sos[k][3] = 1;
			sos[k][4] = (-re_p[0] * wc - 1) / a1;
			sos[k][5] = 0;

			k = 1;
		}
		for (i = k; i < n; i += 2)
		{
			double re_s, re_m, im_s, im_m;

			re_m = re_p[i] * re_p[i + 1] - im_p[i] * im_p[i + 1];
			im_m = re_p[i] * im_p[i + 1] + im_p[i] * re_p[i + 1];

			re_s = re_p[i] + re_p[i + 1];
			im_s = im_p[i] + im_p[i + 1];

			a1 = 1 - re_s * wc + re_m * wc * wc;

			sos[k][0] = wc * wc / a1;
			sos[k][1] = 2 * wc * wc / a1;
			sos[k][2] = wc * wc / a1;

			sos[k][3] = 1;
			sos[k][4] = (2 * re_m * wc * wc - 2) / a1;
			sos[k][5] = (re_m * wc * wc + re_s * wc + 1) / a1;
			k = k + 1;
		}

	}
	g = pow(10.0, g / 20.0);
	sos[0][0] *= g;
	sos[0][1] *= g;
	sos[0][2] *= g;

	return nsection;
}