/* Copyright (C) José Adrián Rodríguez Fonollosa 2004.
 *               Universitat Politècnica de Catalunya, Barcelona, Spain.
 *
 * Permission to copy, use, modify, sell and distribute this software
 * is granted provided this copyright notice appears in all copies.
 * This software is provided "as is" without express or implied
 * warranty, and with no claim as to its suitability for any purpose.
 */
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <vector>
#include <iostream>
#include "pveu.h"
#include "FFTReal.h"
#include "def.h"

using namespace std;

const double PI = 3.14159265358979;
const double DEF_PREEMP = 0.98;

/* Hamming window */
void hamming(float *w, int length)
{
	int i;
	double tmp = PI;

	tmp = (2*tmp)/(length-1);
	for (i=0; i<length; i++) {
		w[i] = (float) (0.54 - 0.46*cos(tmp*i));/* Apply pre-emphasis and window */
	}
}

/* Sinus window */
void sinus(float *w, int lpccep_order)
{
	int i;
	double tmp;

	tmp = PI/lpccep_order;
	for (i=0; i<lpccep_order; i++) {
		w[i] = (float) (1 + 0.5*lpccep_order*sin(tmp*(i+1)));
	}
}

/* Apply pre-emphasis and window */
void pre_emphasis_and_window(const float *x, float *xw, int n, float preemph, const float *w)
{
	int i;
	const float *xm1;

	xm1 = x-1;
	for (i=n-1; i>0; i--)
		xw[i] = (x[i] - preemph*xm1[i])*w[i];
	xw[0] = x[0]*(1-preemph)*w[0];
}

/* From windowed samples to autocorrelation */
void ac(float *x, int frame_len, float *r, int lpc_order)
{
    register int i, j;
    register float tmp;

    for (i=0; i<=lpc_order; i++) {
		tmp = (float) 0;
		for (j=0; j<(frame_len-i); j++)
			 tmp += x[j]*x[j+i];
		r[i] = tmp;
    }
}

/* From autocorrelation to LPC. Levinson algorithm */
void lpc(float *r, float *lpc, int lpc_order, float *v1, float *v2)
{
	int i, j;
	float e;
	float rc;
	float tmp;
	float *a=v1, *a_tmp=v2, *p_tmp;

	a[0] = (float) 1;
	e = r[0];

	for (i=1; i<=lpc_order; i++) {
		tmp = r[i];
		for (j=1; j<i ;j++)
			tmp += a[j]*r[i-j];
		rc = -tmp/e;
		e = e + tmp*rc;
		for (j=1; j<i ;j++)
			a_tmp[j] = a[j] + rc*a[i-j];
		a_tmp[i] = rc;

		/* Swap pointers */
		p_tmp = a;
		a = a_tmp;
		a_tmp = p_tmp;
	}
	/* Copy result */
	for (i=0; i<=lpc_order; i++)
		lpc[i] = a[i];
}

/* From LPC to cepstrum */
void lpc_cepstrum(float *a, int lpc_order, float *c, int lpccep_order)
{
    register int i, j, bound;
    register float tmp;

    for (i=1; i<=lpccep_order; i++) {
		if (i<=lpc_order) {
			bound = i;
			c[i-1] = -a[i];
		}
		else {
			bound = lpc_order+1;
			c[i-1] = (float) 0;
		}
		tmp = (float) 0;
		for (j=1; j<bound; j++)
			tmp += (i-j)*c[i-j-1]*a[j];
		c[i-1] = c[i-1] - (tmp/i);
    }
}

/******************************************************************************/
/* Start of New Functions */
/******************************************************************************/

/* Filter Bank Creation */
void computeHmBank(float *bFrecs, int winpts, int nbands, float *wts, int eMe) 
{
// Output float wts[][] -> The filter bank
//function [wts] = computeHmBank(bFrecs,winpts)
// Spoken Language Processing, PH PTR
// Eq 6.140
	register int k,m;
	for (m = 1; m<(eMe-1); m++) {
		for (k = 0; k<winpts; k++) {
		        if (k < bFrecs[m - 1]) {
		        	wts[(winpts*(m-1))+k] = 0;
		        	}
		        else if (k <= bFrecs[m]) {
				 wts[(winpts*(m-1))+k] = (2 * (k - bFrecs[m-1])) / ((bFrecs[m+1]-bFrecs[m-1]) * (bFrecs[m]-bFrecs[m-1]));
				}
			else if (k <= bFrecs[m+1]) {
				wts[(winpts*(m-1))+k] = (2 * (bFrecs[m+1]-k)) / ((bFrecs[m+1]-bFrecs[m-1]) * (bFrecs[m+1]-bFrecs[m]));
				}
			}
		}
}

