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

#include "fir2.h"


/*************************************************************************/
/* Global Variables */
/*************************************************************************/
void CalcSincFilter(fir_float_t *pY, fir_float_t s, fir_float_t f, int len)
{
	int i;
	fir_float_t x;
	fir_float_t off = ((fir_float_t)len-1)/2;

	for (i=0; i < len; i++)
	{
		x = (fir_float_t)(f*(i-off));
		pY[i] = s*Sinc(x);
	}
}

fir_float_t Sinc(fir_float_t x)
{
	if (x == 0)
		return 1.0;

	return (fir_float_t)(sin(fir_pi*x)/(fir_pi*x));
}

void FIRCalcLowpass(fir_float_t omega, fir_float_t *pCoeff, int N)
{
	CalcSincFilter(pCoeff, omega, omega, N);
	CalcKaiser(pCoeff, pCoeff, 8.0, N);
}

void FIRCalcHighpass(fir_float_t omega, fir_float_t *pCoeff, int N)
{
	int i;

	CalcSincFilter(pCoeff, omega, omega, N);

	for (i=0; i < N; i++)
	{
		pCoeff[i] = -pCoeff[i];
	}
	pCoeff[(N-1)/2] = 1 + pCoeff[(N-1)/2];

	CalcKaiser(pCoeff, pCoeff, 8.0, N);
}

void FIRCalcBandpass(fir_float_t omega, fir_float_t bw, fir_float_t *pCoeff, int N)
{
	int i;

	CalcSincFilter(pCoeff, bw, bw, N);

	for (i=0; i < N; i++)
	{
		pCoeff[i] *= (fir_float_t)cos(2*fir_pi*omega*i);
	}
	CalcKaiser(pCoeff, pCoeff, 8.0, N);
}

fir_float_t CalcFirRC(fir_float_t *pB, fir_float_t fa, fir_float_t Tsym, fir_float_t Alpha, int N)
{
	int n, delay;
	fir_float_t term, k, k0, phi;

	if (N%2)
		delay = (N-1)/2;
	else
		delay = N/2;

	k  = (fir_float_t)2.0/Tsym;
	k0 = (fir_float_t)0.5*Tsym*fa;
	for (n=0; n < N; n++)
	{
		phi = (n-delay)/fa;
		if (fabs(fabs(4*Alpha*phi/Tsym) - 1.0) > sqrt(DBL_EPSILON))
		{
			term = (fir_float_t)4.*Alpha*phi/Tsym;
			pB[n] =  Sinc(2*phi/Tsym)/fa * (fir_float_t)cos(2*fir_pi*Alpha*phi/Tsym) /((fir_float_t)1.0 - term*term);
		}
		else
		{
			pB[n] = Alpha * (fir_float_t)sin(fir_pi/(2*Alpha)) /(2*fa);
		}
		pB[n] *= k;
	}
	return k0;
}

fir_float_t CalcFirSRRC(fir_float_t *pB, fir_float_t fa, fir_float_t Tsym, fir_float_t Alpha, int N)
{
	int n, delay;
	fir_float_t term, k, k0, phi;

	if (N%2)
		delay = (N-1)/2;
	else
		delay = N/2;

	k = (fir_float_t)sqrt(2.0/Tsym);
	k0 = (fir_float_t)0.5*Tsym*fa;
	for (n=0; n < N; n++)
	{
		phi = (n-delay)/fa;
		if (phi == 0.0)
		{
			pB[n] = (fir_float_t)(-k * (fir_pi*(Alpha-1.0) - 4*Alpha) /(fir_pi*fa));
		}
		else
		{
            if (fabs(fabs(8*Alpha*phi/Tsym) - 1.0) < sqrt(DBL_EPSILON))
			{
				pB[n] = (fir_float_t)(k / (2*fir_pi*fa) \
						* (fir_pi*(Alpha+1.0) * sin(fir_pi*(Alpha+1.0)/(4*Alpha)) \
						- 4*Alpha * sin(fir_pi*(Alpha-1.0)/(4*Alpha)) \
						+ fir_pi*(Alpha-1.0) * cos(fir_pi*(Alpha-1.0)/(4*Alpha))));

			}
			else
			{
				term = 8*Alpha*phi/Tsym;
				pB[n] = (fir_float_t)(-4*Alpha/fa * ( cos((1.0+Alpha)*2*fir_pi*phi/Tsym) \
					 + sin((1.0-Alpha)*2*fir_pi*phi/Tsym) / (8*Alpha*phi/Tsym)) \
					 / (fir_pi * sqrt(1.0/(2/Tsym)) * (term*term - 1.0)));

			}
		}
		pB[n] *= k;
	}

	return k0;
}

// Hamming
//                        2*pi*k
// w(k) = 0.54 - 0.46*cos(------), where 0 <= k < N
//                         N-1
//
// len: window length
// pX: Input buffer (in)
// pY: Window weighted input buffer y[n] = x[n] * w[n] (out)
void CalcHamming(fir_float_t *pX, fir_float_t *pY, int len)
{
	int i;

	for (i=0; i < len; i++)
		pY[i] = (fir_float_t)(pX[i]*(0.54-0.46*cos(2*fir_pi*i/(len-1))));

}

// Hanning
//                   2*pi*k
// w = 0.5 - 0.5*cos(------), where 0 < k <= N
//                    N+1
// len: window length
// pX: Input buffer (in)
// pY: Window weighted input buffer y[n] = x[n] * w[n] (out)
void CalcVonHann(fir_float_t *pX, fir_float_t *pY, int len)
{
	int i;

	for (i=0; i < len; i++)
		pY[i] = (fir_float_t)(0.5*pX[i]*(1.0-cos(2*fir_pi*i/(len-1))));

}

