// --------------------------------------------------------------
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include <fir/fir2.h>
#include <crc/crc.h>
#include "radio.h"

// --------------------------------------------------------------
// Real Functions
// --------------------------------------------------------------

radio_float_t dabs(radio_float_t x)
{
	if (x < 0)
		return -x;
	
	return x;
}

radio_float_t dsign(radio_float_t x)
{
	if (x < 0)
		return -1.0;
	
	return 1.0;
}

radio_float_t dmod(radio_float_t x, radio_float_t y)
{
	int xy;

	if (y == 0)
		return x;

	xy = (int)floor(x/y);

	return dsign(y)*(radio_float_t)fabs(x - y*(radio_float_t)xy);
}

radio_float_t dmax(radio_float_t x, radio_float_t y)
{
	if (x > y)
		return x;

	return y;
}

radio_float_t dmin(radio_float_t x, radio_float_t y)
{
	if (x < y)
		return x;

	return y;
}

radio_float_t dclamp(radio_float_t x, radio_float_t limit_low, radio_float_t limit_high)
{
	return dmin(dmax(x, limit_low), limit_high);
}

radio_float_t powerDB(radio_float_t v1, radio_float_t preScale)
{
	return (radio_float_t)(10*log10(RADIO_POWER_EPS + v1*preScale));
}

float fsum(float *pSrc, int len)
{
	float sum = 0;
	
	while(len--)
		sum += *(pSrc++);

	return sum;
}

float fmean(float *pSrc, int len)
{
	return fsum(pSrc, len)/len;
}

radio_float_t dsum(radio_float_t *pSrc, int len)
{
	radio_float_t sum = 0;
	
	while(len--)
		sum += *(pSrc++);

	return sum;
}

radio_float_t dmean(radio_float_t *pSrc, int len)
{
	return dsum(pSrc, len)/len;
}

// --------------------------------------------------------------
// Complex Functions
// --------------------------------------------------------------
cpx_t CpxConjS(cpx_t v1)
{
	register cpx_t temp;

	temp.real = v1.real;
	temp.imag = -v1.imag;

	return temp;

}

cpx_t CpxAddS(cpx_t v1, cpx_t v2)
{
	register cpx_t temp;

	temp.real = v1.real + v2.real;
	temp.imag = v1.imag + v2.imag;

	return temp;
}

cpx_t CpxSubS(cpx_t v1, cpx_t v2)
{
	register cpx_t temp;

	temp.real = v1.real - v2.real;
	temp.imag = v1.imag - v2.imag;

	return temp;
}

cpx_t CpxMulS(cpx_t v1, cpx_t v2)
{
	register cpx_t temp;

	temp.real = v1.real*v2.real - v1.imag*v2.imag;
	temp.imag = v1.real*v2.imag + v1.imag*v2.real;

	return temp;
}

cpx_t CpxScaleRealS(cpx_t v1, radio_float_t v2)
{
	register cpx_t temp;

	temp.real = v1.real*v2;
	temp.imag = v1.imag*v2;

	return temp;
}

cpx_t CpxScaleComplexS(cpx_t x, cpx_t gain)
{
	register cpx_t temp;

	temp.real = x.real*gain.real;
	temp.imag = x.imag*gain.imag;

	return temp;
}

cpx_t CpxFromReal(radio_float_t v1)
{
	cpx_t res = {v1, v1};

	return res;
}

cpx_t Cpx(radio_float_t real, radio_float_t imag)
{
	cpx_t res = {real, imag};

	return res;
}

radio_float_t CpxMagS(cpx_t v1)
{
	radio_float_t mag;

	mag = (radio_float_t)sqrt(v1.real*v1.real + v1.imag*v1.imag);

	return mag;
}

radio_float_t CpxPhiS(cpx_t v1)
{
	return (radio_float_t)atan2(v1.real, v1.imag);
}

cpx_t CpxMinS(cpx_t x1, cpx_t x2)
{
	return Cpx(dmin(x1.real, x2.real), dmin(x1.imag, x2.imag));
}

cpx_t CpxMaxS(cpx_t x1, cpx_t x2)
{
	return Cpx(dmax(x1.real, x2.real), dmax(x1.imag, x2.imag));
}

radio_float_t cpxPowerDB(cpx_t v1, radio_float_t preScale)
{
	return powerDB(CpxMagS(v1), preScale);
}

void CpxCopy(cpx_t *pSrc, cpx_t *pDst, uint32_t len)
{
	memcpy(pDst, pSrc, len*sizeof(cpx_t));
}

void CpxConj(cpx_t *pSrcDst, uint32_t len)
{
	uint32_t i;
	for(i=0; i < len; i++)
		pSrcDst[i] = CpxConjS(pSrcDst[i]);
}

void CpxAdd(cpx_t *pSrc, cpx_t *pSrcDst, uint32_t len)
{
	uint32_t i;
	for(i=0; i < len; i++)
		pSrcDst[i] = CpxAddS(pSrc[i], pSrcDst[i]);
}

void CpxSub(cpx_t *pSrc, cpx_t *pSrcDst, uint32_t len)
{
	uint32_t i;
	for(i=0; i < len; i++)
		pSrcDst[i] = CpxSubS(pSrc[i], pSrcDst[i]);
}

void CpxMul(cpx_t *pSrc, cpx_t *pSrcDst, uint32_t len)
{

	uint32_t i;
	for(i=0; i < len; i++)
		pSrcDst[i] = CpxMulS(pSrc[i], pSrcDst[i]);
}


cpx_t csum(cpx_t *pSrc, int len)
{
	cpx_t sum = {0};
	
	while(len--)
	{
		sum.real += (*(pSrc)).real;
		sum.imag += (*(pSrc++)).imag;
	}

	return sum;
}

cpx_t cmean(cpx_t *pSrc, int len)
{
	cpx_t y;

	y = csum(pSrc, len);
	y.real /= len;
	y.imag /= len;

	return y;
}

// --------------------------------------------------------------
// NCO
// --------------------------------------------------------------
void NCO_Init(nco_t *pObj, radio_float_t omega, radio_float_t phi, uint32_t tbl_size)
{
	uint32_t i;
	radio_float_t dphi_tbl, phi_tbl;

	pObj->dOmega = 0.0;
	pObj->dPhi = 0.0;
	pObj->omega = omega;
	pObj->phi = phi;
	pObj->tbl_index = 0;
	pObj->tbl_size = tbl_size;
	pObj->pTbl = NULL;
	memset(&pObj->state,0,sizeof(cpx_t));

	if (tbl_size > 0)
	{
			dphi_tbl = (radio_float_t)(2.0*PI/tbl_size);
			pObj->pTbl = (cpx_t*)malloc(tbl_size*sizeof(cpx_t));

			phi_tbl = 0;
			for (i=0; i < tbl_size; i++)
			{
					pObj->pTbl[i].real = (radio_float_t)cos(phi_tbl);
					pObj->pTbl[i].imag = (radio_float_t)sin(phi_tbl);
					phi_tbl += dphi_tbl;
			}
	}
}

void NCO_Free(nco_t *pObj)
{
	if (pObj->pTbl)
		free(pObj->pTbl);
}

void NCO_ModPhi(nco_t *pObj, radio_float_t dPhi)
{
	pObj->dPhi += dPhi;
	pObj->phi += dPhi;

	if (pObj->phi < -(radio_float_t)0.5) pObj->phi += (radio_float_t)1.0;
	if (pObj->phi >= +(radio_float_t)0.5) pObj->phi -= (radio_float_t)1.0;
}

void NCO_ModOmega(nco_t *pObj, radio_float_t dOmega)
{
	pObj->dOmega = dOmega;
}

radio_float_t NCO_GetOmega(nco_t *pObj)
{
	return pObj->omega + pObj->dOmega;
}

void NCO_Process(nco_t *pObj)
{
	uint32_t i;

	pObj->phi += (pObj->omega + pObj->dOmega);

	if (pObj->phi < -(radio_float_t)0.5) pObj->phi += (radio_float_t)1.0;
	if (pObj->phi >= +(radio_float_t)0.5) pObj->phi -= (radio_float_t)1.0;

	if (pObj->pTbl == NULL)
	{
			pObj->state.real = +(radio_float_t)cos(2.0*PI*pObj->phi);
			pObj->state.imag = -(radio_float_t)sin(2.0*PI*pObj->phi);
	}
	else
	{
			i = (int)(0.5 + pObj->phi*pObj->tbl_size)%pObj->tbl_size;
			pObj->state.real = +pObj->pTbl[i].real;
			pObj->state.imag = -pObj->pTbl[i].imag;
	}
}

// --------------------------------------------------------------
// Mixer
// Scalar version
// --------------------------------------------------------------
cpx_t NCO_MixComplexS(nco_t *pObj, cpx_t *pSrc, cpx_t gain)
{
	return CpxMulS(pObj->state, CpxScaleComplexS(*pSrc, gain));
}

cpx_t NCO_MixRealS(nco_t *pObj, radio_float_t src, radio_float_t gain)
{
	cpx_t dst;

	dst.real = gain*src;
	dst.imag = 0;

	return CpxMulS(pObj->state, dst);
}

radio_float_t NCO_MixComplexRealS(nco_t *pObj, cpx_t *pSrc, cpx_t gain)
{
	return gain.real*pSrc->real*pObj->state.real + gain.imag*pSrc->imag*pObj->state.imag;
}
// --------------------------------------------------------------
// Mixer
// Vector version
// --------------------------------------------------------------
void NCO_MixComplexV(nco_t *pObj, cpx_t *pSrc, cpx_t *pDst, cpx_t gain, uint32_t len)
{

	while (len--)
	{

		*pDst++ = NCO_MixComplexS(pObj, pSrc++, gain);

		// NCO update
		NCO_Process(pObj);
	}

}

