/*
**
** 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 5.00
    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} amd {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.
  */