#include <cmath>
#include "Receiver.hpp"
#include "MinMaxLemire.h"

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

/***************************************************************/
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.00E-5;		// 3E-5
const radio_float_t		CPR_GAIN_LAG_AQU	= (radio_float_t)0.40E-6;		// 1E-6
const radio_float_t		CPR_GAIN_LEAD_TRK	= (radio_float_t)1.00E-6;		// 1E-5
const radio_float_t		CPR_GAIN_LAG_TRK	= (radio_float_t)1.00E-9;		// 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)8.00E-5;		// 8E-4
const radio_float_t		STR_GAIN_LAG_TRK	= (radio_float_t)1.00E-7;		// 1E-6

const radio_float_t		AGC_INITIAL_VALUE		= (radio_float_t)2.0;
const radio_float_t		AGC_ADAPTION_RATE_ACQ	= (radio_float_t)0.01;
const radio_float_t		AGC_ADAPTION_RATE_TRK	= (radio_float_t)0.0001;

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

/***************************************************************/
Receiver::Receiver(LogHandler *pLogHandler)
	: m_log(pLogHandler)
	, m_pPassbandBuffer(0)
	, m_pBasebandBuffer(0)
	, m_ReceiverEnable(false)
	, m_pDataListener(0)
	, m_timingGenerator()
	, m_polyPhase(m_timingGenerator)
	, m_farrow(m_timingGenerator)
	, m_frameReceiver()
	, m_pSymMapper(0)
	, m_tracker(1.0 - 5.0/EQ_UPD_INTERVAL)
{
	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;

		m_pPassbandBuffer = new cpx_t[size];
		m_pBasebandBuffer = new cpx_t[size];

		m_buf_symerr.resize(size);
		m_buf_sym.resize(size);
	}
	m_bufsize = size;
	m_buffer_agc.resize(2*size);
	m_buffer_ip.resize(size);
	init();
}

Processor::Buffer<sym_err_t>& Receiver::getSoftSyms()
{
	return m_buf_symerr;
}

