/*************************************************************************
** 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, float fa, float fg, float q, unsigned order, unsigned filterType)
{
	unsigned p;
	float 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, float fa, float fg, float Qi, unsigned filterType)
{
	float K, a0;
	float alpha, omega, ks, kc;
	unsigned error;

	omega = (float)(2*pi*fg/fa);
	ks    = (float)sin(omega);
	kc    = (float)cos(omega);
	alpha = (float)(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 = (float)(1.0/a0);
		pCoeff->bk1 = (float)(1.0/a0);
		pCoeff->bk2 = (float)0.0;

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

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

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

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

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

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

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

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

		break;
	
	case IIR_FILTERTYPE_HIGHPASS:

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

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

		break;

	case IIR_FILTERTYPE_BANDPASS:

		a0 = 1 + alpha;
		pCoeff->ak0 =  1.0;
		pCoeff->ak1 = (float)(-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 = (float)(-2.0*kc /a0);
		pCoeff->ak2 = (1 - alpha) /a0;

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

		break;

	case IIR_FILTERTYPE_PEAKING:

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

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

		break;

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

void IIR(struct _sIIRCoeff *pCoeff, float *xn, float *yn, unsigned order, unsigned numPoints)
{
	float 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;
	}
}

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

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

