/* Sound_and_LPC.cpp
 *
 * Copyright (C) 1994-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 20020625 GPL header
 djmw corrected a bug in Sound_into_LPC_Frame_marple that could crash praat when signal has only zero samples.
 djmw 20040303 Removed warning in Sound_to_LPC.
 djmw 20070103 Sound interface changes
 djmw 20080122 float -> double
 djmw 20101009 Filter and inverseFilter with one frame.
*/

#include "Sound_and_LPC.h"
#include "Sound_extensions.h"
#include "Vector.h"
#include "Spectrum.h"
#include "NUM2.h"

#define LPC_METHOD_AUTO 1
#define LPC_METHOD_COVAR 2
#define LPC_METHOD_BURG 3
#define LPC_METHOD_MARPLE 4

/* Markel&Gray, LP of S, page 219
work[1..3*m+2]
r = & work[1]; // r[1..m+1]
a= & work[m+1+1]; // a[1..m+1]
rc = & work[m+1+m+1+1]; // rc[1..m]
for (i=1; i<= m+1+m+1+m;i++) work[i] = 0;
*/

#define LPC_METHOD_AUTO_WINDOW_CORRECTION 1

static void LPC_Frame_and_Sound_filter (LPC_Frame me, Sound thee, int channel)
{
	double *y = thy z[channel], *a = my a;

	for (long i = 1; i <= thy nx; i++)
	{
		long m = i > my nCoefficients ? my nCoefficients : i - 1;
		for (long j = 1; j <= m; j++)
		{
			y[i] -= a[j] * y[i - j];
		}
	}
}

int LPC_Frame_and_Sound_filterInverse (LPC_Frame me, Sound thee, int channel)
{
		double *x = thy z[channel];
		autoNUMvector<double> y (0L, my nCoefficients);
		for (long i = 1; i <= thy nx; i++)
		{
			y[0] = x[i];
			for (long j = 1; j <= my nCoefficients; j++)
			{
				x[i] += my a[j] * y[j];
			}
			for (long j = my nCoefficients; j > 0; j--) y[j] = y[j - 1];
		}
		return 1;
}

static int Sound_into_LPC_Frame_auto (Sound me, LPC_Frame thee)
{
		long i = 1; // For error condition at end
		long m = thy nCoefficients;
		
		autoNUMvector<double> r (1, m + 1);
		autoNUMvector<double> a (1, m + 1);
		autoNUMvector<double> rc (1, m);

		double  *x = my z[1];
		for (long i = 1; i <= m + 1; i++)
		{
			for (long j = 1; j <= my nx - i + 1; j++)
			{
				r[i] += x[j] * x[j+i-1];
			}
		}
		if (r[1] == 0)
		{
			i = 1; /* ! */ goto end;
		}
		a[1] = 1; a[2] = rc[1] = - r[2] / r[1];
		thy gain = r[1] + r[2] * rc[1];
		for (i = 2; i <= m; i++)
		{
			double s = 0;
			for (long j = 1; j <= i; j++)
			{
				s += r[i-j+2] * a[j];
			}
			rc[i] = - s / thy gain;
			for (long j = 2; j <= i/2+1; j++)
			{
				double at = a[j] + rc[i] * a[i-j+2];
				a[i-j+2] += rc[i] * a[j];
				a[j] = at;
			}
			a[i+1] = rc[i]; thy gain += rc[i] * s;
			if (thy gain <= 0) goto end;
		}
end:
		i--;
		for (long j = 1; j <= i; j++) thy a[j] = a[j+1];
		if (i == m) return 1;
		thy nCoefficients = i;
		for (long j = i + 1; j <= m; j++) thy a[j] = 0;
		return 0; // Melder_warning ("Less coefficienst than asked for.");
}