void NCO_MixRealV(nco_t *pObj, radio_float_t *pSrc, cpx_t *pDst, radio_float_t gain, uint32_t len)
{

	while (len--)
	{

		*pDst++ = NCO_MixRealS(pObj, *pSrc++, gain);

		// NCO update
		NCO_Process(pObj);
	}
}

void NCO_MixComplexRealV(nco_t *pObj, cpx_t *pSrc, radio_float_t *pDst, cpx_t gain, uint32_t len)
{

	while (len--)
	{

		*pDst++ = NCO_MixComplexRealS(pObj, pSrc++, gain);

		// NCO update
		NCO_Process(pObj);
	}
}

// --------------------------------------------------------------
// Synchronization
// --------------------------------------------------------------
#if 0
void SamplerInit(sampler_t *pObj, int divide)
{
	pObj->reload = divide;
	pObj->count = divide -1;;
}

int SamplerIsSample(sampler_t *pObj)
{
	return (pObj->count == 0);
}

void SamplerUpdate(sampler_t *pObj)
{
	if (pObj->count == 0)
		pObj->count = pObj->reload -1;
	else
		pObj->count--;
}

// --------------------------------------------------------------
// Phase detector for QPSK
// --------------------------------------------------------------
radio_float_t PhaseErrQPSK(cpx_t x)
{
	radio_float_t perr;
	perr = x.imag*dsign(x.real) - x.real*dsign(x.imag);

	return perr;
}

radio_float_t PhaseErr(cpx_t x, radio_float_t pstep, radio_float_t poffset)
{
	radio_float_t perr, phi, m;

	phi = (radio_float_t)CpxPhiS(x) + poffset;
	
	m = dmod(phi/(radio_float_t)PI,(radio_float_t)2.0/pstep);
	perr = (radio_float_t)1.0/pstep - m;

	return (radio_float_t)(PI*perr);
}

// --------------------------------------------------------------
// Clock generator
// --------------------------------------------------------------
void Clkgen_Init(clkgen_t *pObj)
{
	pObj->lastPhi = 0;
}

int Clkgen_GetPulse(clkgen_t *pObj, nco_t *pNCO)
{
	int pulse = 0;
	radio_float_t tmp1, tmp2;

	tmp1 = (radio_float_t)fmod(pNCO->phi, (radio_float_t)PI);
	tmp2 = (radio_float_t)fmod(pObj->lastPhi, (radio_float_t)PI);

//	if ((pObj->pNCO->phi <= 0) && (pObj->lastPhi > 0)/* && (pObj->pNCO->phi <= 0)*/)
//		pulse = 1;
	
	pulse = tmp1 < tmp2;

	pObj->lastPhi = pNCO->phi;

	return pulse;
}

void LeadLagInit(lead_lag_filter_t *pObj, radio_float_t lead_gain, radio_float_t lag_gain, radio_float_t ic_lag)
{
	pObj->term_lag = ic_lag;
	pObj->term_lead = 0;
	pObj->gain_lag = lag_gain;
	pObj->gain_lead = lead_gain;
};

void LeadLagSetGainLead(lead_lag_filter_t *pObj, radio_float_t gain)
{
	pObj->gain_lead = gain;
}

void LeadLagSetGainLag(lead_lag_filter_t *pObj, radio_float_t gain)
{
	pObj->gain_lag = gain;
}

radio_float_t LeadLagProcess(lead_lag_filter_t *pObj, radio_float_t x)
{
	radio_float_t y;
	
	pObj->term_lag += x * pObj->gain_lag;
	pObj->term_lead = x * pObj->gain_lead;

	y = pObj->term_lag + pObj->term_lead;

	return y;
}

// --------------------------------------------------------------
// Gardner Timing Error Detector
// --------------------------------------------------------------
void STRGardnerInit(str_t *pObj)
{
	memset(pObj->d, 0, sizeof(pObj->d));
}

radio_float_t STRGardnerProcess(str_t *pObj, cpx_t x)
{
	radio_float_t Vd;
	cpx_t t, *pD;

	pD = pObj->d;

	t.real = (pD[1].real - x.real) * pD[0].real;
	t.imag = (pD[1].imag - x.imag) * pD[0].imag;
	Vd = (t.real + t.imag);

	// Adjust delay line
	pD[1] = pD[0];
	pD[0] = x;

	return Vd;
}
#endif
// --------------------------------------------------------------
// Sliding Statistics
// --------------------------------------------------------------
void SlidingMeanInit(sl_mean_t *pObj, int N)
{
	RBufInit(&pObj->buf, N);
	pObj->Nr = (radio_float_t)1.0/N;
	pObj->mean = 0.0;
	pObj->sum = 0.0;
	pObj->normalize_count = N;
}

void SlidingMeanFree(sl_mean_t *pObj)
{
	RBufFree(&pObj->buf);
}

radio_float_t SlidingMeanProcess(sl_mean_t *pObj, radio_float_t x)
{
	uint32_t i;
	radio_float_t last;

	last = RBufGetAfter(&pObj->buf, pObj->buf.size-1);
	RBufPut(&pObj->buf, x);
	pObj->sum += (x - last);
	pObj->mean = pObj->sum*pObj->Nr;

	if (--pObj->normalize_count == 0)
	{
		pObj->normalize_count = pObj->buf.size;
		pObj->sum = 0;
		for (i=0; i < pObj->buf.size; i++)
		{
			pObj->sum += pObj->buf.pData[i];
		}
	}
	return pObj->mean;
}

void SlidingMeanProcessV(sl_mean_t *pObj, radio_float_t *pX, uint32_t len)
{
	uint32_t i;

	for (i=0; i < len; i++)
		SlidingMeanProcess(pObj, *(pX++));
}

radio_float_t SlidingMeanGet(sl_mean_t *pObj)
{
	return pObj->mean;
}

void SlidingVarInit(sl_var_t *pObj, int Npwr, int Nmean)
{
	RBufInit(&pObj->buf, Npwr);
	SlidingMeanInit(&pObj->sl_mean, Nmean);
	pObj->Nr = (radio_float_t)1.0/Npwr;
	pObj->var = 0.0;
	pObj->sum = 0.0;
	pObj->normalize_count = Npwr;
}

void SlidingVarFree(sl_var_t *pObj)
{
	RBufFree(&pObj->buf);
	SlidingMeanFree(&pObj->sl_mean);
}

radio_float_t SlidingVarProcess(sl_var_t *pObj, radio_float_t x)
{
	uint32_t i;
	radio_float_t last, xm, mean, var;

	last = RBufGetAfter(&pObj->buf, pObj->buf.size-1);
	mean = SlidingMeanProcess(&pObj->sl_mean, x);
	xm = x - mean;
	xm *= xm;
	RBufPut(&pObj->buf, xm);
	pObj->sum += (xm - last);

	var = pObj->sum*pObj->Nr;
	if (var >= 0)
		pObj->var = var;

	if (--pObj->normalize_count == 0)
	{
		pObj->normalize_count = pObj->buf.size;
		pObj->sum = 0;
		for (i=0; i < pObj->buf.size; i++)
		{
			pObj->sum += pObj->buf.pData[i];
		}
	}
	return pObj->var;
}

void SlidingVarProcessV(sl_var_t *pObj, radio_float_t *pX, uint32_t len)
{
	uint32_t i;

	for (i=0; i < len; i++)
		SlidingVarProcess(pObj, *(pX++));
}

radio_float_t SlidingVarGet(sl_var_t *pObj)
{
	return pObj->var;
}

radio_float_t SlidingVarGetPowerDB(sl_var_t *pObj, radio_float_t preScale)
{
	return powerDB(pObj->var, preScale);
}

void SlidingMinMaxInit(sl_minmax_t *pObj, uint32_t L, radio_float_t P, radio_float_t mode)
{
	pObj->L = L;
	pObj->P = P;
	pObj->N = 0;
	pObj->max = 0;
	pObj->mode = mode;
	pObj->pP = (uint32_t*)malloc(L*sizeof(uint32_t));
	pObj->pU = (radio_float_t*)malloc(L*sizeof(radio_float_t));
	pObj->pP_last = (uint32_t*)malloc(L*sizeof(uint32_t));
	pObj->pU_last = (radio_float_t*)malloc(L*sizeof(radio_float_t));
	memset(pObj->pP, 0, L*sizeof(uint32_t));
	memset(pObj->pU, 0, L*sizeof(radio_float_t));
	memset(pObj->pP_last, 0, L*sizeof(uint32_t));
	memset(pObj->pU_last, 0, L*sizeof(radio_float_t));
	pObj->pU_last[1] = P;
}
	
void SlidingMinMaxFree(sl_minmax_t *pObj)
{
	if (pObj->pP)
		free(pObj->pP);

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

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

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

	pObj->pP = (uint32_t*)0;
	pObj->pU = (radio_float_t*)0;
	pObj->pP_last = (uint32_t*)0;
	pObj->pU_last = (radio_float_t*)0;
}

