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

Original copyright

  FILE........: lsp.c

  AUTHOR......: David Rowe

  DATE CREATED: 24/2/93

Heavily modified by Jean-Marc Valin (c) 2002-2006 (fixed-point,

                       optimizations, additional functions, ...)

   This file contains functions for converting Linear Prediction

   Coefficients (LPC) to Line Spectral Pair (LSP) and back. Note that the

   LSP coefficients are not in radians format but in the x domain of the

   unit circle.

   Speex License:

   Redistribution and use in source and binary forms, with or without

   modification, are permitted provided that the following conditions

   are met:

   - Redistributions of source code must retain the above copyright

   notice, this list of conditions and the following disclaimer.

   - Redistributions in binary form must reproduce the above copyright

   notice, this list of conditions and the following disclaimer in the

   documentation and/or other materials provided with the distribution.

   - Neither the name of the Xiph.org Foundation nor the names of its

   contributors may be used to endorse or promote products derived from

   this software without specific prior written permission.

   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

   ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR

   A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR

   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,

   EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,

   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR

   PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF

   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING

   NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS

   SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

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

  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}.

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

#ifdef HAVE_CONFIG_H

#include "config.h"

#endif

#include <math.h>

#include "lsp.h"

#include "stack_alloc.h"

#include "math_approx.h"

#ifndef M_PI

#define M_PI           3.14159265358979323846  /* pi */

#endif

#ifndef NULL

#define NULL 0

#endif

#ifdef FIXED_POINT

#define FREQ_SCALE 16384

/*#define ANGLE2X(a) (32768*cos(((a)/8192.)))*/

#define ANGLE2X(a) (SHL16(spx_cos(a),2))

/*#define X2ANGLE(x) (acos(.00006103515625*(x))*LSP_SCALING)*/

#define X2ANGLE(x) (spx_acos(x))

#ifdef BFIN_ASM

#include "lsp_bfin.h"

#endif

#else

/*#define C1 0.99940307

#define C2 -0.49558072

#define C3 0.03679168*/

#define FREQ_SCALE 1.

#define ANGLE2X(a) (spx_cos(a))

#define X2ANGLE(x) (acos(x))

#endif

#ifndef DISABLE_ENCODER

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

   FUNCTION....: cheb_poly_eva()

   AUTHOR......: David Rowe

   DATE CREATED: 24/2/93

   This function evaluates a series of Chebyshev polynomials

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

#ifdef FIXED_POINT

#ifndef OVERRIDE_CHEB_POLY_EVA

static inline spx_word32_t cheb_poly_eva(

  spx_word16_t *coef, /* P or Q coefs in Q13 format               */

  spx_word16_t     x, /* cos of freq (-1.0 to 1.0) in Q14 format  */

  int              m, /* LPC order/2                              */

  char         *stack

)

{

    int i;

    spx_word16_t b0, b1;

    spx_word32_t sum;

    /*Prevents overflows*/

    if (x>16383)

       x = 16383;

    if (x<-16383)

       x = -16383;

    /* Initialise values */

    b1=16384;

    b0=x;

    /* Evaluate Chebyshev series formulation using an iterative approach  */

    sum = ADD32(EXTEND32(coef[m]), EXTEND32(MULT16_16_P14(coef[m-1],x)));

    for(i=2;i<=m;i++)

    {

       spx_word16_t tmp=b0;

       b0 = SUB16(MULT16_16_Q13(x,b0), b1);

       b1 = tmp;

       sum = ADD32(sum, EXTEND32(MULT16_16_P14(coef[m-i],b0)));

    }

    return sum;

}

#endif

#else

static float cheb_poly_eva(spx_word32_t *coef, spx_word16_t x, int m, char *stack)

{

   int k;

   float b0, b1, tmp;

   /* Initial conditions */

   b0=0; /* b_(m+1) */

   b1=0; /* b_(m+2) */

   x*=2;

   /* Calculate the b_(k) */

   for(k=m;k>0;k--)

   {

      tmp=b0;                           /* tmp holds the previous value of b0 */

      b0=x*b0-b1+coef[m-k];    /* b0 holds its new value based on b0 and b1 */

      b1=tmp;                           /* b1 holds the previous value of b0 */

   }

   return(-b1+.5*x*b0+coef[m]);

}

#endif

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

    FUNCTION....: lpc_to_lsp()

    AUTHOR......: David Rowe

    DATE CREATED: 24/2/93

    This function converts LPC coefficients to LSP

    coefficients.

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

#ifdef FIXED_POINT

#define SIGN_CHANGE(a,b) ((((a)^(b))&0x80000000)||(b==0))

#else

#define SIGN_CHANGE(a,b) (((a)*(b))<0.0)