// Blackman
//                       2*pi*k             4*pi*k
// w(k) = 0.42 - 0.5*cos(------) + 0.08*cos(------), where 0 <= k < N
//                        N-1                 N-1
//
// len: window length
// pX: Input buffer (in)
// pY: Window weighted input buffer y[n] = x[n] * w[n] (out)
void CalcBlackman(fir_float_t *pX, fir_float_t *pY, int len)
{
	int i;

	for (i=0; i < len; i++)
		pY[i] = (fir_float_t)(pX[i] * (0.42 - 0.5*cos(2*fir_pi*i/(len-1)) + 0.08*cos(4*fir_pi*i/(len-1))));

}

// Computes the 0th order modified Bessel function of the first kind.
// (Needed to compute Kaiser window)
//
// y = sum( (x/(2*n))^2 )
//      n
//
#define BIZ_EPSILON 1E-11 // Max error acceptable

double besselizero(double x)
{
	double temp;
	double sum   = 1.0;
	double u     = 1.0;
	double halfx = (double)(x/2.0);
	int      n     = 1;

	do
	{
		temp = halfx/(double)n;
		u *=temp * temp;
		sum += u;
		n++;
	} while (u >= BIZ_EPSILON * sum);

	return(sum);
}


// Kaiser
//
// n window length
// w buffer for the window parameters
// b beta parameter of Kaiser window, Beta >= 1
//
// Beta trades the rejection of the low pass filter against the
// transition width from passband to stop band.  Larger Beta means a
// slower transition and greater stop band rejection.  See Rabiner and
// Gold (Theory and Application of DSP) under Kaiser windows for more
// about Beta.  The following table from Rabiner and Gold gives some
// feel for the effect of Beta:
//
// All ripples in dB, width of transition band = D*N where N = window
// length
//
// BETA    D       PB RIP   SB RIP
// 2.120   1.50  +-0.27      -30
// 3.384   2.23    0.0864    -40
// 4.538   2.93    0.0274    -50
// 5.658   3.62    0.00868   -60
// 6.764   4.32    0.00275   -70
// 7.865   5.0     0.000868  -80
// 8.960   5.7     0.000275  -90
// 10.056  6.4     0.000087  -100
void CalcKaiser(fir_float_t *pX, fir_float_t *pY, fir_float_t b, int len)
{
	double tmp, tmp2, *pW;
	double k1  = 1.0/besselizero(b);
	int k2  = 1 - (len & 1);
	int end = (len + 1) >> 1;
	int i;

	pW =  (double*)malloc(len*sizeof(double));

	// Calculate window coefficients
	for (i=0 ; i<end ; i++)
	{
		tmp = (double)(2*i + k2) / ((double)len - 1.0);
		tmp2 = k1 * besselizero(b*sqrt(1.0 - tmp*tmp));
		pW[end-(1&(!k2))+i] = tmp2;
		pW[end-1-i] = tmp2;
	}

	if (pX)
	{
		for (i=0; i < len; i++)
			pY[i] = (fir_float_t)(pW[i] * pX[i]);
	}
	else
	{
		for (i=0; i < len; i++)
			pY[i] = (fir_float_t)pW[i];
	}
	free(pW);
}

void CalcKaiserSR(fir_float_t *pX, fir_float_t *pY, fir_float_t b, int len)
{
	double tmp, tmp2, *pW;
	double k1  = 1.0/besselizero(b);
	int k2  = 1 - (len & 1);
	int end = (len + 1) >> 1;
	int i;

	pW =  (double*)malloc(len*sizeof(double));

	// Calculate window coefficients
	for (i=0 ; i<end ; i++)
	{
		tmp = (double)(2*i + k2) / ((double)len - 1.0);
		tmp2 = k1 * besselizero(b*sqrt(1.0 - tmp*tmp));
		pW[end-(1&(!k2))+i] = tmp2;
		pW[end-1-i] = tmp2;
	}

	if (pX)
	{
		for (i=0; i < len; i++)
			pY[i] = (fir_float_t)((fir_float_t)pow(pW[i], 0.5) * pX[i]);
	}
	else
	{
		for (i=0; i < len; i++)
			pY[i] = (fir_float_t)(fir_float_t)pow(pW[i], 0.5);
	}
	free(pW);
}

void FIR(fir_float_t *pCoeff, fir_float_t *pState, int order, fir_float_t *x, fir_float_t *y, int len)
{
	int i, n;
	fir_float_t *pB, *pS;

	for (i=0; i<len; i++)
	{
		pB = &pCoeff[order-1];
		pS = &pState[order-1];

		y[i] = 0;
		for (n=0; n < order; n++)
		{
			if (pS != &pState[0])
				*pS = *(pS-1);
			else
				*pS = x[i];

			y[i] += *(pB--) * *(pS--);
		}
	}
}

void FIRneu(fir_float_t *pCoeff, fir_float_t *pState, int order, fir_float_t *x, fir_float_t *y, int len)
{
	int i, n;
	fir_float_t *pB, *pS, t;

	for (i=0; i<len; i++)
	{
		pB = &pCoeff[order-1];
		pS = &pState[order-1];

		t = 0;
		*pS = x[i];
		for (n=0; n < order; n++)
		{
			t += *(pB--) * *(pS--);
			*pS = *(pS-1);
		}
		y[i] = t;
	}
}