/*
   ITU-T G.729 Speech Coder with Annex B    ANSI-C Source Code
   Version 1.3    Last modified: August 1997

   Copyright (c) 1996,
   AT&T, France Telecom, NTT, Universite de Sherbrooke, Lucent Technologies,
   Rockwell International
   All rights reserved.
*/

/*-----------------------------------------------------------------*
 *   Functions Coder_ld8k and Init_Coder_ld8k                      *
 *             ~~~~~~~~~~     ~~~~~~~~~~~~~~~                      *
 *-----------------------------------------------------------------*/
#include <stdio.h>
#include <stdlib.h>

#include "typedef.h"
#include "basic_op.h"
#include "ld8k.h"
#include "tab_ld8k.h"
#include "oper_32b.h"
#include "vad.h"
#include "dtx.h"
#include "sid.h"

/*-----------------------------------------------------------*
 *    Coder constant parameters (defined in "ld8k.h")        *
 *-----------------------------------------------------------*
 *   L_WINDOW    : LPC analysis window size.                 *
 *   L_NEXT      : Samples of next frame needed for autocor. *
 *   L_FRAME     : 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        *
 *-----------------------------------------------------------*/

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

        /* Speech vector */

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

                /* Weighted speech vector */

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

                /* Excitation vector */

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

        /* Zero vector */

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

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

 /* For G.729B */
 /* DTX variables */
 static Word16 pastVad;   
 static Word16 ppastVad;
 static Word16 seed;


/*-----------------------------------------------------------------*
 *   Function  Init_Coder_ld8k                                     *
 *            ~~~~~~~~~~~~~~~                                      *
 *                                                                 *
 *  Init_Coder_ld8k(void);                                         *
 *                                                                 *
 *   ->Initialization of variables for the coder section.          *
 *       - initialize pointers to speech buffer                    *
 *       - initialize static  pointers                             *
 *       - set static vectors to zero                              *
 *                                                                 *
 *-----------------------------------------------------------------*/

void Init_Coder_ld8k(void)
{

  /*----------------------------------------------------------------------*
  *      Initialize pointers to speech vector.                            *
  *                                                                       *
  *                                                                       *
  *   |--------------------|-------------|-------------|------------|     *
  *     previous speech           sf1           sf2         L_NEXT        *
  *                                                                       *
  *   <----------------  Total speech vector (L_TOTAL)   ----------->     *
  *   <----------------  LPC analysis window (L_WINDOW)  ----------->     *
  *   |                   <-- present frame (L_FRAME) -->                 *
  * old_speech            |              <-- new speech (L_FRAME) -->     *
  * p_window              |              |                                *
  *                     speech           |                                *
  *                             new_speech                                *
  *-----------------------------------------------------------------------*/

  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;

  /* 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);
  sharp = SHARPMIN;

  /* Initialize lsp_old_q[] */
  Copy(lsp_old, lsp_old_q, M);
  Lsp_encw_reset();
  Init_exc_err();

  /* For G.729B */
  /* Initialize VAD/DTX parameters */
  pastVad = 1;
  ppastVad = 1;
  seed = INIT_SEED;
  vad_init();
  Init_lsfq_noise();

 return;
}

/*-----------------------------------------------------------------*
 *   Functions Coder_ld8k                                          *
 *            ~~~~~~~~~~                                           *
 *  Coder_ld8k(Word16 ana[], Word16 synth[]);                      *
 *                                                                 *
 *   ->Main coder function.                                        *
 *                                                                 *
 *                                                                 *
 *  Input:                                                         *
 *                                                                 *
 *   80 speech data should have been copied to vector new_speech[].*
 *   This vector is global and is declared in this function.       *
 *                                                                 *
 *  Ouputs:                                                        *
 *                                                                 *
 *    ana[]      ->analysis parameters.                            *
 *    synth[]    ->Local synthesis (for debug purpose)             *
 *                                                                 *
 *-----------------------------------------------------------------*/

