#ifndef _INTERPOLATION_POLYPHASE_HPP_
#define _INTERPOLATION_POLYPHASE_HPP_

#pragma once

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

#include <fir/fir2.h>
#include <memory.h>
#include <string.h>

namespace Radio
{
namespace Interpolation
{
// --------------------------------------------------------------
// Complex FIR-based discrete step polyphase interpolation filter
// --------------------------------------------------------------
class PolyPhase : public Processor::Block
{
public:
	PolyPhase(TimingGenerator &timingGenerator)
	: Processor::Block(1, 1)
	, m_timingGenerator(timingGenerator)
	{
		
	}
	
	~PolyPhase()
	{
		
	}
	
	void init(RealScalar fa, RealScalar tsym, RealScalar alpha, uint32_t N, uint32_t M)
	{
		uint32_t i, j;
		fir_float_t *pCoeff;
		RealScalar k;
	
		m_fir.setNumTaps(N);
		m_M = M;
		m_N = N;
		m_coeff.resize(N, M);
		
		pCoeff = (fir_float_t*)malloc((N+1)*M*sizeof(fir_float_t));
		memset(pCoeff, 0, (N+1)*M*sizeof(fir_float_t));

		k = CalcFirSRRC(pCoeff, M*fa, tsym, alpha, N*M);

		m_coeff.resize(N, M);
		for(i=0; i < M; i++)
		{
			for(j=0; j < N; j++)
			{
				m_coeff(j,i) = M*pCoeff[M*j+M/2+i];
			}
		}
		free(pCoeff);
	}

	void process()
	{
		// Get buffers
		auto buf_in = buffer_in<ComplexScalar>();
		auto buf_out = buffer_out<ComplexScalar>();

		// Process input
		while(buf_out->free())
		{
			size_t adv = m_timingGenerator.getAdv();
			radio_float_t mu = m_timingGenerator.getMu();

			if (buf_in->len() < adv)
			{
				break;
			}

			if (adv != 0)
			{
				m_fir.feed(*buf_in, adv);
			}

			// Polyphase filtering
			uint32_t index = (uint32_t)dmod(dmax(0, mu*m_M), (RealScalar)m_M);
			ComplexScalar yout = m_fir.processReal(column(m_coeff, index));

			// Process timing generator
			bool is_sample = m_timingGenerator.process(yout);
			if (is_sample)
			{
				buf_out->write(&yout, 1);
			}
		}
	}	

private:
	TimingGenerator &m_timingGenerator;
	FirComplex m_fir;
	uint32_t m_M;
	uint32_t m_N;
	uint32_t m_r;
	uint32_t m_w;
	RMat m_coeff;
};

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

#endif // _INTERPOLATION_POLYPHASE_HPP_