/*
**
** File:    lpc.c
**
** Description: Functions that implement linear predictive coding 
**      (LPC) operations.  
**
** Functions:
**
**  Computing LPC coefficients:
**
**      Comp_Lpc()
**      Durbin()
**
**  Perceptual noise weighting:
**
**      Wght_Lpc()
**      Error_Wght()
**
**  Computing combined impulse response:
**
**      Comp_Ir()
**
**  Computing ringing response:
**
**      Sub_Ring()
**      Upd_Ring()
**
**  Synthesizing speech:
**
**      Synt()
**      Spf()
*/

/*
    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 "lbccodec.h"
#include "coder.h"
#include "decod.h"
#include "util_lbc.h"
#include "lpc.h"


/*
**
** Function:        Comp_Lpc()
**
** Description:     Computes the tenth-order LPC filters for an
**          entire frame.  For each subframe, a
**          Hamming-windowed block of 180 samples,
**          centered around the subframe, is used to
**          compute eleven autocorrelation coefficients.
**          The Levinson-Durbin algorithm then generates
**          the LPC coefficients.  This function requires
**          a look-ahead of one subframe, and hence
**          introduces a 7.5 ms encoding delay.
**
** Links to text:   Section 2.4
**
** Arguments:       
**
**  Word16 *UnqLpc      Empty Buffer
**  Word16 PrevDat[]    Previous 2 subframes of samples (120 words)
**  Word16 DataBuff[]   Current frame of samples (240 words)
**
** Outputs:

**
**  Word16 UnqLpc[]     LPC coefficients for entire frame (40 words)
**
** Return value:    None
**
*/
void  Comp_Lpc( Word16 *UnqLpc, Word16 *PrevDat, Word16 *DataBuff )
{
   int   i,j,k ;

   Word16   Dpnt[Frame+LpcFrame-SubFrLen] ;
   Word16   Vect[LpcFrame] ;
   Word16   Corr[LpcOrder] ;
   Word16   Err   ;
   Word16   Exp   ;

   Word32   Acc0,Acc1   ;

   /*

    * Generate a buffer of 360 samples.  This consists of 120 samples
    * from the previous frame and 240 samples from the current frame.
    */
   for ( i = 0 ; i < LpcFrame-SubFrLen ; i ++ )
      Dpnt[i] = PrevDat[i] ;
   for ( i = 0 ; i < Frame ; i ++ )
      Dpnt[i+LpcFrame-SubFrLen] = DataBuff[i] ;


   /*
    * Repeat for all subframes 
    */
   for ( k = 0 ; k < SubFrames ; k ++ ) {


     /* 
      * Do windowing
      */
    
     /* Get block of 180 samples centered around current subframe */
      for ( i = 0 ; i < LpcFrame ; i ++ )
         Vect[i] = Dpnt[k*SubFrLen+i] ;

      /* Normalize */
      Vec_Norm( Vect, (Word16) LpcFrame ) ;

      /* Apply the Hamming window */
      for ( i = 0 ; i < LpcFrame ; i ++ )
         Vect[i] = mult_r(Vect[i], HammingWindowTable[i]) ;


      /* 
       * Compute the autocorrelation coefficients
       */

      /* Compute the zeroth-order coefficient (energy) */
      Acc1 = (Word32) 0 ;
      for ( i = 0 ; i < LpcFrame ; i ++ ) {
         Acc0 = L_mult( Vect[i], Vect[i] ) ;
         Acc0 = L_shr( Acc0, (Word16) 1 ) ;
         Acc1 = L_add( Acc1, Acc0 ) ;
         }

      /* Apply a white noise correction factor of (1025/1024) */
      Acc0 = L_shr( Acc1, (Word16) RidgeFact ) ;
      Acc1 = L_add( Acc1, Acc0 ) ;

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

      Err = round( Acc1 ) ;

      /* Compute the rest of the autocorrelation coefficients.
         Multiply them by a binomial coefficients lag window. */
      for ( i = 1 ; i <= LpcOrder ; i ++ ) {
         Acc1 = (Word32) 0 ;
         for ( j = i ; j < LpcFrame ; j ++ ) {
            Acc0 = L_mult( Vect[j], Vect[j-i] ) ;
            Acc0 = L_shr( Acc0, (Word16) 1 ) ;
            Acc1 = L_add( Acc1, Acc0 ) ;
            }
         Acc0 = L_shl( Acc1, Exp ) ;
         Acc0 = L_mls( Acc0, BinomialWindowTable[i-1] ) ;
         Corr[i-1] = round(Acc0) ;
         }


      /*
       * Apply the Levinson-Durbin algorithm to generate the LPC
       * coefficients
       */
      Durbin( &UnqLpc[k*LpcOrder], Corr, Err ) ;
      }

    /* Update sine detector */
    CodStat.SinDet &= 0x7fff ;

    j = CodStat.SinDet ;
    k = 0 ;
    for ( i = 0 ; i < 15 ; i ++ ) {
        k += j & 1 ;
        j >>= 1 ;
    }
    if ( k >= 14 )
        CodStat.SinDet |= 0x8000 ;
}


