/*=======1=========2=========3=========4=========5=========6=========7========*/
/* Demonstration program to accompany the subroutines described in the        */
/* articles by J. Rothweiler, on computing the Line Spectral Frequencies.     */

#include <stdio.h>
#include <math.h>

static void cacm283( double a[], int ord, double r[]);
static void a2lsp( float a[], float lsp[], int order, int type ) ;

/* Operations counters. Not needed in a real application. */
static int mpy=0;
static int add=0;
static int ptr=0;

/*=======1=========2=========3=========4=========5=========6=========7========*/
int main(int argc, char *argv[]) {
	/* Random set of coefficients. Should represent a stable filter */
        /* For any order up to 24. */
	float awc[25] = {
	    1.0,  0.1077, -0.0424, 0.1737, -0.0278,  0.1759,
	-0.1990, -0.0333, -0.1904, 0.0759,  0.0278, -0.0568,
        -0.1325,   0.001, -0.001,  0.001,  -0.001,   0.001,
        -0.0005,   0.001, -0.001,  0.001,  -0.001,   0.001,
         0.0002,
	};
	float a[25];
	float lsp[24];
	int i;
	int type = 0;
	int ord = 12;

	if(argc <= 1) {
		fprintf(stderr,"command: al2sp type order\n");
		fprintf(stderr,"type is 1 to 4\n");
		fprintf(stderr,"order is 2 to 24\n");
		exit(2);
	}
	/* Allow different types of computation, and model orders. */
	if(argc > 1) type = atoi(argv[1]);  /* 1-4 are valid types. */
	if(argc > 2) ord = atoi(argv[2]);

	if(ord > 24) {
		fprintf(stderr,"Model order is 24 max.\n");
		exit(1);
	} 

	/* Copy the coefficients into a working array. */
	a[0] = 1.0;
	for(i=1;i<=ord;i++) a[i] = -awc[i];
	a2lsp(a,lsp,ord,type);

	for(i=0;i<ord;i++) printf("%3d: %12.5f\n",i,lsp[i]);

	printf("mpy %d add %d ptr %d\n",mpy,add,ptr);
	return(0);
}
/*=======1=========2=========3=========4=========5=========6=========7========*/
/* The transformation as proposed in the paper. */
static void cheby1(double *g, int ord) {
	int i, j;
	int k;

	for(i=2; i<= ord; i++) {
		for(j=ord; j > i; j--) {
			g[j-2] -= g[j];           add++;
		}
		g[j-2] -= 2.0*g[j];           mpy++; add++;
		/* for(k=0;k<=ord;k++) printf(" %6.3f",g[k]); printf("\n"); */
	}
}
/*=======1=========2=========3=========4=========5=========6=========7========*/
/* An alternate transformation giving roots between -2 and +2. */
static void cheby2(double *g, int ord) {
	int i, j;
	int k;

	g[0] *= 0.5;                              mpy++;
	for(i=2; i<= ord; i++) {
		for(j=ord; j >= i; j--) {
			g[j-2] -= g[j];           add++;
		}
		g[i-1] *= 0.5;                    mpy++;
		/* for(k=0;k<=ord;k++) printf(" %6.3f",g[k]); printf("\n"); */
	}
	g[ord] *= 0.5;                    mpy++;
}
/*=======1=========2=========3=========4=========5=========6=========7========*/
/* Another transformation giving roots between -1 and +1. */
static void cheby3(double *g, int ord) {
	int i, j;
	int k;

	g[0] *= 0.5;                              mpy++;
	for(i=2; i<= ord; i++) {
		for(j=ord; j >= i; j--) {
			g[j-2] -= g[j];           add++;
			g[j] += g[j];             add++;
		}
		/* for(k=0;k<=ord;k++) printf(" %6.3f",g[k]); printf("\n"); */
	}
}
/*=======1=========2=========3=========4=========5=========6=========7========*/
/* The transformation as proposed by Wu and Chen. */
static void kw(double *r,int n) {
	double s[100];
	double c[100];
	int i, j, k;

	s[0] = 1.0;
	s[1] = -2.0;
	s[2] = 2.0;
	for(i=3;i<=n/2;i++) s[i] = s[i-2];

	for(k=0;k<=n;k++) {
		c[k] = r[k];
		j = 1;
		for(i=k+2;i<=n;i+=2) {
			c[k] += s[j]*r[i];        mpy++; add++;
			s[j] -= s[j-1];           add++;
			j++;                      ptr++;
		}
	}
	for(k=0;k<=n;k++) r[k] = c[k];
}
/*=======1=========2=========3=========4=========5=========6=========7========*/
/* Convert filter coefficients to lsp coefficients.                   */
void a2lsp(
float a[],   /* A(z) = a0 - sum { ai * z^-i } . a0 = 1. */
float lsp[], /* Output ls frequencies in the range 0 to pi. */
int order,   /* A(z) order */
int type
) {
	double g1[100], g2[100];
	double g1r[100], g2r[100];
	int even;
	int g1_order, g2_order;
	int orderd2;
	
	int i, j;
	int swap;
	double Factor;

	/* Compute the lengths of the x polynomials. */

	even = (order & 1) == 0;  /* True if order is even. */
	if(even) g1_order = g2_order = order/2;
	else {
		fprintf(stderr,"Odd order not implemented yet\n");
		exit(1);
		g1_order = (order+1)/2;
		g2_order = g1_order - 1;
	}

	/* Compute the first half of K & R F1 & F2 polynomials. */

	/* Compute half of the symmetric and antisymmetric polynomials. */
	/* Remove the roots at +1 and -1. */

	orderd2=(order+1)/2;
	g1[orderd2] = a[0];
	for(i=1;i<=orderd2;i++) g1[g1_order-i] = a[i]+a[order+1-i];
	g2[orderd2] = a[0];
	for(i=1;i<=orderd2;i++) g2[orderd2-i] = a[i]-a[order+1-i];

	if(even) {
		for(i=1; i<=orderd2;i++) g1[orderd2-i] -= g1[orderd2-i+1];
		for(i=1; i<=orderd2;i++) g2[orderd2-i] += g2[orderd2-i+1];
	} else {
		for(i=2; i<=orderd2;i++) g2[orderd2-i] += g2[orderd2-i+2];   /* Right? */
	}

	/* Convert into polynomials in cos(alpha) */

	if(type == 1) {
		printf("Implementing chebyshev reduction\n");
		cheby1(g1,g1_order);
		cheby1(g2,g2_order);
		Factor = 0.5;
	} else if(type == 2) {
		printf("Implementing first alternate chebyshev reduction\n");
		cheby2(g1,g1_order);
		cheby2(g2,g2_order);
		Factor = 0.5;
	} else if(type == 3) {
		printf("Implementing second alternate chebyshev reduction\n");
		cheby3(g1,g1_order);
		cheby3(g2,g2_order);
		Factor = 1.0;
	} else if(type == 4) {
		printf("Implementing DID reduction\n");
		kw(g1,g1_order);
		kw(g2,g2_order);
		Factor = 0.5;
	} else {
		fprintf(stderr,"valid type values are 1 to 4.\n");
		exit(1);
	}
        /* Print the polynomials to be reduced. */
	for(i=0;i<=g1_order;i++) {
		printf("%3d: %14.6g",i,g1[i]);
		if(i<=g2_order) printf(" %14.6g",g2[i]);
		printf("\n");
	}
	fflush(stdout);

	/* Find the roots of the 2 even polynomials.*/

	cacm283(g1,g1_order,g1r);
	cacm283(g2,g2_order,g2r);

	/* Convert back to angular frequencies in the range 0 to pi. */

 	for(i=0, j=0 ; ; ) {
		lsp[j++] = acos(Factor * g1r[i]);
		if(j >= order) break;
		lsp[j++] = acos(Factor * g2r[i]);
		if(j >= order) break;
		i++;
	}
}
/*=======1=========2=========3=========4=========5=========6=========7========*/
/* A simple rootfinding algorithm, as published in the Collected Algorithms of*/
/* The Association for Computing Machinery, CACM algorithm 283.               */
/* It's basically a Newton iteration, that applies optimization steps to all  */
/* root estimates together. It is stated to work for polynomials whose roots  */
/* are all real and distinct.  I know of no proof of global convergence, but  */
/* in practice it has always worked for the LSF rootfinding problem, although */
/* there may be an initial period of wild divergence before it starts         */
/* converging. */
static void cacm283(
double a[],    /* Input array of coefficients. Length ord+1. */
int ord,
double r[]     /* Holds the found roots. */
)
{
	int i, k;
	double val, p, delta, error;
	double rooti;
	int swap;

	for(i=0; i<ord;i++) r[i] = 2.0 * (i+0.5) / ord - 1.0;

	for(error=1 ; error > 1.e-12; ) {

		error = 0;
		for( i=0; i<ord; i++) {  /* Update each point. */
			rooti = r[i];
			val = a[ord];
			p = a[ord];
			for(k=ord-1; k>= 0; k--) {
				val = val * rooti + a[k];
				if (k != i) p *= rooti - r[k];
			}
			delta = val/p;
			r[i] -= delta;
			error += delta*delta;
		}
	}

	/* Do a simple bubble sort to get them in the right order. */

	do {
		double tmplsp;
		swap = 0;
		for(i=0; i<ord-1;i++) {
			if(r[i] < r[i+1]) {
				tmplsp = r[i];
				r[i]=r[i+1];
				r[i+1]=tmplsp;
				swap++;
			}
		}
	} while (swap > 0);
}