/*
   ITU-T G.729 Annex C+ - Reference C code for floating point
                         implementation of G.729 Annex C+
                         (integration of Annexes B, D and E)
                          Version 2.1 of October 1999
*/

/*
 File : CODLD8CP.C
*/

/*-----------------------------------------------------------------*
 *   Functions coder_ld8c and init_coder_ld8c                      *
 *             ~~~~~~~~~~     ~~~~~~~~~~~~~~~                      *
 *-----------------------------------------------------------------*/
#include <math.h>
#include "typedef.h"
#include "ld8k.h"
#include "ld8cp.h"
#include "tab_ld8k.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 FLOAT old_speech[L_TOTAL];
static FLOAT *speech, *p_window;
FLOAT  *new_speech;                       /* Global variable */

/* Weighted speech vector */
static FLOAT old_wsp[L_FRAME+PIT_MAX];
static FLOAT *wsp;

/* Excitation vector */
static FLOAT old_exc[L_FRAME+PIT_MAX+L_INTERPOL];
static FLOAT *exc;

/* Zero vector */
static FLOAT ai_zero[L_SUBFR+M_BWDP1];
static FLOAT *zero;

/* Lsp (Line spectral pairs) */
static FLOAT lsp_old[M]=
{ (F)0.9595,  (F)0.8413,  (F)0.6549,  (F)0.4154,  (F)0.1423,
(F)-0.1423, (F)-0.4154, (F)-0.6549, (F)-0.8413, (F)-0.9595};
static FLOAT lsp_old_q[M];

/* Filter's memory */
static FLOAT mem_syn[M_BWD], mem_w0[M_BWD], mem_w[M_BWD];
static FLOAT mem_err[M_BWD+L_SUBFR], *error;
static FLOAT pit_sharp;

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

/* for G.729E */
/* for the backward analysis */
static FLOAT prev_filter[M_BWDP1]; /* Previous selected filter */

static FLOAT rexp[M_BWDP1];
static FLOAT synth[L_ANA_BWD];
static FLOAT *synth_ptr;
static int prev_lp_mode ;
static FLOAT gamma1[2], gamma2[2];       /* Weighting factor for the 2 subframes */
static FLOAT A_t_bwd_mem[M_BWDP1];
static int bwd_dominant;
static FLOAT C_int;              /* See file bwfw.c */
static INT16 glob_stat;  /* Mesure of global stationnarity */
static INT16 stat_bwd;       /* Nbre of consecutive backward frames */
static INT16 val_stat_bwd;   /* Value associated with stat_bwd */

/* Last backward A(z) for case of unstable filter */
static FLOAT old_A_bwd[M_BWDP1];
static FLOAT old_rc_bwd[2];
/* Last forkward A(z) for case of unstable filter */
static FLOAT old_A_fwd[MP1];
static FLOAT old_rc_fwd[2];
static FLOAT freq_prev[MA_NP][M];    /* previous LSP vector       */

static int lag_buf[5]={20,20, 20, 20,20};
static FLOAT pgain_buf[5]={(F)0.7,(F)0.7, (F)0.7, (F)0.7,(F)0.7};
#define         AVG(a,b,c,d) (int)(((a)+(b)+(c)+(d))/((F)4.0)+(F)0.5)

/*----------------------------------------------------------------------------
 * init_coder_ld8c - initialization of variables for the encoder
 *----------------------------------------------------------------------------
 */

