/* LPC_utilities.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 20020625 GPL header
 djmw corrected bugs in MGcovar and marple (made them equal to versions in Sound_and_LPC.c)
*/

#include "NUM2.h"
#include "LPC_utilities.h"

/* Markel&Gray, Linear Prediction of Speech, page 232 */

int MGa_to_rc (const float lpc[], float rc[], int predictionOrder)
{
	float *b = NULL, *a = NULL; int m, i, status = 0;
	
	if (((b = NUMfvector(1, predictionOrder)) == NULL) ||
		((a = NUMfvector_copy (lpc, 1, predictionOrder)) == NULL)) goto end;
		
	for (m=predictionOrder; m > 0; m--)
	{
		rc[m] = a[m];
		if (abs (rc[m]) > 1) goto end;
		for (i=1; i < m; i++) b[i] = a[i];
		for (i=1; i < m; i++)
		{
			a[i] = (b[i] - rc[m] * b[m-i]) / (1.0 - rc[m] * rc[m]);
		}
	}
	status = 1;
end:
	NUMfvector_free (a, 1);
	NUMfvector_free (b, 1);
	return status;	
}

void MGrc_to_area (const float rc[], float area[], int predictionOrder)
{
	int i; float s = 1; /* area: 1.0 cm^2 at glottis */
	for (i=predictionOrder; i > 0; i--)
	{
		s *= (1.0 + rc[i]) / (1.0 - rc[i]);
		area[i] = s;
	}
}

int MGfb_to_a (const float f[], const float b[], int nfb, double samplingFrequency, float a[])
{
	int i, j, m = 0; double *lpc;
	
	if ((lpc = NUMdvector (-1, 2 * nfb)) == NULL) return 0;
	
	lpc[0] = 1;
	for (i=1; i <= nfb; i++)
	{ 	/*
			D(z): 1 + p z^-1 + q z^-2
		*/
		double r = exp (- NUMpi * b[i] / samplingFrequency);
		double p = - 2 * r * cos (2 * NUMpi * f[i] / samplingFrequency);
		double q = r * r;
		for (j=m+2; j > 0; j--)
		{
			lpc[j] += p * lpc[j-1] + q * lpc[j-2];
		}
		m += 2;
	}
	for (i=1; i <= 2*nfb; i++) a[i] = lpc[i];
	NUMdvector_free (lpc, -1);
	return 1;
}

/* 
int MGa_to_r (const float a[], int m, float gain, float r[])
{
  int i, j, status = 0; float *sa = NULL, *rc = NULL;
  if (! (sa = NUMfvector (0, m)) ||
  	  ! (rc = NUMfvector (1, m))) goto end;
  NUMfvector_copyElements (a, sa+1, 1, m);
  if (! MGa_to_rc (a, rc, m)) goto end;

  status = 1;
end:
  NUMfvector_free (rc, 1);
  NUMfvector_free (sa, 0);
  return status;	
}
*/

