#include <cmath>
#include "Receiver.h"
#include "MinMaxLemire.h"
#include "radio/Interpolation.hpp"
#include <radio/Frame.hpp>

#if 0
#include <radio/ComplexVector.hpp>
#include <radio/RealVector.hpp>
#endif

#define DISABLE_UNUSED_STATISTICS

/***************************************************************/
const uint32_t			LPF_OVERSAMPLING	= 16;									// Arm-Filter: oversampling
const radio_float_t		LPF_OMEGA			= (radio_float_t)0.48;					// Arm-Filter: RC Roll-off
const uint32_t			RCF_OVERSAMPLING	= 16;									// RCF_TYPE_POLYPHASE_DISCRETE: oversampling
const radio_float_t		RCF_ROLLOFF			= (radio_float_t)0.35;					// RCF_TYPE_POLYPHASE_DISCRETE: RC Roll-off
const uint32_t			RCF_NUM_PHASES		= 256;									// RCF_TYPE_POLYPHASE_DISCRETE: number of discrete phases

const radio_float_t		CPR_GAIN_LEAD_AQU	= (radio_float_t)5.0E-5;		// 3E-5
const radio_float_t		CPR_GAIN_LAG_AQU	= (radio_float_t)0.4E-6;		// 1E-6
const radio_float_t		CPR_GAIN_LEAD_TRK	= (radio_float_t)1.0E-6;		// 1E-5
const radio_float_t		CPR_GAIN_LAG_TRK	= (radio_float_t)2.0E-8;		// 5E-7

const radio_float_t		STR_GAIN_LEAD_AQU	= (radio_float_t)4.00E-4;		// 2E-3
const radio_float_t		STR_GAIN_LAG_AQU	= (radio_float_t)0.50E-6;		// 5E-7
const radio_float_t		STR_GAIN_LEAD_TRK	= (radio_float_t)4.00E-6;		// 8E-4
const radio_float_t		STR_GAIN_LAG_TRK	= (radio_float_t)0.02E-6;		// 1E-6

const radio_float_t		AGC_INITIAL_VALUE	= (radio_float_t)2.0;
const radio_float_t		AGC_ADAPTION_RATE	= (radio_float_t)0.001;

const uint32_t			EQ_UPD_INTERVAL		= 2000;					// 1000
const uint32_t			EQ_DFE_K			= 63;					// 63
const uint32_t			EQ_GROUP_DELAY		= 16;					// 16

const radio_float_t		DFE_MU				= (radio_float_t)1E-3;	// 1E-3
const radio_float_t		CMA_MU				= (radio_float_t)8E-2;	// 8E-2

const radio_float_t		TRACKER_MU			= (radio_float_t)2E-2;
const radio_float_t		TRACKER_EPS			= (radio_float_t)1E-3;

/***************************************************************/
Receiver::Receiver(LogHandler *pLogHandler)
	: m_log(pLogHandler)
	, m_pPassbandBuffer(0)
	, m_pBasebandBuffer(0)
	, m_pSymbolBuffer(0)
	, m_ReceiverEnable(false)
	, m_pDataListener(0)
	, m_frameReceiver(this)
	, m_pSymMapper(0)
{
	m_params.numBitsPerSymbol = 2;
	m_params.samplerate = 48000;
	m_params.symbolrate = m_params.samplerate/4;
	m_params.ddc_freq = m_params.samplerate/4;
	m_params.CPR_phase = 0;

	startTimer (1000 / 10);

}

Receiver::~Receiver(void)
{
	stopTimer();
	free();
}

void Receiver::addStatusListener(ReceiverStatusListener *pListener)
{
	m_statusListeners.add(pListener);
}

void Receiver::addDataListener(ReceiverDataListener *pListener)
{
	m_pDataListener = pListener;
}

void Receiver::setBufSize(uint32 size)
{

	const ScopedLock sl (m_lock);

	if (m_bufsize != size)
	{
		m_passbandBuffer.resize(size);
		m_basebandBuffer.resize(size);

		if (m_pPassbandBuffer)
			delete m_pPassbandBuffer;

		if (m_pBasebandBuffer)
			delete m_pBasebandBuffer;

		if (m_pSymbolBuffer)
			delete m_pSymbolBuffer;

		m_pPassbandBuffer = new cpx_t[size];
		m_pBasebandBuffer = new cpx_t[size];
		m_pSymbolBuffer = new sym_err_t[size];
	}
	m_bufsize = size;
	init();
}

uint32 Receiver::getNumSoftSym()
{
	return m_numSymsInBuffer;
}

sym_err_t Receiver::getSoftSym(uint32 index)
{
	return m_pSymbolBuffer[index];
}

sym_err_t* Receiver::getSoftSyms()
{
	return m_pSymbolBuffer;
}

