#ifndef _INTERPOLATION_FARROW_HPP_
#define _INTERPOLATION_FARROW_HPP_

#pragma once

#include <cpp/radio/Vector.hpp>
#include <cpp/radio/FirComplex.hpp>
#include <cpp/radio/processor/src/Buffer.hpp>
#include <cpp/radio/interpolation/TimingGenerator.hpp>

#include <iostream>

namespace Radio
{
namespace Interpolation
{

// --------------------------------------------------------------
// Complex FIR-based polyphase interpolation filter (Farrow structure)
// From:
// "PERFORMANCE AND DESIGN OF FARROW FILTER USED FOR ARBITRARY RESAMPLING"
// [unknown date], Fred Harris, Signal Processing ChairCommunication Systems and Signal Processing Institute
// College of Engineering, San Diego State University, San Diego, CA 92182-0190 USA
// --------------------------------------------------------------
class Farrow : public FirComplex
{
	typedef struct _ml_farrow_coef_hdr_t
	{
		uint32_t M;
		uint32_t N;
	} ml_farrow_coef_hdr_t;
	
public:
	Farrow(uint32_t M=0, uint32_t N=0)
	: FirComplex(N)
	, m_M(M)
	, m_N(N)
	, m_w(0)
	, m_r(0)
	, m_coeff(N, M)
	, m_b(M)
	, m_h(M)
	, m_fifo(N)
	, m_bufferIn(8*1024)
	, m_bufferOut(8*1024)
	{
		init(M, N);
	}
	
	~Farrow()
	{
		
	}
	
	void init(uint32_t M, uint32_t N)
	{
		FirComplex::setNumTaps(N);
		m_M = M;
		m_N = N;
		m_coeff.resize(N, M);
		m_b.resize(M);
		m_h.resize(M);
		m_fifo.resize(N);
		m_fifo = ComplexScalar(0,0);
//		m_bufferIn.fill(ComplexScalar(0,0));
//		m_bufferOut.fill(ComplexScalar(0,0));
	}

	size_t feed(ComplexScalar const *x, uint32_t size=1)
	{
		m_w = (m_w + size) % m_N;
		m_fifo[m_w] = *x;
		return m_bufferIn.write(x, size);
	}
	
	bool process_new(TimingGenerator &timingGenerator)
	{
#if 0		
		while(m_bufferIn.len())
		{
			ComplexScalar xout;
			m_bufferIn.consume(1);
			m_bufferIn.read(&xout, 1, false);
			m_bufferOut.write(&xout, 1);
			std::cout << "xout: " << xout << std::endl;
		};
		std::cout << "----- SKIP -----" << std::endl;

#else
		while(true)
		{
			size_t adv = timingGenerator.getAdv();
			radio_float_t mu = timingGenerator.getMu();

			if (m_bufferIn.len() < adv)
			{
				break;
			}

			if (adv != 0)
			{
				FirComplex::feed(m_bufferIn, adv);
				
				// Partial filter responses
				for (int i=0; i < m_M; i++)
				{
					m_h[i] = FirComplex::processReal(column(m_coeff, i));
				}
			}
			// Combine
			ComplexScalar yout = horner(mu);
			m_bufferOut.write(&yout, 1);

			timingGenerator.process(yout);

		}
#endif
		return true;
	}

	ComplexScalar process(RealScalar mu, uint32_t pop)
	{
		uint32_t i, r;
		int32_t j;

		m_r = (m_r + pop) % m_N;

		if (pop)
		{
			r = m_r;
			j = m_N-1;
			while(r < m_N)
			{
				m_state[j--] = m_fifo[r++];
			}
			r = 0;
			while(j >= 0)
			{
				m_state[j--] = m_fifo[r++];
			}
		}

		// Partial filter responses
		for (i=0; i < m_M; i++)
		{
			m_h[i] = FirComplex::processReal(column(m_coeff, i));
		}

		// Combine
		return horner(mu);
	}	

	void load(const char *pFilename)
	{
		int i, j;
		FILE *pFile;
		ml_farrow_coef_hdr_t hdr;
		RealScalar *pCoeff;
		
		pFile = fopen(pFilename, "rb");
		if (!pFile)
		{
			printf("Can't open %s\n", pFilename);
			return;
		}
		if (EOF == fread(&hdr, sizeof(hdr), 1, pFile))
		{
			printf("Can't read %s\n", pFilename);
			return;
		}

		init(hdr.M, hdr.N);

		pCoeff = new RealScalar[hdr.M*hdr.N];
		for (i=0; i < (int)hdr.M; i++)
		{
			if (EOF == fread(pCoeff, sizeof(RealScalar), hdr.N, pFile))
			{
				printf("Can't read %s\n", pFilename);
				return;
			}
				
			for (j=0; j < hdr.N; j++)
			{
				m_coeff(j,i) = pCoeff[j];
			}
		}
		printf("Farrow filter coefficients loaded (M=%d, N=%d)\n", m_M, m_N);
		
		delete [] pCoeff;
		fclose(pFile);

	}
	
	Processor::Buffer<ComplexScalar>& getOutputBuffer()
	{
		return m_bufferOut;
	}
	
private:
	uint32_t m_M;
	uint32_t m_N;
	uint32_t m_r;
	uint32_t m_w;
	RMat m_coeff;
	CVec m_b;
	CVec m_h;
	CVec m_fifo;
	Processor::Buffer<ComplexScalar> m_bufferIn;
	Processor::Buffer<ComplexScalar> m_bufferOut;
	TimingGenerator m_timing;

	ComplexScalar horner(RealScalar mu)
	{
		uint32_t i;

		m_b[0] = m_h[0];
		for (i=1; i < m_M; i++)
		{
			m_b[i] = ComplexScalar(m_h[i].real() + m_b[i-1].real() * mu, m_h[i].imag() + m_b[i-1].imag() * mu);
		}
		
		return m_b[m_M-1];
	}

};

} // Radio::Interpolation
} // ::Radio

#endif // _INTERPOLATION_FARROW_HPP_