/*
***********************************************************************
*
* Copyright (C) 1999 Ericsson Radio Systems AB
* 
* The Association of Radio Industries and Businesses (ARIB) has the
* licence to reproduce, distribute and publish the Bit Exact C Code
* programs in this CD-ROM together with the other parts of the RCR
* STD-27 (Personal Digital Cellular Telecommunication System)
* specification.
* 
* Ericsson has declared Option 1 (royalty-free licence) to ARIB for
* its Essential Industrial Property Rights related to the ACELP
* specification and the Bit Exact C Code programs. The royalty-free
* licence is limited to the development and manufacturing of
* products complying with the RCR STD-27H specification (or
* subsequent versions) for sale and use in Japan. Copying of the
* program(s) for any other purpose requires the prior written
* permission from Ericsson.
* 
* The user of the Bit Exact C Code hereby acknowledges the
* following:
* 
* 1.  Ericsson does not provide the user with any warranties,
*     whether express or implied, including but not limited to
*     warranties of merchantability and fitness for a particular
*     purpose relating to the Bit Exact C Code.
* 
* 2.  Ericsson shall have no responsibility whatsoever to indemnify
*     the user against actions or claims for infringement of any
*     intellectual property rights, e.g. patents, copyrights,
*     registered designs or any other rights, by reason of the
*     user's use of the Bit Exact C Code.
* 
***********************************************************************
*/

/***************************************************************************
 *
 *  FILE NAME:    cod_6k7.c
 *
 *  FUNCTIONS DEFINED IN THIS FILE:
 *                   Coder_6k7 and Init_Coder_6k7
 *
 *  Init_Coder_6k7(void):
 *      Initialization of variables for the coder section.
 *
 *  Coder_6k7(Word16 vox_mode, Word16 vox_ctrl, Word16 vad_ctrl, Word16 ana[], Word16 synth[]):
 *      Main encoder routine operating on a frame basis.
 *
 ***************************************************************************/

#include "typedef.h"
#include "basic_op.h"
#include "sig_proc.h"
#include "codec.h"
#include "cnst.h"
#include <stdio.h>

#include "window.tab"

#define VOX_HIST_SIZE 8
/*-----------------------------------------------------------*
 *    Coder constant parameters (defined in "cnst.h")        *
 *-----------------------------------------------------------*
 *   L_WINDOW    : LPC analysis window size.                 *
 *   L_NEXT      : Samples of next frame needed for autocor. *
 *   L_FRAME     : Frame size.                               *
 *   L_FRAME_BY2 : Half the frame size.                      *
 *   L_SUBFR     : Sub-frame size.                           *
 *   M           : LPC order.                                *
 *   MP1         : LPC order+1                               *
 *   L_TOTAL     : Total size of speech buffer.              *
 *   PIT_MIN     : Minimum pitch lag.                        *
 *   PIT_MAX     : Maximum pitch lag.                        *
 *   L_INTERPOL  : Length of filter for interpolation        *
 *   GAMMA1      : Weighting factor (numerator)              *
 *   GAMMA2      : Weighting factor (denominator)            *
 *-----------------------------------------------------------*/

/*--------------------------------------------------------*
 *         Static memory allocation.                      *
 *--------------------------------------------------------*/

 /* Speech vector */

static Word16 old_speech[L_TOTAL];
static Word16 *speech, *p_window;
Word16 *new_speech;             /* Global variable */

 /* Weight speech vector */

static Word16 old_wsp[L_FRAME_BY2 + PIT_MAX];
static Word16 *wsp;

 /* Excitation vector */

static Word16 old_exc[L_SUBFR + PIT_MAX + L_INTERPOL];
static Word16 *exc;

 /* Zero vector */

static Word16 ai_zero[L_SUBFR + MP1];
static Word16 *zero;

 /* Impulse response vector */

static Word16 *h1;
static Word16 hvec[L_SUBFR * 2];

 /* Spectral expansion factors */

static Word16 F_gamma1[M];
static Word16 F_gamma2[M];

 /* Lsp (Line spectral pairs) */

static Word16 lsp_old[M] = {30000, 26000, 21000, 15000, 8000,
                            0, -8000, -15000, -21000, -26000};
