/*************************************************************************/
/* fir.c
/*************************************************************************/
#include <stdio.h>
#include <malloc.h>
#include <memory.h>
#include "math.h"
#include "fir.h"
#include "firinterp.h"
#include "firdecim.h"
#include "resample.h"

/*************************************************************************/
/* Global Variables
/*************************************************************************/
/* Berechnet den ggT rekursiv mit Euklid's Algorithmus */
int GGT(int a, int b)
{
	int r;
	r = a % b;
	if (r == 0)
		return b;
	else
		return GGT(b, r);       /* Rekursion */
}

/***************************************************************/
/* Primzahlenzerlegung */
int GetStages(int n, int *pBuf)
{
	int z;
	int c;

	z = 2;
	c = 0;
	while (z <= n)
	{
		if ((n % z) == 0)
		{
			pBuf[c] = z;
			c++;
			n = n / z;
		}
		else
			z = z + 1;
	}
	return c;
}

int GetNumStages(int n)
{
	int z;
	int c;

	z = 2;
	c = 0;
	while (z <= n)
	{
		if ((n % z) == 0)
		{
			c++;
			n = n / z;
		}
		else
			z = z + 1;
	}
	return c;
}

/***************************************************************/
int ReduceStages(int *pBuf, int nMaxStages, int len)
{
	int i;

	if ((nMaxStages < len) && (len > 1))
	{
		pBuf[0] *= pBuf[1];

		for (i=1; i < len; i++)
			pBuf[i] = pBuf[i+1];

		len--;
		jsort(pBuf, len);

		return ReduceStages(pBuf, nMaxStages, len);
	}
	return len;
}

/***************************************************************/
void jsort(int *pBuf, int len)
{
	int i, temp, unsorted;

	do
	{
		unsorted = 0;
		for (i=0; i < len-1; i++)
		{
			if (pBuf[i] > pBuf[i+1])
			{
				temp = pBuf[i];
				pBuf[i] = pBuf[i+1];
				pBuf[i+1] = temp;
				unsorted = 1;
			}
		}
	} while(unsorted);
}