/*
**
** Function:        Durbin()
**
** Description:     Implements the Levinson-Durbin algorithm for a
**          subframe.  The Levinson-Durbin algorithm
**          recursively computes the minimum mean-squared
**          error (MMSE) linear prediction filter based on the
**          estimated autocorrelation coefficients.
**
** Links to text:   Section 2.4
**
** Arguments:       
**
**  Word16 *Lpc Empty buffer
**  Word16 Corr[]   First- through tenth-order autocorrelations (10 words)
**  Word16 Err  Zeroth-order autocorrelation, or energy
**
** Outputs:     
**
**  Word16 Lpc[]    LPC coefficients (10 words)
**
** Return value:    The error
**
*/
Word16  Durbin( Word16 *Lpc, Word16 *Corr, Word16 Err )
{
    int     i,j   ;

    Word16  Temp[LpcOrder] ;
    Word16  Pk ;

    Word32  Acc0,Acc1,Acc2 ;
    extern  CODSTATDEF  CodStat  ;

   /*
    * Initialize the LPC vector
    */
    for ( i = 0 ; i < LpcOrder ; i ++ )
        Lpc[i] = (Word16) 0 ;

   /*
    * Do the recursion.  At the ith step, the algorithm computes the
    * (i+1)th - order MMSE linear prediction filter.
    */
   for ( i = 0 ; i < LpcOrder ; i ++ ) {

     /*
      * Compute the partial correlation (parcor) coefficient
      */

     /* Start parcor computation */
      Acc0 = L_deposit_h( Corr[i] ) ;
      Acc0 = L_shr( Acc0, (Word16) 2 ) ;
      for ( j = 0 ; j < i ; j ++ )
         Acc0 = L_msu( Acc0, Lpc[j], Corr[i-j-1] ) ;
      Acc0 = L_shl( Acc0, (Word16) 2 ) ;

      /* Save sign */
      Acc1 = Acc0 ;
      Acc0 = L_abs( Acc0 ) ;

      /* Finish parcor computation */
      Acc2 = L_deposit_h( Err ) ;
      if ( Acc0 >= Acc2 )
         break ;

      Pk = div_l( Acc0, Err ) ;

      if ( Acc1 >= 0 )
         Pk = negate(Pk) ;

      /*
       * Sine detector
       */
        if ( i == 1 ) {
        CodStat.SinDet <<= 1 ;
            if ( Pk > 0x799a )
                CodStat.SinDet ++ ;
        }

      /*
       * Compute the ith LPC coefficient 
       */
      Acc0 = L_deposit_h( negate(Pk) ) ;
      Acc0 = L_shr( Acc0, (Word16) 2 ) ;
      Lpc[i] = round( Acc0 ) ;

      /* 
       * Update the prediction error 
       */
      Acc1 = L_mls( Acc1, Pk ) ;
      Acc1 = L_add( Acc1, Acc2 ) ;
      Err = round( Acc1 ) ;

      /* 
       * Compute the remaining LPC coefficients 
       */
      for ( j = 0 ; j < i ; j ++ )
         Temp[j] = Lpc[j] ;

      for ( j = 0 ; j < i ; j ++ ) {
         Acc0 = L_deposit_h( Lpc[j] ) ;
         Acc0 = L_mac( Acc0, Pk, Temp[i-j-1] ) ;
         Lpc[j] = round( Acc0 ) ;
         }
      }
    return Err ;
}


/*
**
** Function:        Wght_Lpc()
**
** Description:     Computes the formant perceptual weighting
**          filter coefficients for a frame.  These
**          coefficients are geometrically scaled versions
**          of the unquantized LPC coefficients.
**
** Links to text:   Section 2.8  
**
** Arguments:       
**
**  Word16 *PerLpc      Empty Buffer
**  Word16 UnqLpc[]     Unquantized LPC coefficients (40 words)
**
** Outputs:     

**
**  Word16 PerLpc[]     Perceptual weighting filter coefficients
**              (80 words)
**
** Return value:    None
**
*/
void  Wght_Lpc( Word16 *PerLpc, Word16 *UnqLpc )
{
   int   i,j   ;


   /*
    * Do for all subframes
    */
   for ( i = 0 ; i < SubFrames ; i ++ ) {


     /*
      * Compute the jth FIR coefficient by multiplying the jth LPC
      * coefficient by (0.9)^j.  
      */
      for ( j = 0 ; j < LpcOrder ; j ++ )
         PerLpc[j] = mult_r( UnqLpc[j], PerFiltZeroTable[j] ) ;
      PerLpc += LpcOrder ;


     /*
      * Compute the jth IIR coefficient by multiplying the jth LPC
      * coefficient by (0.5)^j. 
      */
      for ( j = 0 ; j < LpcOrder ; j ++ )
         PerLpc[j] = mult_r( UnqLpc[j], PerFiltPoleTable[j] ) ;
      PerLpc += LpcOrder ;
      UnqLpc += LpcOrder ;
      }
}