cpx_t Receiver::getTracker(uint32 index)
{
	return toCpx(m_tracker.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_ACQ;
	m_params.agcMu[1] = AGC_ADAPTION_RATE_TRK;
	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_timingGenerator.loopFilterSetup(&m_loop_filter_str, &m_params.str_loopfilter_coeff[m_params.str_loopfilter_coeff_index]);

	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.agc_mode = agc_mode_enabled;
	
	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);

	initDefaultParams();
	m_ReceiverEnable = false;

	// FIR Arm filters
	initFilterArm();

	// FIR RCF filters
	initFilterRcf();

	// NCOs
	initDDC();
	initCPR();

	// Gardner Symbol Timing Recovery
	initSTR();

	// Processor buffer wiring
	m_polyPhase.buffer_in_add(&m_buffer_agc);
	m_polyPhase.buffer_out_add(&m_buffer_ip);
	m_polyPhase.prepare();
	
	m_farrow.buffer_in_add(&m_buffer_agc);
	m_farrow.buffer_out_add(&m_buffer_ip);
	m_farrow.prepare();

	m_frameReceiver.buffer_in_add(&m_buf_sym);
	m_frameReceiver.prepare();

	// 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;

	// 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(RVec const &rf, uint32_t len)
{
	const ScopedLock sl (m_lock);

	uint32_t len_down;
	radio_float_t test4normal;

	if (!m_ReceiverEnable)
	{
		return;
	}

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

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

	// Digital Down Converter
	// @samplerate
	m_nco_ddc.mixRealComplexV(m_passbandBuffer, rf, RealScalar(2)/*m_params.agcGain[0]*/, len);

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

	
}

void Receiver::processBaseband(radio_float_t *pI, radio_float_t *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 = {0,0};
	cpx_t IQ_agc = {0,0};
	cpx_t IQ_cpr = {0,0};
	cpx_t IQ_cma = {0,0};
	cpx_t IQ_dfeOn = {0,0};
	cpx_t IQ_dfeOff = {0,0};
	cpx_t IQ_eq_in = {0,0};
	cpx_t IQ_eq = {0,0};
	cpx_t IQ_soft = {0,0};
	cpx_t IQ_hard = {0,0};
	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_buf_symerr.clear();

	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

	// Blind AGC gain adjustment
	// @2 x symbolrate
	for (n = 0; n < len; n++)
	{
		// AGC Blind gain adjustments
		ComplexScalar iq_agc = toComplexScalar(CpxScaleRealS(toCpx(iq[n]), AGC_GetWeight(&agcBlind)));
		m_buffer_agc.write(&iq_agc, 1);
	}

	// Matched filtering and symbol timing recovery
	// @2 x symbolrate
	if (RCF_TYPE == RCF_TYPE_POLYPHASE_FARROW)
	{
		// Farrow interpolation
		m_farrow.process();
	}
	if (RCF_TYPE == RCF_TYPE_POLYPHASE_DISCRETE)
	{
		// Polyphase interpolation
		m_polyPhase.process();
	}

	// -----------------------------------------------------
	// Big and ugly processing loop
	// Further BB processing
	// @1 x symbolrate
	// -----------------------------------------------------
	while(m_buffer_ip.len())
	{
		// Buffer contains every 2nd sample from interpolator
		ComplexScalar iq_mf = m_buffer_ip.readAt(0);

#ifndef DISABLE_UNUSED_STATISTICS
		minMaxProcess(&m_statistics.ddcMinMax, iq_mf);
#endif
		// -----------------------------------------------------
		// AGC Tracker based training
		// @1 x symbolrate
		// -----------------------------------------------------
		if (m_params.agc_mode != agc_mode_disabled)
		{
			if (m_params.agcMu_index == agc_state_acquisition)
			{
				e_agc = 1.0 - CpxMagS(getTracker(0));
				AGC_Train(&agcBlind, e_agc, m_params.agcMu[m_params.agcMu_index]);
			}
			if (m_params.agcMu_index == agc_state_track)
			{
				e_agc = 1.0 - CpxMagS(getTracker(0));
				AGC_Train(&agcBlind, e_agc, m_params.agcMu[m_params.agcMu_index]);
			}
		}

		// -----------------------------------------------------
		// Carrier Derotator
		// @1 x symbolrate
		// -----------------------------------------------------
		IQ_cpr = toCpx(m_nco_cpr.mixComplexS(iq_mf, ComplexScalar(1,1)));
		m_nco_cpr.process(m_dOmega_vco, m_params.CPR_phase);

		// Carrier Phase Recovery
		if (m_params.cprState == cpr_state_acquisition)
		{
			// Costas loop 
			vPfdCpr = PhaseErrQPSK(IQ_cpr);
		}

		IQ_eq_in = IQ_cpr;

		// -----------------------------------------------------
		// Equalizer Process samples
		// @1 x symbolrate
		// -----------------------------------------------------
		ComplexScalar __IQ_cma;
		ComplexScalar __IQ_dfeOn;
		
		IQ_eq = IQ_eq_in;
		if (m_params.eq_mode == eq_mode_cma)
		{
			__IQ_cma = m_cma2.process(toComplexScalar(IQ_eq_in));
			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_eq_in), toComplexScalar(IQ_hard_last));
			IQ_dfeOn = toCpx(__IQ_dfeOn);
			IQ_eq = IQ_dfeOn;
		}

		IQ_soft = IQ_eq;

		// -----------------------------------------------------
		// Peak-Amplitude Tracker
		// @1 x symbolrate
		// -----------------------------------------------------
		m_tracker.process(toComplexScalar(IQ_soft));

		// -----------------------------------------------------
		// Map sympol
		// @1 x symbolrate
		// -----------------------------------------------------
		sym = SymMapDemap(m_pSymMapper, IQ_soft);
		sym_err = SymMapGetError(m_pSymMapper, IQ_soft, sym);
		IQ_hard = sym_err.hard_sym;

		// Carrier Phase Recovery
		if (m_params.cprState == cpr_state_track)
		{
			// state-based PFD
			vPfdCpr = PfdProcess(&m_pfdCpr, sym_err.err_phi, dabs(sym_err.mag));
		}

		if (m_params.cpr_mode == cpr_mode_enabled)
		{
			// Update frequency from Carrier Derotator
			// vPfdCpr comes from PhaseErrQPSK forming a Costas Loop when
			// cprState == cpr_state_acquisition
			// vPfdCpr comes from SymMapper forming a decision directed phase correction loop when
			// cprState == cpr_state_track
			m_dOmega_vco = LeadLagProcess(&m_loop_filter_cpr, &m_params.cpr_loopfilter_coeff[m_params.cpr_loopfilter_coeff_index], vPfdCpr);
		}

		// -----------------------------------------------------
		// Equalizer Training
		// @1 x symbolrate
		// -----------------------------------------------------
		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_eq_in), 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;

		// Feed symbol buffer
		m_buf_sym.write(&sym, 1);

		// Feed symbol error buffer
		m_buf_symerr.write(&sym_err, 1);

		// Update per-symbol statistic
		SymStatUpDate(&m_sym_stat, sym, &sym_err);
		if (m_pDataListener)
		{
			m_pDataListener->receiverDataChanged(this);
		}

		// 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);

	}

	// Call frame receiver with current symbol
	m_frameReceiver.process();
}

void Receiver::initDDC()
{
	printf("Receiver::initDDC: ddc_freq=%f\n", m_params.ddc_freq);

	m_nco_ddc.init(m_params.ddc_freq/m_params.samplerate, 0);
}

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

void Receiver::initSTR()
{
	// Gardner Symbol Timing Recovery
	LeadLagInit(&m_loop_filter_str, 0.0);
	m_timingGenerator.setOmega(1.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
#ifdef _WINDOWS
		m_farrow.load("C:\\Users\\jens\\farrow_coeff.dat");
#else
		m_farrow.load("/home/jens/farrow_coeff.dat");
#endif
	}

}

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(radio_float_t 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;

	// STR loop filter has changed
	if (lastParams.str_loopfilter_coeff_index != m_params.str_loopfilter_coeff_index)
	{
		m_timingGenerator.loopFilterSetup(&m_loop_filter_str, &m_params.str_loopfilter_coeff[m_params.str_loopfilter_coeff_index]);
	}

	// 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);

	m_params.cprState = cpr_state_acquisition;
	if (params.cpr_loopfilter_coeff_index == 1)
	{
		m_params.cprState = cpr_state_track;
	}

	m_params.strState = str_state_acquisition;
	if (params.str_loopfilter_coeff_index == 1)
	{
		m_params.strState = str_state_track;
	}

	m_params.agc_state = agc_state_acquisition;
	if (params.agcMu_index == 1)
	{
		m_params.agc_state = agc_state_track;
	}
}

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()
{
	m_timingGenerator.reset();
	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);
}