/* 
 * File:   Interpolation.hpp
 * Author: jens
 *
 * Created on 20. Dezember 2014, 19:02
 */

#ifndef _RADIO_INTERPOLATION_HPP_
#define	_RADIO_INTERPOLATION_HPP_

#include <cstdio>
#include "Vector.hpp"
#include "FirComplex.hpp"

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)
	{
		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.setZero();
	}

	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;
		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++];
			}
		}

//		Eigen::internal::set_is_malloc_allowed(true);
		// Partial filter responses
		for (i=0; i < m_M; i++)
		{
			m_h[i] = FirComplex::processReal(m_coeff.col(i));
		}
//		Eigen::internal::set_is_malloc_allowed(false);

		// 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);

	}
	
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;
	
	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];
	}
	
};

// --------------------------------------------------------------
// 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.setZero();
		
		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(m_coeff.col(index));
	}	
	
private:
	uint32_t m_M;
	uint32_t m_N;
	uint32_t m_r;
	uint32_t m_w;
	RMat m_coeff;
	CVec m_fifo;
};

class DownSampler
{
public:
	DownSampler()
	: m_pFir(nullptr)
	, m_pCoeff(nullptr)
	, m_M(0)
	, m_currStage(0)
	{
		
	}
	~DownSampler()
	{
		if (m_pFir)
			delete [] m_pFir;
			
		if (m_pCoeff)
			delete [] m_pCoeff;
		
	}
	void init(uint32_t M, RealScalar const *coeff, uint32_t N)
	{
		uint32_t i, j;
		uint32_t stage_N;
		uint32_t stage;
		
		if (m_pFir)
			delete [] m_pFir;
			
		if (m_pCoeff)
			delete [] m_pCoeff;

		m_pFir = new FirComplex[M];
		m_pCoeff = new RVec[M];
		
		for (stage=0; stage < M; stage++)
		{
			stage_N = 0;
			for (i=stage; i < N; i += M)
			{
				stage_N++;
			}
			m_pFir[stage].setNumTaps(stage_N);
			m_pCoeff[stage].resize(stage_N);
			
			j=0;
			for (i=stage; i < N; i += M)
			{
				if (coeff)
				{
					m_pCoeff[stage](j++) = coeff[i];
				}
			}
		}
		m_M = M;
		m_currStage = m_M-1;

	}
	
	uint32_t process(CVec &dst, CVec const &src, uint32_t len)
	{
		uint32_t i;
		uint32_t j;
		
		j = 0;
		for (i=0; i < len; i++)
		{
			m_pFir[m_currStage].feed(src(i));
			if (m_currStage == (m_M-1))
			{
				dst(j) = m_pFir[m_currStage].processReal(m_pCoeff[m_currStage]);
			}
			else
			{
				dst(j) += m_pFir[m_currStage].processReal(m_pCoeff[m_currStage]);
			}
			if (--m_currStage >= m_M)
			{
				m_currStage = m_M-1;
				j++;
			}
			
		}
		return j;
	}
	
private:
	FirComplex *m_pFir;
	RVec *m_pCoeff;
	uint32_t m_M;
	uint32_t m_currStage;
	
};

class UpSampler
{
public:
	UpSampler()
	: m_pFir(nullptr)
	, m_pCoeff(nullptr)
	, m_L(0)
	{
		
	}
	~UpSampler()
	{
		if (m_pFir)
			delete [] m_pFir;
			
		if (m_pCoeff)
			delete [] m_pCoeff;
		
	}
	void init(uint32_t L, RealScalar const *coeff, uint32_t N)
	{
		uint32_t i, j;
		uint32_t stage_N;
		uint32_t stage;
	
		if (m_pFir)
			delete [] m_pFir;

		if (m_pCoeff)
			delete [] m_pCoeff;

		m_pFir = new FirComplex[L];
		m_pCoeff = new RVec[L];
		
		for (stage=0; stage < L; stage++)
		{
			stage_N = 0;
			for (i=L-stage-1; i < N; i += L)
			{
				stage_N++;
			}
			m_pFir[stage].setNumTaps(stage_N);
			m_pCoeff[stage].resize(stage_N);
			
			j=0;
			for (i=L-stage-1; i < N; i += L)
			{
				if (coeff)
				{
					m_pCoeff[stage](j++) = coeff[i];
				}
			}
		}
		m_L = L;
	}

	uint32_t process(CVec &dst, CVec const &src, uint32_t len)
	{
		uint32_t i;
		uint32_t j;
		uint32_t currStage;
		
		j = 0;
		for (i=0; i < len; i++)
		{
			for (currStage=0; currStage < m_L; currStage++)
			{
				m_pFir[currStage].feed(src(i));
				dst(m_L-1-currStage+j) = m_pFir[currStage].processReal(m_pCoeff[currStage]);
			}
			j += m_L;
		}
		return j;
	}

private:
	FirComplex *m_pFir;
	RVec *m_pCoeff;
	uint32_t m_L;
	
};

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

#endif	/* _RADIO_INTERPOLATION_HPP_ */