/* Markel&Gray, LP of S, page 221
	work[1..m(m+1)/2+m+m+1+m+m+1]
	b = & work[1]
	grc = & work[m*(m+1)/2+1];
	a = & work[m*(m+1)/2+m+1];
	beta = & work [m+1)/2+m+m+1+1];
	cc = & work[m+1)/2+m+m+1+m+1]
	for (i=1; i<=m(m+1)/2+m+m+1+m+m+1;i++) work[i] = 0;
*/
static int Sound_into_LPC_Frame_covar (Sound me, LPC_Frame thee)
{
		long i = 1, n = my nx, m = thy nCoefficients;
		double *x = my z[1];

		autoNUMvector<double> b (1, m * (m+1) / 2);
		autoNUMvector<double> grc (1, m);
		autoNUMvector<double> a (1, m + 1);
		autoNUMvector<double> beta (1, m);
		autoNUMvector<double> cc (1, m + 1);

		thy gain = 0;
		for (i = m + 1; i <= n; i++)
		{
			thy gain += x[i] * x[i];
			cc[1] += x[i] * x[i-1];
			cc[2] += x[i-1] * x[i-1];
		}

		if (thy gain == 0)
		{
			i = 1; /* ! */ goto end;
		}

		b[1] = 1;
		beta[1] = cc[2];
		a[1] = 1;
		a[2] = grc[1] = -cc[1] / cc[2];
		thy gain += grc[1] * cc[1];

		for (i = 2; i <= m; i++) /*130*/
		{
			double s = 0; /* 20 */
			for (long j = 1; j <= i; j++)
			{
				cc[i-j+2] = cc[i-j+1] + x[m-i+1] * x[m-i+j] - x[n-i+1] * x[n-i+j];
			}
			cc[1] = 0;
			for (long j = m + 1; j <= n; j++)
			{
				cc[1] += x[j-i] * x[j]; /* 30 */
			}
			b[i*(i+1)/2] = 1;
			for (long j = 1; j <= i - 1; j++) /* 70 */
			{
				double gam = 0;
				if (beta[j] < 0) goto end;
				else if (beta[j] == 0) continue;
				for (long k = 1; k <= j; k++)
				{
					gam += cc[k+1] * b[j*(j-1)/2+k]; /*50*/
				}
				gam /= beta[j];
				for (long k = 1; k <= j; k++)
				{
					b[i*(i-1)/2+k] -= gam * b[j*(j-1)/2+k]; /*60*/
				}
			}

			beta[i] = 0;
			for (long j = 1; j <= i; j++)
			{
				beta[i] += cc[j+1] * b[i*(i-1)/2+j]; /*80*/
			}
			if (beta[i] <= 0) goto end;

			for (long j = 1; j <= i; j++)
			{
				s += cc[j] * a[j]; /*100*/
			}
			grc[i] = -s / beta[i];

			for (long j = 2; j <= i; j++)
			{
				a[j] += grc[i] * b[i*(i-1)/2+j-1]; /*110*/
			}
			a[i+1] = grc[i];
			s = grc[i] * grc[i] * beta[i];
			thy gain -= s;
			if (thy gain <= 0) goto end;
		}
end:
		i--;

		for (long j = 1; j <= i; j++)
		{
			thy a[j] = a[j+1];
		}
		if (i == m) return 1;

		thy nCoefficients = i;
		for (long j = i + 1; j <= m; j++) thy a[j] = 0;
		return 0; // Melder_warning ("Less coefficienst than asked for.");
}

static int Sound_into_LPC_Frame_burg (Sound me, LPC_Frame thee)
{
		int status = NUMburg (my z[1], my nx, thy a, thy nCoefficients, &thy gain);
		thy gain *= my nx;
		for (long i = 1; i <= thy nCoefficients; i++) thy a[i] = -thy a[i];
		return status;
}

