#ifndef _INTERPOLATION_POLYPHASE_HPP_
#define _INTERPOLATION_POLYPHASE_HPP_

#pragma once

#include <cpp/radio/Vector.hpp>
#include <cpp/radio/FirComplex.hpp>
#include <fir/fir2.h>
#include <memory.h>

namespace Radio
{
namespace Interpolation
{
// --------------------------------------------------------------
// Complex FIR-based discrete step polyphase interpolation filter
// --------------------------------------------------------------
class PolyPhase : public FirComplex
{
public:
	PolyPhase()
	{
		
	}
	
	~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;
	
		FirComplex::setNumTaps(N);
		m_M = M;
		m_N = N;
		m_coeff.resize(N, M);
		m_fifo.resize(N);
		m_fifo = ComplexScalar(0,0);
		
		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 feed(ComplexScalar const &x, uint32_t push)
	{
		m_w = (m_w + push) % m_N;
		m_fifo[m_w] = x;
	}
	
	ComplexScalar process(RealScalar mu, uint32_t pop)
	{
		uint32_t i, r, index;
		int32_t j;

		index = (uint32_t)dmod(dmax(0, mu*m_M), (RealScalar)m_M);
		
		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++];
			}
		}

		return FirComplex::processReal(column(m_coeff, index));
	}	
	
private:
	uint32_t m_M;
	uint32_t m_N;
	uint32_t m_r;
	uint32_t m_w;
	RMat m_coeff;
	CVec m_fifo;
};

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

#endif // _INTERPOLATION_POLYPHASE_HPP_