// Sliding Min/Max based on MAXLIST-Algorithm
// "AN EFFICIENT ALGORITHM FOR RUNNING MAX MIN CALCULATION", 1996, S.C. Douglas, University of Utah
// Dependeding on 'mode' this algorithm calculates either minimum (mode = -1) or maximum (mode = +1)
radio_float_t SlidingMinMaxProcess(sl_minmax_t *pObj, radio_float_t x)
{
	uint32_t m;
	uint32_t i;
	uint32_t j;

	x *= pObj->mode;

	m = 1;
	if (pObj->pP_last[pObj->N] == pObj->L)
	{
		m = 0;
		pObj->pU_last[pObj->N] = pObj->P;
	}

	i = 0;
	while (x >= pObj->pU_last[i+1])
	{
		i++;
	}
	pObj->N = pObj->N - i + m;

	for (j=1; j < pObj->N; j++)
	{
		pObj->pU[j+1] = pObj->pU_last[j+i];
		pObj->pP[j+1] = pObj->pP_last[j+i] + 1;
	}
	pObj->pU[pObj->N+1] = pObj->P;
	pObj->pU[1] = x;
	pObj->pP[1] = 1;
	pObj->max = pObj->pU[pObj->N];

	i=0;
	while(pObj->pU[i] != pObj->P)
	{
		pObj->pU_last[i] = pObj->pU[i];
		pObj->pP_last[i] = pObj->pP[i];
		i++;
	}
	pObj->pU_last[i] = pObj->pU[i];
	pObj->pP_last[i] = pObj->pP[i];

	return pObj->mode*pObj->max;
}

radio_float_t SlidingMinMaxGet(sl_minmax_t *pObj)
{
	return pObj->mode*pObj->max;
}

void SlidingMinMaxProcessV(sl_minmax_t *pObj, radio_float_t *pX, uint32_t len)
{
	uint32_t i;

	for (i=0; i < len; i++)
	{
		SlidingMinMaxProcess(pObj, pX[i]);
	}
}

// --------------------------------------------------------------
// RingBuffer and Fifo
// --------------------------------------------------------------
void RBufInit(rbuf_t *pObj, int size)
{
	pObj->ir = 0;
	pObj->iw = 0;
	pObj->dist = 0;

	pObj->size = size;
	pObj->pData = (radio_float_t*)malloc(size*sizeof(radio_float_t));
	memset(pObj->pData, 0, size*sizeof(radio_float_t));
}

void RBufFree(rbuf_t *pObj)
{
	if (pObj->pData)
		free(pObj->pData);
	
	pObj->pData = NULL;
	pObj->size = 0;
}

void RBufPut(rbuf_t *pObj, radio_float_t x)
{
	(pObj->iw)++;
	if (pObj->iw >= pObj->size)
		pObj->iw = 0;

	pObj->pData[pObj->iw] = x;
	pObj->dist++;

}

radio_float_t RBufGet(rbuf_t *pObj)
{
	radio_float_t y;
	y = pObj->pData[(pObj->ir)++];

	if (pObj->ir >= pObj->size)
		pObj->ir = 0;

	pObj->dist--;
	return y;
}

radio_float_t RBufGetAfter(rbuf_t *pObj, int delay)
{
	pObj->ir = (pObj->iw-delay)%pObj->size;
	if (pObj->ir < 0)
		pObj->ir += pObj->size;

	pObj->dist = delay;
	return pObj->pData[pObj->ir];
}

radio_float_t RBufGetAt(rbuf_t *pObj, int index)
{
	if (index < 0)
		index = 0;
	pObj->ir = (index)%pObj->size;
	pObj->dist = pObj->iw-pObj->ir;
	if (pObj->dist < 0)
		pObj->dist += pObj->size;

	return pObj->pData[pObj->ir];
}

void FifoInit(fifo_t *pObj, uint32_t max_size)
{
	pObj->size_max = max_size;
	pObj->size = 0;
	pObj->iw = (uint32_t)0;
	pObj->ir = (uint32_t)0;

	pObj->pData = (uint32_t*)malloc(max_size*sizeof(uint32_t));
	memset(pObj->pData, 0, max_size*sizeof(uint32_t));
}

void FifoFree(fifo_t *pObj)
{
	if (pObj->pData)
		free(pObj->pData);
	
	pObj->pData = NULL;
	pObj->size_max = 0;
	pObj->size = 0;
}

uint32_t FifoSize(fifo_t *pObj)
{
	return pObj->size;
}

uint32_t FifoSizeMax(fifo_t *pObj)
{
	return pObj->size_max;
}

uint32_t FifoFront(fifo_t *pObj)
{
	return pObj->pData[pObj->ir];
}

uint32_t FifoBack(fifo_t *pObj)
{
	return pObj->pData[pObj->iw];
}

void FifoPushBack(fifo_t *pObj, uint32_t item)
{
	pObj->pData[pObj->iw++] = item;
	if (pObj->iw >= pObj->size_max)
		pObj->iw = 0;

	if (pObj->size < pObj->size_max)
	{
		pObj->size++;
	}
	else
	{
		if (++pObj->ir >= pObj->size_max)
			pObj->ir = 0;
	}
}

void FifoPushFront(fifo_t *pObj, uint32_t item)
{
	pObj->ir--;
	if (pObj->ir >= pObj->size_max)
		pObj->ir = 0;

	pObj->pData[pObj->ir] = item;
	if (pObj->size < pObj->size_max)
	{
		pObj->size++;
	}
	else
	{
		if (--pObj->iw >= pObj->size_max)
			pObj->iw = 0;
	}
}

void FifoPopBack(fifo_t *pObj)
{
	pObj->iw--;
	if (pObj->iw >= pObj->size_max)
		pObj->iw = 0;

	if (pObj->size > 0)
		pObj->size--;
}

void FifoPopFront(fifo_t *pObj)
{
	pObj->ir++;
	if (pObj->ir >= pObj->size_max)
		pObj->ir = 0;

	if (pObj->size > 0)
		pObj->size--;
}

// --------------------------------------------------------------
// Interpolation
// --------------------------------------------------------------
#if 0
void FDly_Init(fdly_t *pObj, int order)
{
	pObj->Nf = order + 1;

	pObj->hf = (radio_float_t*)malloc(pObj->Nf*sizeof(radio_float_t));
	pObj->statef = (radio_float_t*)malloc(pObj->Nf*sizeof(radio_float_t));
	memset(pObj->hf, 0, pObj->Nf*sizeof(radio_float_t));
	memset(pObj->statef, 0, pObj->Nf*sizeof(radio_float_t));
}

void FDly_Free(fdly_t *pObj)
{
	if (pObj->hf)
		free(pObj->hf);

	pObj->hf = NULL;

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

	pObj->statef = NULL;

}

radio_float_t FDly_Process(fdly_t *pObj, radio_float_t x)
{
	radio_float_t y;

	FIR(pObj->hf, pObj->statef, pObj->Nf,  &x, &y, 1);

	return y;
}

void FDly_CalcCoeff(fdly_t *pObj, radio_float_t delay)
{
	int n, k;
	radio_float_t hn;

	for (n=0; n < pObj->Nf; n++)
	{
		hn = 1.0;
		for (k=0; k < pObj->Nf; k++)
		{
			if (k == n)
				continue;

			hn *= ((delay - (radio_float_t)k) / (radio_float_t)(n-k));
		}
		pObj->hf[n] = hn;
	}
}

void LGIpInit(lgip_t *pObj, int order, int bufsize)
{
	pObj->N = order + 1;
	pObj->bufsize = bufsize;
	RBufInit(&pObj->rbuf, bufsize);
}

void LGIpFree(lgip_t *pObj)
{
	RBufFree(&pObj->rbuf);
	pObj->N = 0;
}

radio_float_t LGIpProcess(lgip_t *pObj, radio_float_t x, int m, radio_float_t mu)
{
	int n, k;
	radio_float_t hn, y, delay;

	RBufPut(&pObj->rbuf, x);
	y = 0;
	delay = 1-mu;
	for (n=0; n < pObj->N; n++)
	{
		hn = 1.0;
		for (k=0; k < pObj->N; k++)
		{
			if (k == n)
				continue;

			hn *= ((delay - (radio_float_t)k) / (radio_float_t)(n-k));
		}
		y += hn*RBufGetAt(&pObj->rbuf, pObj->bufsize/2 + m-n);
	}
	return y;
}

void LGCpxIpInit(lgip_cpx_t *pObj, int order)
{
	// FIFO
	pObj->N = order + 1;
	pObj->w = 0;
	pObj->r = 0;
	pObj->L = 64*pObj->N;	// Influences STR-Loop delay

	pObj->pFifo = (cpx_t*)malloc(pObj->L*sizeof(cpx_t));
	memset(pObj->pFifo, 0, pObj->L*sizeof(cpx_t));

}

void LGCpxIpFree(lgip_cpx_t *pObj)
{
	free(pObj->pFifo);
	pObj->pFifo = NULL;
	pObj->N = 0;
}

cpx_t LGCpxIpProcess(lgip_cpx_t *pObj, cpx_t x, int m, radio_float_t mu)
{
	uint32_t i, j, k, N, L, r;
	radio_float_t h;
	cpx_t y;

	L = pObj->L;
	N = pObj->N;

	pObj->pFifo[pObj->w++] = x;
	if (pObj->w == L)
		pObj->w = 0;

	r = (m-N) % L;

	k = 0;

	y.real = 0.0;
	y.imag = 0.0;
	for (i=0; i < pObj->N; i++)
	{
		h = 1.0;
		for (j=0; j < pObj->N; j++)
		{
			if (i == j)
				continue;

			h *= (((radio_float_t)pObj->N/2 + mu - (radio_float_t)j - (radio_float_t)0.5) / (radio_float_t)(i-j));
		}
		y.real += pObj->pFifo[r].real * h;
		y.imag += pObj->pFifo[r].imag * h;
		r = (r+1) % L;
	}
	return y;
}