#endif

int lpc_to_lsp (spx_coef_t *a,int lpcrdr,spx_lsp_t *freq,int nb,spx_word16_t delta, char *stack)

/*  float *a          lpc coefficients      */

/*  int lpcrdr      order of LPC coefficients (10)    */

/*  float *freq           LSP frequencies in the x domain         */

/*  int nb      number of sub-intervals (4)     */

/*  float delta     grid spacing interval (0.02)    */

{

    spx_word16_t temp_xr,xl,xr,xm=0;

    spx_word32_t psuml,psumr,psumm,temp_psumr/*,temp_qsumr*/;

    int i,j,m,k;

    VARDECL(spx_word32_t *Q);                  /* ptrs for memory allocation     */

    VARDECL(spx_word32_t *P);

    VARDECL(spx_word16_t *Q16);         /* ptrs for memory allocation    */

    VARDECL(spx_word16_t *P16);

    spx_word32_t *px;                 /* ptrs of respective P'(z) & Q'(z) */

    spx_word32_t *qx;

    spx_word32_t *p;

    spx_word32_t *q;

    spx_word16_t *pt;                 /* ptr used for cheb_poly_eval()

        whether P' or Q'      */

    int roots=0;                /* DR 8/2/94: number of roots found   */

    m = lpcrdr/2;             /* order of P'(z) & Q'(z) polynomials   */

    /* Allocate memory space for polynomials */

    ALLOC(Q, (m+1), spx_word32_t);

    ALLOC(P, (m+1), spx_word32_t);

    /* 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;                      /* initialise ptrs       */

    qx = Q;

    p = px;

    q = qx;

#ifdef FIXED_POINT

    *px++ = LPC_SCALING;

    *qx++ = LPC_SCALING;

    for(i=0;i<m;i++){

       *px++ = SUB32(ADD32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *p++);

       *qx++ = ADD32(SUB32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *q++);

    }

    px = P;

    qx = Q;

    for(i=0;i<m;i++)

    {

       /*if (fabs(*px)>=32768)

          speex_warning_int("px", *px);

       if (fabs(*qx)>=32768)

       speex_warning_int("qx", *qx);*/

       *px = PSHR32(*px,2);

       *qx = PSHR32(*qx,2);

       px++;

       qx++;

    }

    /* The reason for this lies in the way cheb_poly_eva() is implemented for fixed-point */

    P[m] = PSHR32(P[m],3);

    Q[m] = PSHR32(Q[m],3);

#else

    *px++ = LPC_SCALING;

    *qx++ = LPC_SCALING;

    for(i=0;i<m;i++){

       *px++ = (a[i]+a[lpcrdr-1-i]) - *p++;

       *qx++ = (a[i]-a[lpcrdr-1-i]) + *q++;

    }

    px = P;

    qx = Q;

    for(i=0;i<m;i++){

       *px = 2**px;

       *qx = 2**qx;

       px++;

       qx++;

    }

#endif

    px = P;               /* re-initialise ptrs       */

    qx = Q;

    /* now that we have computed P and Q convert to 16 bits to

       speed up cheb_poly_eval */

    ALLOC(P16, m+1, spx_word16_t);

    ALLOC(Q16, m+1, spx_word16_t);

    for (i=0;i<m+1;i++)

    {

       P16[i] = P[i];

       Q16[i] = Q[i];

    }

    /* 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 = FREQ_SCALE;                 /* start at point xl = 1    */

    for(j=0;j<lpcrdr;j++){

  if(j&1)              /* determines whether P' or Q' is eval. */

      pt = Q16;

  else

      pt = P16;

  psuml = cheb_poly_eva(pt,xl,m,stack); /* evals poly. at xl  */

  while(xr >= -FREQ_SCALE){

           spx_word16_t dd;

           /* Modified by JMV to provide smaller steps around x=+-1 */

#ifdef FIXED_POINT

           dd = MULT16_16_Q15(delta,SUB16(FREQ_SCALE, MULT16_16_Q14(MULT16_16_Q14(xl,xl),14000)));

           if (psuml<512 && psuml>-512)

              dd = PSHR16(dd,1);

#else

           dd=delta*(1-.9*xl*xl);

           if (fabs(psuml)<.2)

              dd *= .5;

#endif

           xr = SUB16(xl, dd);                         /* interval spacing   */

      psumr = cheb_poly_eva(pt,xr,m,stack);/* 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(SIGN_CHANGE(psumr,psuml))

            {

    roots++;

    psumm=psuml;

    for(k=0;k<=nb;k++){

#ifdef FIXED_POINT

        xm = ADD16(PSHR16(xl,1),PSHR16(xr,1));          /* bisect the interval  */

#else

                    xm = .5*(xl+xr);          /* bisect the interval  */

#endif

        psumm=cheb_poly_eva(pt,xm,m,stack);

        /*if(psumm*psuml>0.)*/

        if(!SIGN_CHANGE(psumm,psuml))

                    {

      psuml=psumm;

      xl=xm;

        } else {

      psumr=psumm;

      xr=xm;

        }

    }

         /* once zero is found, reset initial interval to xr  */

         freq[j] = X2ANGLE(xm);

         xl = xm;

         break;

      }

      else{

    psuml=temp_psumr;

    xl=temp_xr;

      }

  }

    }

    return(roots);

}