/******************************************************************************/
/* Conversion from Frequency to Mel Scale */
float B_MelScale(float frec)
{
	//// Spoken Language Processing, PH PTR
	//// Eq 2.6
	float bM = 1125 * log(1 + (frec/700));
	return bM;
}

/******************************************************************************/
/* Conversion from Mel Scale to Frequency */
float invB_MelScale(float bM)
{
	//// Spoken Language Processing, PH PTR
	//// Eq 6.144
	float frec = 700 * (exp(bM/1125)-1);
	return frec;
}

/******************************************************************************/
/* Compute the boundaries of the filters */
void computeBoundaries(int eMe, float fLow, float fHigh, int nFFT, int sFrec, float *bFrecs, int sizebFrecs)
{
	// Parameters :
	// eMe - Number of filters
	// fLow - Lower frecuency bound
	// fHigh - Higher frecuency bound
	// nFFT - Number of FFT points
	// sFrec - Sampling frecuency in Hz
	// bFrecs - Output array containing the boundaries of the filters
	// Spoken Language Processing, PH PTR
	// Eq 6.142
	register int m, i;
	
	//cout << "eme: " << eMe << " nFFT: " << nFFT << " sFrec: " << sFrec << "\n";
	
	for (m = 0; m<=(eMe+1); m++) {
		bFrecs[m] = invB_MelScale(B_MelScale(fLow) + (m *((B_MelScale(fHigh) - B_MelScale(fLow)) / (eMe + 1))));
		}
	for (i=0; i < sizebFrecs ; i++) {
		bFrecs[i] = bFrecs[i] * (((float)nFFT)/((float)sFrec));
		}
}

/******************************************************************************/
/* Frecuency Filtering Spectrum */

void frequencyFiltering(float *aspectrum, float *FFSpectra, int size)
{
	register int i;
	float *specChunk= (float *)calloc(size+2, sizeof(float));
	memcpy(specChunk+1, aspectrum, size*sizeof(float));
	for (i = 0; i<size; i++) {
		FFSpectra[i] = specChunk[(i+1)+1] - specChunk[(i+1)-1];
		}
		//cout <<"e\n";
	//printArray(aspectrum,size);
	//printArray(FFSpectra,size);
}

void printArraynl(float *array_in, int size)
{
	printArray(array_in, size);
		cout << "\n";
}

void printArraySemicolon(float *array_in, int size)
{
	printArray(array_in, size);
		cout << ";";
}

void printArray(float *array_in, int size)
{
	for (int i=0;i<size;i++) {
		cout << array_in[i];
		if (i != (size-1)) cout << ", ";
		}
}


void mprint(float array_in[], int rows, int cols)
{
	cout << rows << '\040' << cols << '\n';
	
	
	for (int i=0;i<rows;i++){
      cout << "row > (" << i << ") " ; 
	for(int j=0;j<cols;j++){
		cout << array_in[(cols*(i))+j] << ", ";
		//array_in[(size1*(i))+j]*=2;
	}
	cout << '\n';
	}
}

void mprint2(float array_in[], int rows, int cols)
{
	for (int i=0;i<rows;i++){
		for(int j=0;j<cols;j++){
			cout << array_in[(cols*(i))+j] << ", ";
		//array_in[(size1*(i))+j]*=2;
			}
		cout << '\n';
		}
}

/* End of New Functions */
/******************************************************************************/

// All the size and param. declarations are in ../include/def.h
struct PRMREG {
	PRMTYPE type; // Defines the type of algorithm, LPCC, MFCC, FF (init argument)
	float preemp; // Pre-emphasis coefficient (init argument)
	int frame_len; // Length of each frame/window (240 def, init arg)
	int param_len; // Number of parameters (12 def, init arg)
	int order;
	float *xw; // Windowed Signal
	float *ac; // Autocorrelation of the Signal
	float *a;
	float *v1;
	float *v2;
	float *wframe;
	float *wcep;
};