radio_float_t PolyPhaseSRRCInit(fir_pp_t *pObj, radio_float_t fa, radio_float_t tsym, radio_float_t alpha, uint32_t N, uint32_t M)
{
	uint32_t i, j;
	fir_float_t *pCoeff;
	radio_float_t k;

	pCoeff = (fir_float_t*)malloc((N+1)*M*sizeof(fir_float_t));
	memset(pCoeff, 0, M*(N+1)*sizeof(fir_float_t));

	k = CalcFirSRRC(pCoeff, M*fa, tsym, alpha, M*N);

	pObj->ppCoeff = (fir_float_t**)malloc(M*sizeof(fir_float_t*));
	for(i=0; i < M; i++)
	{
		pObj->ppCoeff[i] = (fir_float_t*)malloc(N*sizeof(fir_float_t));
		memset(pObj->ppCoeff[i], 0, N*sizeof(fir_float_t));
	}
	pObj->pState = (fir_float_t*)malloc(N*sizeof(fir_float_t));
	memset(pObj->pState, 0, N*sizeof(fir_float_t));

	for(i=0; i < M; i++)
	{
		for(j=0; j < N; j++)
		{
			pObj->ppCoeff[i][j] = M*pCoeff[M*j+M/2+i];
		}
	}

	pObj->M = M;
	pObj->N = N;

	free(pCoeff);

	return k/M;
}

void PolyPhaseSRRCFree(fir_pp_t *pObj)
{
	uint32_t i;

	for(i=0; i < pObj->M; i++)
	{
		free(pObj->ppCoeff[i]);
	}
	free(pObj->ppCoeff);
	pObj->ppCoeff = NULL;

	free(pObj->pState);
	pObj->pState = NULL;

}

uint32_t PolyPhaseSRRCProcess(fir_pp_t *pObj, radio_float_t mu, radio_float_t *pX, radio_float_t *pY, uint32_t len)
{
	uint32_t m;

//	m = (pObj->M/2 + (int)(mu * pObj->M/2)) % pObj->M;
	m = (int)dmod(mu*pObj->M, (radio_float_t)pObj->M);

	FIR(pObj->ppCoeff[m], pObj->pState, pObj->N, pX, pY, len);

	return len;
}

// --------------------------------------------------------------
radio_float_t PolyPhaseIpInit(ppip_t *pObj, radio_float_t fa, radio_float_t tsym, radio_float_t alpha, uint32_t N, uint32_t M)
{
	uint32_t i, j;
	fir_float_t *pCoeff;
	radio_float_t k;

	pCoeff = (fir_float_t*)malloc((N+1)*M*sizeof(fir_float_t));
	memset(pCoeff, 0, (N+1)*M*sizeof(fir_float_t));

	k = CalcFirSRRC(pCoeff, M*fa, tsym, alpha, N*M);

	pObj->ppCoeff = (fir_float_t**)malloc(M*sizeof(fir_float_t*));
	for(i=0; i < M; i++)
	{
		pObj->ppCoeff[i] = (fir_float_t*)malloc(N*sizeof(fir_float_t));
		memset(pObj->ppCoeff[i], 0, N*sizeof(fir_float_t));
	}
	for(i=0; i < M; i++)
	{
		for(j=0; j < N; j++)
		{
			pObj->ppCoeff[i][j] = M*pCoeff[M*j+M/2+i];
		}
	}
	pObj->pState = (fir_float_t*)malloc(N*sizeof(fir_float_t));
	memset(pObj->pState, 0, N*sizeof(fir_float_t));

	pObj->M = M;
	pObj->N = N;

	RBufInit(&pObj->rbuf, 2*N);		// Influences STR-Loop delay

	free(pCoeff);

	return k/M;

}

void PolyPhaseIpFree(ppip_t *pObj)
{
	uint32_t i;

	for(i=0; i < pObj->M; i++)
	{
		free(pObj->ppCoeff[i]);
	}
	free(pObj->ppCoeff);
	pObj->ppCoeff = NULL;

	free(pObj->pState);
	pObj->pState = NULL;

	RBufFree(&pObj->rbuf);

}

radio_float_t PolyPhaseIpProcess(ppip_t *pObj, radio_float_t x, int m, radio_float_t mu)
{
	uint32_t index, i;
	radio_float_t y;

	index = (int)dmod(mu*pObj->M, (radio_float_t)pObj->M);

	for(i=0; i < pObj->N; i++)
	{
		pObj->pState[i] = RBufGetAt(&pObj->rbuf, m-i);
	}

	FIR(pObj->ppCoeff[index], pObj->pState, pObj->N, &x, &y, 1);
	RBufPut(&pObj->rbuf, x);

	return y;
}

void FirCpxInit(fir_cpx_t *pObj, uint32_t N)
{
	pObj->pX = (cpx_t*)malloc(N*sizeof(cpx_t));
	memset(pObj->pX, 0, N*sizeof(cpx_t));
	pObj->N = N;
	pObj->r = 0;
	pObj->w = 0;
}

void FirCpxFree(fir_cpx_t *pObj)
{
	free(pObj->pX);
}

void FirCpxProcessReal(fir_cpx_t *pObj, radio_float_t *pW, cpx_t *pSrc, cpx_t *pDst, uint32_t downSampleFactor, uint32_t len)
{

	uint32_t i, j, w, r, N, k;
	
	w = pObj->w;
	r = pObj->r;
	N = pObj->N;

	for (i=0; i < len/downSampleFactor; i++)
	{
		
		for (k=0; k < downSampleFactor; k++)
		{
			pObj->pX[w++] = pSrc[i*downSampleFactor + k];
			if (w >= N)
				w = 0;
		}
		pDst[i].real = pDst[i].imag = 0;
		for (j=0; j < N; j++)
		{
			
			pDst[i].real += pObj->pX[r].real * pW[j];
			pDst[i].imag += pObj->pX[r].imag * pW[j];

			r--;
			if (r >= N)
				r = N-1;

		}
		for (k=0; k < downSampleFactor; k++)
		{
			r++;
			if (r >= N)
				r = 0;
		}
	}
	pObj->r = r;
	pObj->w = w;
}

void FirCpxProcessComplex(fir_cpx_t *pObj, cpx_t *pW, cpx_t *pSrc, cpx_t *pDst, uint32_t len)
{
	(void)pObj;
	(void)pW;
	(void)pSrc;
	(void)pDst;
	(void)len;
}

// --------------------------------------------------------------
radio_float_t PolyPhaseCpxIpInit(ppip_cpx_t *pObj, radio_float_t fa, radio_float_t tsym, radio_float_t alpha, uint32_t N, uint32_t M)
{
	uint32_t i, j;
	fir_float_t *pCoeff;
	radio_float_t k;

	// Calculate polyphase filter
	pCoeff = (fir_float_t*)malloc((N+1)*M*sizeof(fir_float_t));
	memset(pCoeff, 0, (N+1)*M*sizeof(fir_float_t));

	k = CalcFirSRRC(pCoeff, M*fa, tsym, alpha, N*M);
	CalcKaiser(pCoeff, pCoeff, (radio_float_t)2.4, N*M);

	pObj->ppCoeff = (fir_float_t**)malloc(M*sizeof(fir_float_t*));
	for(i=0; i < M; i++)
	{
		pObj->ppCoeff[i] = (fir_float_t*)malloc(N*sizeof(fir_float_t));
		memset(pObj->ppCoeff[i], 0, N*sizeof(fir_float_t));
	}
	for(i=0; i < M; i++)
	{
		for(j=0; j < N; j++)
		{
			pObj->ppCoeff[i][j] = M*pCoeff[M*j+M/2+i];
		}
	}

	// FIFO
	pObj->w = 0;
	pObj->r = 0;
	pObj->L = 2*N;	// Influences STR-Loop delay

	pObj->pFifo = (cpx_t*)malloc(pObj->L*sizeof(cpx_t));
	memset(pObj->pFifo, 0, pObj->L*sizeof(cpx_t));

	pObj->M = M;
	pObj->N = N;

	free(pCoeff);

	return k/M;

}

void PolyPhaseCpxIpFree(ppip_cpx_t *pObj)
{
	uint32_t i;

	for(i=0; i < pObj->M; i++)
	{
		free(pObj->ppCoeff[i]);
	}
	free(pObj->ppCoeff);
	pObj->ppCoeff = NULL;

	free(pObj->pFifo);
	pObj->pFifo = NULL;

}

cpx_t PolyPhaseCpxIpProcess(ppip_cpx_t *pObj, cpx_t x, INT32 m, radio_float_t mu)
{
	uint32_t index, j, N, L, r;
	radio_float_t *pW;
	cpx_t y;
	int i;

	index = (uint32_t)dmod(mu*pObj->M, (radio_float_t)pObj->M);

	pW = pObj->ppCoeff[index];

	L = pObj->L;
	N = pObj->N;
	
	r = m % L;

	pObj->pFifo[pObj->w++] = x;
	if (pObj->w == L)
		pObj->w = 0;


	y.real = pObj->pFifo[r].real * pW[0];
	y.imag = pObj->pFifo[r].imag * pW[0];

	for (j=1; j < N; j++)
	{

		i = (int)m - j;
		if (i < 0)
		{
			i = 0;
		}
	
		r = i % L;

		y.real += pObj->pFifo[r].real * pW[j];
		y.imag += pObj->pFifo[r].imag * pW[j];

	}

	return y;
}

void FarrowPPIPCpxInit(ppip_farrow_cpx_t *pObj, uint32_t M, uint32_t N)
{
	uint32_t i;

	pObj->ppCoeff = (fir_float_t**)malloc(M*sizeof(fir_float_t*));
	for(i=0; i < M; i++)
	{
		pObj->ppCoeff[i] = (fir_float_t*)malloc(N*sizeof(fir_float_t));
		memset(pObj->ppCoeff[i], 0, N*sizeof(fir_float_t));
	}
	pObj->M = M;
	pObj->N = N;
	pObj->pH = (cpx_t*)malloc(M*sizeof(cpx_t));
	pObj->pB = (cpx_t*)malloc(M*sizeof(cpx_t));

	pObj->w = 0;
	pObj->r = 0;
	pObj->L = 2*N;	// Influences STR-Loop delay

	pObj->pFifo = (cpx_t*)malloc(pObj->L*sizeof(cpx_t));
	memset(pObj->pFifo, 0, pObj->L*sizeof(cpx_t));


}