void init_coder_ld8c(
    int dtx_enable   /* input : DTX enable flag */
)
{
/*-----------------------------------------------------------------------*
*      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_wind            |              |                                *
*                     speech           |                                *
*                             new_speech                                *
*-----------------------------------------------------------------------*/
    int   i;

    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 + M_BWDP1;
    error  = mem_err + M_BWD;

    /* 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_BWD);
    set_zero(mem_w,   M_BWD);
    set_zero(mem_w0,  M_BWD);
    set_zero(mem_err, M_BWD);
    set_zero(zero, L_SUBFR);
    pit_sharp = SHARPMIN;
    
    /* Initialize lsp_old_q[] */
    copy(lsp_old, lsp_old_q, M);
    
    lsp_encw_resete(freq_prev);
    init_exc_err();
    
    /* For G.729B */
    /* Initialize VAD/DTX parameters */
    if(dtx_enable == 1) {
        pastVad = 1;
        ppastVad = 1;
        seed = INIT_SEED;
        vad_init();
        init_lsfq_noise();
    }
    /* for G.729E */
    /* for the backward analysis */
    set_zero(synth, L_ANA_BWD);
    synth_ptr = synth + MEM_SYN_BWD;
    prev_lp_mode = 0;
    bwd_dominant = 0;              /* See file bwfw.c */
    C_int = (F)1.1;       /* Filter interpolation parameter */
    glob_stat = 10000;  /* Mesure of global stationnarity */
    stat_bwd = 0;       /* Nbre of consecutive backward frames */
    val_stat_bwd = 0;   /* Value associated with stat_bwd */

    for(i=0; i<M_BWDP1; i++) rexp[i] = (F)0.;

    A_t_bwd_mem[0] = (F)1.;
    for (i=1; i<M_BWDP1; i++) A_t_bwd_mem[i] = (F)0.;
    set_zero(prev_filter, M_BWDP1);
    prev_filter[0] = (F)1.;

    set_zero(old_A_bwd, M_BWDP1);
    old_A_bwd[0]= (F)1.;
    set_zero(old_rc_bwd, 2);

    set_zero(old_A_fwd, MP1);
    old_A_fwd[0]= (F)1.;
    set_zero(old_rc_fwd, 2);


    return;
}

/*----------------------------------------------------------------------------
* coder_ld8c - encoder routine ( speech data should be in new_speech )
*----------------------------------------------------------------------------
*/
void coder_ld8c(
    int ana[],        /* output: analysis parameters */
    int frame,        /* input : frame counter */
    int dtx_enable,   /* input : DTX enable flag */
    int rate          /* input : rate selector/ G729, G729D, and G729E */
)
{
    /* LPC analysis */
    FLOAT r_fwd[NP+1];          /* Autocorrelations (forward) */
    FLOAT r_bwd[M_BWDP1];      /* Autocorrelations (backward) */
    FLOAT rc_fwd[M];           /* Reflection coefficients : forward analysis */
    FLOAT rc_bwd[M_BWD];       /* Reflection coefficients : backward analysis */
    FLOAT A_t_fwd[MP1*2];      /* A(z) forward unquantized for the 2 subframes */
    FLOAT A_t_fwd_q[MP1*2];    /* A(z) forward quantized for the 2 subframes */
    FLOAT A_t_bwd[2*M_BWDP1];  /* A(z) backward for the 2 subframes */
    FLOAT *Aq;           /* A(z) "quantized" for the 2 subframes */
    FLOAT *Ap;           /* A(z) "unquantized" for the 2 subframes */
    FLOAT *pAp, *pAq;
    FLOAT Ap1[M_BWDP1];          /* A(z) with spectral expansion         */
    FLOAT Ap2[M_BWDP1];          /* A(z) with spectral expansion         */
    FLOAT lsp_new[M], lsp_new_q[M]; /* LSPs at 2th subframe                 */
    FLOAT lsf_int[M];               /* Interpolated LSF 1st subframe.       */
    FLOAT lsf_new[M];
    int lp_mode;                  /* LP Backward (1) / Forward (0) Indication mode */
    int m_ap, m_aq;
    int code_lsp[2];
    
    /* Other vectors */
    FLOAT h1[L_SUBFR];           /* Impulse response h1[]              */
    FLOAT xn[L_SUBFR];           /* Target vector for pitch search     */
    FLOAT xn2[L_SUBFR];          /* Target vector for codebook search  */
    FLOAT code[L_SUBFR];         /* Fixed codebook excitation          */
    FLOAT y1[L_SUBFR];           /* Filtered adaptive excitation       */
    FLOAT y2[L_SUBFR];           /* Filtered fixed codebook excitation */
    FLOAT res2[L_SUBFR];         /* Pitch prediction residual          */
    FLOAT g_coeff[5];            /* Correlations between xn, y1, & y2:
                                 <y1,y1>, <xn,y1>, <y2,y2>, <xn,y2>,<y1,y2>*/
                                 
    /* Scalars */
    int   i, j, i_gamma, i_subfr;
    int   T_op, t0, t0_min, t0_max, t0_frac;
    int   index, taming;
    FLOAT gain_pit, gain_code;

    /* for G.729E */
    int sat_filter;
    FLOAT freq_cur[M];
    
    /* For G.729B */
    FLOAT r_nbe[MP1];
    FLOAT lsfq_mem[MA_NP][M];
    int Vad;
    FLOAT Energy_db;