static int Sound_into_LPC_Frame_marple (Sound me, LPC_Frame thee, double tol1, double tol2)
{
		long k, m = 1, n = my nx, mmax = thy nCoefficients;
		int status = 1;
		double *a = thy a, *x = my z[1];

		autoNUMvector<double> c (1, mmax + 1);
		autoNUMvector<double> d (1, mmax + 1);
		autoNUMvector<double> r (1, mmax + 1);
		double e0 = 0;
		for (long k = 1; k <= n; k++)
		{
			e0 += x[k] * x[k];
		}
		e0 *= 2;
		if (e0 == 0)
		{
			m = 0; thy gain *= 0.5; /* because e0 is twice the energy */
			thy nCoefficients = m; return 0; // warning no signal
		}
		double q1 = 1.0 / e0;
		double q2 = q1 * x[1], q = q1 * x[1] * x[1], w = q1 * x[n] * x[n];
		double v = q, u = w;
		double den = 1.0 - q - w;
		double q4 = 1.0 / den, q5 = 1.0 - q, q6 = 1.0 - w;
		double h = q2 * x[n], s = h, s1;
		thy gain = e0 * den;
		q1 = 1.0 / thy gain;
		c[1] = q1 * x[1];
		d[1] = q1 * x[n];
		s1 = 0.0;
		for (long k = 1; k <= n - 1; k++)
		{
			s1 += x[k+1] * x[k];
		}
		r[1] = 2.0 * s1;
		a[1] = - q1 * r[1];
		thy gain *= (1.0 - a[1] * a[1]);
		while (m < mmax)
		{
			double eOld = thy gain, f = x[m+1], b = x[n-m]; /*n-1 ->n-m*/
			for (k = 1; k <= m; k++)
			{	/* n-1 -> n-m */
				f += x[m+1-k] * a[k];
				b += x[n-m+k] * a[k];
			}
			q1 = 1.0 / thy gain;
			q2 = q1 * f;
			double q3 = q1 * b;
			for (k = m; k >= 1; k--)
			{
				c[k+1] = c[k] + q2 * a[k];
				d[k+1] = d[k] * q3 * a[k];
			}
			c[1] = q2; d[1] = q3;
			double q7 = s * s;
			double y1 = f * f;
			double y2 = v * v;
			double y3 = b * b;
			double y4 = u * u;
			double y5 = 2.0 * h * s;
			q += y1 * q1 + q4 * (y2 * q6 + q7 * q5 + v * y5);
			w += y3 * q1 + q4 * (y4 * q5 + q7 * q6 + u * y5);
			for (h = s = u = v = 0, k = 0; k <= m; k++)
			{
				h += x[n-m+k] * c[k+1];
				s += x[n-k] * c[k+1];
				u += x[n-k] * d[k+1];
				v += x[k+1] * c[k+1];
			}
			q5 = 1.0 - q;
			q6 = 1.0 - w;
			den = q5 * q6 - h * h;
			if (den <= 0)
			{
				status = 2; goto end; /* 2: ill-conditioning */
			}
			q4 = 1.0 / den;
			q1 *= q4;
			double alf = 1.0 / (1.0 + q1 * (y1 * q6 + y3 * q5 + 2.0 * h * f * b));
			thy gain *= alf;
			y5 = h * s;
			double c1 = q4 * (f * q6 + b * h);
			double c2 = q4 * (b * q5 + h * f);
			double c3 = q4 * (v * q6 + y5);
			double c4 = q4 * (s * q5 + v * h);
			double c5 = q4 * (s * q6 + h * u);
			double c6 = q4 * (u * q5 + y5);
			for (k = 1; k <= m; k++)
			{
				a[k] = alf * (a[k] + c1 * c[k+1] + c2 * d[k+1]);
			}
			for (k = 1; k <= m / 2 + 1; k++)
			{
				double s1 = c[k], s2 = d[k], s3 = c[m+2-k], s4 = d[m+2-k];
				c[k] += c3 * s3 + c4 * s4;
				d[k] += c5 * s3 + c6 * s4;
				if (m + 2 - k == k) continue;
				c[m+2-k] += c3 * s1 + c4 * s2;
				d[m+2-k] += c5 * s1 + c6 * s2;
			}
			m++; c1 = x[n+1-m]; c2 = x[m];
			double delta = 0;
			for (long k = m - 1; k >= 1; k--)
			{
				r[k+1] = r[k] - x[n+1-k] * c1 - x[k] * c2;
				delta += r[k+1] * a[k];
			}
			s1 = 0.0;
			for (long k = 1; k <= n - m; k++)
			{
				s1 += x[k+m] * x[k];
			}
			r[1] = 2.0 * s1;
			delta += r[1];
			q2 = - delta / thy gain;
			a[m] = q2;
			for (long k = 1; k <= m / 2; k++)
			{
				s1 = a[k];
				a[k] += q2 * a[m-k];
				if (k == m-k) continue;
				a[m-k] += q2 * s1;
			}
			y1 = q2 * q2;
			thy gain *= 1.0 - y1;
			if (y1 >= 1.0)
			{
				status = 3; goto end; /* |a[m]| > 1 */
			}
			if (thy gain < e0 * tol1)
			{
				status = 4; goto end;
			}
			if (eOld - thy gain < eOld * tol2)
			{
				status = 5; goto end;
			}
		}
end:
		thy gain *= 0.5; /* because e0 is twice the energy */
		thy nCoefficients = m;
		return status == 1 || status == 4 || status == 5;
}