void FarrowPPIPCpxFree(ppip_farrow_cpx_t *pObj)
{
	uint32_t i;

	for(i=0; i < pObj->M; i++)
	{
		free(pObj->ppCoeff[i]);
	}
	free(pObj->ppCoeff);
	pObj->ppCoeff = NULL;

	free(pObj->pH);

	free(pObj->pFifo);
	pObj->pFifo = NULL;

}

cpx_t horner(cpx_t *a, cpx_t *b, uint32_t M, radio_float_t mu)
{
	uint32_t i;

	b[0] = a[0];
	for (i=1; i < M; i++)
	{
		b[i].real = a[i].real+mu*b[i-1].real;
		b[i].imag = a[i].imag+mu*b[i-1].imag;
	}

	return b[M-1];
}

cpx_t FarrowPPIPCpxProcess(ppip_farrow_cpx_t *pObj, cpx_t x, INT32 m, radio_float_t mu)
{
	INT32 i;
	uint32_t n, j, N, L, r;
	cpx_t y;

	L = pObj->L;
	N = pObj->N;
	
	pObj->pFifo[pObj->w++] = x;
	if (pObj->w == L)
		pObj->w = 0;


	// Partial filter responses
	for (n=0; n < pObj->M; n++)
	{
		r = (uint32_t)m % L;

		j=0;
		pObj->pH[n].real = pObj->pFifo[r].real * pObj->ppCoeff[n][j];
		pObj->pH[n].imag = pObj->pFifo[r].imag * pObj->ppCoeff[n][j];
		j++;
		for (; j < N; j++)
		{

			i = (INT32)m - j;
			if (i < 0)
				i = 0;
		
			r = i % L;

			pObj->pH[n].real += pObj->pFifo[r].real * pObj->ppCoeff[n][j];
			pObj->pH[n].imag += pObj->pFifo[r].imag * pObj->ppCoeff[n][j];

		}
	}
	
	// Combine
	y = horner(pObj->pH, pObj->pB, pObj->M, mu);

	return y;
}

#endif

// --------------------------------------------------------------
// Data functions
// --------------------------------------------------------------
uint32_t Scramble(uint8_t *pSrcDst, uint32_t state, uint32_t poly, uint32_t len)
{
	int i;

	while(len)
	{
		*pSrcDst ^= (uint8_t)state;
		pSrcDst++;
		len--;

		for (i=0; i < 8; i++)
		{
			if (state & 0x0001)
				state = (state >> 1) ^ poly;
			else
				state = (state >> 1);
		}

	}
	return state;
}

// --------------------------------------------------------------
uint32_t FrameCalcNumStreamBytes(uint32_t len_raw)
{
	uint32_t len, num_frames;
	num_frames = (int)ceil((float)len_raw / FRAME_NUM_BYTES_PER_FRAME);
	len = num_frames*sizeof(frame_t);

	if (len < sizeof(frame_t))
		len = sizeof(frame_t);

	return len;
}

// --------------------------------------------------------------
uint32_t FrameFormat(uint8_t *pRaw, uint8_t *pFormatted, uint32_t len_raw, uint32_t *pPrn_state, uint16_t frame_type)
{
	frame_t frame;

	uint32_t bytes_remain, len_formatted, len;
	uint32_t prn_state;

	if (len_raw == 0)
		return 0;

	prn_state = *pPrn_state;

	// Set preamble
	frame.hdr.preamble[0] = FRAME_PREAMBLE_IV;
	frame.hdr.preamble[1] = ~(frame.hdr.preamble[0]);

	bytes_remain = len_raw;
	len_formatted = 0;
	while(bytes_remain)
	{
		len = FRAME_NUM_BYTES_PER_FRAME;
		if (bytes_remain < FRAME_NUM_BYTES_PER_FRAME)
			len = bytes_remain;

		memset(frame.data, 0, FRAME_NUM_BYTES_PER_FRAME);
		memcpy(frame.data, pRaw, len);
		frame.hdr.crc16 = Crc16(FRAME_CRC16_POLY, FRAME_CRC16_IV, frame.data, FRAME_NUM_BYTES_PER_FRAME);
		frame.hdr.type = frame_type;
		frame.hdr.prn_state = prn_state;
		frame.hdr.length = (uint16_t)len;
		prn_state = Scramble(frame.data, prn_state, FRAME_SCRAMBLER_POLY, FRAME_NUM_BYTES_PER_FRAME);
		prn_state = Scramble((uint8_t*)&frame.hdr.crc16, prn_state, FRAME_SCRAMBLER_POLY, sizeof(frame.hdr.crc16));
		prn_state = Scramble((uint8_t*)&frame.hdr.type, prn_state, FRAME_SCRAMBLER_POLY, sizeof(frame.hdr.type));
		prn_state = Scramble((uint8_t*)&frame.hdr.length, prn_state, FRAME_SCRAMBLER_POLY, sizeof(frame.hdr.length));

		pRaw += len;
		bytes_remain -= len;

		memcpy(pFormatted, &frame, sizeof(frame_t));
		pFormatted += sizeof(frame_t);
		len_formatted += sizeof(frame_t);

	}
	*pPrn_state = prn_state;
	return len_formatted;
}

// --------------------------------------------------------------
uint32_t FrameDeformat(uint8_t *pFormatted, uint8_t *pRaw, uint32_t len_formatted, uint16_t frame_type_mask)
{
	frame_hdr_t hdr;
	uint32_t bytes_remain, len_raw, sync_state, frame_sync, i;
	uint16_t crc16_ist;
	uint8_t temp[FRAME_NUM_BYTES_PER_FRAME];
	uint32_t prn_state;

	if (len_formatted == 0)
		return 0;

	i = 0;
	bytes_remain = len_formatted;
	len_raw = 0;
	while(bytes_remain)
	{

		sync_state = 0;
		do
		{
			// Find frame boundaries
			switch(sync_state)
			{
				case 0:
				if (pFormatted[i] == FRAME_PREAMBLE_IV)
				{
					sync_state = 1;
				}
				break;

				case 1:
				if ((uint8_t)(~pFormatted[i]) == pFormatted[i-1])
				{
					sync_state = 2;
				}
				else
				{
					sync_state = 0;
				}
				break;
			}
			i++;
			bytes_remain--;
			frame_sync = (sync_state == 2);

		} while(bytes_remain && !frame_sync);

		memcpy(&hdr, &pFormatted[i-2], sizeof(frame_hdr_t));
		memcpy(temp, &pFormatted[i-2+sizeof(frame_hdr_t)], FRAME_NUM_BYTES_PER_FRAME);
		
		prn_state = Scramble(temp, hdr.prn_state, FRAME_SCRAMBLER_POLY, FRAME_NUM_BYTES_PER_FRAME);
		prn_state = Scramble((uint8_t*)&hdr.crc16, prn_state, FRAME_SCRAMBLER_POLY, sizeof(hdr.crc16));
		prn_state = Scramble((uint8_t*)&hdr.type, prn_state, FRAME_SCRAMBLER_POLY, sizeof(hdr.type));
		Scramble((uint8_t*)&hdr.length, prn_state, FRAME_SCRAMBLER_POLY, sizeof(hdr.length));

		crc16_ist = Crc16(FRAME_CRC16_POLY, FRAME_CRC16_IV, temp, FRAME_NUM_BYTES_PER_FRAME);

		if (crc16_ist == hdr.crc16)
		{
			
			if ((hdr.type & frame_type_mask) == frame_type_mask)
			{
				memcpy(&pRaw[len_raw], temp, hdr.length);
				len_raw += hdr.length;
			}
			i += FRAME_NUM_BYTES_PER_FRAME + sizeof(frame_hdr_t) - 2;
			bytes_remain -= (FRAME_NUM_BYTES_PER_FRAME + sizeof(frame_hdr_t) - 2);
		}

		if ((int)bytes_remain < sizeof(frame_t))
			bytes_remain = 0;
	}
	return len_raw;
}

// --------------------------------------------------------------
// Transmission order MSB first
uint32_t FrameSerialize(uint8_t *pSrc, uint8_t *pDst, uint32_t nBitsPerSym, uint32_t srclen)
{

	uint32_t bits_remain, i, j;
	uint8_t src, dst;

	if (srclen == 0)
		return 0;

	if (nBitsPerSym > 8)
		return (uint32_t)-1;

	j = 0;
	src = *pSrc;
	bits_remain = 8;
	while(srclen)
	{
		dst = 0;
		for (i=0; i < nBitsPerSym; i++)
		{
			dst <<= 1;
			dst |= (0x01 & (src >> 7));
			src <<= 1;

			bits_remain--;
			if (bits_remain == 0)
			{
				pSrc++;
				bits_remain = 8;
				src = *pSrc;
				srclen--;
			}

		}
		pDst[j++] = dst;
	}
	return j;
}