void MGa_to_fb (const float a[], int m, double samplingFrequency, 
	float f[], float b[], int *nFound)
{
	fcomplex ccof[500], zi[500];
	fcomplex z10 = fcomplex_create (1.0, 0.0); int i;
	
	if (m > 499) 
	{
		(void) Melder_error("MGa_to_fb: too many coefficients.");
		*nFound = 0; return;
	}
	
	ccof[m] = z10; *nFound = 0;
	for (i=0; i < m; i++)
	{
		ccof[i] = fcomplex_create (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 > 50.0 && fmnt < samplingFrequency / 2 - 50)
		{
			(*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);
}

void MGa_to_cepstrum (const float a[], float c[], int predictionOrder, float alpha)
{
	int i, k;
	c[0] = 0.5 * log (alpha);
	c[1] = -a[1];
	for (i=2; i <= predictionOrder; i++)
	{
		c[i] = 0;
		for (k=1; k < i; k++) c[i] += a[i-k]*c[k]*k;
		c[i] = - a[i] - c[i] / i;
	}
}

void MGcepstrum_to_a (float c[], float a[], int predictionOrder, float *alpha)
{
	int i, j;
	*alpha = exp (2 * c[0]);
	a[1] = -c[1];
	for (i=2; i <= predictionOrder; i++) c[i] *= i;
	for (i=2; i <= predictionOrder; i++)
	{
		a[i] = c[i];
		for (j=1; j < i; j++) a[i] += a[j]*c[i-j];
		a[i] /= -i;
	}
}

/* voor mel-based cepstral coeffs : z' = ( 1 - a z^-1)/( z^-1 - a) en a =0.6 */

/* test conversions */
static void LPC_utilities_test (void)
{
	int i, nf;
	float rc1[8] ={0,0.66992,-0.13410,-0.29675,-0.58978,0.04873,0.40587,0.39438};
	float area1[8], rc2[8], lpc3[10], f4[6], b4[6];
	float a2[9]={1,-2.34644,1.65697,-0.00599,0.32305,-1.48213,1.15463,-0.18966,-0.05899};
	float f3[6]={0,500,1500,2500,3500,4500}, b3[6]={0,50,150,250,350,450};

/* rc to area MG page 80 */
	MGrc_to_area(rc1, area1, 7);
	for (i=1; i <= 7; i++) Melder_casual("rc_to_a area[%d] = %f", i, area1[i]);
	
/* a to refl coeffs MG page 97 */		
	MGa_to_rc (a2, rc2, 8);
	for (i=1; i <= 8; i++) Melder_casual("a_to_rc i rc[%d] = %f", i, rc2[i]);

/* fb to a */	
	MGfb_to_a (f3, b3, 5, 10000, lpc3);
	for (i=1; i <= 2*5; i++) Melder_casual("fb_to_a a[%d] = %f", i, lpc3[i]);
	
/* a to fb */	
	MGa_to_fb (lpc3, 10, 10000, f4, b4, &nf);
	for(i=1; i <= nf; i++) Melder_casual("a_to_fb f[%d] b[%d] = %f %f", i, i, f4[i], b4[i]);
}

/* Return:
 *  lpc[1..m] the prediction coefficients
 *  on output lpc[1] is the first coefficient ("lpc[0] = 1") */
static int MGauto (float x[], long n, float lpc[], int m, float *alpha)
{
	int i = 0, j; float *r = NULL, *a = NULL, *rc = NULL;
	
	if (((r = NUMfvector (1, m + 1)) == NULL) ||
		((a = NUMfvector (1, m + 1)) == NULL) ||
		((rc = NUMfvector (1, m)) == NULL)) goto error;
		
	for (i=1; i <= m+1; i++)
	{
		for (j=1; j <= n-i+1; j++) r[i] += x[j] * x[j+i-1];
	}
	if (r[1] == 0) goto error;
	
	a[1] = 1; a[2] = rc[1] = - r[2] / r[1];
	*alpha = r[1] + r[2] * rc[1];
	
	for (i=2; i <= m; i++)
	{
		float s = 0;
		for (j=1; j <= i; j++) s += r[i-j+2] * a[j];
		rc[i] = - s / *alpha;
		for (j=2; j <= i/2+1; j++)
		{
			float 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]; *alpha += rc[i] * s;
		if (*alpha <= 0) goto error;
	}
	for (j=1; j <= m; j++) lpc[j] = a[j+1];
	NUMfvector_free (r, 1);
	NUMfvector_free (a, 1);
	NUMfvector_free (rc, 1);
	return 1;
error:
	for (j=1; j <= i; j++) lpc[j] = a[j+1];
	for (j=i+1; j <= m; j++) lpc[j] = 0;
	NUMfvector_free (r, 1);
	NUMfvector_free (a, 1);
	NUMfvector_free (rc, 1);
	return 0;
}


static int MGcovar (float x[], long n, float lpc[], int m, float *alpha)
{
	long i = 1, j, k;
	float *b = NULL, *grc = NULL, *a = NULL, *beta = NULL, *cc = NULL;
	
	if (((b = NUMfvector (1, m * (m + 1) / 2)) == NULL) ||
		((grc = NUMfvector (1, m)) == NULL) ||
		((a = NUMfvector (1, m + 1)) == NULL) ||
		((beta = NUMfvector (1, m)) == NULL) ||
		((cc = NUMfvector (1, m + 1))) == NULL) goto end;
		
	for (i = 1; i <= (m + 1) * m / 2; i++) b[i] = 0;
	*alpha = 0;
	for (i = m + 1; i <= n; i++)
	{
		*alpha += x[i] * x[i];
		cc[1] += x[i] * x[i-1];
		cc[2] += x[i-1] * x[i-1];
	}
	if (*alpha == 0) { i = 1; goto end;}
	b[1] = 1;
	beta[1] = cc[2];
	a[1] = 1; a[2] = grc[1] = -cc[1] / cc[2];
	*alpha += grc[1]*cc[1];
	for (i=2; i <= m; i++) /*130*/
	{
		float s = 0; /* 20 */
		for (j=1; j <= i; j++) cc[i-j+2] = cc[i-j+1] + x[m+1-i] * x[m-i+j] - x[n+1-i] * x[n-i+j];
		cc[1] = 0;
		for (j=m+1; j <= n; j++) cc[1] += x[j-i] * x[j]; /* 30 */
		b[i*(i+1)/2] = 1;
		for (j=1; j <= i-1; j++) /* 70 */
		{
			float gam = 0;
			if (beta[j] < 0) goto end;
			else if (beta[j] == 0) continue;
			for (k=1; k <= j; k++) gam += cc[k+1] * b[j*(j-1)/2+k]; /*50*/
			gam /= beta[j];
			for (k=1; k <= j; k++) b[i*(i-1)/2+k] -= gam * b[j*(j-1)/2+k]; /*60*/
		}
		beta[i] = 0;
		for (j=1; j <= i; j++) beta[i] += cc[j+1] * b[i*(i-1)/2+j]; /*80*/
		if (beta[i] <= 0) goto end;
		for (j=1; j <= i; j++) s += cc[j] * a[j]; /*100*/
		grc[i] = -s / beta[i];
		for (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];
		*alpha -= s;
		if (*alpha <= 0) goto end;
	}
end:
	i--;
	for (j=1; j <= i; j++) lpc[j] = a[j+1];
	NUMfvector_free (b, 1); NUMfvector_free (grc, 1);
	NUMfvector_free (a, 1); NUMfvector_free (beta, 1);
	NUMfvector_free (cc, 1);
	if (i == m) return 1;
	for (j=i+1; j <= m; j++) lpc[j] = 0;
	return 0;
}

static int burg2 (float x[], long n, float a[], int m, float *alpha)
{
	long i;
	NUMmemcof (x, n, m, alpha, a);
	for (i=1; i <= m; i++) a[i] = -a[i];
	return 1;
}

static int marple (float x[], long n, float a[], int mmax, float *alpha)
{
	int k, m = 1, status = 1;
	float tol1 = 1e-6; /* stop recursion when e(m)/e(0) < tol1 */
	float tol2 = 1e-6; /* stop recursion when (e(m)-e(m-1))/e(m-1) < tol2 */
	float *c = NULL, *d = NULL, *r = NULL;
	float den, e0, h, q, q1, q2, q3, q4, q5, q6, q7, s, s1, u, v, w;

	if (((c = NUMfvector (1, mmax + 1)) == NULL) ||
		((d = NUMfvector (1, mmax + 1)) == NULL) ||
		((r = NUMfvector (1, mmax + 1)) == NULL)) { status = 0; goto end; }
	for (e0=0, k=1; k <= n; k++) e0 += x[k] * x[k];
	e0 *= 2;
	if (e0 == 0)
	{
		m = 0; goto end;
	}
	
	q1 = 1.0 / e0; q2 = q1 * x[1]; v = q = q1 * x[1] * x[1]; u = w = q1 * x[n] * x[n];
	den = 1.0 - q - w; q4 = 1.0 / den; q5 = 1.0 - q; q6 = 1.0 - w; s = h = q2 * x[n];
	*alpha = e0 * den; q1 = 1.0 / *alpha; c[1] = q1 * x[1]; d[1] = q1 * x[n];
	for (s1=0.0, k=1; k <= n-1; k++) s1 += x[k+1] * x[k];
	r[1] = 2.0 * s1; a[1] = - q1 * r[1]; *alpha *= ( 1.0 - a[1] * a[1]);
	m = 1; while (m < mmax)
	{
		float delta, eOld = *alpha, f = x[m+1], b = x[n-m]; /*n-1 ->n-m*/
		float alf, c1, c2, c3, c4, c5, c6, y1, y2, y3, y4, y5;
		for (k=1; k <= m; k++) { f += x[m+1-k] * a[k]; b += x[n-m+k] * a[k]; }/*n-1 ->n-m*/
		q1 = 1.0 / *alpha; q2 = q1 * f; 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;
		q7 = s * s; y1 = f * f; y2 = v * v; y3 = b * b; y4 = u * u; 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;
		alf = 1.0 / ( 1.0 + q1 * ( y1 * q6 + y3 * q5 + 2.0 * h * f * b));
 		*alpha *= alf; y5 = h * s;
		c1 = q4 * ( f * q6 + b * h); c2 = q4 * ( b * q5 + h * f);
		c3 = q4 * ( v * q6 + y5); c4 = q4 * ( s * q5 + v * h);
 		c5 = q4 * ( s * q6 + h * u); 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++)
		{
			float 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];
		for (delta=0, 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];
		}
		for (s1=0.0, k=1; k <= n-m; k++) s1 += x[k+m] * x[k];
		r[1] = 2.0 * s1; delta += r[1]; q2 = - delta / *alpha; a[m] = q2;
		for (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; *alpha *= 1.0 - y1;
		if (y1 >= 1.0) { status = 3; goto end; } /* |a[m]| > 1 */
		if (*alpha < e0 * tol1) { status = 4; goto end; } 
		if (eOld - *alpha < eOld * tol2) { status = 5; goto end; }
	}
end:
	NUMfvector_free (c, 1);
	NUMfvector_free (d, 1);
	NUMfvector_free (r, 1);
	return status == 1 || status == 4 || status == 5;
}

/* End of file LPC_utilities.c */