/*---------------------------------------------------------------------------*\

  FILE........: lsp.c
  AUTHOR......: David Rowe
  DATE CREATED: 24/2/93


  This file contains functions for LPC to LSP conversion and LSP to
  LPC conversion. Note that the LSP coefficients are not in radians
  format but in the x domain of the unit circle.

\*---------------------------------------------------------------------------*/

/*
  Copyright (C) 2009 David Rowe

  All rights reserved.

  This program is free software; you can redistribute it and/or modify
  it under the terms of the GNU Lesser General Public License version 2.1, as
  published by the Free Software Foundation.  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 Lesser General Public License
  along with this program; if not, see <http://www.gnu.org/licenses/>.
*/

#include "defines.h"
#include "lsp.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

/*---------------------------------------------------------------------------*\

  Introduction to Line Spectrum Pairs (LSPs)
  ------------------------------------------

  LSPs are used to encode the LPC filter coefficients {ak} for
  transmission over the channel.  LSPs have several properties (like
  less sensitivity to quantisation noise) that make them superior to
  direct quantisation of {ak}.

  A(z) is a polynomial of order lpcrdr with {ak} as the coefficients.

  A(z) is transformed to P(z) and Q(z) (using a substitution and some
  algebra), to obtain something like:

    A(z) = 0.5[P(z)(z+z^-1) + Q(z)(z-z^-1)]  (1)

  As you can imagine A(z) has complex zeros all over the z-plane. P(z)
  and Q(z) have the very neat property of only having zeros _on_ the
  unit circle.  So to find them we take a test point z=exp(jw) and
  evaluate P (exp(jw)) and Q(exp(jw)) using a grid of points between 0
  and pi.

  The zeros (roots) of P(z) also happen to alternate, which is why we
  swap coefficients as we find roots.  So the process of finding the
  LSP frequencies is basically finding the roots of 5th order
  polynomials.

  The root so P(z) and Q(z) occur in symmetrical pairs at +/-w, hence
  the name Line Spectrum Pairs (LSPs).

  To convert back to ak we just evaluate (1), "clocking" an impulse
  thru it lpcrdr times gives us the impulse response of A(z) which is
  {ak}.

\*---------------------------------------------------------------------------*/

/*---------------------------------------------------------------------------*\

  FUNCTION....: cheb_poly_eva()
  AUTHOR......: David Rowe
  DATE CREATED: 24/2/93

  This function evalutes a series of chebyshev polynomials

  FIXME: performing memory allocation at run time is very inefficient,
  replace with stack variables of MAX_P size.

\*---------------------------------------------------------------------------*/


static float
cheb_poly_eva(float *coef,float x,int order)
/*  float coef[]  	coefficients of the polynomial to be evaluated 	*/
/*  float x   		the point where polynomial is to be evaluated 	*/
/*  int order 		order of the polynomial 			*/
{
    int i;
    float *t,*u,*v,sum;
    float T[(order / 2) + 1];

    /* Initialise pointers */

    t = T;                          	/* T[i-2] 			*/
    *t++ = 1.0;
    u = t--;                        	/* T[i-1] 			*/
    *u++ = x;
    v = u--;                        	/* T[i] 			*/

    /* Evaluate chebyshev series formulation using iterative approach 	*/

    for(i=2;i<=order/2;i++)
	*v++ = (2*x)*(*u++) - *t++;  	/* T[i] = 2*x*T[i-1] - T[i-2]	*/

    sum=0.0;                        	/* initialise sum to zero 	*/
    t = T;                          	/* reset pointer 		*/

    /* Evaluate polynomial and return value also free memory space */

    for(i=0;i<=order/2;i++)
	sum+=coef[(order/2)-i]**t++;

    return sum;
}


/*---------------------------------------------------------------------------*\

  FUNCTION....: lpc_to_lsp()
  AUTHOR......: David Rowe
  DATE CREATED: 24/2/93

  This function converts LPC coefficients to LSP coefficients.

\*---------------------------------------------------------------------------*/

