/*
**
** File:    lsp.c
**
** Description: Functions that implement line spectral pair 
**      (LSP) operations.  
**
** Functions:
**
**  Converting between linear predictive coding (LPC) coefficients
**  and LSP frequencies:
**
**      AtoLsp()
**      LsptoA()
**
**  Vector quantization (VQ) of LSP frequencies:
**
**      Lsp_Qnt()
**      Lsp_Svq()
**      Lsp_Inq()
**
**  Interpolation of LSP frequencies:
**
**      Lsp_Int()
*/

/*
    ITU-T G.723 Speech Coder   ANSI-C Source Code     Version 4.1
    copyright (c) 1995, AudioCodes, DSP Group, France Telecom,
    Universite de Sherbrooke.  All rights reserved.
*/

#include <stdio.h>

#include "typedef.h"
#include "basop.h"
#include "cst_lbc.h"
#include "tab_lbc.h"
#include "lsp.h"


/*
**
** Function:            AtoLsp()
**
** Description:     Transforms 10 LPC coefficients to the 10
**          corresponding LSP frequencies for a subframe.
**          This transformation is done once per frame,
**          for subframe 3 only.  The transform algorithm
**          generates sum and difference polynomials from
**          the LPC coefficients.  It then evaluates the
**          sum and difference polynomials at uniform
**          intervals of pi/256 along the unit circle.
**          Intervals where a sign change occurs are
**          interpolated to find the zeros of the
**          polynomials, which are the LSP frequencies.
**
** Links to text:   Section 2.5
**
** Arguments:       
**
**  Word16 *LspVect     Empty Buffer
**  Word16 Lpc[]        Unquantized LPC coefficients (10 words)
**  Word16 PrevLsp[]    LSP frequencies from the previous frame (10 words)
**
** Outputs:
**
**  Word16 LspVect[]    LSP frequencies for the current frame (10 words)
**
** Return value:        None
**
**/
void AtoLsp( Word16 *LspVect, Word16 *Lpc, Word16 *PrevLsp )
{

   int   i,j,k ;

   Word32   Lpq[LpcOrder+2] ;
   Word16   Spq[LpcOrder+2] ;

   Word16   Exp   ;
   Word16   LspCnt ;

   Word32   PrevVal,CurrVal   ;
   Word32   Acc0,Acc1   ;


   /* 
    * Perform a bandwidth expansion on the LPC coefficients.  This
    * scales the poles of the LPC synthesis filter by a factor of
    * 0.994.
    */
   for ( i = 0 ; i < LpcOrder ; i ++ )
      LspVect[i] = mult_r( Lpc[i], BandExpTable[i] ) ;

   
   /*
    * Compute the sum and difference polynomials with the roots at z =
    * -1 (sum) or z = +1 (difference) removed.  Let these polynomials
    * be P(z) and Q(z) respectively, and let their coefficients be
    * {p_i} and {q_i}.  The coefficients are stored in the array Lpq[]
    * as follows: p_0, q_0, p_1, q_1, ..., p_5, q_5.  There is no need
    * to store the other coefficients because of symmetry.  
    */


  /* Set p_0 = q_0 = 1.  The LPC coefficients are already scaled by
     1/4.  P(z) and Q(z) are scaled by an additional scaling factor of
     1/16, for an overall factor of 1/64 = 0x02000000L. */
   Lpq[0] = Lpq[1] = (Word32) 0x02000000L ;

   /* This loop computes the coefficients of P(z) and Q(z).  The long
      division (to remove the real zeros) is done recursively. */
   for ( i = 0 ; i < LpcOrder/2 ; i ++ ) {

    /* P(z) */
      Acc0 = L_negate( Lpq[2*i+0] ) ;
      Acc1 = L_deposit_h( LspVect[i] ) ;
      Acc1 = L_shr( Acc1, (Word16) 4 ) ;
      Acc0 = L_sub( Acc0, Acc1 ) ;
      Acc1 = L_deposit_h( LspVect[LpcOrder-1-i] ) ;
      Acc1 = L_shr( Acc1, (Word16) 4 ) ;
      Acc0 = L_sub( Acc0, Acc1 ) ;
      Lpq[2*i+2] = Acc0 ;

    /* Q(z) */
      Acc0 = Lpq[2*i+1] ;
      Acc1 = L_deposit_h( LspVect[i] ) ;
      Acc1 = L_shr( Acc1, (Word16) 4 ) ;

      Acc0 = L_sub( Acc0, Acc1 ) ;
      Acc1 = L_deposit_h( LspVect[LpcOrder-1-i] ) ;
      Acc1 = L_shr( Acc1, (Word16) 4 ) ;
      Acc0 = L_add( Acc0, Acc1 ) ;
      Lpq[2*i+3] = Acc0 ;
      }

  /* Divide p_5 and q_5 by 2 for proper weighting during polynomial
     evaluation. */
   Lpq[LpcOrder+0] = L_shr( Lpq[LpcOrder+0], (Word16) 1 ) ;
   Lpq[LpcOrder+1] = L_shr( Lpq[LpcOrder+1], (Word16) 1 ) ;


  /*
   * Normalize the polynomial coefficients and convert to shorts 
   */

   /* Find the maximum */
   Acc1 = L_abs( Lpq[0] ) ;
   for ( i = 1 ; i < LpcOrder+2 ; i ++ ) {
      Acc0 = L_abs( Lpq[i] ) ;
      if ( Acc0 > Acc1 )
         Acc1 = Acc0 ;
      }

   /* Compute the normalization factor */
   Exp = norm_l( Acc1 ) ;


   /* Normalize and convert to shorts */
   for ( i = 0 ; i < LpcOrder+2 ; i ++ ) {
      Acc0 = L_shl( Lpq[i], Exp ) ;
      Spq[i] = round( Acc0 ) ;
      }


  /*
   * Initialize the search loop
   */  

  /* The variable k is a flag that indicates which polynomial (sum or
     difference) the algorithm is currently evaluating.  Start with
     the sum. */
   k = 0 ;

  /* Evaluate the sum polynomial at frequency zero */
   PrevVal = (Word32) 0 ;
   for ( j = 0 ; j <= LpcOrder/2 ; j ++ )
      PrevVal = L_mac( PrevVal, Spq[2*j], CosineTable[0] ) ;


  /* 
   * Search loop.  Evaluate P(z) and Q(z) at uniform intervals of
   * pi/256 along the unit circle.  Check for zero crossings.  The
   * zeros of P(w) and Q(w) alternate, so only one of them need by
   * evaluated at any given step.
   */
   LspCnt = (Word16) 0 ;
   for ( i = 1 ; i < CosineTableSize/2 ; i ++ ) {

     /* Evaluate the selected polynomial */
      CurrVal = (Word32) 0 ;
      for ( j = 0 ; j <= LpcOrder/2 ; j ++ )
         CurrVal = L_mac( CurrVal, Spq[LpcOrder-2*j+k],
CosineTable[i*j%CosineTableSize] ) ;

    /* Check for a sign change, indicating a zero crossing */
      if ( (CurrVal ^ PrevVal) < (Word32) 0 ) {

    /* Interpolate to find the bottom 7 bits of the 
       zero-crossing frequency */
         Acc0 = L_abs( CurrVal ) ;
         Acc1 = L_abs( PrevVal ) ;
         Acc0 = L_add( Acc0, Acc1 ) ;

         /* Normalize the sum */
         Exp = norm_l( Acc0 ) ;
         Acc0 = L_shl( Acc0, Exp ) ;
         Acc1 = L_shl( Acc1, Exp ) ;

         Acc1 = L_shr( Acc1, (Word16) 8 ) ;

         LspVect[LspCnt] = div_l( Acc1, extract_h( Acc0 ) ) ;

     /* Add the upper part of the zero-crossing frequency,
        i.e. bits 7-15 */
         Exp = shl( (Word16) (i-1), (Word16) 7 ) ;
         LspVect[LspCnt] = add( LspVect[LspCnt], Exp ) ;
         LspCnt ++ ;

         /* Check if all zeros have been found */
         if ( LspCnt == (Word16) LpcOrder )
            break ;

     /* Switch the pointer between sum and difference polynomials */
         k ^= 1 ;

     /* Evaluate the new polynomial at the current frequency */
         CurrVal = (Word32) 0 ;
         for ( j = 0 ; j <= LpcOrder/2 ; j ++ )
            CurrVal = L_mac( CurrVal, Spq[LpcOrder-2*j+k],
CosineTable[i*j%CosineTableSize] ) ;
         }

      /* Update the previous value */
      PrevVal = CurrVal ;
      }


   /*
    * Check if all 10 zeros were found.  If not, ignore the results of
    * the search and use the previous frame's LSP frequencies instead.
    */
   if ( LspCnt != (Word16) LpcOrder ) {
      for ( j = 0 ; j < LpcOrder ; j ++ )
         LspVect[j] = PrevLsp[j] ;
   }

    return ;
}


