#include <cpp/radio/Vector.hpp>
#include <cpp/radio/FirComplex.hpp>
#include <cpp/radio/interpolation/Farrow.hpp>
#include <cpp/radio/interpolation/PolyPhase.hpp>
#include <radio/synchronization.h>
#include <wav/wav.h>

#include <string.h>
#include <iostream>

using namespace std;
using namespace Radio;

CVec wavLoad(const char *pName)
{
	WAV wav;
	WavOpen(&wav, pName);
	CVec res(wav.m_numSamples);

	const size_t BLOCK_SIZE = 65536;
	int16_t *pWavInterleaved = (int16_t*)malloc(BLOCK_SIZE*wav.m_nChannels*sizeof(int16_t));
	
	float *pWav[2];
	
	pWav[0] = (float*)malloc(BLOCK_SIZE*sizeof(float));
	pWav[1] = (float*)malloc(BLOCK_SIZE*sizeof(float));

	size_t remain = wav.m_numSamples;
	size_t k=0;
	while(remain)
	{
		size_t size = std::min(remain, BLOCK_SIZE);
		size_t numRead = WavRead(&wav, pWavInterleaved, size);
		WavDeIlvdFloat(&wav, pWavInterleaved, pWav, numRead);
		for (int j=0; j < numRead; j++)
		{
			res[k++] = ComplexScalar(pWav[0][j], pWav[1][j]);
		}
		remain -= numRead;
	}
	return res;

}

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)1.00E-9;		// 1E-6

void test()
{

	// Setup Farrow
	Interpolation::Farrow farrow;
	farrow.load("/home/jens/farrow_coeff.dat");

	// Setup IQ data
	CVec iq = wavLoad("m2_12k_12k_i_q.wav");

	// Loop-Filter
	lead_lag_filter_t loop_filter_str;
	lead_lag_coeff_t str_loopfilter_coeff;

	// Gardner Symbol Timing Recovery
	str_t m_SymbolTimingRevovery;
	timing_corrector_t m_timing_corrector;

	LeadLagInit(&loop_filter_str, 1.0);
	LeadLagSetCoeff(&str_loopfilter_coeff, STR_GAIN_LEAD_AQU, STR_GAIN_LAG_AQU);

	radio_float_t vc=0;
	radio_float_t vd=0;
	for (size_t n = 0; n < iq.size(); n++)
	{
		// @2 x symbolrate

		vc = LeadLagProcess(&loop_filter_str, &str_loopfilter_coeff, vd);

		// Matched filtering and interpolation
		ComplexScalar iq_ip = farrow.process_new(iq[n], vc);

		// Symbol timing recovery
		vd = STRGardnerProcess(&m_SymbolTimingRevovery, toCpx(iq_ip));
		cout << "Vd = " << vd << ", Vc = " << vc << endl;

	}

}