#endif /* DISABLE_ENCODER */

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

  FUNCTION....: lsp_to_lpc()

  AUTHOR......: David Rowe

  DATE CREATED: 24/2/93

        Converts LSP coefficients to LPC coefficients.

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

#ifdef FIXED_POINT

void lsp_to_lpc(const spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)

/*  float *freq   array of LSP frequencies in the x domain  */

/*  float *ak     array of LPC coefficients       */

/*  int lpcrdr    order of LPC coefficients       */

{

    int i,j;

    spx_word32_t xout1,xout2,xin;

    spx_word32_t mult, a;

    VARDECL(spx_word16_t *freqn);

    VARDECL(spx_word32_t **xp);

    VARDECL(spx_word32_t *xpmem);

    VARDECL(spx_word32_t **xq);

    VARDECL(spx_word32_t *xqmem);

    int m = lpcrdr>>1;

    /*

       Reconstruct P(z) and Q(z) by cascading second order polynomials

       in form 1 - 2cos(w)z(-1) + z(-2), where w is the LSP frequency.

       In the time domain this is:

       y(n) = x(n) - 2cos(w)x(n-1) + x(n-2)

       This is what the ALLOCS below are trying to do:

         int xp[m+1][lpcrdr+1+2]; // P matrix in QIMP

         int xq[m+1][lpcrdr+1+2]; // Q matrix in QIMP

       These matrices store the output of each stage on each row.  The

       final (m-th) row has the output of the final (m-th) cascaded

       2nd order filter.  The first row is the impulse input to the

       system (not written as it is known).

       The version below takes advantage of the fact that a lot of the

       outputs are zero or known, for example if we put an inpulse

       into the first section the "clock" it 10 times only the first 3

       outputs samples are non-zero (it's an FIR filter).

    */

    ALLOC(xp, (m+1), spx_word32_t*);

    ALLOC(xpmem, (m+1)*(lpcrdr+1+2), spx_word32_t);

    ALLOC(xq, (m+1), spx_word32_t*);

    ALLOC(xqmem, (m+1)*(lpcrdr+1+2), spx_word32_t);

    for(i=0; i<=m; i++) {

      xp[i] = xpmem + i*(lpcrdr+1+2);

      xq[i] = xqmem + i*(lpcrdr+1+2);

    }

    /* work out 2cos terms in Q14 */

    ALLOC(freqn, lpcrdr, spx_word16_t);

    for (i=0;i<lpcrdr;i++)

       freqn[i] = ANGLE2X(freq[i]);

    #define QIMP  21   /* scaling for impulse */

    xin = SHL32(EXTEND32(1), (QIMP-1)); /* 0.5 in QIMP format */

    /* first col and last non-zero values of each row are trivial */

    for(i=0;i<=m;i++) {

     xp[i][1] = 0;

     xp[i][2] = xin;

     xp[i][2+2*i] = xin;

     xq[i][1] = 0;

     xq[i][2] = xin;

     xq[i][2+2*i] = xin;

    }

    /* 2nd row (first output row) is trivial */

    xp[1][3] = -MULT16_32_Q14(freqn[0],xp[0][2]);

    xq[1][3] = -MULT16_32_Q14(freqn[1],xq[0][2]);

    xout1 = xout2 = 0;

    /* now generate remaining rows */

    for(i=1;i<m;i++) {

      for(j=1;j<2*(i+1)-1;j++) {

  mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);

  xp[i+1][j+2] = ADD32(SUB32(xp[i][j+2], mult), xp[i][j]);

  mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);

  xq[i+1][j+2] = ADD32(SUB32(xq[i][j+2], mult), xq[i][j]);

      }

      /* for last col xp[i][j+2] = xq[i][j+2] = 0 */

      mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);

      xp[i+1][j+2] = SUB32(xp[i][j], mult);

      mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);

      xq[i+1][j+2] = SUB32(xq[i][j], mult);

    }

    /* process last row to extra a{k} */

    for(j=1;j<=lpcrdr;j++) {

      int shift = QIMP-13;

      /* final filter sections */

      a = PSHR32(xp[m][j+2] + xout1 + xq[m][j+2] - xout2, shift);

      xout1 = xp[m][j+2];

      xout2 = xq[m][j+2];

      /* hard limit ak's to +/- 32767 */

      if (a < -32767) a = -32767;

      if (a > 32767) a = 32767;

      ak[j-1] = (short)a;

    }

}