/*
**
** Function:            Lsp_Qnt()
**
** Description:     Vector quantizes the LSP frequencies.  The LSP
**          vector is divided into 3 sub-vectors, or
**          bands, of dimension 3, 3, and 4.  Each band is
**          quantized separately using a different VQ
**          table.  Each table has 256 entries, so the
**          quantization generates three indices of 8 bits
**          each.  (Only the LSP vector for subframe 3 is
**          quantized per frame.)
**
** Links to text:   Section 2.5
**
** Arguments:       
**
**  Word16 CurrLsp[]    Unquantized LSP frequencies for the current frame (10 words)
**  Word16 PrevLsp[]    LSP frequencies from the previous frame (10 words)
**
** Outputs:             Quantized LSP frequencies for the current frame (10 words)
**
** Return value:
**
**  Word32      Long word packed with the 3 VQ indices.  Band 0
**          corresponds to bits [23:16], band 1 corresponds
**          to bits [15:8], and band 2 corresponds to bits [7:0].
**          (Bit 0 is the least significant.)
**
*/
Word32   Lsp_Qnt( Word16 *CurrLsp, Word16 *PrevLsp )
{
   int   i ;

   Word16   Wvect[LpcOrder] ;

   Word16   Tmp0,Tmp1   ;
   Word16   Exp   ;


   /* 
    * Compute the VQ weighting vector.  The weights assign greater
    * precision to those frequencies that are closer together.  
    */

   /* Compute the end differences */
   Wvect[0] = sub( CurrLsp[1], CurrLsp[0] ) ;
   Wvect[LpcOrder-1] = sub( CurrLsp[LpcOrder-1], CurrLsp[LpcOrder-2] ) ;

   /* Compute the rest of the differences */
   for ( i = 1 ; i < LpcOrder-1 ; i ++ ) {
      Tmp0 = sub( CurrLsp[i+1], CurrLsp[i] ) ;
      Tmp1 = sub( CurrLsp[i], CurrLsp[i-1] ) ;
      if ( Tmp0 > Tmp1 )
         Wvect[i] = Tmp1 ;
      else
         Wvect[i] = Tmp0 ;
      }

   /* Invert the differences */
   Tmp0 = (Word16) 0x0020 ;
   for ( i = 0 ; i < LpcOrder ; i ++ ) {

      if ( Wvect[i] > Tmp0 )
         Wvect[i] = div_s( Tmp0, Wvect[i] ) ;
      else
         Wvect[i] = MAX_16 ;
      }

   /* Normalize the weight vector */
   Tmp0 = (Word16) 0 ;
   for ( i = 0 ; i < LpcOrder ; i ++ )
      if ( Wvect[i] > Tmp0 )
         Tmp0 = Wvect[i] ;

   Exp = norm_s( Tmp0 ) ;
   for ( i = 0 ; i < LpcOrder ; i ++ )
      Wvect[i] = shl( Wvect[i], Exp ) ;


   /*
    * Compute the VQ target vector.  This is the residual that remains
    * after subtracting both the DC and predicted
    * components.
    */

   /* Subtract the DC component from both the current and previous LSP
      vectors.  */
   for ( i = 0 ; i < LpcOrder ; i ++ ) {
      CurrLsp[i] = sub( CurrLsp[i], LspDcTable[i] ) ;
      PrevLsp[i] = sub( PrevLsp[i], LspDcTable[i] ) ;
      }

  /* Generate the prediction vector and subtract it.  Use a constant
     first-order predictor based on the previous (DC-free) LSP
     vector. */
   for ( i = 0 ; i < LpcOrder ; i ++ ) {
      Tmp0 = mult_r( PrevLsp[i], (Word16) LspPrd0 ) ;
      CurrLsp[i] = sub( CurrLsp[i], Tmp0 ) ;
      }

   /* Add the DC component back to the previous LSP vector.  This
      vector is needed in later routines. */
   for ( i = 0 ; i < LpcOrder ; i ++ )
      PrevLsp[i] = add( PrevLsp[i], LspDcTable[i] ) ;

   /* Do the vector quantization for all three bands */
   return Lsp_Svq( CurrLsp, Wvect ) ;
}