// --------------------------------------------------------------
uint32_t FrameDeSerialize(uint8_t *pSrc, uint8_t *pDst, uint32_t nBitsPerSym, uint32_t srclen, uint16_t frame_type_mask)
{

	uint32_t size, err, bits_remain, i, j, byte_sync, srcRemain, bitStart, numCorrupt;
	uint8_t src, dst, *pBitStart;
	uint16_t dst16, t;
	uint8_t frame[sizeof(frame_t)], data[FRAME_NUM_BYTES_PER_FRAME];

	if (srclen == 0)
		return 0;

	if (nBitsPerSym > 8)
		return (uint32_t)-1;

	src = *pSrc;

	bits_remain = nBitsPerSym;
	bitStart = bits_remain;
	pBitStart = pSrc;
	numCorrupt = 0;

	t = (FRAME_PREAMBLE_IV << 8) | (0xFF & ~FRAME_PREAMBLE_IV);
	size = 0;
	do
	{
		j = 0;
		dst16 = 0;
		byte_sync = 0;
		do
		{
			bits_remain--;
			dst16 <<= 1;
			dst16 |= (0x01 & (src >> (bits_remain)));

			if (bits_remain == 0)
			{
				pSrc++;
				bits_remain = nBitsPerSym;
				src = *pSrc;
				srclen--;
				if (!srclen)
					break;
			}
			
			// Find frame boundaries
			if (dst16 == t)
			{
				pBitStart = pSrc;
				bitStart = bits_remain;
				srcRemain = srclen;
				byte_sync = 1;
				frame[j++] = 0xFF & (t >> 8);
				frame[j++] = 0xFF & t;

			}

		} while(srclen && !byte_sync);
		if (!srclen)
			break;

		for(j; j < sizeof(frame_t); j++)
		{
			dst = 0;
			for (i=0; i < 8; i++)
			{
				bits_remain--;
				dst <<= 1;
				dst |= (0x01 & (src >> (bits_remain)));

				if (bits_remain == 0)
				{
					pSrc++;
					bits_remain = nBitsPerSym;
					src = *pSrc;
					srclen--;
					if (!srclen)
						break;
				}

			}
			frame[j] = dst;
			if (!srclen)
				break;

		}
		if (!srclen)
			break;

		err = FrameDeformat(frame, data, sizeof(frame_t), frame_type_mask);
		if (err > 0)
		{
			memcpy(&pDst[size], frame, sizeof(frame_t));
			size += sizeof(frame_t);

		}
		else
		{
			if (size > 0)
				numCorrupt++;
			bits_remain = bitStart;
			pSrc = pBitStart;
			srclen = srcRemain;
		}

	} while(srclen);

	printf("Number of corrupted frames = %d\n", numCorrupt);	
	return size;
}

// --------------------------------------------------------------
// Differential M-PSK symbol mapping
uint32_t FrameSymbolsMap(uint8_t *pBits, cpx_t *pSym, uint32_t nBitsPerSym, uint32_t num_syms, uint32_t type)
{
	uint32_t i;
	sym_map_t sym_mapper;

	if (num_syms == 0)
		return 0;

	SymMapInit(&sym_mapper, nBitsPerSym, type);

	for(i=0; i < num_syms; i++)
	{
		pSym[i] = SymMapEncode(&sym_mapper, pBits[i]);
	}
	
	return num_syms;
}

// --------------------------------------------------------------
// Differential M-PSK symbol de-mapping
uint32_t FrameSymbolsDemap(cpx_t *pSym, uint8_t *pBits, uint32_t nBitsPerSym, uint32_t num_syms, uint32_t type)
{
	uint32_t i;
	sym_map_t sym_demapper;
	sym_err_t sym_err;

	if (num_syms == 0)
		return 0;

	memset(&sym_err, 0, sizeof(sym_err_t));

	SymMapInit(&sym_demapper, nBitsPerSym, type);

	for(i=0; i < num_syms; i++)
	{
		pBits[i] = SymMapDecode(&sym_demapper, pSym[i], &sym_err);
	}

	return num_syms;
}

// --------------------------------------------------------------
// Symbol
// --------------------------------------------------------------
void SymErrInit(sym_err_t *pObj)
{
	memset(pObj, 0, sizeof(sym_err_t));
}

// --------------------------------------------------------------
void SymStatInit(sym_stat_t *pObj, uint32_t nBitsPerSym)
{
	uint32_t i;

	memset(pObj, 0, sizeof(sym_stat_t));

	pObj->num_const = 1;
	for(i=0; i < nBitsPerSym; i++)
	{
		pObj->num_const *= 2;
	}

	pObj->pPerSymStat = (per_sym_stat_t*)malloc(pObj->num_const*sizeof(per_sym_stat_t));

	for(i=0; i < pObj->num_const; i++)
	{
		pObj->pPerSymStat[i].count = 0;
		pObj->pPerSymStat[i].var_err_mag = 0;
		pObj->pPerSymStat[i].var_err_phi = 0;
		SlidingVarInit(&pObj->pPerSymStat[i].sl_err_mag, 1000, 1000);
		SlidingVarInit(&pObj->pPerSymStat[i].sl_err_phi, 1000, 1000);
	}

}

void SymStatFree(sym_stat_t *pObj)
{
	if(pObj->pPerSymStat)
		free(pObj->pPerSymStat);
}

void SymStatUpDate(sym_stat_t *pObj, uint8_t sym, sym_err_t *pSym_err)
{
	// Statistics
	pObj->sym_cnt++;
	pObj->pPerSymStat[sym].count++;
	pObj->pPerSymStat[sym].p = (radio_float_t)pObj->pPerSymStat[sym].count/pObj->sym_cnt;
	pObj->pPerSymStat[sym].var_err_mag = SlidingVarProcess(&pObj->pPerSymStat[sym].sl_err_mag, pSym_err->err_mag);
	pObj->pPerSymStat[sym].var_err_phi = SlidingVarProcess(&pObj->pPerSymStat[sym].sl_err_phi, pSym_err->err_phi);

}

void SymStatPrint(sym_stat_t *pObj)
{
	uint32_t i;

	printf("Number of constellations : %d\n", pObj->num_const);
	printf("Number of total symbols  : %d\n", pObj->sym_cnt);
	printf("Sym#\tOccur.\t\tProb.\t\tVar(MagErr)[dB]\tVar(PhiErr/PI)[dB]\n");
	for(i=0; i < pObj->num_const; i++)
	{
		printf("%4.1d\t%6.d\t\t%1.2g\t\t%.2f\t\t%.2f\n", 
		i,
		pObj->pPerSymStat[i].count,
		pObj->pPerSymStat[i].p,
		10*log10(pObj->pPerSymStat[i].var_err_mag),
		10*log10(pObj->pPerSymStat[i].var_err_phi/(PI*PI)));
	}
}
// --------------------------------------------------------------
// Symbol demapper for receiver works on each symbol
// --------------------------------------------------------------
void SymMapInit(sym_map_t *pObj, uint32_t nBitsPerSym, uint32_t type)
{
	cpx_t iq;
	uint32_t i, j, k, m, n, u, Ns, Nq, Nsq, map_idx_i, map_idx_q;
	radio_float_t phi, dphi, ii, qq, iii, qqq, temp, stepIQ;
	radio_float_t mag, sum2, sum4;
	int signI[4] = {+1, -1, -1, +1};
	int signQ[4] = {+1, +1, -1, -1};
	int even;
	pObj->mask = 0;
	pObj->nBitsPerSym = nBitsPerSym;
	pObj->num_const = 1;
	pObj->type = type;
	pObj->iq_step = 0;
	pObj->iq_max = 0;
	pObj->Pref = 0;

	for(k=0; k < nBitsPerSym; k++)
	{
		pObj->num_const *= 2;
		pObj->mask <<= 1;
		pObj->mask |= 1;
	}
	pObj->pMapTbl = (map_t*)malloc(pObj->num_const*sizeof(map_t));
	pObj->ppMapTbl = NULL;

	Ns = (uint32_t)sqrt(pObj->num_const);
	Nq = pObj->num_const/4;
	Nsq = (uint32_t)sqrt(Nq);
	stepIQ = (radio_float_t)sqrt(2.0)/(Ns-1);
	sum2 = sum4 = 0;
	switch(type)
	{

		case MAP_MODE_PSK:

		pObj->Es = 1.0;						// Symbol transmit power
		pObj->Eb = pObj->Es/nBitsPerSym;
		pObj->Pref = (radio_float_t)(1.0/sqrt(2.0));

		dphi = 2*(radio_float_t)PI/pObj->num_const;
		phi = (radio_float_t)PI/pObj->num_const;
		
		for (k=0; k < pObj->num_const; k++)
		{
			iq.real = (radio_float_t)cos(phi);
			iq.imag = (radio_float_t)sin(phi);
			pObj->pMapTbl[k].phi = (radio_float_t)CpxPhiS(iq);
			pObj->pMapTbl[k].mag = (radio_float_t)CpxMagS(iq);
			pObj->pMapTbl[k].rect = iq;
			phi += dphi;
		}
		break;

		case MAP_MODE_QAM:
		pObj->Es = (radio_float_t)0.5*stepIQ*stepIQ;
		pObj->Eb = pObj->Es/nBitsPerSym;
		pObj->ppMapTbl = (map_t**)malloc(Ns*sizeof(map_t*));
		for (i=0; i< Ns; i++)
			pObj->ppMapTbl[i] = (map_t*)malloc(Ns*sizeof(map_t));

		ii = (radio_float_t)(1.0/sqrt(2.0));
		pObj->iq_max = ii;
		qq = ii;
		even = 1;

		k = 0;
		for (m=0; m < Nsq; m++)
		{
			for (n=0; n < Nsq; n++)
			{
				iii = ii;
				qqq = qq;
				for (j=0; j < 4; j++)
				{
					u = Nq*j+k;
					iq.real = iii*signI[j];
					iq.imag = qqq*signQ[j];
					mag = (radio_float_t)CpxMagS(iq);
					phi = (radio_float_t)CpxPhiS(iq);
					pObj->pMapTbl[u].mag = mag;
					pObj->pMapTbl[u].phi = phi;
					pObj->pMapTbl[u].rect = iq;
					sum2 += mag*mag;
					sum4 += mag*mag*mag*mag;
					temp = iii;
					iii = qqq;
					qqq = temp;
					map_idx_i = (int)(0.5 +(0.5*iq.real/pObj->iq_max + 0.5)*(Ns-1));
					map_idx_q = (int)(0.5 +(0.5*iq.imag/pObj->iq_max + 0.5)*(Ns-1));
					pObj->ppMapTbl[map_idx_i][map_idx_q].mag = mag;
					pObj->ppMapTbl[map_idx_i][map_idx_q].phi = phi;
					pObj->ppMapTbl[map_idx_i][map_idx_q].rect = iq;
					pObj->ppMapTbl[map_idx_i][map_idx_q].sym = u;
				}
				ii = ii - even*stepIQ;
				k++;
			}
			qq = qq - stepIQ;
			ii = ii + even*stepIQ;
			even = -even;
		}
		
		switch(pObj->num_const)
		{
			// QPSK
			case 4:
				pObj->Pref = (radio_float_t)0.707;
				break;
			// 16-QAM
			case 16:
				pObj->Pref = (radio_float_t)0.385;
				break;
			// 64-QAM
			case 64:
				pObj->Pref = (radio_float_t)0.31;
				break;
			// 256-QAM
			case 256:
				pObj->Pref = (radio_float_t)0.27;
				break;
			default:
			break;
		}
		break;

		default:
		break;
	}
	pObj->iq_step = stepIQ;
	pObj->iq_side_len = Ns;
	sum2 /= pObj->num_const;
	sum4 /= pObj->num_const;

	pObj->R2 = sum4/sum2;
	pObj->R4 = sum4/(sum2*sum2);

	pObj->sym_last = 0;

}