cpx_t& Receiver::getTracker(uint32 index)
{
	return m_trackers[index];
}

void Receiver::initDefaultParams()
{

	m_params.CPR_phase = 0;

	m_params.agc_state = agc_state_acquisition;
	m_params.agc_mode = agc_mode_disabled;
	m_params.agcMu[0] = AGC_ADAPTION_RATE;
	m_params.agcMu[1] = AGC_ADAPTION_RATE/10;
	m_params.agcMu_index = 0;

	m_params.strState = str_state_acquisition;
	m_params.str_mode = str_mode_enabled;
	LeadLagSetCoeff(&m_params.str_loopfilter_coeff[0], STR_GAIN_LEAD_AQU, STR_GAIN_LAG_AQU);
	LeadLagSetCoeff(&m_params.str_loopfilter_coeff[1], STR_GAIN_LEAD_TRK, STR_GAIN_LAG_TRK);
	m_params.str_loopfilter_coeff_index = 0;

	m_params.cprState = cpr_state_acquisition;
	m_params.cpr_mode = cpr_mode_enabled;
	LeadLagSetCoeff(&m_params.cpr_loopfilter_coeff[0], CPR_GAIN_LEAD_AQU, CPR_GAIN_LAG_AQU);
	LeadLagSetCoeff(&m_params.cpr_loopfilter_coeff[1], CPR_GAIN_LEAD_TRK, CPR_GAIN_LAG_TRK);
	m_params.cpr_loopfilter_coeff_index = 0;

	m_params.eq_mode = eq_mode_disabled;
	m_params.cmaType = cma_type_cma;
	m_params.cmaMode = cma_mode_training_enabled;
	m_params.dfeMode = dfe_mode_training_enabled;
	m_params.dfeAutoUpdateEnable = true;
	
	m_params.eqMuCma = CMA_MU;
	m_params.eqMuDfe = DFE_MU;
}

#if 0
void UpdateWeigths(CVec &x, CVec &e, CVec &w, CVec &w_conj, radio_float_t mu)
{

	e *= CVec(mu, 0);
	e.print("e * mu");

	w += x * e.conj();
	w.print("x * e*");

	w_conj = w.conj();
	w_conj.print("w*");

}
#endif