static LPC _Sound_to_LPC (Sound me, int predictionOrder, double analysisWidth, double dt,
	double preEmphasisFrequency, int method, double tol1, double tol2)
{
		double t1, samplingFrequency = 1.0 / my dx;
		double windowDuration = 2 * analysisWidth; /* gaussian window */
		long nFrames, frameErrorCount = 0;

		if (floor (windowDuration / my dx) < predictionOrder + 1) Melder_throw ("Analysis window duration too short.\n"
			"For a prediction order of ", predictionOrder, " the analysis window duration has to be greater than ", my dx * (predictionOrder+1),
		"Please increase the analysis window duration or lower the prediction order.");
		// Convenience: analyse the whole sound into one LPC_frame
		if (windowDuration > my dx * my nx) windowDuration = my dx * my nx;
		Sampled_shortTermAnalysis (me, windowDuration, dt, & nFrames, & t1);
		autoSound sound = (Sound) Data_copy (me);
		autoSound sframe = Sound_createSimple (1, windowDuration, samplingFrequency);
		autoSound window = Sound_createGaussian (windowDuration, samplingFrequency);
		autoLPC thee = LPC_create (my xmin, my xmax, nFrames, dt, t1, predictionOrder, my dx);
		
		autoMelderProgress progress (L"LPC analysis");

		if (preEmphasisFrequency < samplingFrequency / 2) Sound_preEmphasis (sound.peek(), preEmphasisFrequency);

		for (long i = 1; i <= nFrames; i++)
		{
			LPC_Frame lpcframe = (LPC_Frame) & thy frame[i];
			double t = Sampled_indexToX (thee.peek(), i);
			LPC_Frame_init (lpcframe, predictionOrder);
			Sound_into_Sound (sound.peek(), sframe.peek(), t - windowDuration / 2);
			Vector_subtractMean (sframe.peek());
			Sounds_multiply (sframe.peek(), window.peek());
			if (method == LPC_METHOD_AUTO)
			{
				if (! Sound_into_LPC_Frame_auto (sframe.peek(), lpcframe)) frameErrorCount++;
			}
			else if (method == LPC_METHOD_COVAR)
			{
				if (! Sound_into_LPC_Frame_covar (sframe.peek(), lpcframe)) frameErrorCount++;
			}
			else if (method == LPC_METHOD_BURG)
			{
				if (! Sound_into_LPC_Frame_burg (sframe.peek(), lpcframe)) frameErrorCount++;
			}
			else if (method == LPC_METHOD_MARPLE)
			{
				if (! Sound_into_LPC_Frame_marple (sframe.peek(), lpcframe, tol1, tol2)) frameErrorCount++;
			}
			if ((i % 10) == 1) {
				Melder_progress5 ((double)i / nFrames, L"LPC analysis of frame ",
					Melder_integer (i), L" out of ", Melder_integer (nFrames), L"."); therror
			}
		}
		return thee.transfer();
}