// --------------------------------------------------------------
void SymMapFree(sym_map_t *pObj)
{
	uint32_t i;

	if (pObj->pMapTbl)
	{
		free(pObj->pMapTbl);
		pObj->pMapTbl = NULL;
	}

	if (pObj->ppMapTbl)
	{
		for (i=0; i< (uint32_t)sqrt(pObj->num_const); i++)
			free(pObj->ppMapTbl[i]);

		free(pObj->ppMapTbl);
		pObj->ppMapTbl = NULL;
	}
}

// --------------------------------------------------------------
cpx_t SymMapEncode(sym_map_t *pObj, uint8_t bit)
{
	uint32_t sym;
	cpx_t iq;
	
	// Differential encode
	sym = pObj->mask & ((uint32_t)bit + pObj->sym_last);
	pObj->sym_last = sym;

	iq = pObj->pMapTbl[sym].rect;

	return iq;
}

// --------------------------------------------------------------
uint8_t SymMapDecode2(sym_map_t *pObj, cpx_t x, sym_err_t *pSymErr)
{
	
	cpx_t err;
	uint32_t k;
	uint32_t sym, bit; 
	radio_float_t dist, best_dist, min_err_mag, err_mag, mag, phi;

	mag = (radio_float_t)CpxMagS(x);
	phi = (radio_float_t)CpxPhiS(x);

	best_dist = 1000;
	min_err_mag = 1000;
	for(k=0; k < pObj->num_const; k++)
	{
		err = CpxSubS(pObj->pMapTbl[k].rect, x);
		err_mag = pObj->pMapTbl[k].mag - mag;
		dist = err.real*err.real + err.imag*err.imag;
		if (dist < best_dist)
		{
			best_dist = dist;
//			pSymErr->err_mag = err_mag;
			pSymErr->err_phi = pObj->pMapTbl[k].phi - phi;
			pSymErr->err = err;
			pSymErr->mag = mag;
			pSymErr->phi = phi;
			pSymErr->hard_sym = pObj->pMapTbl[k].rect;
			sym = k;

		}
		if ((radio_float_t)fabs(err_mag) < min_err_mag)
		{
			pSymErr->err_mag = err_mag;
			pSymErr->hard_mag = pObj->pMapTbl[k].mag;
			min_err_mag = (radio_float_t)fabs(err_mag);
		}

	}

	// Differential decode
	bit = (uint8_t)(pObj->mask & (sym - pObj->sym_last));
	pObj->sym_last = sym;
	
	return bit;
}

map_t SymMapGetSymbol(sym_map_t *pObj, cpx_t x)
{
	map_t map;
	uint32_t map_idx_i, map_idx_q;
	radio_float_t rx_i, rx_q;

	rx_i = dclamp(x.real, -pObj->iq_max, pObj->iq_max);
	rx_q = dclamp(x.imag, -pObj->iq_max, pObj->iq_max);

	map_idx_i = (int)(0.5 +(0.5*rx_i/pObj->iq_max + 0.5)*(pObj->iq_side_len-1));
	map_idx_q = (int)(0.5 +(0.5*rx_q/pObj->iq_max + 0.5)*(pObj->iq_side_len-1));
	
	map = pObj->ppMapTbl[map_idx_i][map_idx_q];
	
	return map;
}

uint8_t SymMapDecode(sym_map_t *pObj, cpx_t x, sym_err_t *pSymErr)
{
	
	map_t map;
	uint32_t bit; 

	map = SymMapGetSymbol(pObj, x);

	pSymErr->err_phi = map.phi - (radio_float_t)CpxPhiS(x);
	pSymErr->err_mag = map.mag - (radio_float_t)CpxMagS(x);
	pSymErr->err = CpxSubS(map.rect, x);
	pSymErr->mag = CpxMagS(x);
	pSymErr->phi = CpxPhiS(x);
	pSymErr->soft_sym = x;
	pSymErr->hard_sym = map.rect;
	pSymErr->hard_mag = map.mag;

	// Differential decode
	bit = (uint8_t)(pObj->mask & (map.sym - pObj->sym_last));
	pObj->sym_last = map.sym;
	
	return bit;
}

// --------------------------------------------------------------
radio_float_t SymMapGetModulus(sym_map_t *pObj, cpx_t x)
{
	
	uint32_t k;
	radio_float_t best_mod, min_err_mag, err_mag, mag;

	mag = (radio_float_t)CpxMagS(x);

	min_err_mag = 1000;
	for(k=0; k < pObj->num_const; k++)
	{
		err_mag = pObj->pMapTbl[k].mag - mag;
		if ((radio_float_t)fabs(err_mag) < min_err_mag)
		{
			best_mod = pObj->pMapTbl[k].mag;
			min_err_mag = (radio_float_t)fabs(err_mag);
		}

	}
	return best_mod;
}
// --------------------------------------------------------------
// Equalizer
// --------------------------------------------------------------
#if 0
void EQComplexInit(eq_cpx_t *pObj, uint32_t N)
{
	pObj->N = N;
	pObj->pX = (cpx_t*)malloc(pObj->N*sizeof(cpx_t));
	pObj->pW = (cpx_t*)malloc(pObj->N*sizeof(cpx_t));
	memset(pObj->pX, 0, pObj->N*sizeof(cpx_t));
	memset(pObj->pW, 0, pObj->N*sizeof(cpx_t));
}

void EQComplexFree(eq_cpx_t *pObj)
{
	free(pObj->pX);
	free(pObj->pW);
}

cpx_t EQComplexProcess(eq_cpx_t *pObj, cpx_t x)
{
	uint32_t i;
	cpx_t y, t;

	for(i=pObj->N-1; (int)i > 0; i--)
	{
		pObj->pX[i] = pObj->pX[i-1];
	}

	// fill M top positions with M new data
	pObj->pX[i] = x;

	y.real = 0;
	y.imag = 0;
	for(i=0; i < pObj->N; i++)
	{
		t = CpxMulS(pObj->pX[i], CpxConjS(pObj->pW[i]));
		y = CpxAddS(y, t);
	}
	return y;
}

// --------------------------------------------------------------
void CMAInit(cma_t *pObj, uint32_t ntaps, uint32_t nSamplesPerSym)
{
	pObj->M = nSamplesPerSym;
	pObj->N = ntaps;
	pObj->Px = 0;
	pObj->Px_last = 0;

	EQComplexInit(&pObj->eq, pObj->N);
}

void CMAFree(cma_t *pObj)
{
	EQComplexFree(&pObj->eq);
}

cpx_t CMAProcess(cma_t *pObj, cpx_t x)
{
	cpx_t y;
	radio_float_t x2, x1;

	y = EQComplexProcess(&pObj->eq, x);
	x1 = CpxMagS(x);
	x2 = CpxMagS(pObj->eq.pX[pObj->eq.N-1]);

	pObj->Px += (x1*x1 - pObj->Px_last);
	pObj->Px_last = x2*x2;

	return y;
}

// Proakis: page 698
cpx_t CMAGodard(cpx_t sym_s_eq, radio_float_t mag_h, radio_float_t R2)
{
	cpx_t d;
	radio_float_t mag_s, t;

	mag_s = CpxMagS(sym_s_eq);
	
	t = (radio_float_t)((mag_h + R2*mag_s - mag_s*mag_s*mag_s)/(1E-4+mag_h));
	d = CpxScaleRealS(sym_s_eq, t);

	return d;
}

radio_float_t CMATrain(cma_t *pObj, cpx_t y, cpx_t d, radio_float_t mu)
{

	cpx_t e;

	e = CpxSubS(d, y);

	LMSUpdateWeigths(&pObj->eq, e, mu/(1.f+pObj->Px));

	return CpxMagS(e);
}

// --------------------------------------------------------------
void DFEComplexInit(dfe_cpx_t *pObj, uint32_t K)
{
	pObj->N = 2*K+1;
	pObj->N_ff = K+1;
	pObj->N_fb = K;

	EQComplexInit(&pObj->eq, pObj->N);

}

void DFEComplexFree(dfe_cpx_t *pObj)
{
	EQComplexFree(&pObj->eq);
}