void Receiver::init()
{
	const ScopedLock sl (m_lock);

	m_trackers[0] = Cpx(1,1);
	m_trackers[1] = Cpx(-1,1);
	m_trackers[2] = Cpx(-1,-1);
	m_trackers[3] = Cpx(1,-1);
	m_trackers_[0] = Cpx(1,1);
	m_trackers_[1] = Cpx(-1,1);
	m_trackers_[2] = Cpx(-1,-1);
	m_trackers_[3] = Cpx(1,-1);

	initDefaultParams();
	m_ReceiverEnable = false;

	ClockInit(&m_symClock, NUM_BASEBAND_SAMPLES_PER_SYM);
	ClockSetPhase(&m_symClock, 1);

	// FIR Arm filters
	initFilterArm();

	// FIR RCF filters
	initFilterRcf();

	// NCOs
	initDDC();
	initCPR();

	// Gardner Symbol Timing Recovery
	initSTR();

	// Symbol demapper
	initSymbolMapper();

	// AGC
	AGC_Init(&agcBlind, EQ_UPD_INTERVAL, AGC_INITIAL_VALUE);

	// Channel estimation filter
	m_cma2.init(2*EQ_DFE_K+1);
	m_dfe_off2.init(EQ_DFE_K);
	m_dfe_on2.init(EQ_DFE_K);
	
	m_dfe_e_cpx.real = m_dfe_e_cpx.imag = 0.f;

	cmaReset();
	dfeReset();

	// Power detectors
	SlidingVarInit(&m_statistics.RF, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.I_DDC, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.Q_DDC, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.I_MF, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.Q_MF, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.I_CPR, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.Q_CPR, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.I_AGC, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.Q_AGC, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.I_EQ, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.Q_EQ, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.I_decision, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.Q_decision, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.MagDecision, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.PhiDecision, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.noise, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.noise_str, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.noise_cpr, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.noise_cma, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.noise_dfe_on, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.noise_dfe_off, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.sym_err_mag, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingVarInit(&m_statistics.sym_err_phi, EQ_UPD_INTERVAL, EQ_UPD_INTERVAL);
	SlidingMinMaxInit(&m_statistics.sl_min_I, EQ_UPD_INTERVAL, (radio_float_t)1E12, -1);
	SlidingMinMaxInit(&m_statistics.sl_max_I, EQ_UPD_INTERVAL, (radio_float_t)1E12, +1);
	SlidingMinMaxInit(&m_statistics.sl_min_Q, EQ_UPD_INTERVAL, (radio_float_t)1E12, -1);
	SlidingMinMaxInit(&m_statistics.sl_max_Q, EQ_UPD_INTERVAL, (radio_float_t)1E12, +1);
	SlidingMinMaxInit(&m_statistics.sl_min_mag, EQ_UPD_INTERVAL, (radio_float_t)1E12, -1);
	SlidingMinMaxInit(&m_statistics.sl_max_mag, EQ_UPD_INTERVAL, (radio_float_t)1E12, +1);

	m_ReceiverEnable = true;

	// --------------------------------------
	// Eval

	TraceOpen(&m_trace, "D:\\home\\jens\\Dokumente\\trace.txt");

}

void Receiver::free()
{
	const ScopedLock sl (m_lock);

	TraceClose(&m_trace);

	m_ReceiverEnable = false;

	NCO_Free(&m_nco_ddc);
	NCO_Free(&m_nco_cpr);

	// Symbol demapper
	if (m_pSymMapper)
	{
		SymMapFree(m_pSymMapper);
		delete m_pSymMapper;
	}
	m_pSymMapper = nullptr;

	SymStatFree(&m_sym_stat);

	// Power detectors
	SlidingVarFree(&m_statistics.RF);
	SlidingVarFree(&m_statistics.I_DDC);
	SlidingVarFree(&m_statistics.Q_DDC);
	SlidingVarFree(&m_statistics.I_MF);
	SlidingVarFree(&m_statistics.Q_MF);
	SlidingVarFree(&m_statistics.I_CPR);
	SlidingVarFree(&m_statistics.Q_CPR);
	SlidingVarFree(&m_statistics.I_AGC);
	SlidingVarFree(&m_statistics.Q_AGC);
	SlidingVarFree(&m_statistics.I_EQ);
	SlidingVarFree(&m_statistics.Q_EQ);
	SlidingVarFree(&m_statistics.I_decision);
	SlidingVarFree(&m_statistics.Q_decision);
	SlidingVarFree(&m_statistics.MagDecision);
	SlidingVarFree(&m_statistics.PhiDecision);
	SlidingVarFree(&m_statistics.noise);
	SlidingVarFree(&m_statistics.noise_str);
	SlidingVarFree(&m_statistics.noise_cpr);
	SlidingVarFree(&m_statistics.noise_cma);
	SlidingVarFree(&m_statistics.noise_dfe_on);
	SlidingVarFree(&m_statistics.noise_dfe_off);
	SlidingVarFree(&m_statistics.sym_err_mag);
	SlidingVarFree(&m_statistics.sym_err_phi);
	SlidingMinMaxFree(&m_statistics.sl_min_I);
	SlidingMinMaxFree(&m_statistics.sl_max_I);
	SlidingMinMaxFree(&m_statistics.sl_min_Q);
	SlidingMinMaxFree(&m_statistics.sl_max_Q);
	SlidingMinMaxFree(&m_statistics.sl_min_mag);
	SlidingMinMaxFree(&m_statistics.sl_max_mag);

	// AGC
	AGC_Free(&agcBlind);

}

// The Receiver Controller
void Receiver::timerCallback()
{
	static uint32_t forceStatusChangedCounter;
	const ScopedLock sl (m_lock);
	bool statusHasChanged = false;

	if (!m_ReceiverEnable)
		return;

	if (!forceStatusChangedCounter)
	{
		statusHasChanged = true;
		forceStatusChangedCounter = 10;
	}
	forceStatusChangedCounter--;

	// DFE Auto update
	if (m_params.dfeAutoUpdateEnable && (m_params.dfeMode == dfe_mode_training_enabled))
	{
		if ((getStatus().snrDfeOff_dB - getStatus().snrDfeOn_dB) > (radio_float_t)1.5)
		{
			dfeOnUpdateFromDfeOff();
			statusHasChanged = true;
		}
	}

	// Control
	m_params.cprState = cpr_state_acquisition;
	m_params.strState = str_state_acquisition;
	if (getStatus().snrCurrent_dB > 30)
	{
//		statusHasChanged = (m_params.cprState != cpr_state_track) || (m_params.strState != str_state_track);
		m_params.cprState = cpr_state_track;
		m_params.strState = str_state_track;
	}

	// Announce status changed
	if (statusHasChanged)
	{
		m_statusListeners.call(&ReceiverStatusListener::receiverStatusChanged, this);
	}
}

void Receiver::processPassband(float *pRF, uint32_t len)
{
	const ScopedLock sl (m_lock);

	uint32_t len_down;
	float test4normal;

	if (!m_ReceiverEnable)
	{
		return;
	}

	test4normal = 0;
	for (uint32_t i=0; i < len; i++)
	{
		test4normal += pRF[i];
	}
	if (isnan(test4normal))
	{
		return;
	}
	if (dabs(test4normal/len) > 10)
	{
		return;
	}

#ifndef DISABLE_UNUSED_STATISTICS
	SlidingVarProcessV(&m_statistics.RF, pRF, len);
#endif

	// Digital Down Converter
	// @samplerate
	NCO_MixRealComplexV(&m_nco_ddc, m_pPassbandBuffer, pRF, 2/*m_params.agcGain[0]*/, len);
	
	for (uint32_t i=0; i < len; i++)
	{
		m_passbandBuffer[i] = ComplexScalar(m_pPassbandBuffer[i].real, m_pPassbandBuffer[i].imag);
	}

	// Arm Filtering and downsampling
	len_down = m_firArmDown.process(m_basebandBuffer, m_passbandBuffer, len);
	processBaseband(m_basebandBuffer, len_down);
}

void Receiver::processBaseband(float *pI, float *pQ, uint32_t len)
{
	for (uint32_t i=0; i < len/2; i++)
	{
		m_basebandBuffer[i] = ComplexScalar(pI[2*i], pQ[2*i]);
	}
	processBaseband(m_basebandBuffer, len/2);
}

// Baseband processing
void Receiver::processBaseband(CVec const &iq, uint32_t len)
{
	const ScopedLock sl (m_lock);

	static cpx_t IQ_hard_last;

	uint32_t n;
	static cpx_t IQ_mf;
	cpx_t IQ_agc;
	cpx_t IQ_cpr;
	cpx_t IQ_cma;
	cpx_t IQ_dfeOn;
	cpx_t IQ_dfeOff;
	cpx_t IQ_eq;
	cpx_t IQ_soft;
	cpx_t IQ_hard;
	radio_float_t e_cma;
	radio_float_t e_dfe_on;
	radio_float_t e_dfe_off;
	radio_float_t e_agc;
	sym_err_t sym_err;

	radio_float_t Vd;
	radio_float_t vPfdCpr;

	symbol_t sym;
	map_t sym_cma;
	static map_t sym_dfe_off;
	m_numSymsInBuffer = 0;

	if (!m_ReceiverEnable)
		return;

#ifndef DISABLE_UNUSED_STATISTICS
	// @2 x symbolrate
	for (n = 0; n < len; n++)
	{
		SlidingVarProcess(&m_statistics.I_DDC, iq[n].real());
		SlidingVarProcess(&m_statistics.Q_DDC, iq[n].imag());
	}
#endif
	
	for (n = 0; n < len; n++)
	{

		if (RCF_TYPE == RCF_TYPE_POLYPHASE_DISCRETE)
		{
			// Matched filtering and interpolation
			m_polyPhase.feed(iq[n], 1);
		}

		if (RCF_TYPE == RCF_TYPE_POLYPHASE_FARROW)
		{
			// Matched filtering and interpolation
			m_farrow.feed(iq[n], 1);
		}

		do
		{
			// Timing corrector
			TimingCorrectorProcess(&m_timing_corrector, m_Vc);

#ifndef _DEBUG
//			TracePrint(&m_trace, "%d\t%8f\t%.8f\t%.8f\n", TimingCorrectorIsSkip(&m_timing_corrector)-TimingCorrectorIsStuff(&m_timing_corrector), TimingCorrectorGetMu(&m_timing_corrector), TimingCorrectorGetMu(&m_timing_corrector), (powerDB(m_pSymMapper->Eb, 2) - powerDB((SlidingVarGet(&m_statistics.sym_err_mag) + SlidingVarGet(&m_statistics.sym_err_phi)), 1)));
#endif

			if (RCF_TYPE == RCF_TYPE_POLYPHASE_DISCRETE)
			{
				ComplexScalar __IQ_mf = m_polyPhase.process(TimingCorrectorGetMu(&m_timing_corrector), !TimingCorrectorIsStuff(&m_timing_corrector));
				IQ_mf = toCpx(__IQ_mf);
			}

			if (RCF_TYPE == RCF_TYPE_POLYPHASE_FARROW)
			{
				ComplexScalar __IQ_mf = m_farrow.process(TimingCorrectorGetMu(&m_timing_corrector), !TimingCorrectorIsStuff(&m_timing_corrector));
				IQ_mf = toCpx(__IQ_mf);
			}

			// AGC Blind process samples
			IQ_agc = CpxScaleRealS(IQ_mf, AGC_GetWeight(&agcBlind));

			// Symbol timing recovery
			Vd = STRGardnerProcess(&m_SymbolTimingRevovery, IQ_agc);

			// Carrier Derotator
			IQ_cpr = NCO_MixComplexS(&m_nco_cpr, IQ_agc, Cpx(1,1));
			NCO_Process(&m_nco_cpr, m_dOmega_vco, m_params.CPR_phase);

			if (TimingCorrectorIsSkip(&m_timing_corrector))
			{
				if (!TimingCorrectorIsStuff(&m_timing_corrector))
					break;
			}

			// Operate at symbol clock
			// @1 x symbolrate
			if (ClockIsTick(&m_symClock, 1))
			{
				// AGC Blind Training
				if (m_params.agc_mode != agc_mode_disabled)
				{
					if (m_params.agcMu_index == agc_state_acquisition)
					{
						AGC_Process(&agcBlind, CpxMagS(IQ_cpr), m_params.agcMu[m_params.agcMu_index], 1.0);
					}
					if (m_params.agcMu_index == agc_state_track)
					{
						e_agc = CpxMagS(IQ_hard_last) - CpxMagS(IQ_cpr);
						AGC_Train(&agcBlind, e_agc, m_params.agcMu[m_params.agcMu_index]);
					}
				}
#ifndef DISABLE_UNUSED_STATISTICS
				minMaxProcess(&m_statistics.ddcMinMax, IQ_cpr);
#endif
				// -----------------------------------------------------
				// Equalizer Process samples
				// -----------------------------------------------------
				ComplexScalar __IQ_cma;
				ComplexScalar __IQ_dfeOn;
				
				IQ_eq = IQ_cpr;
				if (m_params.eq_mode == eq_mode_cma)
				{
					__IQ_cma = m_cma2.process(toComplexScalar(IQ_cpr));
					IQ_cma = toCpx(__IQ_cma);
					IQ_eq = IQ_cma;
				}

				if (m_params.eq_mode == eq_mode_dfe)
				{
					__IQ_dfeOn = m_dfe_on2.process(toComplexScalar(IQ_cpr), toComplexScalar(IQ_hard_last));
					IQ_dfeOn = toCpx(__IQ_dfeOn);
					IQ_eq = IQ_dfeOn;
				}

				IQ_soft = IQ_eq;

				// Map sympol
				sym = SymMapDemap(m_pSymMapper, IQ_soft);
				sym_err = SymMapGetError(m_pSymMapper, IQ_soft, sym);
				IQ_hard = sym_err.hard_sym;

				// Timing detector
				if (m_params.str_mode == str_mode_enabled)
				{
					m_Vc = LeadLagProcess(&m_loop_filter_str, &m_params.str_loopfilter_coeff[m_params.str_loopfilter_coeff_index], Vd);
				}
			
				// Carrier Phase Recovery
				if (m_params.cpr_mode == cpr_mode_enabled)
				{
					// state-based PFD
					vPfdCpr = PfdProcess(&m_pfdCpr, sym_err.err_phi, dabs(sym_err.mag));
				
					// use phase error from symbol mapper
					m_dOmega_vco = LeadLagProcess(&m_loop_filter_cpr, &m_params.cpr_loopfilter_coeff[m_params.cpr_loopfilter_coeff_index], vPfdCpr);
				}

				// -----------------------------------------------------
				// Equalizer Training
				// -----------------------------------------------------
				sym_cma = SymMapGetSymbolInfo(m_pSymMapper, SymMapDemap(m_pSymMapper, IQ_cma));
				if (m_params.cmaMode == cma_mode_training_enabled)
				{
					if (m_params.cmaType == cma_type_cma)
					{
						// CMA: Train
						m_cma2.trainGodard(__IQ_cma, CpxMagS(sym_cma.rect), m_pSymMapper->R2_cma, m_params.eqMuCma);
					}
					if (m_params.cmaType == cma_type_mma)
					{
						// MMA: Train
						m_cma2.trainMma(__IQ_cma, toComplexScalar(m_pSymMapper->R_mma), m_params.eqMuCma);
					}
					if (m_params.cmaType == cma_type_smma)
					{
						// S-MMA: Train
						m_cma2.trainSmma(__IQ_cma, CpxMagS(sym_cma.rect), toComplexScalar(m_pSymMapper->R_smma), m_params.eqMuCma);
					}
				}

				if (m_params.dfeMode == dfe_mode_training_enabled)
				{
					// DFE: LMS Update coefficients
					m_dfe_off2.train(toComplexScalar(m_dfe_e_cpx), m_params.eqMuDfe);
		
					// DFE: Calc offline response
					ComplexScalar __IQ_dfeOff = m_dfe_off2.process(toComplexScalar(IQ_cpr), toComplexScalar(sym_dfe_off.rect));
					IQ_dfeOff = toCpx(__IQ_dfeOff);
					
					sym_dfe_off = SymMapGetSymbolInfo(m_pSymMapper, SymMapDemap(m_pSymMapper, IQ_soft));

					// DFE: Calculate current error
					ComplexScalar __dfe_e_cpx =  toComplexScalar(sym_dfe_off.rect) - __IQ_dfeOff;
					m_dfe_e_cpx = toCpx(__dfe_e_cpx);
				}
			
				// Calculate current error
				// CMA
				e_cma = CpxMagS(CpxSubS(sym_cma.rect, IQ_cma));

				// DFE-Offline
				e_dfe_off = CpxMagS(m_dfe_e_cpx);

				// DFE-Online
				e_dfe_on = CpxMagS(CpxSubS(IQ_hard, IQ_dfeOn));
				// -----------------------------------------------------

				// Update IQ_H
				IQ_hard_last = IQ_hard;

				// Lock-detector development
				cpx_t winner_diff = Cpx(0,0);
				float winner_d = 1000;
				int winner_i = 0;
				cpx_t diff;
				float d;
				for (int i=0; i < 4; i++)
				{
					diff = CpxSubS(IQ_soft, m_trackers_[i]);
					d = CpxMagS(diff);
					if (d < winner_d)
					{
						winner_d = d;
						winner_i = i;
						winner_diff = diff;
					}
				}
				m_trackers_[winner_i] = CpxAddS(m_trackers_[winner_i], CpxScaleRealS(winner_diff, winner_d*TRACKER_MU+TRACKER_EPS));
				for (int i=0; i < 4; i++)
				{
					m_trackers[i] = CpxAddS(CpxScaleRealS(m_trackers[i], 0.995f), CpxScaleRealS(m_trackers_[i], 0.005f));
				}

				// Statistics
#ifndef DISABLE_UNUSED_STATISTICS
				SlidingVarProcess(&m_statistics.I_MF, IQ_mf.real);
				SlidingVarProcess(&m_statistics.Q_MF, IQ_mf.imag);
				SlidingVarProcess(&m_statistics.I_CPR, IQ_cpr.real);
				SlidingVarProcess(&m_statistics.Q_CPR, IQ_cpr.imag);
				SlidingVarProcess(&m_statistics.I_AGC, IQ_agc.real);
				SlidingVarProcess(&m_statistics.Q_AGC, IQ_agc.imag);
				SlidingVarProcess(&m_statistics.I_decision, IQ_hard.real);
				SlidingVarProcess(&m_statistics.Q_decision, IQ_hard.imag);
				SlidingVarProcess(&m_statistics.MagDecision, sym_err.hard_mag);
				SlidingVarProcess(&m_statistics.PhiDecision, sym_err.hard_phi);
#endif				
				SlidingVarProcess(&m_statistics.I_EQ, IQ_eq.real);
				SlidingVarProcess(&m_statistics.Q_EQ, IQ_eq.imag);
				SlidingVarProcess(&m_statistics.sym_err_mag, sym_err.err_mag);
				SlidingVarProcess(&m_statistics.sym_err_phi, sym_err.err_phi*(radio_float_t)(1.0/PI));
				SlidingVarProcess(&m_statistics.noise, CpxMagS(CpxSubS(IQ_hard, IQ_soft)));
				SlidingVarProcess(&m_statistics.noise_str, LeadLagGetState(&m_loop_filter_str));
				SlidingVarProcess(&m_statistics.noise_cpr, LeadLagGetState(&m_loop_filter_cpr));
				SlidingVarProcess(&m_statistics.noise_cma, e_cma);
				SlidingVarProcess(&m_statistics.noise_dfe_on, e_dfe_on);
				SlidingVarProcess(&m_statistics.noise_dfe_off, e_dfe_off);

				// -----------------------------------------------------
				m_pSymbolBuffer[m_numSymsInBuffer++] = sym_err;
				if ((m_params.strState == str_state_track) && (m_params.cprState == cpr_state_track))
				{
					// Update per-symbol statistic
					SymStatUpDate(&m_sym_stat, sym, &sym_err);
					m_frameReceiver.process((symbol_t)sym);
				}
				if (m_pDataListener)
					m_pDataListener->receiverDataChanged(this);
			}
		} while (TimingCorrectorIsStuff(&m_timing_corrector));

	}
}

void Receiver::initDDC()
{
	NCO_Init(&m_nco_ddc, m_params.ddc_freq/m_params.samplerate, 0);
}

void Receiver::initCPR()
{
	PfdInit(&m_pfdCpr);
	NCO_Init(&m_nco_cpr, 0, 0);
	LeadLagInit(&m_loop_filter_cpr, 0.0);
	m_dOmega_vco = 0.0;
}

void Receiver::initSTR()
{
	// Gardner Symbol Timing Recovery
	STRGardnerInit(&m_SymbolTimingRevovery);
	LeadLagInit(&m_loop_filter_str, 0.0);
	TimingCorrectorInit(&m_timing_corrector, (radio_float_t)0.1);
	m_Vc = 0;
}

void Receiver::initSymbolMapper()
{
	if (m_pSymMapper)
	{
		SymMapFree(m_pSymMapper);
		SymStatFree(&m_sym_stat);
	}
	else
	{
		m_pSymMapper = new sym_map_t();
	}

	SymMapInit(m_pSymMapper, m_params.numBitsPerSymbol, MODULATION_TYPE);
	SymStatInit(&m_sym_stat, m_params.numBitsPerSymbol);
	m_frameReceiver.setNumBitsPerSymbol(m_params.numBitsPerSymbol);
}

void Receiver::initFilterRcf()
{
	uint32_t nrcf =  (uint32_t)(RCF_OVERSAMPLING*m_params.samplerate/m_params.symbolrate)+1;

	if (RCF_TYPE == RCF_TYPE_POLYPHASE_DISCRETE)
	{
		printf("Calculating %d-tap %d-phase SRRC matched Filter (total %d coefficients)\n",nrcf, RCF_NUM_PHASES, nrcf*RCF_NUM_PHASES);
		m_polyPhase.init(NUM_BASEBAND_SAMPLES_PER_SYM, (radio_float_t)NUM_BASEBAND_SAMPLES_PER_SYM, RCF_ROLLOFF, nrcf, RCF_NUM_PHASES);
	}

	// SRRC poly-phase matched filter with Lagrange interpolator (Farrow)
	if (RCF_TYPE == RCF_TYPE_POLYPHASE_FARROW)
	{
		// Fixed at symbol duration 2/fs
		// -> RCF_ROLLOFF and symbol rate parameter have no influence 
		m_farrow.load("/home/jens/farrow_coeff.dat");
	}

}

void Receiver::initFilterArm()
{
	uint32_t down;

	uint32_t narm;
	RealScalar *coefArm;
	
	narm = (uint32_t)(LPF_OVERSAMPLING*m_params.samplerate/m_params.symbolrate)+1;
	coefArm = new RealScalar[narm];

	// FIR Arm filters
	printf("Calculating %d-tap Arm LP-Filter fc = %g Hz\n",narm, NUM_PASSBAND_SAMPLES_PER_SYM*LPF_OMEGA*m_params.symbolrate);
	FIRCalcLowpass((radio_float_t)(NUM_PASSBAND_SAMPLES_PER_SYM*LPF_OMEGA*m_params.symbolrate/m_params.samplerate), coefArm, narm);
//	printf("Calculating %d-tap Arm BP-Filter fc = %g Hz, bw=%g Hz\n",narm, m_params.symbolrate, 4*m_params.symbolrate/3);
//	FIRCalcBandpass((radio_float_t)(m_params.symbolrate/m_params.samplerate), 4*(m_params.symbolrate/m_params.samplerate)/3, coefArm, narm);

	down = (uint32_t)(m_params.samplerate/(2*m_params.symbolrate) + 0.5);
	m_firArmDown.init(down, coefArm, narm);

	delete [] coefArm;
}

// Interface
void Receiver::setSamplerate(float samplerate_hz)
{
	const ScopedLock sl (m_lock);

	m_params.samplerate = samplerate_hz;
	m_params.symbolrate = samplerate_hz/4;
	m_params.ddc_freq = samplerate_hz/4;

	initDDC();
	initFilterArm();
	initFilterRcf();
}

void Receiver::setParams(const params_t &params)
{
	const ScopedLock sl (m_lock);
	params_t lastParams = m_params;
		
	m_params = params;

	// DDC frequency has changed
	if (lastParams.ddc_freq != m_params.ddc_freq)
	{
		initDDC();
	}

	// Symbol rate has changed
	if (lastParams.symbolrate != m_params.symbolrate)
	{
		initFilterArm();
		initFilterRcf();
	}

	// Number of bits per symbol has changed
	if (MAX_NUMBITS_PERSYM < m_params.numBitsPerSymbol)
	{
		m_params.numBitsPerSymbol = MAX_NUMBITS_PERSYM;
	}

	if (lastParams.numBitsPerSymbol != m_params.numBitsPerSymbol)
	{
		initSymbolMapper();
	}
	m_statusListeners.call(&ReceiverStatusListener::receiverStatusChanged, this);
}

params_t& Receiver::getParams()
{
	return m_params;
}

status_t& Receiver::getStatus()
{
	static status_t status;
	radio_float_t powerSoft_dB;
	radio_float_t powerHard_dB;

	if (!m_pSymMapper)
		return status;
	
	status.frameStatRx = m_frameReceiver.getStats();
	status.numSymbolsReceived = m_sym_stat.sym_cnt;
	status.noiseStr = powerDB(SlidingVarGet(&m_statistics.noise_str), 1.0f);
	status.noiseCpr = powerDB(SlidingVarGet(&m_statistics.noise_cpr), 1.0f);

	status.agcGain[0] = AGC_GetWeight(&agcBlind);
	status.powerRF_dB = powerDB(SlidingVarGet(&m_statistics.RF), 1.0f);
	status.powerDDC_dB = cpxPowerDB(Cpx(SlidingVarGet(&m_statistics.I_DDC), SlidingVarGet(&m_statistics.Q_DDC)), 1.0f);
	status.powerMF_dB = cpxPowerDB(Cpx(SlidingVarGet(&m_statistics.I_MF), SlidingVarGet(&m_statistics.Q_MF)), 1.0f);
	status.powerCPR_dB = cpxPowerDB(Cpx(SlidingVarGet(&m_statistics.I_CPR), SlidingVarGet(&m_statistics.Q_CPR)), 1.0f);
	status.powerAGC_dB = cpxPowerDB(Cpx(SlidingVarGet(&m_statistics.I_AGC), SlidingVarGet(&m_statistics.Q_AGC)), 1.0f);
	status.powerEQ_dB = cpxPowerDB(Cpx(SlidingVarGet(&m_statistics.I_EQ), SlidingVarGet(&m_statistics.Q_EQ)), 1.0f);
	status.powerDecison_dB = cpxPowerDB(Cpx(SlidingVarGet(&m_statistics.I_decision), SlidingVarGet(&m_statistics.Q_decision)), 1.0f);
	status.powerMagDecision_dB = powerDB(SlidingVarGet(&m_statistics.MagDecision), 1);
	status.powerPhiDecision_dB = powerDB(SlidingVarGet(&m_statistics.PhiDecision), 1);
	powerSoft_dB = status.powerEQ_dB;
	powerHard_dB = status.powerDecison_dB;

	status.snrCurrent_dB = -(powerDB(SlidingVarGet(&m_statistics.noise), 1.0f) + powerSoft_dB);
	status.snrCma_dB = -(powerDB(SlidingVarGet(&m_statistics.noise_cma), 1.0f) + powerSoft_dB);
	status.snrDfeOn_dB = -(powerDB(SlidingVarGet(&m_statistics.noise_dfe_on), 1.0f) + powerSoft_dB);
	status.snrDfeOff_dB = -(powerDB(SlidingVarGet(&m_statistics.noise_dfe_off), 1.0f) + powerSoft_dB);
	status.snrSymbolMagnitude_dB = -(powerDB(SlidingVarGet(&m_statistics.sym_err_mag), 1.0f));
	status.snrSymbolPhase_dB = -(powerDB(SlidingVarGet(&m_statistics.sym_err_phi), 1));
	status.EB_N0 = (powerDB(m_pSymMapper->Eb, 2) - powerDB((SlidingVarGet(&m_statistics.sym_err_mag) + SlidingVarGet(&m_statistics.sym_err_phi)), 1));

	status.deltaFrequencyCPR = m_params.ddc_freq + NUM_BASEBAND_SAMPLES_PER_SYM*m_params.symbolrate*LeadLagGetState(&m_loop_filter_cpr);
	status.deltaFrequencySTR = m_params.symbolrate * (1-LeadLagGetState(&m_loop_filter_str));

	status.ddcMinMax = m_statistics.ddcMinMax;

	return status;
}

void Receiver::reset()
{
	free();
	init();
	m_statusListeners.call(&ReceiverStatusListener::receiverStatusChanged, this);
}

void Receiver::statisticsReset()
{
	m_frameReceiver.resetStats();
	SymStatReset(&m_sym_stat);
	m_statusListeners.call(&ReceiverStatusListener::receiverStatusChanged, this);
}

void Receiver::strReset()
{
	LeadLagSetState(&m_loop_filter_str, 0);
	STRGardnerInit(&m_SymbolTimingRevovery);
	m_statusListeners.call(&ReceiverStatusListener::receiverStatusChanged, this);
}

void Receiver::cprReset()
{
	LeadLagSetState(&m_loop_filter_cpr, 0);
	m_statusListeners.call(&ReceiverStatusListener::receiverStatusChanged, this);
}

void Receiver::dfeReset()
{
	m_dfe_on2.setUnit(EQ_GROUP_DELAY);
	m_dfe_off2.setUnit(EQ_GROUP_DELAY);
	m_statusListeners.call(&ReceiverStatusListener::receiverStatusChanged, this);
}

void Receiver::cmaReset()
{
	m_cma2.setUnit(EQ_GROUP_DELAY);
	m_statusListeners.call(&ReceiverStatusListener::receiverStatusChanged, this);
}

void Receiver::dfeOffUpdateFromCma()
{
	(Equalizer::AEqualizer &)m_dfe_off2 = (Equalizer::AEqualizer const &)m_cma2;
	m_statusListeners.call(&ReceiverStatusListener::receiverStatusChanged, this);
}

void Receiver::dfeOnUpdateFromDfeOff()
{
	m_dfe_on2 = m_dfe_off2;
	m_statusListeners.call(&ReceiverStatusListener::receiverStatusChanged, this);
}