/* LPC_and_Formant.c */
/* David Weenink 20010628 */

#include "LPC_and_Formant.h"
#include "NUM2.h"

int LPC_Frame_into_Formant_Frame (LPC_Frame me, Formant_Frame thee, double samplingFrequency)
{
	int nFound, i, m = my nCoefficients;
	float *f = NULL, *b = NULL;
	float fLow = 50, fHigh = samplingFrequency / 2 - 50; /* bij voldoende hoge Fs */
	fcomplex *ccof = NULL, *zi = NULL;
	fcomplex z10 = fcomplex_create (1.0, 0.0);
	
	if (((f = NUMfvector (1, m)) == NULL) ||
		 ((b = NUMfvector (1, m)) == NULL) ||
		 ((ccof = NUMfcvector (0, m)) == NULL) ||
		 ((zi = NUMfcvector (0, m)) == NULL)) goto end;
	
	ccof[m] = z10; nFound = 0;
	for (i=0; i < m; i++)  ccof[i] = fcomplex_create (my a[m-i], 0.0);
	NUMfindRoots_f (ccof, m, zi);
	
	/*
		Fix roots into unit circle.
	*/
	
	for (i=1; i <= m; i++)
	{
		if (fcomplex_abs (zi[i]) > 1.0) zi[i] = fcomplex_div (z10, fcomplex_conjugate (zi[i]));
	}
	for (i=m; i >= 1; i--) if (zi[i].im >= 0)
	{
		double fmnt = fabs (atan2 (zi[i].im, zi[i].re)) * samplingFrequency / 2 / NUMpi;
		if (fmnt > fLow && fmnt < fHigh)
		{
			nFound++;
			f[nFound] = fmnt;
			b[nFound] = - log (zi[i].re * zi[i].re + zi[i].im * zi[i].im) * samplingFrequency / 2 / NUMpi;
		}
	}
	NUMsort2_ff (nFound, f, b);
	if (nFound > 0)
	{
		int j, n = thy nFormants > nFound ? nFound : thy nFormants;
		for (j=1; j <= n; j++)
		{
			thy formant[j].frequency = f[j];
			thy formant[j].bandwidth = b[j];
		}
	}
	thy intensity = my gain;
	
end:
	NUMfvector_free (f, 1);
	NUMfvector_free (b, 1);
	NUMfcvector_free (ccof, 0);
	NUMfcvector_free (zi, 0);
	return ! Melder_hasError();
}


Formant LPC_to_Formant (LPC me)
{
	Formant thee = NULL; float *f = NULL, *b = NULL;
	double samplingFrequency = 1.0 / my samplingInterval;
	int nmax = my maxnCoefficients;
	long i, interval = nmax > 20 ? 1 : 10;
	
	if (nmax > 99) return Melder_errorp ("LPC_to_Formant: we cannot find the roots "
		"of a polynomial of order > 99.");
		
	if (((f = NUMfvector (1, nmax)) == NULL) ||
		((b = NUMfvector (1, nmax)) == NULL) ||
		((thee = Formant_create (my xmin, my xmax, my nx, my dx,
			my x1, (nmax+1)/2)) == NULL)) goto end;
	
	(void) Melder_progress (0.0, "LPC to Formant");
	for (i=1; i <= my nx; i++)
	{
		Formant_Frame frame = & thy frame[i];
		int j, nFound, n = my frame[i].nCoefficients;
		MGa_to_fb (my frame[i].a, n, samplingFrequency, f, b, & nFound);
		if (nFound > n / 2)
		{	/* this could occur for large prediction orders (say > 30) */
			Melder_casual("LPC_to_Formant: frame %ld, %d formants found, "
				"only %ld saved.", i, nFound, n / 2);
			nFound = n /2;
		}
		if (nFound > 0)
		{
			if ((frame->formant = NUMstructvector (Formant_Formant, 1, nFound)) == NULL) goto end;
			for (j=1; j <= nFound; j++)
			{
				frame->formant[j].frequency = f[j];
				frame->formant[j].bandwidth = b[j];
			}
		}
		frame->nFormants = nFound;
		frame->intensity = my frame[i].gain;
		if ((interval == 1 || (i % interval) == 1) && ! Melder_progress ((double)i / my nx,
			"LPC to Formant: frame %ld out of %ld", i, my nx)) goto end;	
	}
	
end:

	(void) Melder_progress (1.0, NULL);
	NUMfvector_free (f, 1);
	NUMfvector_free (b, 1);
	if (Melder_hasError()) forget (thee);
	return thee;
}

int Formant_Frame_into_LPC_Frame (Formant_Frame me, LPC_Frame thee, double samplingFrequency)
{
	int i, j, m = 0, n = 2 * my nFormants; double *lpc;
	
	if (my nFormants < 1) return 1;
	if ((lpc = NUMdvector (1, n)) == NULL) return 0;
	
	for (i=1; i <= my nFormants; i++)
	{ 	/* D(z): 1 + p z^-1 + q z^-2 */
		double r = exp (- NUMpi * my formant[i].bandwidth / samplingFrequency);
		double p = - 2 * r * cos (2 * NUMpi * my formant[i].frequency / samplingFrequency);
		double q = r * r;
		for (j=m+2; j > 2; j--) lpc[j] += p * lpc[j-1] + q * lpc[j-2];
		lpc[2] += p * lpc[1] + q; lpc[1] += p;
		m += 2;
	}
	n = thy nCoefficients < n ? thy nCoefficients : n;
	for (i=1; i <= n ; i++) thy a[i] = lpc[i];
	thy gain = my intensity;
	NUMdvector_free (lpc, 1);
	return 1;
}

LPC Formant_to_LPC (Formant me, double samplingFrequency)
{
	LPC thee = NULL; long i;
	float *f = NULL, *b = NULL;
	
	if (((f = NUMfvector (1, my maxnFormants)) == NULL) ||
		((b = NUMfvector (1, my maxnFormants)) == NULL) ||
		((thee = LPC_create (my xmin, my xmax, my nx, my dx, my x1,
		2 * my maxnFormants, 1.0 / samplingFrequency)) == NULL)) goto end;
	for (i=1; i <= my nx; i++)
	{
		Formant_Frame frame = & my frame[i];
		int j, m = thy frame[i].nCoefficients = 2 * frame->nFormants;
		if (m != 0 && ((thy frame[i].a = NUMfvector (1, m)) == NULL)) goto end;
		for (j=1; j <= frame->nFormants; j++)
		{
			f[j] = frame->formant[j].frequency; 
			b[j] = frame->formant[j].bandwidth;
		}
		thy frame[i].gain = frame->intensity;
		if (! MGfb_to_a (f, b, frame->nFormants, samplingFrequency, thy frame[i].a)) Melder_warning(
				"Formant_to_LPC: no memory at frame %ld", i);
	}
end:
	NUMfvector_free (f, 1);
	NUMfvector_free (b, 1);
	if (Melder_hasError()) forget (thee);
	return thee;
}

/* End of file LPC_and_Formant.c */
