/* LPC_and_VocalTract.c
 *
 * Copyright (C) 1994-2002 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 20020812 GPL header
*/

#include "LPC_and_VocalTract.h"

static double wakitaLength (const float a[], int m, double samplingFrequency, double refLength)
{
	float length = 0.14, lMin = 1, epsMin = 1.e35, l = 0.0; int nf, i;
	float *f = NULL, *b = NULL, *f1 = NULL, *b1 = NULL;
	float *a1 = NULL, *rc = NULL, *area = NULL;

	if (((f  = NUMfvector (1, m)) == NULL) ||
		((b  = NUMfvector (1, m)) == NULL) ||
		((f1 = NUMfvector (1, m)) == NULL) ||
		((b1 = NUMfvector (1, m)) == NULL) ||
		((a1 = NUMfvector (1, m)) == NULL) ||
		((rc = NUMfvector (1, m)) == NULL) ||
		((area = NUMfvector (1, m)) == NULL)) goto end;
		
	MGa_to_fb (a, m, samplingFrequency, f, b, & nf); 			/* step 2 */
	if (nf < 1) goto end;
	
	while (length <= 0.22)
	{
		float eps = 0, logSum = 0;
		for (i=1; i <= nf; i++)
		{
			f1[i] = f[i] * refLength / length; 					/* step 3 */
			b1[i] = b[i] * refLength / length;
		}
	/*
	20000125: Bovenstaande schaling van f1/b1 kan ook gedaan worden door
		MGfb_to_a (f, b, nf, samplingFrequency*length/refLength, a1)
		De berekening is extreem gevoelig voor de samplefrequentie: een zelfde
		stel f,b waardes geven andere lengtes afhankelijk van Fs. Ook het
		weglaten van een hogere formant heeft consekwenties.
		De refLength zou eigenlijk vast moeten liggen op
		refLength=c*aantalFormanten/Fs waarbij c=340 m/s (geluidssnelheid).
		Bij Fs=10000 zou aantalFormanten=5 zijn en refLength -> 0.17 m
	*/
		if (! MGfb_to_a (f1, b1, nf, samplingFrequency, a1) ||	/* step 4 */
			! MGa_to_rc (a1, rc, 2*nf)) goto end;				/* step 5 */
		MGrc_to_area (rc, area, 2*nf);							/* step 6.1 */
		for (i=1; i <= 2*nf; i++)
		{
			area[i] = log (area[i]);							/* step 6.2 */
			logSum += area[i];
		}
		for (i=1; i <= 2*nf; i++)								/* step 6.3 */
		{
			float lnS = area[i] - logSum / 2 / nf;				/* step 6.3 */
			eps += lnS * lnS;									/* step 7 */
		}
		if (eps < epsMin) { lMin = length; epsMin = eps; }
		length += 0.001;
	}
	l = lMin;
	
end:
	NUMfvector_free ( f, 1);
	NUMfvector_free ( b, 1);
	NUMfvector_free (f1, 1);
	NUMfvector_free (b1, 1);
	NUMfvector_free (a1, 1);
	NUMfvector_free (rc, 1);
	NUMfvector_free (area, 1);
	return l;
}

VocalTract LPC_to_VocalTract(LPC me, double time, double length, int wakita)
{
	VocalTract thee = NULL; double refLength = length;
	float *rc = NULL, *area = NULL, *a;
	long i, n, iframe = Sampled_xToIndex (me, time);
	
	if (iframe < 1) iframe =1;
	if (iframe > my nx) iframe = my nx;
	
	a = my frame[iframe].a;
	n = my frame[iframe].nCoefficients;
	
	if (((rc = NUMfvector (1, n)) == NULL) ||
		((area = NUMfvector (1, n)) == NULL)) goto end;
		
	if (wakita && (length = wakitaLength (a, n, 1.0 / my samplingInterval, refLength)) == 0)
	{
		length = refLength;
		Melder_warning ("LPC_to_VocalTract: cannot determine VocalTract length.\n"
			"VocalTract length set to %f cm.", 100 * refLength);
	}
	
	if (! MGa_to_rc (a, rc, n) ||
		((thee = VocalTract_create (n, length / n)) == NULL)) goto end;
		
	MGrc_to_area (rc, area, n);
	
	/* 
		area[lips..glottis] (cm^2) to VocalTract[glottis..lips] (m^2)
	*/
	
	for (i=1; i <= n; i++)
	{
		thy z[1][i] = 0.0001 * area[n + 1 - i];
	}
	
end:
	NUMfvector_free (area, 1);
	NUMfvector_free (rc, 1);
	return thee;
}

/* End of file LPC_and_VocalTract.c */