//Allocation: 	prmreg = prm_alloc(MFCC, PREEMP, DEF_N, DEF_P);
PRMREG* prm_alloc(PRMTYPE type, float preemp, int frame_len, int param_len)
{
	PRMREG* prm_reg = (PRMREG*) calloc(1, sizeof(PRMREG));
	memset(prm_reg, 0, sizeof(PRMREG));

	prm_reg->type = type;
	prm_reg->preemp = preemp;
	prm_reg->frame_len = frame_len;
	prm_reg->param_len = param_len;
	prm_reg->order = (2*param_len)/3;

	prm_reg->xw = (float*) calloc(prm_reg->frame_len, sizeof(float));
	prm_reg->ac = (float*) calloc(prm_reg->order+1, sizeof(float));
	prm_reg->a  = (float*) calloc(prm_reg->order+1, sizeof(float));
	prm_reg->v1 = (float*) calloc(prm_reg->order+1, sizeof(float));
	prm_reg->v2 = (float*) calloc(prm_reg->order+1, sizeof(float));

	prm_reg->wframe = (float*) calloc(prm_reg->frame_len, sizeof(float));
	hamming(prm_reg->wframe, prm_reg->frame_len);

	prm_reg->wcep = (float*) calloc(prm_reg->param_len, sizeof(float));
	sinus(prm_reg->wcep, prm_reg->param_len);

	return prm_reg;
}