LPC Sound_to_LPC_auto (Sound me, int predictionOrder, double analysisWidth, double dt,
	double preEmphasisFrequency)
{
	try {
		autoLPC thee = _Sound_to_LPC (me, predictionOrder, analysisWidth, dt, preEmphasisFrequency, LPC_METHOD_AUTO, 0, 0);
		return thee.transfer ();
	} catch (MelderError) { Melder_thrown (me, ": no LPC (auto) created."); }
}

LPC Sound_to_LPC_covar (Sound me, int predictionOrder, double analysisWidth, double dt,
	double preEmphasisFrequency)
{
	try {
		autoLPC thee = _Sound_to_LPC (me, predictionOrder, analysisWidth, dt, preEmphasisFrequency, LPC_METHOD_COVAR, 0, 0);
		return thee.transfer ();
	} catch (MelderError) { Melder_thrown (me, ": no LPC (covar) created."); }
}

LPC Sound_to_LPC_burg (Sound me, int predictionOrder, double analysisWidth, double dt,
	double preEmphasisFrequency)
{
	try {
		autoLPC thee = _Sound_to_LPC (me, predictionOrder, analysisWidth, dt, preEmphasisFrequency, LPC_METHOD_BURG, 0, 0);
		return thee.transfer ();
	} catch (MelderError) { Melder_thrown (me, ": no LPC (burg) created."); }
}

LPC Sound_to_LPC_marple (Sound me, int predictionOrder, double analysisWidth, double dt,
	double preEmphasisFrequency, double tol1, double tol2)
{
	try {
		autoLPC thee = _Sound_to_LPC (me, predictionOrder, analysisWidth, dt, preEmphasisFrequency, LPC_METHOD_MARPLE, tol1, tol2);
		return thee.transfer ();
	} catch (MelderError) { Melder_thrown (me, ": no LPC (marple) created."); }
}

Sound LPC_and_Sound_filterInverse (LPC me, Sound thee)
{
	try {
		if (my samplingPeriod != thy dx) Melder_throw ("Sampling frequencies are not the same.");
		if (my xmin != thy xmin || thy xmax != my xmax) Melder_throw ("Domains of LPC and Sound are not equal.");
		autoSound him = (Sound) Data_copy (thee);

		double *e = his z[1], *x = thy z[1];
		for (long i = 1; i <= his nx; i++)
		{
			double t = his x1 + (i - 1) * his dx; /* Sampled_indexToX (him, i) */
			long iFrame = floor ((t - my x1) / my dx + 1.5); /* Sampled_xToNearestIndex (me, t) */
			double *a; long m, j;
			if (iFrame < 1 || iFrame > my nx) { e[i] = 0; continue; }
			a = my frame[iFrame].a;
			m = i > my frame[iFrame].nCoefficients ? my frame[iFrame].nCoefficients : i-1;
			for (long j = 1; j <= m; j++) e[i] += a[j] * x[i-j];
		}
		return him.transfer();
	} catch (MelderError) { Melder_thrown (thee, ": not inverse filtered."); }
}