#else

void lsp_to_lpc(const spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)

/*  float *freq   array of LSP frequencies in the x domain  */

/*  float *ak     array of LPC coefficients       */

/*  int lpcrdr    order of LPC coefficients       */

{

    int i,j;

    float xout1,xout2,xin1,xin2;

    VARDECL(float *Wp);

    float *pw,*n1,*n2,*n3,*n4=NULL;

    VARDECL(float *x_freq);

    int m = lpcrdr>>1;

    ALLOC(Wp, 4*m+2, float);

    pw = Wp;

    /* initialise contents of array */

    for(i=0;i<=4*m+1;i++){         /* set contents of buffer to 0 */

  *pw++ = 0.0;

    }

    /* Set pointers up */

    pw = Wp;

    xin1 = 1.0;

    xin2 = 1.0;

    ALLOC(x_freq, lpcrdr, float);

    for (i=0;i<lpcrdr;i++)

       x_freq[i] = ANGLE2X(freq[i]);

    /* 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<=lpcrdr;j++){

       int i2=0;

  for(i=0;i<m;i++,i2+=2){

      n1 = pw+(i*4);

      n2 = n1 + 1;

      n3 = n2 + 1;

      n4 = n3 + 1;

      xout1 = xin1 - 2.f*x_freq[i2] * *n1 + *n2;

      xout2 = xin2 - 2.f*x_freq[i2+1] * *n3 + *n4;

      *n2 = *n1;

      *n4 = *n3;

      *n1 = xin1;

      *n3 = xin2;

      xin1 = xout1;

      xin2 = xout2;

  }

  xout1 = xin1 + *(n4+1);

  xout2 = xin2 - *(n4+2);

  if (j>0)

     ak[j-1] = (xout1 + xout2)*0.5f;

  *(n4+1) = xin1;

  *(n4+2) = xin2;

  xin1 = 0.0;

  xin2 = 0.0;

    }

}

#endif

#ifdef FIXED_POINT

void lsp_interpolate(spx_lsp_t *old_lsp, spx_lsp_t *new_lsp, spx_lsp_t *lsp, int len, int subframe, int nb_subframes, spx_word16_t margin)

{

   int i;

   spx_word16_t m = margin;

   spx_word16_t m2 = 25736-margin;

   spx_word16_t tmp = DIV32_16(SHL32(EXTEND32(1 + subframe),14),nb_subframes);

   spx_word16_t tmp2 = 16384-tmp;

   for (i=0;i<len;i++)

      lsp[i] = MULT16_16_P14(tmp2,old_lsp[i]) + MULT16_16_P14(tmp,new_lsp[i]);

   /* Enforce margin to sure the LSPs are stable*/

   if (lsp[0]<m)

      lsp[0]=m;

   if (lsp[len-1]>m2)

      lsp[len-1]=m2;

   for (i=1;i<len-1;i++)

   {

      if (lsp[i]<lsp[i-1]+m)

         lsp[i]=lsp[i-1]+m;

      if (lsp[i]>lsp[i+1]-m)

         lsp[i]= SHR16(lsp[i],1) + SHR16(lsp[i+1]-m,1);

   }

}

#else

void lsp_interpolate(spx_lsp_t *old_lsp, spx_lsp_t *new_lsp, spx_lsp_t *lsp, int len, int subframe, int nb_subframes, spx_word16_t margin)

{

   int i;

   float tmp = (1.0f + subframe)/nb_subframes;

   for (i=0;i<len;i++)

      lsp[i] = (1-tmp)*old_lsp[i] + tmp*new_lsp[i];

   /* Enforce margin to sure the LSPs are stable*/

   if (lsp[0]<LSP_SCALING*margin)

      lsp[0]=LSP_SCALING*margin;

   if (lsp[len-1]>LSP_SCALING*(M_PI-margin))

      lsp[len-1]=LSP_SCALING*(M_PI-margin);

   for (i=1;i<len-1;i++)

   {

      if (lsp[i]<lsp[i-1]+LSP_SCALING*margin)

         lsp[i]=lsp[i-1]+LSP_SCALING*margin;

      if (lsp[i]>lsp[i+1]-LSP_SCALING*margin)

         lsp[i]= .5f* (lsp[i] + lsp[i+1]-LSP_SCALING*margin);

   }

}

#endif