static Word16 lsp_old_q[M];

 /* Filter's memory */

static Word16 mem_syn[M], mem_w0[M], mem_w[M];
static Word16 mem_err[M + L_SUBFR], *error;

static Word16 sharp;

 /* VOX parameters */

static Word16 vox_lsp_hist[M*VOX_HIST_SIZE];  /* history of the last 8 frames lsp's */
static Word16 vox_log_en_hist[VOX_HIST_SIZE];
static Word16 vox_hist_ptr = 0;
static Word16 vox_log_en_index=0;
static Word16 vox_lsp[M] = {30000, 26000, 21000, 15000, 8000,
                            0, -8000, -15000, -21000, -26000};

Word16 lsp_init[M] = {30000, 26000, 21000, 15000, 8000,
                   0, -8000, -15000, -21000, -26000};
                   
static Word16 gp[N_FRAME];
extern Word16 vox_vad_hangover;
extern Word16 since_last_sid;
extern Word16 vox_frames;
extern Word16 vox_average_ok;
extern Word16 prev_vox_avg_ok;  

/***************************************************************************
 *  FUNCTION:   Init_Coder_6k7
 *
 *  PURPOSE:   Initialization of variables for the coder section.
 *
 *  DESCRIPTION:
 *       - initilize pointers to speech buffer
 *       - initialize static  pointers
 *       - set static vectors to zero
 *
 ***************************************************************************/


void Init_Coder_6k7(void)
{
    Word16 i;
/*--------------------------------------------------------------------------*
 *          Initialize pointers to speech vector.                           *
 *--------------------------------------------------------------------------*/
    
    new_speech = old_speech + L_TOTAL - L_FRAME;    /* New speech     */
    speech = new_speech - L_NEXT;    /* Present frame  */
    p_window = old_speech + L_TOTAL - L_WINDOW;    /* For LPC window */

    /* Initialize static pointers */

    wsp = old_wsp + PIT_MAX;
    exc = old_exc + PIT_MAX + L_INTERPOL;
    zero = ai_zero + MP1;
    error = mem_err + M;
    h1 = &hvec[L_SUBFR];

    /* Static vectors to zero */

    Set_zero(old_speech, L_TOTAL);
    Set_zero(old_exc, PIT_MAX + L_INTERPOL);
    Set_zero(old_wsp, PIT_MAX);
    Set_zero(mem_syn, M);
    Set_zero(mem_w, M);
    Set_zero(mem_w0, M);
    Set_zero(mem_err, M);
    Set_zero(zero, L_SUBFR);
    Set_zero(hvec, L_SUBFR);    /* set to zero "h1[-L_SUBFR..-1]" */

    /* Initialize lsp_old_q[] */

    Copy(lsp_old, lsp_old_q, M);

    /* Compute LPC spectral expansion factors */

    Weight_Fac(GAMMA1, F_gamma1);
    Weight_Fac(GAMMA2, F_gamma2);

    Set_zero(gp, N_FRAME);    /* Init Gp_Clipping */

    sharp = SHARPMIN;

    /* Initialize vox_lsp_hist */
    for (i=0; i < VOX_HIST_SIZE; i++)
        Copy(lsp_init, &vox_lsp_hist[i*M], M);

    return;
}

/***************************************************************************
 *  FUNCTION:   reset_cod_states
 *
 *  PURPOSE:   reset internal coder states for VOX
 *
 *  DESCRIPTION:
 *
 ***************************************************************************/
void reset_cod_states(void)
{
    Set_zero(old_exc, PIT_MAX + L_INTERPOL);
    Set_zero(old_wsp, PIT_MAX);
    Set_zero(mem_w, M);
    Set_zero(mem_w0, M);
    Set_zero(mem_err, M);
    Set_zero(zero, L_SUBFR);
    Set_zero(hvec, L_SUBFR);    /* set to zero "h1[-L_SUBFR..-1]" */
    sharp = SHARPMIN;
    Copy(lsp_init, lsp_old,M);
    Copy(lsp_init, lsp_old_q,M);
}


