/* LPC_and_Formant.c
 *
 * Copyright (C) 1994-2003 David Weenink
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/*
 djmw 20030410 Latest modification
*/

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


int Roots_into_Formant_Frame (Roots me, Formant_Frame thee, double samplingFrequency, double margin)
{
	double f, b, fLow = margin, fHigh = samplingFrequency / 2 - margin;
	float *fc = NULL, *bc = NULL;
	long i, n = my max - my min + 1;
	
	fc = NUMfvector (1, n);
	if (fc == NULL) return 0;
	bc = NUMfvector (1, n);
	if (bc == NULL) goto end;
	
	/*
		Determine the formants and bandwidths
	*/
	 
    thy nFormants = 0;
    for (i = my min; i <= my max; i++)
    {
		if (my v[i].im < 0) continue;
		f = fabs (atan2 (my v[i].im, my v[i].re)) * samplingFrequency / 2 / NUMpi;
        if (f >= fLow && f <= fHigh)
        {
			/*b = - log (my v[i].re * my v[i].re + my v[i].im * my v[i].im) * samplingFrequency / 2 / NUMpi;*/
			b = - log (dcomplex_abs (my v[i])) * samplingFrequency / NUMpi;
			thy nFormants++;
			fc[thy nFormants] = f;
			bc[thy nFormants] = b;
		}
	}
	
    thy formant = NUMstructvector (Formant_Formant, 1, thy nFormants);
    if (thy formant == NULL) goto end;
	
	for (i = 1; i <= thy nFormants; i++)
	{
		thy formant[i].frequency = fc[i];
		thy formant[i].bandwidth = bc[i];	
	}
	
end:

	NUMfvector_free (fc, 1); NUMfvector_free (bc, 1);
	return ! Melder_hasError ();   
}

int LPC_Frame_into_Formant_Frame (LPC_Frame me, Formant_Frame thee, 
	double samplingFrequency, double margin)
{
	Polynomial p;
	Roots r = NULL;
	long status = 0;
	
	thy intensity = my gain;

	if (my nCoefficients == 0) return 1;
	
    p = LPC_Frame_to_Polynomial (me);
	if (p == NULL) return 0;
	
    r = Polynomial_to_Roots (p);
    if (r == NULL) goto end;
    
    Roots_fixIntoUnitCircle (r);
    
    status = Roots_into_Formant_Frame (r, thee, samplingFrequency, margin);

end:

	forget (r); forget (p);

	return status;
}

Formant LPC_to_Formant (LPC me, double margin)
{
	char *proc = "LPC_to_Formant";
	Formant thee = NULL;
	double samplingFrequency = 1.0 / my samplingInterval;
	int nmax = my maxnCoefficients;
	long err = 0, i, interval = nmax > 20 ? 1 : 10;

	if (nmax > 99) return Melder_errorp ("%s: We cannot find the roots of a polynomial "
		"of order > 99.", proc);
	if (margin >= samplingFrequency/4) return Melder_errorp ("%s: Margin must be smaller than %s",
		proc, Melder_double (samplingFrequency/4));
	 
	thee = Formant_create (my xmin, my xmax, my nx, my dx, my x1, (nmax+1)/2);	
	if (thee == NULL) return NULL;

	(void) Melder_progress (0.0, "LPC to Formant");
	
	for (i = 1; i <= my nx; i++)
	{
		Formant_Frame formant = & thy frame[i];
		LPC_Frame lpc = & my frame[i];
		
		if (! LPC_Frame_into_Formant_Frame (lpc, formant, samplingFrequency, margin))
		{
			err++;
		}
		
		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;
	}
	
	Formant_sort (thee);

end:

	(void) Melder_progress (1.0, NULL);
	
	if (err > 0) Melder_warning ("%s: %d formant frames out of %d suspect.", proc, err, my nx);
	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 */