/* LPC_and_Tube.cpp
 *
 * Copyright (C) 1993-2011 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 20020612 GPL header
 djmw 20041020 struct Tube_Frame -> struct structTube_Frame; struct LPC_Frame -> struct structLPC_Frame;
 	struct Formant_Frame->struct structFormant_Frame
 djmw 20051005 Always make a VocalTract with length 0.01 m when wakita_length==NUMundefined.
*/

#include "LPC_and_Tube.h"
#include "LPC_and_Formant.h"

void LPC_Frame_into_Tube_Frame_rc (LPC_Frame me, Tube_Frame thee)
{
	long p = my nCoefficients;
	Melder_assert (p <= thy nSegments); //TODO

	autoNUMvector<double> b (1, p);
	autoNUMvector<double> a (1, p);

	for (long i = 1; i <= p; i++) a[i] = my a[i];

	double *rc = thy c;
	for (long m = p; m > 0; m--)
	{
		rc[m] = a[m];
		if (fabs (rc[m]) > 1) Melder_throw ("rc larger 1."); // TODO kan er geen Tube worden gemaakt?
		for (long i = 1; i < m; i++) b[i] = a[i];
		for (long i = 1; i < m; i++)
		{
			a[i] = (b[i] - rc[m] * b[m-i]) / (1.0 - rc[m] * rc[m]);
		}
	}
}

void LPC_Frame_into_Tube_Frame_area (LPC_Frame me, Tube_Frame thee)
{
	struct structTube_Frame rc_struct = { 0 };
	Tube_Frame rc = & rc_struct;
	Tube_Frame_init (rc, my nCoefficients, thy length);
	LPC_Frame_into_Tube_Frame_rc (me, rc);
	Tube_Frames_rc_into_area (rc, thee);
	rc -> destroy ();
}

double LPC_Frame_getVTL_wakita (LPC_Frame me, double samplingPeriod, double refLength)
{
	struct structLPC_Frame lpc_struct;
	LPC_Frame lpc = & lpc_struct;
	struct structFormant_Frame f_struct;
	Formant_Frame f = & f_struct;
	struct structTube_Frame rc_struct, af_struct;
	Tube_Frame rc = & rc_struct, af = & af_struct;
	try {
		long m = my nCoefficients;
		double length, dlength = 0.001, wakita_length = NUMundefined;
		double varMin = 1e38;
		double fscale = 1;

		memset(& lpc_struct, 0, sizeof(lpc_struct));
		memset(& f_struct, 0, sizeof(f_struct));
		memset(& rc_struct, 0, sizeof(rc_struct));
		memset(& af_struct, 0, sizeof(af_struct));


		LPC_Frame_init  (lpc, m);
		Tube_Frame_init (rc, m, refLength);
		Tube_Frame_init (af, m, refLength);

		// Step 2

		LPC_Frame_into_Formant_Frame (me, f, samplingPeriod, 0);

		// LPC_Frame_into_Formant_Frame performs the Formant_Frame_init !!

		if (f -> nFormants < 1) Melder_throw ("Not enough formants.");

		double *area = af -> c;
		double lmin = length = 0.10;
		double plength = refLength;
		while (length <= 0.25)
		{
			// Step 3

			double fscale = plength / length;
			for (long i = 1; i <= f -> nFormants; i++)
			{
				f -> formant[i].frequency *= fscale;
				f -> formant[i].bandwidth *= fscale;
			}

			/*
			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
			*/

			// step 4

			Formant_Frame_into_LPC_Frame (f, lpc, samplingPeriod);

			// step 5

			rc -> length = length;
			LPC_Frame_into_Tube_Frame_rc (lpc, rc);

			// step 6.1

			Tube_Frames_rc_into_area (rc, af);

			// step 6.2 Log(areas)

			double logSum = 0;
			for (long i = 1; i <= af -> nSegments; i++)
			{
				area[i] = log (area[i]);
				logSum += area[i];
			}

			// step 6.3 and 7
			double var = 0;
			for (long i = 1; i <= af -> nSegments; i++)
			{
				double delta = area[i] - logSum / af -> nSegments;
				var += delta * delta;
			}

			if (var < varMin)
			{
				lmin = length; varMin = var;
			}
			plength = length;
			length += dlength;
		}

		wakita_length = lmin;
		f -> destroy ();
		lpc -> destroy ();
		rc -> destroy ();
		af -> destroy ();
		return wakita_length;
	} catch (MelderError) {
		f -> destroy ();
		lpc -> destroy ();
		rc -> destroy ();
		af -> destroy ();
		return NUMundefined;
	}
}

int Tube_Frame_into_LPC_Frame_area (Tube_Frame me, LPC_Frame thee)
{
	(void) me;
	(void) thee;
	return 0;
}

int Tube_Frame_into_LPC_Frame_rc (Tube_Frame me, LPC_Frame thee)
{
	(void) me;
	(void) thee;
	return 0;
}


VocalTract LPC_to_VocalTract (LPC me, double time, double length, int wakita)
{
	autoVocalTract thee = 0;
	struct structTube_Frame area_struct = { 0 };   // David, moet dit niet op nul geinitialiseerd? = { 0 };ja
	Tube_Frame area = & area_struct;
	try {
		long iframe = Sampled_xToIndex (me, time);

		if (iframe < 1) iframe = 1;
		if (iframe > my nx) iframe = my nx;

		//memset (& area_struct, 0, sizeof(area_struct));   // David, ja zo kan het ook (is dit op tijd voor alle throws? kan je niet weten...); init op nul is boven

		LPC_Frame lpc = & my d_frames[iframe];
		long m = lpc -> nCoefficients;

		Tube_Frame_init (area, m, length);
		LPC_Frame_into_Tube_Frame_area (lpc, area);

		thee.reset(VocalTract_create (m, area -> length / m));

		// area[lips..glottis] (m^2) to VocalTract[glottis..lips] (m^2)

		for (long i = 1; i <= m; i++)
		{
			thy z[1][i] = area -> c[m + 1 - i];
		}
		//if (wakita) double wakita_length =  LPC_Frame_getVTL_wakita (lpc, my samplingPeriod, length);   // David, dit heeft geen betekenis, toch?
		area -> destroy ();
		return thee.transfer();
	} catch (MelderError) {
		thy xmax = thy x1 = thy dx = NUMundefined;
		area -> destroy ();
		Melder_throw (me, ": no VocalTract created.");
	}
}

/* End of file LPC_and_Tube.cpp */