/***************************************************************************
 *   FUNCTION:   Coder_6k7
 *
 *   PURPOSE:  Main encoder routine.
 *
 *   DESCRIPTION: This function is called every 20 ms speech frame,
 *       operating on the newly read 160 speech samples. It performs the
 *       principle encoding functions to produce the set of encoded parameters
 *       which include the LSP, adaptive codebook, and fixed codebook
 *       quantization indices (addresses and gains).
 *
 *   INPUTS:
 *       No input argument are passed to this function. However, before
 *       calling this function, 160 new speech data should be copied to the
 *       vector new_speech[]. This is a global pointer which is declared in
 *       this file (it points to the end of speech buffer minus 160).
 *
 *   OUTPUTS:
 *
 *       ana[]:     vector of analysis parameters.
 *       synth[]:   Local synthesis speech (for debugging purposes)
 *
 ***************************************************************************/

void Coder_6k7(
    Word16 vox_mode,/* (i)  : vox mode flag       */ 
    Word16 vox_ctrl,/* (i)  : vox control flag    */ 
    Word16 vad_ctrl,/* (i)  : VAD control flag    */ 
    Word16 ana[],   /* (o)  : Analysis parameters */
    Word16 synth[]  /* (o)  : Local synthesis     */
)
{
    /* LPC coefficients */

    Word16 r_l[MP1], r_h[MP1];  /* Autocorrelations low and hi          */
    Word16 A_t[(MP1) * 4];      /* A(z) unquantized for the 4 subframes */
    Word16 Aq_t[(MP1) * 4];     /* A(z)   quantized for the 4 subframes */
    Word16 Ap1[MP1];            /* A(z) with spectral expansion         */
    Word16 Ap2[MP1];            /* A(z) with spectral expansion         */
    Word16 *A, *Aq;             /* Pointer on A_t and Aq_t              */
    Word16 lsp_new[M], lsp_new_q[M];  /* LSPs at 4th subframe           */

    /* Other vectors */

    Word16 xn[L_SUBFR];         /* Target vector for pitch search     */
    Word16 xn2[L_SUBFR];        /* Target vector for codebook search  */
    Word16 code[L_SUBFR];       /* Fixed codebook excitation          */
    Word16 y1[L_SUBFR];         /* Filtered adaptive excitation       */
    Word16 y2[L_SUBFR];         /* Filtered fixed codebook excitation */
    Word16 g_coeff[6];          /* Correlations between xn, y1, & y2: */


    /* Scalars */

    Word16 i, j, k, i_subfr;
    Word16 T_op, T0, T0_min, T0_max, T0_frac;
    Word16 gain_pit, gain_code, index, pit_flag;
    Word16 temp;
    Word32 L_temp;
    Word16 lsp_flag;
    Word16 GpC_flag;

    /* VAD and VOX variables */
    static Word16 VAD_flag = 1;
    Word16 lags[2], rc[4];
    static Word16 vox_state = 0;
    /* vox state has the following values :
       0 = normal speech
       1 = POST0
       2 = POST1
       3 = PRE
       4 = No transmission
    */

    Word16 vox_frame_type = 0;
    /* vox frame type has the following values :
       0 = normal speech frame or POST1 frame
       1 = POST0
       2 = PRE
       3 = No transmission
    */

    Word32 L_vox_frame_en;
    Word16 vox_log_en_e;
    Word16 vox_log_en_m;
    Word16 vox_log_en;
    Word16 vox_lsp_q[M];
    Word32 L_vox_lsp[M];
    Word16 init_lsf_vq_index;

/*------------------------------------------------------------------------*
 *  - Perform LPC analysis:                                               *
 *       * autocorrelation + lag windowing                                *
 *       * Levinson-durbin algorithm to find a[]                          *
 *       * convert a[] to lsp[]                                           *
 *       * quantize and code the LSPs                                     *
 *       * find the interpolated LSPs and convert to a[] for all          *
 *         subframes (both quantized and unquantized)                     *
 *------------------------------------------------------------------------*/

    /* LP analysis centered at 4th subframe */

    Autocorr(p_window, M, r_h, r_l, window_200_40);  /* Autocorrelations */
    Lag_window(M, r_h, r_l);  /* Lag windowing */
    Levinson(r_h, r_l, &A_t[MP1 * 3], rc);  /* Levinson Durbin  */

    /* VOX vad algorithm */
    if (vox_mode == 1)
    {
        /* VOX enabled or disabled depending on vad_input file */
        VAD_flag = vad_ctrl;
        if (vox_ctrl == 0) VAD_flag=1;
        vox_tx_handler(&vox_state, VAD_flag, &vox_frame_type);
        if (vox_frame_type == 3)
            *ana++ = 1;   /* BFI set i.e no transmission */
        else
            *ana++ = 0;
        *ana++ = vox_frame_type;
    }
    else
    {
        /* VOX disabled, active speech in every frame */
        VAD_flag = 1;

        *ana++ = 0;   /* bit position used as BFI at receiver */
        *ana++ = 0;   /* Two dummy bits used only in VOX mode */

    }
    
    /*---------------------------------------------------------------------*
     * DSP power saving action, Az_lsp allowed to execute only when needed *
     * i.e in speech mode, PRE and when a lsp's for a new POST frame       *
     * are about to be calculated                                          *
     *---------------------------------------------------------------------*/ 
    if (vox_state != 4 || 
        since_last_sid >= 42 ||  
        vox_frames <= 10) {
    
       /* Conversion directform to LSP */
       Az_lsp(&A_t[MP1 * 3], lsp_new, lsp_old);  /* From A(z) to lsp */
       lsp_flag = Check_LSP(lsp_new);
       
       /* LSP quantization */
       if (vox_state == 0) {
          Q_plsf_3(lsp_new, 0, lsp_new_q, ana, &init_lsf_vq_index);
          ana += 3;  /* Advance analysis parameters pointer */
       } else
          Copy(lsp_new, lsp_new_q, M);
       
    } else 
       Copy(lsp_init, lsp_old,M);
    
    /*------------------------------------------------------------------*
     * DSP power saving action, code below are only allowed to execute  *
     * in speech mode and PRE                                           *
     *------------------------------------------------------------------*/ 
    if (vox_state == 0 || 
        vox_state == 3) { 

       /*--------------------------------------------------------------------*
        * Find interpolated LPC parameters in all subframes (both quantized  *
        * and unquantized).                                                  *
        * The interpolated parameters are in array A_t[] of size (M+1)*4     *
        * and the quantized interpolated parameters are in array Aq_t[]      *
        *--------------------------------------------------------------------*/
      
       Int_lpc2(lsp_old, lsp_new, A_t);
       Int_lpc(lsp_old_q, lsp_new_q, Aq_t);
      
       /* update the LSPs for the next frame */
      
       for (i = 0; i < M; i++) {
           lsp_old[i] = lsp_new[i];
           lsp_old_q[i] = lsp_new_q[i];
       }
      
      
       /*----------------------------------------------------------------------*
        * - Find the weighted input speech w_sp[] for the whole speech frame   *
        * - Find the open-loop pitch delay for first 2 subframes               *
        * - Set the range for searching closed-loop pitch in 1st subframe      *
        * - Find the open-loop pitch delay for last 2 subframes                *
        *----------------------------------------------------------------------*/
      
      
       /* Find open loop pitch lag for first two subframes */
      
       Weight_Ai(&A_t[0], F_gamma1, Ap1);
       Weight_Ai(&A_t[0], F_gamma2, Ap2);
       Residu(Ap1, &speech[0], &wsp[0], L_SUBFR);
       Syn_filt(Ap2, &wsp[0], &wsp[0], L_SUBFR, mem_w, 1);
      
       Weight_Ai(&A_t[MP1], F_gamma1, Ap1);
       Weight_Ai(&A_t[MP1], F_gamma2, Ap2);
       Residu(Ap1, &speech[L_SUBFR], &wsp[L_SUBFR], L_SUBFR);
       Syn_filt(Ap2, &wsp[L_SUBFR], &wsp[L_SUBFR], L_SUBFR, mem_w, 1);
       
       
       T_op = Pitch_ol(wsp, PIT_MIN, PIT_MAX, L_FRAME_BY2);
       lags[0] = T_op;
      
       Copy(&old_wsp[L_FRAME_BY2], &old_wsp[0], PIT_MAX);
      
       /* Range for closed loop pitch search in 1st subframe */
      
       T0_min = sub(T_op, 3);
       if (sub(T0_min, PIT_MIN) < 0) {
           T0_min = PIT_MIN;
       }
       T0_max = add(T0_min, 6);
       if (sub(T0_max, PIT_MAX) > 0) {
           T0_max = PIT_MAX;
           T0_min = sub(T0_max, 6);
       }
       /* Find open loop pitch lag for last two subframes */
      
       Weight_Ai(&A_t[2 * MP1], F_gamma1, Ap1);
       Weight_Ai(&A_t[2 * MP1], F_gamma2, Ap2);
       Residu(Ap1, &speech[2 * L_SUBFR], &wsp[0], L_SUBFR);
       Syn_filt(Ap2, &wsp[0], &wsp[0], L_SUBFR, mem_w, 1);
      
       Weight_Ai(&A_t[3 * MP1], F_gamma1, Ap1);
       Weight_Ai(&A_t[3 * MP1], F_gamma2, Ap2);
       Residu(Ap1, &speech[3 * L_SUBFR], &wsp[L_SUBFR], L_SUBFR);
       Syn_filt(Ap2, &wsp[L_SUBFR], &wsp[L_SUBFR], L_SUBFR, mem_w, 1);
      
       T_op = Pitch_ol(wsp, PIT_MIN, PIT_MAX, L_FRAME_BY2);
       lags[1] = T_op;
      
       Copy(&old_wsp[L_FRAME_BY2], &old_wsp[0], PIT_MAX);
    }      

    /*------------------------------------------------------------------------*
     *  When in any of the VOX states 1-4 the subframe processing should      *
     *  not be performed, instead a separate SID frame processing             *
     *  is done for every POST1 frame                                         *
     * -----------------------------------------------------------------------*/
    if (vox_state == 0) {
   
         /*------------------------------------------------------------------------*
          *          Loop for every subframe in the analysis frame                 *
          *------------------------------------------------------------------------*
          *  To find the pitch and innovation parameters. The subframe size is     *
          *  L_SUBFR and the loop is repeated L_FRAME/L_SUBFR times.               *
          *     - find the weighted LPC coefficients                               *
          *     - find the LPC residual signal res[]                               *
          *     - compute the target signal for pitch search                       *
          *     - compute impulse response of weighted synthesis filter (h1[])     *
          *     - find the closed-loop pitch parameters                            *
          *     - encode the pitch dealy                                           *
          *     - update the impulse response h1[] by including fixed-gain pitch   *
          *     - find target vector for codebook search                           *
          *     - codebook search                                                  *
          *     - encode codebook address                                          *
          *     - VQ of pitch and codebook gains                                   *
          *     - find synthesis speech                                            *
          *     - update states of weighting filter                                *
          *------------------------------------------------------------------------*/
    
    
         A = A_t;      /* pointer to interpolated LPC parameters */
         Aq = Aq_t;    /* pointer to interpolated quantized LPC parameters */
        
         for (i_subfr = 0; i_subfr < L_FRAME; i_subfr += L_SUBFR) {
        
         /*---------------------------------------------------------------*
          * Find the weighted LPC coefficients for the weighting filter.  *
          *---------------------------------------------------------------*/
        
             Weight_Ai(A, F_gamma1, Ap1);
             Weight_Ai(A, F_gamma2, Ap2);
        
         /*---------------------------------------------------------------*
          * Compute impulse response, h1[], of weighted synthesis filter  *
          *---------------------------------------------------------------*/
        
             for (i = 0; i <= M; i++) {
                 ai_zero[i] = Ap1[i];
             }
        
             Syn_filt(Aq, ai_zero, h1, L_SUBFR, zero, 0);
             Syn_filt(Ap2, h1, h1, L_SUBFR, zero, 0);
        
             /*------------------------------------------------------------------------*
              *                                                                        *
              *          Find the target vector for pitch search:                      *
              *                                                                        *
              *------------------------------------------------------------------------*/
        
             Residu(Aq, &speech[i_subfr], exc, L_SUBFR);    /* LPC residual */
        
             Syn_filt(Aq, exc, error, L_SUBFR, mem_err, 0);
        
             Residu(Ap1, error, xn, L_SUBFR);
        
             Syn_filt(Ap2, xn, xn, L_SUBFR, mem_w0, 0);    /* target signal xn[]*/
        
             /*----------------------------------------------------------------------*
              *                 Closed-loop fractional pitch search                  *
              *----------------------------------------------------------------------*/
        
             pit_flag = i_subfr;    /* flag for first and 3th subframe  */
             if (sub(i_subfr, L_FRAME_BY2) == 0) { /* set t0_min and t0_max for 3th subf.*/
                T0_min = sub(T_op, 3);
                if (sub(T0_min, PIT_MIN) < 0) {
                   T0_min = PIT_MIN;
                }
                T0_max = add(T0_min, 6);
                if (sub(T0_max, PIT_MAX) > 0) {
                   T0_max = PIT_MAX;
                   T0_min = sub(T0_max, 6);
                }
                pit_flag = 0;
             }
             T0 = Pitch_fr3(exc, xn, h1, L_SUBFR, T0_min, T0_max,
                            pit_flag, &T0_frac);
        
             *ana++ = Enc_lag3(T0, T0_frac, &T0_min, &T0_max, PIT_MIN, PIT_MAX, pit_flag);
             /*-----------------------------------------------------------------*
              *   - find unity gain pitch excitation (adapitve codebook entry)  *
              *     with fractional interpolation.                              *
              *   - find filtered pitch exc. y1[]=exc[] convolve with h1[])     *
              *   - compute pitch gain and limit between 0 and 1.2              *
              *   - update target vector for codebook search                    *
              *   - find LTP residual.                                          *
              *-----------------------------------------------------------------*/
        
             Pred_lt_3(exc, T0, T0_frac, L_SUBFR);
             Convolve(exc, h1, y1, L_SUBFR);
             gain_pit = G_pitch(xn, y1, g_coeff, L_SUBFR);
        
             GpC_flag = 0;
             if ((lsp_flag != 0) && (sub(gain_pit, GP_CLIP) > 0)) {
                 GpC_flag = Check_Gp_Clipping(&gain_pit);
             }
        
             for (i = 0; i < L_SUBFR; i++) {
                 L_temp = L_mult(y1[i], gain_pit);
                 L_temp = L_shl(L_temp, 1);
                 xn2[i] = sub(xn[i], extract_h(L_temp));
             }
        
             /*-----------------------------------------------------------------*
              * - Inovative codebook search (find index and gain)               *
              *-----------------------------------------------------------------*/
        
             *ana++ = code_3i40_14bits(xn2, h1, T0, sharp, code, y2, &index);
             *ana++ = index;    /* sign index */
        
             /*------------------------------------------------------*
              * - Quantization of gains.                             *
              *------------------------------------------------------*/
             
             *ana++ = Qua_gain(xn, y1, y2, code, g_coeff, L_SUBFR,
                               &gain_pit, &gain_code, GpC_flag);
   
             Update_Gp_Clipping(gain_pit);
   
             /*------------------------------------------------------------*
              * - Update pitch sharpening "sharp" with quantized gain_pit  *
              *------------------------------------------------------------*/
        
             sharp = gain_pit;
             if (sub(sharp, SHARPMAX) > 0) {
                 sharp = SHARPMAX;
             }
             /*------------------------------------------------------*
              * - Find the total excitation                          *
              * - find synthesis speech corresponding to exc[]       *
              * - update filters memories for finding the target     *
              *   vector in the next subframe                        *
              *   (update error[-m..-1] and mem_w_err[])             *
              *------------------------------------------------------*/
        
             for (i = 0; i < L_SUBFR; i++) {
                 /* exc[i] = gain_pit*exc[i] + gain_code*code[i]; */
        
                 L_temp = L_mult(exc[i], gain_pit);
                 L_temp = L_mac(L_temp, code[i], gain_code);
                 L_temp = L_shl(L_temp, 1);
                 exc[i] = round(L_temp);
             }
        
             Syn_filt(Aq, exc, &synth[i_subfr], L_SUBFR, mem_syn, 1);
        
             for (i = L_SUBFR - M, j = 0; i < L_SUBFR; i++, j++) {
                 mem_err[j] = sub(speech[i_subfr + i], synth[i_subfr + i]);
                 temp = extract_h(L_shl(L_mult(y1[i], gain_pit), 1));
                 k = extract_h(L_shl(L_mult(y2[i], gain_code), 2));
                 mem_w0[j] = sub(xn[i], add(temp, k));
             }
        
             A += MP1;    /* interpolated LPC parameters for next subframe */
             Aq += MP1;
        
             /*--------------------------------------------------*
              * Update signal for next subframe.                 *
              * -> shift to the left by L_SUBFR:                 *
              *     exc[]                                        *
              *--------------------------------------------------*/
        
             Copy(&old_exc[L_SUBFR], &old_exc[0], PIT_MAX + L_INTERPOL);
         }
    } 
  

    if (VAD_flag == 0 || vox_state != 0) {
        /* VOX mode computation of SID parameters */

      if(VAD_flag == 0) {  
        /* update pointer to circular buffer      */
        vox_hist_ptr = add(vox_hist_ptr,1);
        if (vox_hist_ptr == VOX_HIST_SIZE)
           vox_hist_ptr = 0;

        /* copy lsp vector into buffer */
        Copy(lsp_new, &vox_lsp_hist[vox_hist_ptr*M],M);

        /* compute log energy based on frame energy */
        L_vox_frame_en = 0;     /* Q0 */
        for (i=0; i < L_FRAME; i++)
            L_vox_frame_en = L_mac(L_vox_frame_en, speech[i], speech[i]); 

        Log2(L_vox_frame_en, &vox_log_en_e, &vox_log_en_m);
        /* convert exponent and mantissa to Word16 Q10 */
        vox_log_en = shl(vox_log_en_e, 10);  /* Q10 */
        vox_log_en = add(vox_log_en, shr(vox_log_en_m, 15-10));

        /* divide with L_FRAME i.e subtract with 1 + log2(L_FRAME) =
           1 + 7.32193 */
        vox_log_en = sub(vox_log_en, 8522);

        /* insert into log energy buffer with division by 2 */
        vox_log_en = shr(vox_log_en,1);
        vox_log_en_hist[vox_hist_ptr] = vox_log_en; /* Q10 */
      }

        if (vox_state == 2) {
            /* POST1 mode, compute SID frame */
           if (vox_average_ok || prev_vox_avg_ok) {
              vox_log_en_index = 0;
              /* compute new SID frame if safe i.e don't
               * compute immediately after a talk spurt */
              vox_log_en = 0;
              for (i = 0; i < M; i++)
                 L_vox_lsp[i] = 0;

              /* average energy and lsp */
              for (i=0; i < VOX_HIST_SIZE; i++) {
                 vox_log_en = add(vox_log_en,
                                  shr(vox_log_en_hist[i],2));
                 for (j=0; j < M; j++)
                    L_vox_lsp[j] =
                       L_add(L_vox_lsp[j],
                                L_deposit_l(vox_lsp_hist[i*M+j]));
              }
              vox_log_en = shr(vox_log_en, 1);
              for (j=0; j < M; j++)
                 vox_lsp[j] = extract_l(L_shr(L_vox_lsp[j],3));

              /*  quantize logarithmic energy to 5 bits */
              vox_log_en_index = add(vox_log_en, 2560); /* +2.5 in Q10 */
              vox_log_en_index = add(vox_log_en_index, 128); /* add 0.5/4 in Q10 */
              vox_log_en_index = shr(vox_log_en_index, 8);
              if (vox_log_en_index > 63)
               vox_log_en_index = 63;
              if (vox_log_en_index < 0)
                 vox_log_en_index = 0;

              /* update gain predictor memory */
              vox_log_en = shl(vox_log_en_index,-2+10); /* Q10 and divide by 4 */
              vox_log_en = sub(vox_log_en, 2560);/* subtract 2.5 in Q10 */

              vox_log_en = sub(vox_log_en, 9000);
              if (vox_log_en > 0)
                 vox_log_en = 0;
              if (sub(vox_log_en,-14436) < 0)
                 vox_log_en = -14436;
              reset_gainq_mem(vox_log_en);
           }   

           /* Quantize lsp and put on parameter list */
           Q_plsf_3(vox_lsp, 1, vox_lsp_q, ana, &init_lsf_vq_index);
           ana += 3;  /* Advance analysis parameters pointer */
           *ana++ = shl(vox_log_en_index,2);
           /* gain bits occupy 6 msb of 8 bit lag value for first subframe */
                       
           /* here the rest of the bits are set, all bits
            * are set to zero except for 3 msb of lag for 
            * 3rd subframe                                         */
           *ana++ = 0;
           *ana++ = 0;
           *ana++ = 0;
           *ana++ = 0;
           *ana++ = 0;
           *ana++ = 0;
           *ana++ = 0;
           *ana++ = shl(init_lsf_vq_index,5);
           /* prediction residual index occupy 3 msb of lag 
            * value for third subframe                             */
           *ana++ = 0;
           *ana++ = 0;
           *ana++ = 0;
           *ana++ = 0;
           *ana++ = 0;
           *ana++ = 0;
           *ana++ = 0;
        }

    }

    if (!(vox_state == 0 || vox_state == 2)) {
       /* Set output to zero if POST0, PRE or no TX */
       Set_zero(ana, 19);
    }
    
   /*--------------------------------------------------*
    * Update signal for next frame.                    *
    * -> shift to the left by L_FRAME:                 *
    *     speech[]                                     *
    *--------------------------------------------------*/

    Copy(&old_speech[L_FRAME], &old_speech[0], L_TOTAL - L_FRAME);

    return;
}

