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

using namespace std;
using namespace Radio;
using namespace Processor;

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)2.00E-2;		// 2E-3
const radio_float_t		STR_GAIN_LAG_AQU	= (radio_float_t)5.00E-4;		// 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;
	bool is_time;
	radio_float_t m_dOmega;

	public:
	TimingGeneratorGardner()
	: Interpolation::TimingGenerator()
	{
		LeadLagInit(&loop_filter_str, 0.01);
		LeadLagSetCoeff(&str_loopfilter_coeff, STR_GAIN_LEAD_AQU, STR_GAIN_LAG_AQU);
		STRGardnerInit(&m_SymbolTimingRevovery);
		is_time = true;
	}

	virtual ~TimingGeneratorGardner() = default;

	void process(ComplexScalar const &iq)
	{
		// @2 x symbolrate
		// Symbol timing recovery
		radio_float_t vd = STRGardnerProcess(&m_SymbolTimingRevovery, toCpx(iq));
		if (is_time)
		{
			// @1 x symbolrate
#ifdef TEST_DEBUG
			cout << "Vd = " << vd << ", omega = " << m_dOmega << ", mu = " << getMu() << endl;
#endif
			m_dOmega = LeadLagProcess(&loop_filter_str, &str_loopfilter_coeff, vd);
		}
		is_time = not is_time;

		update(m_dOmega);
	}
};

void test()
{

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

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

	// Setup IQ data
	WAVEHEADER header;
#ifdef TEST_DEBUG
	CVec iq_in = wavLoad("m2_12k_12k_i_q_short_2sps.wav", header);
//	CVec iq_in = wavLoad("sine.wav", header);
#else
	CVec iq_in = wavLoad("m2_12k_12k_i_q_2sps.wav", header);
#endif

	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;

	// Create input buffer for Farrow Block
	auto farrow_buf_in = new Buffer<ComplexScalar>(BLOCKSIZE);
	
	// Create output buffer for Farrow Block
	auto farrow_buf_out = new Buffer<ComplexScalar>(BLOCKSIZE);

	// Assign buffers to Farrow Block
	farrow.buffer_in_add(farrow_buf_in);
	farrow.buffer_out_add(farrow_buf_out);

	while(remain)
	{
		// @2 x symbolrate

		// Matched filtering and interpolation
		size_t size = std::min(BLOCKSIZE, remain);
		size_t numFed = farrow_buf_in->write(&iq_in.data()[n], size);
		n += numFed;
		remain -= numFed;

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

}