/*************************************************************************/
/* iir.c
/*************************************************************************/
#include "stdio.h"
#include "math.h"
#include "iir.h"

/*************************************************************************/
/* Global Variables
/*************************************************************************/
const char *filterTypeString[] =
{
	"Unknown filter type",
	"Butterworth-Lowpass",
	"Butterworth-Highpass",
    "Butterworth-Bandpass",
	"Butterworth-Bandstop",
	"Peaking-EQ",
	"Low Shelving-EQ",
	"High Shelving-EQ"
};

/******************************************************************************/
void IIRCalcFilterCoeff(struct _sIIRCoeff *pCoeff, double fa, double fg, double q, unsigned order, unsigned filterType)
{
	unsigned p;
	double qp;
	
	IIRInit(pCoeff, order);

	for(p=0; p < order/2;p++)
	{
		qp = q * IIRCalcQp(p+1, order);
		IIRCalcPartFilterCoeff2(&pCoeff[p], 1.0, fa, fg, qp, filterType);
	}
}

int IIRCalcPartFilterCoeff1(struct _sIIRCoeff *pCoeff, double fa, double fg, double Qi, unsigned filterType)
{
	double K, a0;
	double alpha, omega, ks, kc;
	unsigned error;

	omega = 2*pi*fg/fa;
	ks    = sin(omega);
	kc    = cos(omega);
	alpha = 0.5*ks /Qi;

	K  = IIRBilTrans(fg, fa);
	a0 = K/Qi + 1;

	switch(filterType)
	{
	case IIR_FILTERTYPE_LOWPASS:
	
		pCoeff->ak0 = 1.0;
		pCoeff->ak1 = (1 - K/Qi)/a0;
		pCoeff->ak2 = 0.0;

		pCoeff->bk0 = 1.0/a0;
		pCoeff->bk1 = 1.0/a0;
		pCoeff->bk2 = 0.0;

		break;
	
	case IIR_FILTERTYPE_HIGHPASS:
	
		pCoeff->ak0 = 1.0;
		pCoeff->ak1 = (1 - K/Qi) /a0;
		pCoeff->ak2 = 0.0;

		pCoeff->bk0 =  1.0*K /a0;
		pCoeff->bk1 = -1.0*K /a0;
		pCoeff->bk2 =  0.0;
		
		break;

	default:
		error = -1;
		break;
	}
	return error;
}

int IIRCalcPartFilterCoeff2(struct _sIIRCoeff *pCoeff, double A, double fa, double fg, double qp, unsigned filterType)
{
	double a0;
	double alpha, omega, ks, kc;
	unsigned error;

	omega = 2*pi*fg/fa;
	ks    = sin(omega);
	kc    = cos(omega);
	alpha = 0.5*ks /qp;

	error = 0;
	switch(filterType)
	{
	case IIR_FILTERTYPE_LOWPASS:

		a0 = 1 + alpha;
		pCoeff->ak0 =  1.0;
		pCoeff->ak1 = -2.0*kc /a0;
		pCoeff->ak2 = (1 - alpha) /a0;

		pCoeff->bk0 = 0.5*(1 - kc) /a0;
		pCoeff->bk1 = (1 - kc) /a0;
		pCoeff->bk2 = 0.5*(1 - kc) /a0;

		break;
	
	case IIR_FILTERTYPE_HIGHPASS:

		a0 = 1 + alpha;
		pCoeff->ak0 =  1.0;
		pCoeff->ak1 = -2.0*kc /a0;
		pCoeff->ak2 = (1 - alpha) /a0;

		pCoeff->bk0 = 0.5*(1 + kc) /a0;
		pCoeff->bk1 = -(1 + kc) /a0;
		pCoeff->bk2 = 0.5*(1 + kc) /a0;

		break;

	case IIR_FILTERTYPE_BANDPASS:

		a0 = 1 + alpha;
		pCoeff->ak0 =  1.0;
		pCoeff->ak1 = -2.0*kc /a0;
		pCoeff->ak2 = (1 - alpha) /a0;

		pCoeff->bk0 = alpha /a0;
		pCoeff->bk1 = 0;
		pCoeff->bk2 = -alpha /a0;

		break;

	case IIR_FILTERTYPE_BANDSTOP:

		a0 = 1 + alpha;
		pCoeff->ak0 =  1.0;
		pCoeff->ak1 = -2.0*kc /a0;
		pCoeff->ak2 = (1 - alpha) /a0;

		pCoeff->bk0 = 1.0 /a0;
		pCoeff->bk1 = -2.0*kc /a0;
		pCoeff->bk2 = 1.0 /a0;

		break;

	case IIR_FILTERTYPE_PEAKING:

		a0 = 1 + (alpha/A);
		pCoeff->ak0 =  1.0;
		pCoeff->ak1 = -2.0*kc /a0;
		pCoeff->ak2 = (1 - (alpha/A)) /a0;

		pCoeff->bk0 =  (1 + (alpha*A)) /a0;
		pCoeff->bk1 = -2.0*kc /a0;
		pCoeff->bk2 =  (1 - (alpha*A)) /a0;

		break;

	default:
		error = -1;
		break;
	}
	return error;
}

void IIR(struct _sIIRCoeff *pCoeff, double *xn, double *yn, unsigned order, unsigned numPoints)
{
	double xp, yp;
	unsigned i, p;
	unsigned numSec = order/2;

	for (i=0; i<numPoints; i++)
	{
		xp = xn[i];
		for (p=0; p < numSec; p++)
		{
			yp = pCoeff[p].bk0*xp 
			   + pCoeff[p].bk1*pCoeff[p].xn1
			   + pCoeff[p].bk2*pCoeff[p].xn2
			   - pCoeff[p].ak1*pCoeff[p].yn1
			   - pCoeff[p].ak2*pCoeff[p].yn2;
			
			pCoeff[p].yn2 = pCoeff[p].yn1;
			pCoeff[p].yn1 = yp;
			pCoeff[p].xn2 = pCoeff[p].xn1;
			pCoeff[p].xn1 = xp;
			xp	  = yp;
		}
		yn[i] = yp;	
	}
}

void IIRInit(struct _sIIRCoeff *pCoeff, unsigned order)
{
	unsigned n;
	for(n=0; n < order/2; n++)
	{
		pCoeff[n].ak0 = 0;
		pCoeff[n].ak1 = 0;
		pCoeff[n].ak2 = 0;
		pCoeff[n].bk0 = 0;
		pCoeff[n].bk1 = 0;
		pCoeff[n].bk2 = 0;
		pCoeff[n].xn1 = 0;
		pCoeff[n].xn2 = 0;
		pCoeff[n].yn1 = 0;
		pCoeff[n].yn2 = 0;
	}
}

double IIRBilTrans(double fg, double fa)
{
	return 1.0/(tan(pi*fg/fa));
}

double IIRCalcQp(unsigned p, unsigned N)
{
	return 1.0/(2*sin(pi*(2*p-1)/(2*N)));
}