cpx_t DFEComplexProcess(dfe_cpx_t *pObj, cpx_t xs, cpx_t xh)
{
	INT32 i;
	cpx_t y, t, *pW, *pX;

	pX = pObj->eq.pX;
	pW = pObj->eq.pW;

	// Shift Feedback part
	for(i=pObj->N-1; i > (INT32)pObj->N_ff; i--)
	{
		pX[i] = pX[i-1];
	}

	// put last x into buffer
	pX[i] = xh;

	// Shift Feed forward part
	for(i=pObj->N_ff-1; i > 0; i--)
	{
		pX[i] = pX[i-1];
	}

	// put current x into buffer
	pX[i] = xs;

	y.real = 0;
	y.imag = 0;
	for(i=0; i < (INT32)pObj->N; i++)
	{
		t = CpxMulS(pX[i], CpxConjS(pW[i]));
		y = CpxAddS(y, t);
	}
	return y;

}

void DFEAdaptLMS(dfe_cpx_t *pObj, cpx_t e, radio_float_t mu)
{
	LMSUpdateWeigths(&pObj->eq, e, mu);
}

// --------------------------------------------------------------
void LMSUpdateWeigths(eq_cpx_t *pObj, cpx_t e, radio_float_t mu)
{
	uint32_t i;

	for(i=0; i < pObj->N; i++)
	{
		pObj->pW[i] = CpxAddS(pObj->pW[i], CpxMulS(CpxScaleRealS(CpxConjS(e), mu), pObj->pX[i]));
	}
}

// --------------------------------------------------------------
void CoeffComplexUnitAt(cpx_t *pW, uint32_t N, uint32_t dly)
{
	if (dly >= N)
		dly = 0;

	memset(pW, 0, N*sizeof(cpx_t));
	pW[dly].real = 1.0;
	pW[dly].imag = 0.0;
}

void CoeffComplexConv(cpx_t *pSrc, cpx_t *pSrcDst, uint32_t N, uint32_t dly)
{
	uint32_t i, j, N2;
	cpx_t *pX, *pW, t, energy = {0};
	radio_float_t energy_correct;

	N2 = N+dly;

	pX = (cpx_t*)malloc(N2*sizeof(cpx_t));
	memset(pX, 0, N2*sizeof(cpx_t));
	pW = (cpx_t*)malloc(N2*sizeof(cpx_t));
	memset(pW, 0, N2*sizeof(cpx_t));

	CpxCopy(pSrc, pX, N);

	for(j=0; j < N; j++)
		energy = CpxAddS(energy, pSrcDst[j]);

	energy_correct = (radio_float_t)(1.0/CpxMagS(energy));

	for(i=0; i < N2; i++)
	{
		for(j=0; j < N; j++)
		{
			t = CpxMulS(pX[N-j+i-1], pSrcDst[j]);
			pW[i] = CpxScaleRealS(CpxAddS(pW[i], t), energy_correct);
		}
	}
	CpxCopy(&pW[N/2], pSrcDst, N);
}

// --------------------------------------------------------------
void RLSInit(rls_t *pObj, uint32_t N, uint32_t M)
{
	uint32_t i, j;
	radio_float_t *pR;

	pObj->w = (radio_float_t*)malloc(N*sizeof(radio_float_t));
	memset(pObj->w, 0, N*sizeof(radio_float_t));
	pObj->w[0] = 1.0f;

	pObj->pW = (cpx_t*)malloc(N*sizeof(cpx_t));
	memset(pObj->pW, 0, N*sizeof(cpx_t));

	pObj->z = (radio_float_t*)malloc(N*sizeof(radio_float_t));
	memset(pObj->z, 0, N*sizeof(radio_float_t));
	
	pObj->x = (radio_float_t*)malloc(N*sizeof(radio_float_t));
	memset(pObj->x, 0, N*sizeof(radio_float_t));
	
	pObj->pX = (cpx_t*)malloc(N*M*sizeof(cpx_t));
	memset(pObj->pX, 0, N*M*sizeof(cpx_t));

	pObj->R = (radio_float_t*)malloc(N*N*sizeof(radio_float_t));
	memset(pObj->R, 0, N*N*sizeof(radio_float_t));

	pObj->T = (radio_float_t*)malloc(N*N*sizeof(radio_float_t));
	memset(pObj->T, 0, N*N*sizeof(radio_float_t));

	pObj->T2 = (radio_float_t*)malloc(N*N*sizeof(radio_float_t));
	memset(pObj->T2, 0, N*N*sizeof(radio_float_t));

	pR = pObj->R;
	for(i=0; i < N; i++)
		for(j=0; j < N; j++)
			pR[i+j*N] = (radio_float_t)1E6*(i==j);

	pObj->N = N;
	pObj->M = M;
}

void RLSFree(rls_t *pObj)
{
	if(pObj->w)
		free(pObj->w);

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

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

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

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

	if(pObj->T2)
		free(pObj->T2);
}
/*
void RLSProcess(rls_t *pObj, radio_float_t rho, radio_float_t mu, radio_float_t x, radio_float_t d, radio_float_t *pE, radio_float_t *pDD)
{
	int i, j, k, N;
	radio_float_t *pR, *pX, *pZ, *pT, *pT2, v, t, rho_i;

	N = pObj->N;

	FIR(pObj->w, pObj->x, N, &x, pDD, 1);

	*pE = d - *pDD;

	// Whitening
	pR = pObj->R;
	pX = pObj->x;
	for(i=0; i < N; i++)
	{
		t = 0;
		for(j=0; j < N; j++)
			t += pR[j*N+i] * pX[j];

		pObj->z[i] = t;
	}

	// Berechnunng der Normierungskonstante
	v = rho;
	for(i=0; i < N; i++)
		v += pObj->x[i] * pObj->z[i];

	v = (radio_float_t)1.0/v;

	// Normierung
	for(i=0; i < N; i++)
		pObj->z[i] *= v;


	// Filter update
	for(i=0; i < N; i++)
		pObj->w[i] += (mu * (*pE)*pObj->z[i]);


	// Matrix update
	pZ = pObj->z;
	pX = pObj->x;
	pT = pObj->T;
	for(i=0; i < N; i++)
	{
		for(j=0; j < N; j++)
			pT[j*N+i] = pZ[i] * pX[j];
	}

	pT = pObj->T;
	pT2 = pObj->T2;
	pR = pObj->R;

	for (i=0;i<N;i++)
	{
		for (j=0;j<N;j++)
		{
			pT2[j*N+i] = 0;
			for (k=0;k<N;k++)
			{
				pT2[j*N+i] += pT[k*N+i] * pR[j*N+k];
			}
		}
	}

	for(i=0; i < N*N; i++)
		pR[i] -= pT2[i];

	rho_i = (radio_float_t)1.0/rho;
	for(i=0; i < N*N; i++)
		pR[i] *= rho_i;
}
*/
cpx_t RLSProcess(rls_t *pObj, radio_float_t i, radio_float_t q)
{
	uint32_t j;
	cpx_t y, t;

	y.real = 0;
	y.imag = 0;
	for(j=pObj->N-1; j != 0; j--)
	{
		pObj->pX[j] = pObj->pX[j-1];
	}
	pObj->pX[j].real = i;
	pObj->pX[j].imag = q;

	for(j=0; j < pObj->N; j++)
	{
		pObj->pW[j].real = pObj->w[j];
		pObj->pW[j].imag = pObj->w[j];
	}

///	for(j=0; j < pObj->N; j++)
//	{
//		t = CpxMuls(pObj->pX[M*j], CpxConjs(pObj->pW[j]));
//		y = CpxAdds(y, t);
//	}

	for(j=0; j < pObj->N; j++)
	{
		t.real = pObj->pX[j].real * pObj->pW[j].real;
		t.imag = pObj->pX[j].imag * pObj->pW[j].imag;
		
		y = CpxAddS(y, t);
	}

	for(j=0; j < pObj->N; j++)
	{
		pObj->x[j] = pObj->pX[j].real;
	}

	return y;

}

radio_float_t RLSUpdate(rls_t *pObj, radio_float_t rho, radio_float_t mu, radio_float_t y, radio_float_t d)
{
	int i, j, k, N;
	radio_float_t *pR, *pX, *pZ, *pT, *pT2, v, t, rho_i, e;

	N = pObj->N;

	e = d - y;

	pR = pObj->R;
	pX = pObj->x;

//	for(j=0; j < pObj->N; j++)
//	{
//		pObj->x[j] = pObj->pX[j*pObj->M].real;
//	}

	// Whitening
	for(i=0; i < N; i++)
	{
		t = 0;
		for(j=0; j < N; j++)
			t += pR[j*N+i] * pX[j];

		pObj->z[i] = t;
	}

	// Berechnunng der Normierungskonstante
	v = rho;
	for(i=0; i < N; i++)
		v += pObj->x[i] * pObj->z[i];

	v = (radio_float_t)1.0/v;

	// Normierung
	for(i=0; i < N; i++)
		pObj->z[i] *= v;


	// Filter update
	for(i=0; i < N; i++)
		pObj->w[i] += (mu * e * pObj->z[i]);


	// Matrix update
	pZ = pObj->z;
	pX = pObj->x;
	pT = pObj->T;
	for(i=0; i < N; i++)
	{
		for(j=0; j < N; j++)
			pT[j*N+i] = pZ[i] * pX[j];
	}

	pT = pObj->T;
	pT2 = pObj->T2;
	pR = pObj->R;

	for (i=0;i<N;i++)
	{
		for (j=0;j<N;j++)
		{
			pT2[j*N+i] = 0;
			for (k=0;k<N;k++)
			{
				pT2[j*N+i] += pT[k*N+i] * pR[j*N+k];
			}
		}
	}

	for(i=0; i < N*N; i++)
		pR[i] -= pT2[i];

	rho_i = (radio_float_t)1.0/rho;
	for(i=0; i < N*N; i++)
		pR[i] *= rho_i;

	return e;
}
#endif

// --------------------------------------------------------------