#include <cpp/radio/Vector.hpp>
#include <cpp/radio/FirComplex.hpp>
#include <cpp/radio/interpolation/Farrow.hpp>
#include <cpp/radio/interpolation/PolyPhase.hpp>
#include <cpp/radio/interpolation/TimingGenerator.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, WAVEHEADER &format)
{
	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;
	}
	format = wav.m_Format;
	WavClose(&wav);
	return res;

}

void wavSave(const char *pName, WAVEHEADER const &format, CVec &iq)
{
	WAV wav;
	WavCreate(&wav, pName, FILETYPE_WAV, &format);

	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 = iq.size();
	size_t k=0;
	while(remain)
	{
		size_t size = std::min(remain, BLOCK_SIZE);
		for (int j=0; j < size; j++)
		{
			pWav[0][j] = iq[k].real();
			pWav[1][j] = iq[k].imag();
			k++;
		}
		WavIlvdFloat(&wav, pWav, pWavInterleaved, size);
		WavWrite(&wav, pWavInterleaved, size);

		remain -= size;
	}
	WavClose(&wav);

}

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

class TimingGeneratorGardner : public Interpolation::TimingGenerator
{
	// Loop-Filter
	lead_lag_filter_t loop_filter_str;
	lead_lag_coeff_t str_loopfilter_coeff;

	// Gardner Symbol Timing Recovery
	str_t m_SymbolTimingRevovery;

	public:
	TimingGeneratorGardner()
	: Interpolation::TimingGenerator()
	{
		LeadLagInit(&loop_filter_str, 1.0);
		LeadLagSetCoeff(&str_loopfilter_coeff, STR_GAIN_LEAD_AQU, STR_GAIN_LAG_AQU);
	}

	virtual ~TimingGeneratorGardner() = default;

	void process(ComplexScalar const &iq)
	{
		radio_float_t vc=0;
		radio_float_t vd=0;

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

		setOmega(vc);
		update();
	}
};

void test()
{

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

	// Setup timing generator
	TimingGeneratorGardner timingGenerator;
	timingGenerator.setOmega(1.0);

	// Setup IQ data
	WAVEHEADER header;
	CVec iq_in = wavLoad("m2_12k_12k_i_q_short.wav", header);
//	CVec iq_in = wavLoad("sine_220.wav", header);
	CVec iq_out;
	iq_out.reserve(8*iq_in.size());

	ComplexScalar iq_ip;
	size_t k=0;
	size_t n=0;
	size_t remain = iq_in.size();
	const size_t BLOCKSIZE=1024;
	while(remain)
	{
		size_t size = std::min(BLOCKSIZE, remain);
		// @2 x symbolrate

		// Matched filtering and interpolation
		size_t numFed = farrow.feed(&iq_in.data()[n], size);
		n += numFed;
		remain -= numFed;

		farrow.process_new(timingGenerator);
		while(farrow.getOutputBuffer().len())
		{
			size_t size = std::min(BLOCKSIZE, farrow.getOutputBuffer().len());
			iq_out.resize(k+size);
			for (size_t i=0; i < size; i++)
			{
				iq_out[k++] = farrow.getOutputBuffer().readAt(0);
			}
		}
	}
	wavSave("out.wav", header, iq_out);
	cout << "End of program" << endl;

}