/******************************************************************************/
#include <math.h>
#include <float.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "iir.h"

/******************************************************************************/
const char *filterTypeString[] =
{
	"Unknown filter type",
	"Butterworth-Lowpass",
	"Butterworth-Highpass",
    "Butterworth-Bandpass",
	"Butterworth-Bandstop"
};

/******************************************************************************/
int IIRCalcPartFilterCoeff1(CIIRCoeff *pCoeff, double fg, double fa, double Qi, unsigned filterType)
{
	double K, A0;
	unsigned error;
	
	error = 0;
	switch(filterType)
	{
	case IIR_FILTERTYPE_LOWPASS:
	
		K = IIRBilTrans(fg, fa);
		A0 = 1.0 /(K/Qi + 1);

		pCoeff->m_pak[0] = 1.0;
		pCoeff->m_pak[1] = (1 - K/Qi) * A0;

		pCoeff->m_pbk[0] = 1.0;// * A0;
		pCoeff->m_pbk[1] = 1.0;// * A0;
		pCoeff->m_aScale = A0;
		break;
	
	case IIR_FILTERTYPE_HIGHPASS:
	
		K  = IIRBilTrans(fg, fa);
		A0 = 1.0 /(K/Qi + 1);

		pCoeff->m_pak[0] = 1.0;
		pCoeff->m_pak[1] = (1 - K/Qi) * A0;

		pCoeff->m_pbk[0] =  1.0*K;// * A0;
		pCoeff->m_pbk[1] = -1.0*K;// * A0;
		pCoeff->m_aScale = A0;
		break;

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

int IIRCalcPartFilterCoeff2(CIIRCoeff *pCoeff, double fg, double fa, double Qi, unsigned filterType)
{
	double K, KK, A0,B0;
	double alpha, omega, sn, cs;
	unsigned error;

	K = IIRBilTrans(fg, fa);
	KK = K*K;
	A0 = 1.0 /(1 + KK + K/Qi);

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

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

		pCoeff->m_pak[0] = 1.0 ;
		pCoeff->m_pak[1] = 2 *(1 - KK) * A0;
		pCoeff->m_pak[2] = (1 + KK - K/Qi) * A0;

		pCoeff->m_pbk[0] = 1.0*A0;
		pCoeff->m_pbk[1] = 2.0*A0;
		pCoeff->m_pbk[2] = 1.0*A0;

		pCoeff->m_aScale = A0;

		break;
	
	case IIR_FILTERTYPE_HIGHPASS:

		pCoeff->m_pak[0] = 1.0;
		pCoeff->m_pak[1] = 2 *(1 - KK) * A0;
		pCoeff->m_pak[2] = (KK - K/Qi + 1) * A0;

		pCoeff->m_pbk[0] =  1.0*KK * A0;
		pCoeff->m_pbk[1] = -2.0*KK * A0;
		pCoeff->m_pbk[2] =  1.0*KK * A0;
		pCoeff->m_aScale = A0;
		break;

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

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

void IIR(double *xn, double *yn, CIIRCoeff *pCoeff, unsigned numPoints)
{
	unsigned n, k;
	double y1, y2;
	
	for (n=0; n < numPoints; n++)
	{
		y1 = 0;
		y2 = 0;
		for (k=0; k <= pCoeff->m_Nb; k++)
		{
			y1 = y1 + (pCoeff->m_pbk[k] * xn[pCoeff->m_Nb-k+n]);
			if (!_finite(y1))
				printf("\nException: MATH ERROR!\n");
		}
//		y1 = y1 *pCoeff->m_aScale;

		for (k=1; k <= pCoeff->m_Na; k++)
		{
			y2 = y2 - (pCoeff->m_pak[k] * yn[pCoeff->m_Na-k+n]);
			if (!_finite(y2))
				printf("\nException: MATH ERROR!\n");
		}
//		y2 = y2 *pCoeff->m_bScale;

		yn[pCoeff->m_Nb+n] = (y1 + y2);
		if (!_finite(yn[pCoeff->m_Nb+n]))
			printf("\nException: MATH ERROR!\n");

	}
}	

int IIRCalcFilterCoeff(double fg, double fa, double Qi, unsigned N, CIIRCoeff *pCoeff, unsigned filterType)
{
	unsigned p, order_ap, order_bp;
	div_t result;
	unsigned numEvenFilterParts, filterCnt;
	
	CIIRCoeff Temp(N, N);
	CIIRCoeff Coeff1(1,1);
	CIIRCoeff Coeff2(2,2);

	double Qp;
	FILE *pFile;

	pFile = fopen("filter.out","w");
	fprintf(pFile,"IIR-Filter Version 1.0\n");
	fprintf(pFile,"Filter Coefficients for %u-Order-%s, Qi = %4.2f\n",N, filterTypeString[filterType],Qi);
	fprintf(pFile,"fg = %9.2f Hz\nfa = %9.2f Hz\n",fg, fa);
	
	p = 1;
	filterCnt = 1;
	
	switch (N)
	{
	case 0:
		break;

	default:
		order_ap = 2;
		order_bp = 2;
		result = div(N,2);
		numEvenFilterParts = result.quot;

		if (result.rem != 0)
		{
			IIRCalcPartFilterCoeff1(&Coeff1, fg, fa, 1.0*Qi, filterType);
			fprintf(pFile,"\n1.Partfilter Np = 1, Qp = 1.00\n");
			IIRPrintCoeff(pFile,&Coeff1, 1);		
			filterCnt++;
		}

		while (p <= numEvenFilterParts)
		{
			Qp = IIRCalcQp(p, N);
			if (p == 1)
			{
				IIRCalcPartFilterCoeff2(&Temp, fg, fa, Qp*Qi, filterType);
				fprintf(pFile,"\n%u.Partfilter Np = 2, Qp = %4.2f\n", filterCnt, Qp);
				IIRPrintCoeff(pFile, &Temp, 2);		
				filterCnt++;
				p++;
				if (numEvenFilterParts > 1)
					continue;
				memcpy(pCoeff->m_pak, Temp.m_pak, (order_ap+1)*sizeof(double));
				memcpy(pCoeff->m_pbk, Temp.m_pbk, (order_bp+1)*sizeof(double));
				continue;
			}

			IIRCalcPartFilterCoeff2(&Coeff2, fg, fa, Qp*Qi, filterType);
			fprintf(pFile,"\n%u.Partfilter Np = 2, Qp = %4.2f\n", filterCnt, Qp);
			IIRPrintCoeff(pFile,&Coeff2, 2);		
			order_ap = IIRMulPolynom(Coeff2.m_pak, 2, Temp.m_pak, order_ap, pCoeff->m_pak);
			order_bp = IIRMulPolynom(Coeff2.m_pbk, 2, Temp.m_pbk, order_bp, pCoeff->m_pbk);
			memcpy(Temp.m_pak, pCoeff->m_pak, (order_ap+1)*sizeof(double));
			memcpy(Temp.m_pbk, pCoeff->m_pbk, (order_bp+1)*sizeof(double));
			filterCnt++;
			p++;
		}

		if (result.rem != 0)
		{
			if (result.quot == 0)
			{
				memcpy(pCoeff->m_pak, Coeff1.m_pak, 2*sizeof(double));
				memcpy(pCoeff->m_pbk, Coeff1.m_pbk, 2*sizeof(double));
			}
			else
			{
				order_ap = IIRMulPolynom(Coeff1.m_pak, 1, Temp.m_pak, order_ap, pCoeff->m_pak);
				order_bp = IIRMulPolynom(Coeff1.m_pbk, 1, Temp.m_pbk, order_bp, pCoeff->m_pbk);
			}
		}
		
		fprintf(pFile,"\n\nResulted Filter N = %u\n",N);
		IIRPrintCoeff(pFile,pCoeff, N);		
//		ScaleCoeff(pCoeff);
		fprintf(pFile,"\n\nNormalized Filterkernel N = %u\n",N);
		IIRPrintCoeff(pFile,pCoeff, N);		
		break;
	}

	fclose(pFile);
return 0;
}


unsigned IIRMulPolynom(double *pA, unsigned orderA, double *pB, unsigned orderB, double *pProduct)
{
	unsigned cntA, cntB, newOrder;
	
	newOrder = orderA+orderB;
	memset(pProduct, 0, (newOrder+1)*sizeof(double));

	for (cntA=0; cntA <= orderA; cntA++)
	{
		for (cntB=0; cntB <= orderB; cntB++)
			pProduct[cntA+cntB] += pA[cntA] * pB[cntB];
	}
	
	return newOrder;
}

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

void IIRPrintCoeff(FILE *pFile, CIIRCoeff *pCoeff, unsigned N)
{
	unsigned k;
	
	
	for (k=0; k <= N; k++)
	{
		fprintf(pFile,"a[%2u] = %9.6g, b[%2u] = %9.6g\n",k,pCoeff->m_pak[k],k,pCoeff->m_pbk[k]);
	}
	fprintf(pFile,"aScale = %9.6g, bScale = %9.6g\n\n",pCoeff->m_aScale, pCoeff->m_bScale);

	fprintf(pFile,";DSP Coefficients\ncoef\n");
	for (k=N; k > 0; k--)
	{
		fprintf(pFile,"\tdc\t%9.7g\t; a%u\n",pCoeff->m_pak[k]/2.0,k);
	}
	for (k=N; k > 0; k--)
	{
		fprintf(pFile,"\tdc\t%9.7g\t; b%u\n",pCoeff->m_pbk[k]/2.0,k);
	}

}


void ScaleCoeff(CIIRCoeff *pCoeff)
{
	unsigned i;
	double val;

	val=0;
	for(i=0; i <= pCoeff->m_Na; i++)
		val=MaxMag(val, pCoeff->m_pak[i]);

	for(i=0; i <= pCoeff->m_Na; i++)
		pCoeff->m_pak[i] /= val;

	pCoeff->m_aScale = val;

	val=0;
	for(i=0; i <= pCoeff->m_Nb; i++)
		val=MaxMag(val, pCoeff->m_pbk[i]);

	for(i=0; i <= pCoeff->m_Nb; i++)
		pCoeff->m_pbk[i] /= val;

	pCoeff->m_bScale = val;

}


double MinMag(double val1, double val2)
{
	if(fabs(val1) < fabs(val2))
		return fabs(val1);

	return fabs(val2);
}

double MaxMag(double val1, double val2)
{
	if(fabs(val1) > fabs(val2))
		return fabs(val1);

	return fabs(val2);
}