#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>

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;
	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[j] = ComplexScalar(pWav[0][j], pWav[1][j]);
		}
		remain -= numRead;
	}
	return res;

}
void test()
{

	Interpolation::Farrow m_farrow;
	m_farrow.load("/home/jens/farrow_coeff.dat");
	
	// Loop-Filter
	lead_lag_filter_t m_loop_filter_str;
	lead_lag_coeff_t str_loopfilter_coeff;

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

	radio_float_t m_Vc;

	CVec iq = wavLoad("m2_12k_12k_i_q.wav");
	size_t len = iq.size();
	for (size_t n = 0; n < len; n++)
	{
		// @2 x symbolrate
		// Matched filtering and interpolation
		ComplexScalar iq_ip = m_farrow.process_new(iq[n], m_Vc);

		// Symbol timing recovery
		radio_float_t Vd = STRGardnerProcess(&m_SymbolTimingRevovery, toCpx(iq_ip));
		m_Vc = LeadLagProcess(&m_loop_filter_str, &str_loopfilter_coeff, Vd);
	}

}