void Coder_ld8k(
     Word16 ana[],      /* output  : Analysis parameters */
     Word16 synth[],    /* output  : Local synthesis     */
     Word16 frame,      /* input   : frame counter       */
     Word16 vad_enable  /* input   : VAD enable flag     */         
)
{

  /* LPC analysis */

  Word16 r_l[NP+1], r_h[NP+1];     /* Autocorrelations low and hi          */
  Word16 rc[M];                    /* Reflection coefficients.             */
  Word16 A_t[(MP1)*2];             /* A(z) unquantized for the 2 subframes */
  Word16 Aq_t[(MP1)*2];            /* A(z)   quantized for the 2 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 2th subframe                 */
  Word16 lsf_int[M];               /* Interpolated LSF 1st subframe.       */
  Word16 lsf_new[M];
  Word16 gamma1[2], gamma2[2];     /* Weighting factor for the 2 subframes */

  /* Other vectors */

  Word16 h1[L_SUBFR];            /* Impulse response h1[]              */
  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[4];             /* Correlations between xn & y1       */

  Word16 g_coeff_cs[5];
  Word16 exp_g_coeff_cs[5];      /* Correlations between xn, y1, & y2
                                     <y1,y1>, -2<xn,y1>,
                                          <y2,y2>, -2<xn,y2>, 2<y1,y2> */
  /* Scalars */

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

  /* For G.729B */
  Word16 rh_nbe[MP1];             
  Word16 lsfq_mem[MA_NP][M];
  Word16 exp_R0, Vad;
  
/*------------------------------------------------------------------------*
 *  - 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 the 2        *
 *         subframes (both quantized and unquantized)                     *
 *------------------------------------------------------------------------*/

  /* LP analysis */
  Autocorr(p_window, NP, r_h, r_l, &exp_R0);    /* Autocorrelations */
  Copy(r_h, rh_nbe, MP1);
  Lag_window(NP, r_h, r_l);                     /* Lag windowing    */
  Levinson(r_h, r_l, &A_t[MP1], rc, &temp);     /* Levinson Durbin  */
  Az_lsp(&A_t[MP1], lsp_new, lsp_old);          /* From A(z) to lsp */

  /* For G.729B */
  /* ------ VAD ------- */
  Lsp_lsf(lsp_new, lsf_new, M);
  vad(rc[1], lsf_new, r_h, r_l, exp_R0, p_window, frame, 
      pastVad, ppastVad, &Vad);
  Update_cng(rh_nbe, exp_R0, Vad);

  /*--------------------------------------------------------------------*
   * Find interpolated LPC parameters in all subframes (unquantized)    *
   * The interpolated parameters are in array A_t[] of size (M+1)*4     *
   *--------------------------------------------------------------------*/
  Int_lpc(lsp_old, lsp_new, lsf_int, lsf_new,  A_t);
  for (i=0; i<M; i++) lsp_old[i] = lsp_new[i];

  /*----------------------------------------------------------------------*
  * - Find the weighting factors                                         *
  *----------------------------------------------------------------------*/
  perc_var(gamma1, gamma2, lsf_int, lsf_new, rc);

 /*----------------------------------------------------------------------*
  * - Find the weighted input speech w_sp[] for the whole speech frame   *
  *----------------------------------------------------------------------*/
  Weight_Az(&A_t[0], gamma1[0], M, Ap1);
  Weight_Az(&A_t[0], gamma2[0], M, Ap2);
  Residu(Ap1, &speech[0], &wsp[0], L_SUBFR);
  Syn_filt(Ap2, &wsp[0], &wsp[0], L_SUBFR, mem_w, 1);

  Weight_Az(&A_t[MP1], gamma1[1], M, Ap1);
  Weight_Az(&A_t[MP1], gamma2[1], M, 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);

  /* ---------------------- */
  /* Case of Inactive frame */
  /* ---------------------- */
  
  if ((Vad == 0) && (vad_enable == 1)){
    Get_freq_prev(lsfq_mem);
    Cod_cng(exc, pastVad, lsp_old_q, Aq_t, ana, lsfq_mem, &seed);
    Update_freq_prev(lsfq_mem);
    ppastVad = pastVad;
    pastVad = Vad;

    /* UPDATE wsp, mem_w, mem_syn, mem_err, and mem_w0 */
    A  = A_t;     /* pointer to interpolated LPC parameters           */
    Aq = Aq_t;    /* pointer to interpolated quantized LPC parameters */
    i_gamma = 0;
    for(i_subfr=0; i_subfr < L_FRAME; i_subfr += L_SUBFR) {
      Weight_Az(A, gamma1[i_gamma], M, Ap1);
      Weight_Az(A, gamma2[i_gamma], M, Ap2);
      i_gamma = add(i_gamma,1);

      /* update mem_syn */
      Syn_filt(Aq, &exc[i_subfr], &synth[i_subfr], L_SUBFR, mem_syn, 1);

      /* update mem_w0 */
      for (i=0; i<L_SUBFR; i++)
        error[i] = sub(speech[i_subfr+i], synth[i_subfr+i]);
      Residu(Ap1, error, xn, L_SUBFR);
      Syn_filt(Ap2, xn, xn, L_SUBFR, mem_w0, 1);

      /* update mem_err */
      for (i = L_SUBFR-M, j = 0; i < L_SUBFR; i++, j++)
        mem_err[j] = error[i];
       
      A  += MP1;
      Aq += MP1;
    }
    
    sharp = SHARPMIN;
    
    /* Update memories for next frames */
    Copy(&old_speech[L_FRAME], &old_speech[0], L_TOTAL-L_FRAME);
    Copy(&old_wsp[L_FRAME], &old_wsp[0], PIT_MAX);
    Copy(&old_exc[L_FRAME], &old_exc[0], PIT_MAX+L_INTERPOL);
    
    return;

  }  /* End of inactive frame case */
    
  /* -------------------- */
  /* Case of Active frame */
  /* -------------------- */