/***************************************************************/
void RSinit(struct _sRESAMPLER *pObj, int nCoef, float fs1, float fs2, int nMaxStagesL, int nMaxStagesM, int blockSize)
{
	int ggt, s;

	pObj->pFirL = 0;
	pObj->pFirM = 0;

	pObj->fs1 = fs1;
	pObj->fs2 = fs2;

	printf("Initial fs  = %g Hz\n",pObj->fs1);
	printf("Target  fs  = %g Hz\n",pObj->fs2);

	ggt = GGT((int)fs1,(int)fs2);
	printf("GGT         = %d\n",ggt);

	pObj->L = (int)fs2/ggt;
	pObj->M = (int)fs1/ggt;

	printf("Up factor   = %d\n",pObj->L);
	printf("Down factor = %d\n",pObj->M);

	pObj->nStagesL = GetNumStages(pObj->L);
	pObj->nStagesM = GetNumStages(pObj->M);

	pObj->pStagesL = (int*)malloc((pObj->nStagesL+1)*sizeof(int));
	pObj->pStagesM = (int*)malloc((pObj->nStagesM+1)*sizeof(int));

	memset(pObj->pStagesL, 0, (pObj->nStagesL+1)*sizeof(int));
	memset(pObj->pStagesM, 0, (pObj->nStagesM+1)*sizeof(int));

	GetStages(pObj->L, pObj->pStagesL);
	GetStages(pObj->M, pObj->pStagesM);

	printf("N up stages [%d]   = ",pObj->nStagesL);
	for (s=0; s < pObj->nStagesL; s++)
		printf("%d ",pObj->pStagesL[s]);
	printf("\n");

	printf("N down stages [%d] = ",pObj->nStagesM);
	for (s=0; s < pObj->nStagesM; s++)
		printf("%d ",pObj->pStagesM[s]);
	printf("\n");

	if (nMaxStagesL > 0)
	{
		pObj->nStagesL = ReduceStages(pObj->pStagesL, nMaxStagesL, pObj->nStagesL);
		printf("N up stages required [%d]   = ",pObj->nStagesL);
		for (s=0; s < pObj->nStagesL; s++)
			printf("%d ",pObj->pStagesL[s]);
		printf("\n");
	}
	if (nMaxStagesM > 0)
	{
		pObj->nStagesM = ReduceStages(pObj->pStagesM, nMaxStagesM, pObj->nStagesM);
		printf("N down stages required [%d] = ",pObj->nStagesM);
		for (s=0; s < pObj->nStagesM; s++)
			printf("%d ",pObj->pStagesM[s]);
		printf("\n");
	}

	pObj->pTemp = (float**)malloc(2*sizeof(float*));
	pObj->pTemp[0] = (float*)malloc(pObj->L*blockSize*sizeof(float));
	pObj->pTemp[1] = (float*)malloc(pObj->L*blockSize*sizeof(float));

	if (pObj->nStagesL > 0)
		pObj->pFirL = (FIRIP*)malloc(pObj->nStagesL*sizeof(FIRIP));

	if (pObj->nStagesM > 0)
		pObj->pFirM = (FIRDECIM*)malloc(pObj->nStagesM*sizeof(FIRDECIM));

	for (s=0; s < pObj->nStagesL; s++)
	{
		if (pObj->pStagesL[s] > 1)
			FIinit(&pObj->pFirL[s], pObj->pStagesL[s], nCoef);
	}

	for (s=0; s < pObj->nStagesM; s++)
	{
		if (pObj->pStagesM[s] > 1)
			FDinit(&pObj->pFirM[s], pObj->pStagesM[s], nCoef);
	}
	printf("\n");

	pObj->currPhase = pObj->L;
	pObj->nTapsPerPhase = nCoef;
	pObj->sizeH = pObj->nTapsPerPhase*pObj->L;
	pObj->pH = (float*)malloc(pObj->sizeH*sizeof(float));
	pObj->pZ = (float*)malloc(pObj->sizeH*sizeof(float));
	Sinc(pObj->pH, pObj->L, 1.f, 1.f/pObj->L, pObj->sizeH);
	memset(pObj->pZ, 0, pObj->sizeH*sizeof(float));

}

void RSfree(struct _sRESAMPLER *pObj)
{
	int s;

	for (s=0; s < pObj->nStagesL; s++)
		FIfree(&pObj->pFirL[s]);

	for (s=0; s < pObj->nStagesM; s++)
		FDfree(&pObj->pFirM[s]);

	if (pObj->pFirL)
		free(pObj->pFirL);
	
	if (pObj->pFirM)
		free(pObj->pFirM);

	free(pObj->pStagesL);
	free(pObj->pStagesM);
	free(pObj->pTemp[0]);
	free(pObj->pTemp[1]);

}

int RSresample(struct _sRESAMPLER *pObj, float *pIn, float *pOut, int len)
{
	int s, id1, id2, t;

	id1 = 0;
	id2 = 1;
	if (pObj->L > 1)
	{
		len = FIfilter(&pObj->pFirL[0], pIn, pObj->pTemp[id1], len);
		for (s=1; s < pObj->nStagesL; s++)
		{
			len = FIfilter(&pObj->pFirL[s], pObj->pTemp[id1], pObj->pTemp[id2], len);
			t = id1;
			id1 = id2;
			id2 = t;
		}
	}
	else
	{
		memcpy(pObj->pTemp[id1], pIn, len*sizeof(float));
	}
	
	if (pObj->M > 1)
	{
		for (s=0; s < pObj->nStagesM-1; s++)
		{
			len = FDfilter(&pObj->pFirM[s], pObj->pTemp[id1], pObj->pTemp[id2], len);
			t = id1;
			id1 = id2;
			id2 = t;
		}
		len = FDfilter(&pObj->pFirM[pObj->nStagesM-1], pObj->pTemp[id1],  pOut, len);
	}
	else
	{
		memcpy(pOut, pObj->pTemp[id1], len*sizeof(float));
	}

	return len;

}

