/*************************************************************************/
/* iir.c */
/*************************************************************************/
#include "stdio.h"
#include "stdlib.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, iir_float_t fa, iir_float_t fg, iir_float_t q, unsigned order, unsigned filterType)
{
	unsigned p;
	iir_float_t 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, iir_float_t fa, iir_float_t fg, iir_float_t Qi, unsigned filterType)
{
	iir_float_t K, a0;
	iir_float_t alpha, omega, ks, kc;
	unsigned error;

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

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

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

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

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

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

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

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

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

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

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

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

		break;
	
	case IIR_FILTERTYPE_HIGHPASS:

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

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

		break;

	case IIR_FILTERTYPE_BANDPASS:

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

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

		break;

	case IIR_FILTERTYPE_PEAKING:

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

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

		break;

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

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

iir_float_t IIRBilTrans(iir_float_t fg, iir_float_t fa)
{
	return 1.0f/(iir_float_t)(tan(iir_pi*fg/fa));
}

iir_float_t IIRCalcQp(unsigned p, unsigned N)
{
	return 1.0f/(iir_float_t)(2*sin(iir_pi*(2*p-1)/(2*N)));
}

iir_float_t IIRS(struct _sIIRCoeff *pCoeff, iir_float_t xn, unsigned order)
{
	iir_float_t xp, yp;
	unsigned p;

	xp = xn;
	yp = 0;
	for (p=0; p < order/2; p++)
	{

		yp = (iir_float_t) (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;
	}
	return yp;
}

void IIR_lin_init(iir_lin_t *pObj, unsigned order)
{
	pObj->order = order;
	pObj->pX = (iir_float_t*)malloc((order+1)*sizeof(iir_float_t));
	pObj->pY = (iir_float_t*)malloc((order+1)*sizeof(iir_float_t));
	memset(pObj->pX, 0, (order+1)*sizeof(iir_float_t));
	memset(pObj->pY, 0, (order+1)*sizeof(iir_float_t));
}

void IIR_lin_free(iir_lin_t *pObj)
{
	if (pObj->pX)
		free(pObj->pX);

	if (pObj->pY)
		free(pObj->pY);

	pObj->order = 0;

}

iir_float_t IIR_lin_process(iir_lin_t *pObj, iir_float_t *pB, iir_float_t *pA, iir_float_t x)
{
	unsigned i;
	iir_float_t y;

	if (!pObj->order)
		return 0;

	for (i=pObj->order; i >= 1; i--)
		pObj->pX[i]	= pObj->pX[i-1];

	for (i=pObj->order; i >= 1; i--)
		pObj->pY[i]	= pObj->pY[i-1];

	pObj->pX[0] = x;

	y = 0;
	for (i=0; i <= pObj->order; i++)
		y += pObj->pX[i]*pB[i];

	for (i=1; i <= pObj->order; i++)
		y -= pObj->pY[i]*pA[i];

	pObj->pY[0] = y;

	return y;

}