/*************************************************************************/
/* fir.c
/*************************************************************************/
#include <stdio.h>
#include <malloc.h>
#include "math.h"
#include "fir.h"

/*************************************************************************/
/* Global Variables
/*************************************************************************/
void Sinc(float *pY, int M, float s, float f, int len)
{
	int i, m;
	float x,h;

	m = (len-1)/2;
	for (i=1; i <= m; i++)
	{
		x = (float)(pi*f*i);
		h = (float)cos(i*0.5*pi/m);
		pY[m+i] = h*s*(float)sin(x)/x;
		pY[m-i] = pY[m+i];
	}

	pY[m] = s;
}

void FIRCalcDCRemovalCoeff(struct _sFIRCoeff *pCoeff, int N)
{
	int n;
	float k = 1.f/N;

	FIRInit(pCoeff, 0, N);

	for (n=0; n < N; n++)
		pCoeff->pB[n] = -k;

	pCoeff->pB[0] += 1.f;
}

void FIR(struct _sFIRCoeff *pCoeff, float *xn, float *yn, int order, int len)
{
	int i, n;
	float *pB, *pX;

	for (i=0; i<len; i++)
	{
		pB = &pCoeff->pB[order-1];
		pX = &pCoeff->pX[order-1];
		pCoeff->pX[0] = xn[i];

		yn[i] = 0;
		for (n=0; n < order; n++)
		{
			yn[i] += *(pB--) * *pX; 
			*(pX) = *(pX-1);
			pX--;
		}
	}
}

void FIRInit(struct _sFIRCoeff *pCoeff, float *pB, int order)
{
	int n;
	if (pB)
	{
		for(n=0; n < order; n++)
		{
			pCoeff->pB[n] = pB[n];
			pCoeff->pX[n] = 0;
		}
	}
	else
	{
		for(n=0; n < order; n++)
		{
			pCoeff->pB[n] = 0;
			pCoeff->pX[n] = 0;
		}
	}
}

void FIRAlloc(struct _sFIRCoeff *pCoeff, int order)
{
	pCoeff->pB = malloc(order*sizeof(float));
	pCoeff->pX = malloc(order*sizeof(float));
}

void FIRFree(struct _sFIRCoeff *pCoeff)
{
	free(pCoeff->pB);
	free(pCoeff->pX);
}