/***************************************************************************
 *  Function  Check_LSP()                                                  *
 *                                                                         *
 *  Check the LSP's to detect resonances          .                        *
 *                                                                         *
 ***************************************************************************/

Word16 Check_LSP(
    Word16 *lsp             /* Unquantized LSP's */
)
{
    Word16 i, dist, dist_min1, dist_min2, dist_th;
    static Word16 count = 0;

  /* Check for a resonance:                             */
  /* Find minimum distance between lsp[i] and lsp[i+1]  */

    dist_min1 = MAX_16;
    for(i=3; i<M-2; i++) {
        dist = sub(lsp[i], lsp[i+1]);
        if(sub(dist, dist_min1) < 0){
            dist_min1 = dist;
        }
    }
    dist_min2 = MAX_16;
    for(i=1; i<3; i++) {
        dist = sub(lsp[i], lsp[i+1]);
        if(sub(dist, dist_min2) < 0){
            dist_min2 = dist;
        }
    }

    if (sub(lsp[1], 32000) > 0) dist_th = 600;
    else if (sub(lsp[1], 30500) > 0) dist_th = 800;
    else dist_th = 1100;

    if(sub(dist_min1, 1500) < 0 || sub(dist_min2, dist_th) < 0  )
        count = add(count, 1);
    else
        count = 0;

  /* Need 12 consecutive frames to set the flag */

    if( sub(count, 12) >= 0) {
        count = 12;
      return(1);
    } else
      return(0);
}

/***************************************************************************
 *  FUNCTION:  Check_Gp_Clipping()                                          
 *                                                                         
 *  Verify that the sum of the last (N_FRAME+1) pitch  
 *  gains is under a certain threshold.              
 *                                                                         
 ***************************************************************************/

Word16 Check_Gp_Clipping(
    Word16 *g_pitch        /* Pitch gain */
)
{
    Word16 i, sum;

    sum = shr(*g_pitch, 3);       /* Division by 8 */
    for(i=0; i<N_FRAME; i++)
        sum = add(sum, gp[i]);

    if (sub(sum, GP_CLIP) > 0) {
        *g_pitch = GP_CLIP;
        return(1);
    } else
        return(0);}

/***************************************************************************
 *  FUNCTION:  Update_Gp_Clipping()                                          
 *                                                                         
 *  Update past pitch gain memory
 *                                                                         
 ***************************************************************************/

void Update_Gp_Clipping(Word16 g_pitch)
{
    Copy(&gp[1], &gp[0], N_FRAME-1);
    gp[N_FRAME-1] = shr(g_pitch, 3);
}