/*
	gain used as a constant amplitude multiplyer within a frame of duration my dx.
	future alternative: convolve gain with a  smoother.
*/
Sound LPC_and_Sound_filter (LPC me, Sound thee, int useGain)
{
	try {
		if (my samplingPeriod != thy dx) Melder_throw ("Sampling frequencies are not the same.");
		if (my xmin != thy xmin || my xmax != thy xmax) Melder_throw ("Time domains of source and filter do not match.");
		autoSound him = (Sound) Data_copy (thee);

		double *x = his z[1];
		long ifirst = 0, ilast = 0;
		for (long i = 1; i <= his nx; i++)
		{
			double t = his x1 + (i - 1) * his dx; /* Sampled_indexToX (him, i) */
			long iFrame = floor ((t - my x1) / my dx + 1.5); /* Sampled_xToNearestIndex (me, t) */
			if (iFrame < 1) { ifirst = i; continue; }
			if (iFrame > my nx) { ilast = i; break; }
			double *a = my frame[iFrame].a;
			long m = i > my frame[iFrame].nCoefficients ? my frame[iFrame].nCoefficients : i-1;
			for (long j = 1; j <= m; j++) x[i] -= a[j] * x[i-j];
		}

		// Make samples before first frame and after last frame zero.

		for (long i = 1; i < ifirst; i++) x[i] = 0;
		if (ilast > 0) for (long i= ilast + 1; i <= his nx; i++) x[i] = 0;
		if (useGain)
		{
			for (long i = 1; i <= his nx; i++)
			{
				double t = his x1 + (i - 1) * his dx; /* Sampled_indexToX (him, i) */
				double riFrame = (t - my x1) / my dx + 1; /* Sampled_xToIndex (me, t); */
				long iFrame = floor (riFrame);
				double phase = riFrame - iFrame;
				if (iFrame < 0 || iFrame > my nx) x[i] = 0;
				else if (iFrame == 0) x[i] *= sqrt (my frame[1].gain) * phase;
				else if (iFrame == my nx) x[i] *= sqrt (my frame [my nx].gain) * (1 - phase);
				else x[i] *=
					phase * sqrt (my frame[iFrame+1].gain)+(1-phase)*sqrt (my frame[iFrame].gain);
			}
		}
		return him.transfer();
	} catch (MelderError) { Melder_thrown (thee, ": not filtered."); }
}

int LPC_and_Sound_filterWithFilterAtTime_inline (LPC me, Sound thee, int channel, double time)
{
		long frameIndex = Sampled_xToNearestIndex (me, time);
		if (frameIndex < 1) frameIndex = 1;
		if (frameIndex > my nx) frameIndex = my nx;
		if (channel > thy ny) channel = 1;
		if (frameIndex < 1 || frameIndex > my nx) Melder_throw ("Frame number out of range.");
		if (channel > 0) LPC_Frame_and_Sound_filter (&(my frame[frameIndex]), thee, channel);
		else
		{
			for (long ichan = 1; ichan <= thy ny; ichan++)
			{
				LPC_Frame_and_Sound_filter (&(my frame[frameIndex]), thee, ichan);
			}
		}
		return 1;
}

Sound LPC_and_Sound_filterWithFilterAtTime (LPC me, Sound thee, int channel, double time)
{
	try {
		autoSound him = (Sound) Data_copy (thee);
		LPC_and_Sound_filterWithFilterAtTime_inline (me, him.peek(), channel, time);
		return him.transfer();
	} catch (MelderError) { Melder_thrown (thee, ": not filtered."); }
}

int LPC_and_Sound_filterInverseWithFilterAtTime_inline (LPC me, Sound thee, int channel, double time)
{
		long frameIndex = Sampled_xToNearestIndex (me, time);
		if (frameIndex < 1) frameIndex = 1;
		if (frameIndex > my nx) frameIndex = my nx;
		if (channel > thy ny) channel = 1;
		if (channel > 0) LPC_Frame_and_Sound_filterInverse (&(my frame[frameIndex]), thee, channel);
		else
		{
			for (long ichan = 1; ichan <= thy ny; ichan++)
			{
				LPC_Frame_and_Sound_filterInverse (&(my frame[frameIndex]), thee, ichan);
			}
		}
		return 1;
}

Sound LPC_and_Sound_filterInverseWithFilterAtTime (LPC me, Sound thee, int channel, double time)
{
	try {
		autoSound him = (Sound) Data_copy (thee);
		LPC_and_Sound_filterInverseWithFilterAtTime_inline (me, him.peek(), channel, time);
		return him.transfer();
	} catch (MelderError) { Melder_thrown (thee, ": not inverse filtered."); }
}

/* End of file Sound_and_LPC.cpp */