/*
**
** Function:            Lsp_Svq()
**
** Description:     Performs the search of the VQ tables to find
**          the optimum LSP indices for all three bands.
**          For each band, the search finds the index which 
**          minimizes the weighted squared error between 
**          the table entry and the target.
**
** Links to text:   Section 2.5
**
** Arguments:       
**
**  Word16 Tv[]     VQ target vector (10 words)
**  Word16 Wvect[]      VQ weight vector (10 words)
**
** Outputs:         None
**
** Return value:    
**
**  Word32      Long word packed with the 3 VQ indices.  Band 0
**          corresponds to bits [23:16], band 1 corresponds
**          to bits [15:8], and band 2 corresponds to bits [7:0].
**              
*/
Word32   Lsp_Svq( Word16 *Tv, Word16 *Wvect )
{
   int   i,j,k ;

   Word32   Rez,Indx    ;
   Word32   Acc0,Acc1   ;

   Word16   Tmp[LpcOrder] ;
   Word16  *LspQntPnt  ;


   /* 
    * Initialize the return value
    */
   Rez = (Word32) 0 ;


   /* 
    * Quantize each band separately
    */
   for ( k = 0 ; k < LspQntBands ; k ++ ) {


      /* 
       * Search over the entire VQ table to find the index that
       * minimizes the error.  
       */

      /* Initialize the search */
      Acc1 = (Word32) -1 ;
      Indx = (Word32) 0 ;
      LspQntPnt = BandQntTable[k] ;

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

         /* Generate the metric, which is the negative error with the
            constant component removed. */