int lpc_to_lsp (float *a, int order, float *freq, int nb, float delta)
/*  float *a 		     	lpc coefficients			*/
/*  int order			order of LPC coefficients (10) 		*/
/*  float *freq 	      	LSP frequencies in radians      	*/
/*  int nb			number of sub-intervals (4) 		*/
/*  float delta			grid spacing interval (0.02) 		*/
{
    float psuml,psumr,psumm,temp_xr,xl,xr,xm = 0;
    float temp_psumr;
    int i,j,m,flag,k;
    float *px;                	/* ptrs of respective P'(z) & Q'(z)	*/
    float *qx;
    float *p;
    float *q;
    float *pt;                	/* ptr used for cheb_poly_eval()
				   whether P' or Q' 			*/
    int roots=0;              	/* number of roots found 	        */
    float Q[order + 1];
    float P[order + 1];

    flag = 1;
    m = order/2;            	/* order of P'(z) & Q'(z) polynimials 	*/

    /* Allocate memory space for polynomials */

    /* determine P'(z)'s and Q'(z)'s coefficients where
      P'(z) = P(z)/(1 + z^(-1)) and Q'(z) = Q(z)/(1-z^(-1)) */

    px = P;                      /* initilaise ptrs */
    qx = Q;
    p = px;
    q = qx;
    *px++ = 1.0;
    *qx++ = 1.0;
    for(i=1;i<=m;i++){
	*px++ = a[i]+a[order+1-i]-*p++;
	*qx++ = a[i]-a[order+1-i]+*q++;
    }
    px = P;
    qx = Q;
    for(i=0;i<m;i++){
	*px = 2**px;
	*qx = 2**qx;
	 px++;
	 qx++;
    }
    px = P;             	/* re-initialise ptrs 			*/
    qx = Q;

    /* Search for a zero in P'(z) polynomial first and then alternate to Q'(z).
    Keep alternating between the two polynomials as each zero is found 	*/

    xr = 0;             	/* initialise xr to zero 		*/
    xl = 1.0;               	/* start at point xl = 1 		*/


    for(j=0;j<order;j++){
	if(j%2)            	/* determines whether P' or Q' is eval. */
	    pt = qx;
	else
	    pt = px;

	psuml = cheb_poly_eva(pt,xl,order);	/* evals poly. at xl 	*/
	flag = 1;
	while(flag && (xr >= -1.0)){
	    xr = xl - delta ;                  	/* interval spacing 	*/
	    psumr = cheb_poly_eva(pt,xr,order);/* poly(xl-delta_x) 	*/
	    temp_psumr = psumr;
	    temp_xr = xr;

        /* if no sign change increment xr and re-evaluate
           poly(xr). Repeat til sign change.  if a sign change has
           occurred the interval is bisected and then checked again
           for a sign change which determines in which interval the
           zero lies in.  If there is no sign change between poly(xm)
           and poly(xl) set interval between xm and xr else set
           interval between xl and xr and repeat till root is located
           within the specified limits  */

	    if(((psumr*psuml)<0.0) || (psumr == 0.0)){
		roots++;

		psumm=psuml;
		for(k=0;k<=nb;k++){
		    xm = (xl+xr)/2;        	/* bisect the interval 	*/
		    psumm=cheb_poly_eva(pt,xm,order);
		    if(psumm*psuml>0.){
			psuml=psumm;
			xl=xm;
		    }
		    else{
			psumr=psumm;
			xr=xm;
		    }
		}

	       /* once zero is found, reset initial interval to xr 	*/
	       freq[j] = (xm);
	       xl = xm;
	       flag = 0;       		/* reset flag for next search 	*/
	    }
	    else{
		psuml=temp_psumr;
		xl=temp_xr;
	    }
	}
    }

    /* convert from x domain to radians */

    for(i=0; i<order; i++) {
	freq[i] = acosf(freq[i]);
    }

    return(roots);
}

/*---------------------------------------------------------------------------*\

  FUNCTION....: lsp_to_lpc()
  AUTHOR......: David Rowe
  DATE CREATED: 24/2/93

  This function converts LSP coefficients to LPC coefficients.  In the
  Speex code we worked out a way to simplify this significantly.

\*---------------------------------------------------------------------------*/

void lsp_to_lpc(float *lsp, float *ak, int order)
/*  float *freq         array of LSP frequencies in radians     	*/
/*  float *ak 		array of LPC coefficients 			*/
/*  int order     	order of LPC coefficients 			*/


{
    int i,j;
    float xout1,xout2,xin1,xin2;
    float *pw,*n1,*n2,*n3,*n4 = 0;
    float freq[order];
    float Wp[(order * 4) + 2];

    /* convert from radians to the x=cos(w) domain */

    for(i=0; i<order; i++)
	freq[i] = cosf(lsp[i]);

    pw = Wp;

    /* initialise contents of array */

    for(i=0;i<=4*(order/2)+1;i++){       	/* set contents of buffer to 0 */
	*pw++ = 0.0;
    }

    /* Set pointers up */

    pw = Wp;
    xin1 = 1.0;
    xin2 = 1.0;

    /* reconstruct P(z) and Q(z) by cascading second order polynomials
      in form 1 - 2xz(-1) +z(-2), where x is the LSP coefficient */

    for(j=0;j<=order;j++){
	for(i=0;i<(order/2);i++){
	    n1 = pw+(i*4);
	    n2 = n1 + 1;
	    n3 = n2 + 1;
	    n4 = n3 + 1;
	    xout1 = xin1 - 2*(freq[2*i]) * *n1 + *n2;
	    xout2 = xin2 - 2*(freq[2*i+1]) * *n3 + *n4;
	    *n2 = *n1;
	    *n4 = *n3;
	    *n1 = xin1;
	    *n3 = xin2;
	    xin1 = xout1;
	    xin2 = xout2;
	}
	xout1 = xin1 + *(n4+1);
	xout2 = xin2 - *(n4+2);
	ak[j] = (xout1 + xout2)*0.5;
	*(n4+1) = xin1;
	*(n4+2) = xin2;

	xin1 = 0.0;
	xin2 = 0.0;
    }
}