int prm_prm(PRMREG* prm_reg, const float *x, float *c)
{
	int i;
	pre_emphasis_and_window(x, prm_reg->xw, prm_reg->frame_len, prm_reg->preemp, prm_reg->wframe);
	
	// Check comment in LPCC case
	ac(prm_reg->xw, prm_reg->frame_len, prm_reg->ac, prm_reg->order);
	
	switch (prm_reg->type) {
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
	case LPCC:
		{
		// ac was originally here,but we moved it up so we can use the results in MFCC/FF	
		
		/* Get cepstrum */
		if (prm_reg->ac[0]>0) {
			lpc(prm_reg->ac, prm_reg->a, prm_reg->order, prm_reg->v1, prm_reg->v2);
			lpc_cepstrum(prm_reg->a, prm_reg->order, c, prm_reg->param_len);
			for (i=0; i<prm_reg->param_len; i++)
				c[i] *= prm_reg->wcep[i];
		}
		else {
			for (i=0; i<prm_reg->param_len; i++)
				c[i] = 0;
		}
		break;
		}
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
	case MFCC:
		{
		/// Extra constants
		const int numcep =  DEF_P; // Number of cepstral components
		const float sr = DEF_FREQ; // Sampling rate, imported from def.h
		const float wintime = DEF_M/DEF_FREQ; // Window time in seconds
		const int winpts = DEF_M;
//		cout<<"\n\n\nTHIS IS WINTIME: " << wintime << "\n\n";
		const float lifterexp = .9; // lifting exponent
		const int sumpower = 0; // fft to Mel using 0 = abs or 0 = abs^2
		const int dither = 1; // Add something to spectrum to avoid digital cero
		const int ditherDC = DEF_M;
		const int minfreq = 0; // Minimum frecuency
		const int maxfreq = 4000; // Maximum frecuency
		const int nbands = 22; // Number of bands or filters to the Mel frecuencies
		int NOVERLAP, nfreqs;
		long NFFT;
		register int i, j; // counters
		
		/***************************************************
		*********** Compute FFT power spectrum*************
		***************************************************/
		// Number of fft points to nearest high power of 2
		NFFT = (long) pow(2, (ceil(log(winpts)/log(2))));
		NFFT = (long) ceil(float(NFFT/2));

		/***************************************************
		******************** FFT ***************************
		***************************************************/
		const FFTReal fft_obj(NFFT); // Creating FFTReal Object with NFFT points
		int nbins = (int)((float)NFFT/2);
		float Fourier [NFFT];
		float pspectrum [nbins];
		
		fft_obj.do_fft(Fourier, prm_reg->xw); // FFT
		pspectrum [0] = sqrt(pow(Fourier[0],2));
		for (i=1; i < nbins ; i++) {
			pspectrum [i] = sqrt(pow(Fourier[i],2) + pow(Fourier	[nbins + i],2));
			}
			
		nfreqs = sizeof pspectrum / sizeof *pspectrum;
		// Dither coeff, incresing the FFT values (scaling)
		if (dither)
		{
			for (i=0; i < nfreqs ; i++)
				pspectrum[i] = pspectrum[i] + ditherDC; // use to be winpts
		}
		
		/***************************************************
		************ Matrix to convert fft to mel **************
		***************************************************/
		float bFrecs [nbands+2]; // Move up to declarations
		computeBoundaries(nbands, minfreq, maxfreq, NFFT, (int) sr, bFrecs,(sizeof bFrecs / sizeof * bFrecs));
		float wts [(nbands+4)*winpts];
		computeHmBank(bFrecs, winpts, nbands,wts, (sizeof bFrecs / sizeof * bFrecs));

		/********************************************************************
		******* Integrate FFT bins into Mel bins, in abs or abs^2 domains ********
		********************************************************************/
		float aspectrum [nbands]; // Move up to declarations
		memset(aspectrum, 0, nbands * 4); /// Ojo ...	
		int offset = winpts - nfreqs;
		if (sumpower) {
			for (i=0; i<(nbands); i++) {
				for (j=0; j <(nfreqs) ; j++) {
						aspectrum[i] += wts[(nfreqs)*i + j +(i*offset)]*pspectrum[j];
			//if (aspectrum[i] < 0) cout << "aspec[ i=" << i << "] " << aspectrum[i] << " = {"<< (nfreqs)*i + j +(i*offset) <<"}(  " << wts[(nfreqs)*i + j +(i*offset)] << " ) *  {j="<< j <<"}" << pspectrum[j] << "\n";
					}
				//if (aspectrum[i] <= 0) cin >> o;
				//cout <<aspectrum[i] << "\n";
				}
			}
		else {
			for (i=0; i<(nbands); i++) {
				for (j=0; j < (nfreqs) ; j++) {
					aspectrum[i] += wts[(nfreqs)*i + j +(i*offset)]* sqrt(pspectrum[j]);
					}
				aspectrum[i] = aspectrum[i] * aspectrum[i];
				}
			}

		/********************************************************************
		******************** Convert to cepstra via DCT  **********************
		********************************************************************/
		float dctm [numcep][nbands];
		for (i = 0; i<numcep; i++) {
			//tmp = sqrt(2/nbands);
			//tmp2 = (i / ((2*nrow)*pi));
			//tmp3 = [1:2:(2*nbands-1)];
			//tmp4 = tmp2 * tmp3;
			for (j=0; j<nbands; j++) {
				dctm[i][j] = (float)cos((float)(2*(float)j +1)  * (i / ((2*(float)nbands)*(float)PI))) * (float)sqrt(2/(float)nbands);
				if (i==0) { dctm[i][j] = dctm[i][j] / sqrt(2);} // Making it unitary
				}
			}
		float cepstra [numcep]; // Move it to declarations	
		//mprint((float*)dctm,numcep,nbands);
		memset(cepstra, 0, numcep * 4); /// Ojo ...
		//printArray(cepstra, numcep);
		//cepstra = dctm*log(aspectrum);
		for (i=0; i<(numcep); i++) {
			for (j=0; j < (nbands) ; j++) {				
				//cout << "cepstra[i] += dctm *log(aspectrum);" << "cep: " << cepstra[i] << " dctm: " << dctm[i][j] << " log(aspectrum[j]): " << log(aspectrum[j]) << " aspect: " << aspectrum[j] << "\n";
				// Crappy patch:
				//if (aspectrum[j]<0) aspectrum[j] = aspectrum[j] * (-1);
				cepstra[i] += dctm[i][j]*log(aspectrum[j]); // cepstra is of size numcep
				
				}
				// cout << "\nCepstra: " << cepstra[i];			
			}

		/********************************************************************
		************************* Lifting Coeficients **************************
		********************************************************************/
		float liftwts [numcep]; // Allocating the array of size numcep
		liftwts[0] = 1;
		for (i=1; i <= (numcep -1); i++)
			liftwts[i] = (float)pow(i,lifterexp);
			
		//printArray(liftwts,numcep);	
		//liftwts = [1, ([1:(numcep-1)].^lifterexp)];
		//cepstra = diag(liftwts)*cepstra;	
			
		for (i=0; i<numcep; i++) { // rows = size of liftwts = numcep
			c[i] = liftwts[i]*cepstra[i];
			//c[i] = cepstra[i];
			//cout << "\n cepstra: " << cepstra[i] << ", Output: " << c[i];			
			}
				
	break;
	}/* End of MFCC */
	
/****************************************************************************************************************************************/
/****************************************************************************************************************************************/
	case FF:
	{
		/// Extra constants
		const int numcep =  DEF_P; // Number of cepstral components
		const int nbands = DEF_P; // Number of bands, equal to the num of coeff for FF
		const float sr = DEF_FREQ; // Sampling rate, imported from def.h
		const float wintime = DEF_M/DEF_FREQ; // Window time in seconds
		const int winpts = DEF_M;
		const int ditherDC = DEF_M;
		const float lifterexp = 0.6; // lifting exponent
		const int sumpower = 1; // fft to Mel using abs or abs^2
		const int dither = 1; // Add something to spectrum to avoid digital cero
		const int minfreq = 0; // Minimum frecuency
		const int maxfreq = 4000; // Maximum frecuency
		// Extra declarations
		int NOVERLAP, nfreqs;
		register int i, j; // counters
		long NFFT;
		
		/***************************************************
		*********** Compute FFT power spectrum*************
		***************************************************/
		// Number of fft points to nearest high power of 2
		NFFT = (long) pow(2, (ceil(log(winpts)/log(2))));
		NFFT = (long) ceil(float(NFFT/2));

		/***************************************************
		******************** FFT ***************************
		***************************************************/
		const FFTReal fft_obj(NFFT); // Creating FFTReal Object with NFFT points
		int nbins = (int)((float)NFFT/2);
		float Fourier [NFFT];
		float pspectrum [nbins];
		fft_obj.do_fft(Fourier, prm_reg->xw); // FFT
		pspectrum [0] = sqrt(pow(Fourier[0],2));
		for (i=1; i < nbins ; i++) {
			pspectrum [i] = sqrt(pow(Fourier[i],2) + pow(Fourier	[nbins + i],2));
			}
		nfreqs = sizeof pspectrum / sizeof *pspectrum;
		// Dither coeff, incresing the FFT values (scaling)
		if (dither)
		{
			for (i=0; i < nfreqs ; i++)
				pspectrum[i] = pspectrum[i] + ditherDC; // use to be winpts
		}
		
		/***************************************************
		************ Matrix to convert fft to mel **************
		***************************************************/
		float bFrecs [nbands+2]; // Move up to declarations
		computeBoundaries(nbands, minfreq, maxfreq, NFFT, (int) sr, bFrecs,(sizeof bFrecs / sizeof * bFrecs));
		float wts [(nbands+4)*winpts];
		computeHmBank(bFrecs, winpts, nbands,wts, (sizeof bFrecs / sizeof * bFrecs));

		/********************************************************************
		******* Integrate FFT bins into Mel bins, in abs or abs^2 domains ********
		********************************************************************/
		float aspectrum [nbands]; // Move up to declarations
		memset(aspectrum, 0, nbands * 4); /// Ojo ...	
		int offset = winpts - nfreqs;
		if (sumpower) {
			for (i=0; i<(nbands); i++) {
				for (j=0; j <(nfreqs) ; j++) {
						aspectrum[i] += wts[(nfreqs)*i + j +(i*offset)]*pspectrum[j];
					}
				}
			}
		else {
			for (i=0; i<(nbands); i++) {
				for (j=0; j < (nfreqs) ; j++) {
					aspectrum[i] += wts[(nfreqs)*i + j +(i*offset)]* sqrt(pspectrum[j]);
					}
				aspectrum[i] = aspectrum[i] * aspectrum[i];
				}
			}
		
		/********************************************************************
		******************** Convert to FF coeff ******************************
		********************************************************************/
		float FFspectra [nbands]; // Move it to declarations
		frequencyFiltering(aspectrum, FFspectra, nbands); // FFspectra is of size nbands
		
		/********************************************************************
		************************* Lifting Coeficients **************************
		********************************************************************/
		float liftwts [numcep]; // Allocating the array of size numcep
		liftwts[0] = 1;
		for (i=1; i <= (numcep -1); i++) {
			liftwts[i] = (float)pow(i,lifterexp);
			}
		for (i=0; i<numcep; i++) { // rows = size of liftwts = numcep
			c[i] = liftwts[i]*FFspectra[i];			
			}
	break;
	} /* End of FF */
	default:
		return -1; /* Not implemented */
	}
	
	return 0;
}

void prm_free(PRMREG *prm_reg)
{
	free(prm_reg->xw);
	free(prm_reg->ac);
	free(prm_reg->a);
	free(prm_reg->v1);
	free(prm_reg->v2);
	free(prm_reg->wframe);
	free(prm_reg->wcep);
	free(prm_reg);
}