int RSresample2(struct _sRESAMPLER *pObj, float *pIn, float *pOut, int len)
{
//void resamp0(int interp_factor_L, int decim_factor_M, int num_taps_per_phase,
//             int *p_current_phase, const double *const p_H,
//             double *const p_Z, int num_inp, const double *p_inp,
//             double *p_out, int *p_num_out)

    int tap, num_out, phase_num = pObj->currPhase;
    const float *p_coeff;
	float *p_Z = pObj->pZ;
	float *p_H = pObj->pH;
	int interp_factor_L = pObj->L;
	int decim_factor_M = pObj->M;
	int num_taps_per_phase = pObj->nTapsPerPhase;
	int num_inp = len;

    float sum;

    num_out = 0;
    while (num_inp > 0) 
	{
        /* shift input samples into Z delay line */
        while (phase_num >= interp_factor_L) 
		{
            /* decrease phase number by interpolation factor L */
            phase_num -= interp_factor_L;

            /* shift Z delay line up to make room for next sample */
            for (tap = num_taps_per_phase - 1; tap >= 1; tap--)
			{
                p_Z[tap] = p_Z[tap - 1];
            }

            /* copy next sample from input buffer to bottom of Z delay line */
            p_Z[0] = *pIn++;

            if (--num_inp == 0) 
			{
                break;
            }
        }

        /* calculate outputs */
        while (phase_num < interp_factor_L)
		{
            /* point to the current polyphase filter */
            p_coeff = p_H + phase_num;

            /* calculate FIR sum */
            sum = 0.0;
            for (tap = 0; tap < num_taps_per_phase; tap++)
			{
                sum += *p_coeff * p_Z[tap];
                p_coeff += interp_factor_L;  /* point to next coefficient */
            }
            *pOut++ = sum;     /* store sum and point to next output */
            num_out++;

            /* increase phase number by decimation factor M */
            phase_num += decim_factor_M;
        }
    }

    /* pass phase number and number of outputs back to caller */
    pObj->currPhase = phase_num;
    return num_out;

}

int RSresample3(struct _sRESAMPLER *pObj, float *pIn, float *pOut, int len)
{
    int tap, num_out, num_new_samples, phase_num = pObj->currPhase;
    const float *p_coeff;
	float *p_Z = pObj->pZ;
	float *p_H = pObj->pH;
	int interp_factor_L = pObj->L;
	int decim_factor_M = pObj->M;
	int num_taps_per_phase = pObj->nTapsPerPhase;
	int num_inp = len;
	
    float sum;

    num_out = 0;
    while (num_inp > 0)
	{

        /* figure out how many new samples to shift into Z delay line */
        num_new_samples = 0;
        while (phase_num >= interp_factor_L)
		{
            /* decrease phase number by interpolation factor L */
            phase_num -= interp_factor_L;
            num_new_samples++;
            if (--num_inp == 0) 
			{
                break;
            }
        }

        if (num_new_samples >= num_taps_per_phase) 
		{
            /* the new samples are bigger than the size of Z:
               fill the entire Z with the tail of new inputs */
            pIn += (num_new_samples - num_taps_per_phase);
            num_new_samples = num_taps_per_phase;
        }

        /* copy new samples into Z */

        /* shift Z delay line up to make room for next samples */
        for (tap = num_taps_per_phase - 1; tap >= num_new_samples; tap--) 
		{
            p_Z[tap] = p_Z[tap - num_new_samples];
        }

        /* copy next samples from input buffer to bottom of Z */
        for (tap = num_new_samples - 1; tap >= 0; tap--)
		{
            p_Z[tap] = *pIn++;
        }

        /* calculate outputs */
        while (phase_num < interp_factor_L)
		{
            /* point to the current polyphase filter */
            p_coeff = p_H + phase_num;

            /* calculate FIR sum */
            sum = 0.0;
            for (tap = 0; tap < num_taps_per_phase; tap++) 
			{
                sum += *p_coeff * p_Z[tap];
                p_coeff += interp_factor_L;   /* point to next coefficient */
            }
            *pOut++ = sum;     /* store sum and point to next output */
            num_out++;

            /* decrease phase number by decimation factor M */
            phase_num += decim_factor_M;
        }
    }

    /* pass back to caller phase number (for next call) and number of
       outputs */
    pObj->currPhase = phase_num;
    return num_out;
}