/*
**
** Function:        Error_Wght()
**
** Description:     Implements the formant perceptual weighting
**          filter for a frame. This filter effectively
**          deemphasizes the formant frequencies in the
**          error signal.
**
** Links to text:   Section 2.8
**
** Arguments:       
**
**  Word16 Dpnt[]       Highpass filtered speech x[n] (240 words)
**  Word16 PerLpc[]     Filter coefficients (80 words)
**
** Inputs:
**
**  CodStat.WghtFirDl[] FIR filter memory from previous frame (10 words)
**  CodStat.WghtIirDl[] IIR filter memory from previous frame (10 words)

**
** Outputs:     
**
**  Word16 Dpnt[]       Weighted speech f[n] (240 words)
**
** Return value:    None
**
*/
void  Error_Wght( Word16 *Dpnt, Word16 *PerLpc )
{
   int   i,j,k ;

   Word32   Acc0  ;


   /* 
    * Do for all subframes
    */
   for ( k = 0 ; k < SubFrames ; k ++ ) {
      for ( i = 0 ; i < SubFrLen ; i ++ ) {


         /* 
      * Do the FIR part 
      */

    /* Filter */
         Acc0 = L_mult( *Dpnt, (Word16) 0x2000 ) ;
         for ( j = 0 ; j < LpcOrder ; j ++ )
            Acc0 = L_msu( Acc0, PerLpc[j], CodStat.WghtFirDl[j] ) ;

     /* Update memory */
         for ( j = LpcOrder-1 ; j > 0 ; j -- )
            CodStat.WghtFirDl[j] = CodStat.WghtFirDl[j-1] ;
         CodStat.WghtFirDl[0] = *Dpnt ;


         /* 
      * Do the IIR part 
      */

     /* Filter */
         for ( j = 0 ; j < LpcOrder ; j ++ )
            Acc0 = L_mac( Acc0, PerLpc[LpcOrder+j], CodStat.WghtIirDl[j] ) ;
         for ( j = LpcOrder-1 ; j > 0 ; j -- )
            CodStat.WghtIirDl[j] = CodStat.WghtIirDl[j-1] ;

         Acc0 = L_shl( Acc0, (Word16) 2 ) ;

     /* Update memory */
         CodStat.WghtIirDl[0] = round( Acc0 ) ;
         *Dpnt ++ = CodStat.WghtIirDl[0] ;
         }
      PerLpc += 2*LpcOrder ;
      }
}


/*
**
** Function:        Comp_Ir()
**
** Description:     Computes the combined impulse response of the
**          formant perceptual weighting filter, harmonic
**          noise shaping filter, and synthesis filter for
**          a subframe.  
**
** Links to text:   Section 2.12
**
** Arguments:       
**
**  Word16 *ImpResp     Empty Buffer
**  Word16 QntLpc[]     Quantized LPC coefficients (10 words)
**  Word16 PerLpc[]     Perceptual filter coefficients (20 words)
**  PWDEF Pw        Harmonic noise shaping filter parameters
**
** Outputs:     
**
**  Word16 ImpResp[]    Combined impulse response (60 words)
**
** Return value:    None
**
*/
void  Comp_Ir( Word16 *ImpResp, Word16 *QntLpc, Word16 *PerLpc, PWDEF Pw )
{
   int   i,j   ;

   Word16   FirDl[LpcOrder] ;
   Word16   IirDl[LpcOrder] ;
   Word16   Temp[PitchMax+SubFrLen] ;

   Word32   Acc0,Acc1 ;


   /* 
    * Clear all memory.  Impulse response calculation requires
    * an all-zero initial state.
    */

   /* Perceptual weighting filter */
   for ( i = 0 ; i < LpcOrder ; i ++ )
      FirDl[i] = IirDl[i] = (Word16) 0 ;

   /* Harmonic noise shaping filter */
   for ( i = 0 ; i < PitchMax+SubFrLen ; i ++ )
      Temp[i] = (Word16) 0 ;


   /* 
    * Input a single impulse
    */
   Acc0 = (Word32) 0x04000000L ;


   /* 
    * Do for all elements in a subframe
    */
   for ( i = 0 ; i < SubFrLen ; i ++ ) {


      /* 
       * Synthesis filter 
       */
      for ( j = 0 ; j < LpcOrder ; j ++ )
         Acc0 = L_mac( Acc0, QntLpc[j], FirDl[j] ) ;
      Acc1 = L_shl( Acc0, (Word16) 2 ) ;