/***************************************************************************
 *
 *   File Name: vad.c
 *
 *   Purpose:   Contains all functions for voice activity detection, as
 *              described in the high level specification of VAD.
 *
 *     Below is a listing of all the functions appearing in the file.
 *     The functions are arranged according to their purpose.  Under
 *     each heading, the ordering is hierarchical.
 *
 *     Resetting of static variables of VAD:
 *       reset_vad()
 *
 *     Main routine of VAD (called by the speech encoder):
 *       vad_computation()
 *       Adaptive filtering and energy computation:
 *         energy_computation()
 *       Averaging of autocorrelation function values:
 *         acf_averaging()
 *       Computation of predictor values:
 *         predictor_values()
 *           schur_recursion()
 *           step_up()
 *           compute_rav1()
 *       Spectral comparison:
 *         spectral_comparison()
 *       Information tone detection:
 *         tone_detection()
 *           step_up()
 *       Threshold adaptation:
 *         threshold_adaptation()
 *       VAD decision:
 *         vad_decision()
 *       VAD hangover addition:
 *         vad_hangover()
 *
 *     Periodicity detection routine (called by the speech encoder):
 *       periodicity_detection()
 *
 **************************************************************************/

#include "gsm_efr.h"
#include "typedef.h"
#include "namespace.h"
#include "cnst.h"
#include "basic_op.h"
#include "oper_32b.h"
#include "no_count.h"
#include "vad.h"
#include "enc_state.h"

/* Constants of VAD hangover addition */

#define HANGCONST 10
#define BURSTCONST 3

/* Constant of spectral comparison */

#define STAT_THRESH 3670L       /* 0.056 */

/* Constants of periodicity detection */

#define LTHRESH 2
#define NTHRESH 4

/* Pseudo floating point representations of constants
   for threshold adaptation */

#define M_PTH    32500          /*** 130000.0 ***/
#define E_PTH    17
#define M_PLEV   21667          /*** 346666.7 ***/
#define E_PLEV   19
#define M_MARGIN 16927          /*** 69333340 ***/
#define E_MARGIN 27

#define FAC 17203               /* 2.1 */

/* Constants of tone detection */

#define FREQTH 3189
#define PREDTH 1464

/* forward declarations for internal functions */

static void energy_computation (
    Word16 r_h[],
    Word16 scal_acf,
    Word16 rvad[],
    Word16 scal_rvad,
    Pfloat * acf0,
    Pfloat * pvad
);

static void acf_averaging (
    struct EFR_encoder_state *st,
    Word16 r_h[],
    Word16 r_l[],
    Word16 scal_acf,
    Word32 L_av0[],
    Word32 L_av1[]
);

static void predictor_values (
    Word32 L_av1[],
    Word16 rav1[],
    Word16 *scal_rav1
);

static void schur_recursion (
    Word32 L_av1[],
    Word16 vpar[]
);

static void step_up (
    Word16 np,
    Word16 vpar[],
    Word16 aav1[]
);

static void compute_rav1 (
    Word16 aav1[],
    Word16 rav1[],
    Word16 *scal_rav1
);

static Word16 spectral_comparison (
    struct EFR_encoder_state *st,
    Word16 rav1[],
    Word16 scal_rav1,
    Word32 L_av0[]
);

static void threshold_adaptation (
    struct EFR_encoder_state *st,
    Word16 stat,
    Word16 ptch,
    Word16 tone,
    Word16 rav1[],
    Word16 scal_rav1,
    Pfloat pvad,
    Pfloat acf0,
    Word16 rvad[],
    Word16 *scal_rvad,
    Pfloat * thvad
);

static void tone_detection (
    Word16 rc[],
    Word16 *tone
);

static Word16 vad_decision (
    Pfloat pvad,
    Pfloat thvad
);

static Word16 vad_hangover (
    struct EFR_encoder_state *st,
    Word16 vvad
);

/*************************************************************************
 *
 *   FUNCTION NAME: vad_reset
 *
 *   PURPOSE:  Resets the static variables of the VAD to their
 *             initial values
 *
 *************************************************************************/

void vad_reset (struct EFR_encoder_state *st)
{
    struct vad_state *vst = &st->vad;
    Word16 i;

    /* Initialize rvad variables */
    vst->rvad[0] = 0x6000;
    for (i = 1; i < 9; i++)
    {
        vst->rvad[i] = 0;
    }
    vst->scal_rvad = 7;

    /* Initialize threshold level */
    vst->thvad.e = 20;               /*** exponent ***/
    vst->thvad.m = 27083;            /*** mantissa ***/

    /* Initialize ACF averaging variables */
    for (i = 0; i < 27; i++)
    {
        vst->L_sacf[i] = 0L;
    }
    for (i = 0; i < 36; i++)
    {
        vst->L_sav0[i] = 0L;
    }
    vst->pt_sacf = 0;
    vst->pt_sav0 = 0;

    /* Initialize spectral comparison variable */
    vst->L_lastdm = 0L;

    /* Initialize threshold adaptation variable */
    vst->adaptcount = 0;

    /* Initialize VAD hangover addition variables */
    vst->burstcount = 0;
    vst->hangcount = -1;

    /* Initialize periodicity detection variables */
    vst->oldlagcount = 0;
    vst->veryoldlagcount = 0;
    vst->oldlag = 18;

    st->ptch = 1;

    return;
}

/****************************************************************************
 *
 *     FUNCTION:  vad_computation
 *
 *     PURPOSE:   Returns a decision as to whether the current frame being
 *                processed by the speech encoder contains speech or not.
 *
 *     INPUTS:    r_h[0..8]     autocorrelation of input signal frame (msb)
 *                r_l[0..8]     autocorrelation of input signal frame (lsb)
 *                scal_acf      scaling factor for the autocorrelations
 *                rc[0..3]      speech encoder reflection coefficients
 *                ptch          flag to indicate a periodic signal component
 *
 *     OUTPUTS:   none
 *
 *     RETURN VALUE: vad decision
 *
 ***************************************************************************/

Word16 vad_computation (
    struct EFR_encoder_state *st,
    Word16 r_h[],
    Word16 r_l[],
    Word16 scal_acf,
    Word16 rc[],
    Word16 ptch
)
{
    struct vad_state *vst = &st->vad;
    Word32 L_av0[9], L_av1[9];
    Word16 vad, vvad, rav1[9], scal_rav1, stat, tone;
    Pfloat acf0, pvad;

    energy_computation (r_h, scal_acf, vst->rvad, vst->scal_rvad, &acf0, &pvad);
    acf_averaging (st, r_h, r_l, scal_acf, L_av0, L_av1);
    predictor_values (L_av1, rav1, &scal_rav1);
    stat = spectral_comparison (st, rav1, scal_rav1, L_av0);
    tone_detection (rc, &tone);
    threshold_adaptation (st, stat, ptch, tone, rav1, scal_rav1, pvad, acf0,
                          vst->rvad, &vst->scal_rvad, &vst->thvad);
    vvad = vad_decision (pvad, vst->thvad);
    vad = vad_hangover (st, vvad);

    return vad;
}

/****************************************************************************
 *
 *     FUNCTION:  energy_computation
 *
 *     PURPOSE:   Computes the input and residual energies of the adaptive
 *                filter in a floating point representation.
 *
 *     INPUTS:    r_h[0..8]      autocorrelation of input signal frame (msb)
 *                scal_acf       scaling factor for the autocorrelations
 *                rvad[0..8]     autocorrelated adaptive filter coefficients
 *                scal_rvad      scaling factor for rvad[]
 *
 *     OUTPUTS:   *acf0          signal frame energy (mantissa+exponent)
 *                *pvad          filtered signal energy (mantissa+exponent)
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void energy_computation (
    Word16 r_h[],
    Word16 scal_acf,
    Word16 rvad[],
    Word16 scal_rvad,
    Pfloat * acf0,
    Pfloat * pvad
)
{
    Word16 i, temp, norm_prod;
    Word32 L_temp;

    /* r[0] is always greater than zero (no need to test for r[0] == 0) */

    /* Computation of acf0 (exponent and mantissa) */

    acf0->e = sub (32, scal_acf);       move16 (); 
    acf0->m = r_h[0] & 0x7ff8;          move16 (); logic16 (); 

    /* Computation of pvad (exponent and mantissa) */

    pvad->e = add (acf0->e, 14);        move16 (); 
    pvad->e = sub (pvad->e, scal_rvad); move16 (); 

    L_temp = 0L;                        move32 (); 

    for (i = 1; i <= 8; i++)
    {
        temp = shr (r_h[i], 3);
        L_temp = L_mac (L_temp, temp, rvad[i]);
    }

    temp = shr (r_h[0], 3);
    L_temp = L_add (L_temp, L_shr (L_mult (temp, rvad[0]), 1));

    test (); 
    if (L_temp <= 0L)
    {
        L_temp = 1L;                    move32 (); 
    }
    norm_prod = norm_l (L_temp);
    pvad->e = sub (pvad->e, norm_prod); move16 (); 
    pvad->m = extract_h (L_shl (L_temp, norm_prod));
                                        move16 (); 

    return;
}

/****************************************************************************
 *
 *     FUNCTION:  acf_averaging
 *
 *     PURPOSE:   Computes the arrays L_av0[0..8] and L_av1[0..8].
 *
 *     INPUTS:    r_h[0..8]     autocorrelation of input signal frame (msb)
 *                r_l[0..8]     autocorrelation of input signal frame (lsb)
 *                scal_acf      scaling factor for the autocorrelations
 *
 *     OUTPUTS:   L_av0[0..8]   ACF averaged over last four frames
 *                L_av1[0..8]   ACF averaged over previous four frames
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void acf_averaging (
    struct EFR_encoder_state *st,
    Word16 r_h[],
    Word16 r_l[],
    Word16 scal_acf,
    Word32 L_av0[],
    Word32 L_av1[]
)
{
    struct vad_state *vst = &st->vad;
    Word32 L_temp;
    Word16 scale;
    Word16 i;

    scale = add (9, scal_acf);

    for (i = 0; i <= 8; i++)
    {
        L_temp = L_shr (L_Comp (r_h[i], r_l[i]), scale);
        L_av0[i] = L_add (vst->L_sacf[i], L_temp);
        L_av0[i] = L_add (vst->L_sacf[i + 9], L_av0[i]);
        L_av0[i] = L_add (vst->L_sacf[i + 18], L_av0[i]);
        vst->L_sacf[vst->pt_sacf + i] = L_temp;
        L_av1[i] = vst->L_sav0[vst->pt_sav0 + i];
        vst->L_sav0[vst->pt_sav0 + i] = L_av0[i];
    }

    /* Update the array pointers */

    if (vst->pt_sacf == 18)
    {
        vst->pt_sacf = 0;
    }
    else
    {
        vst->pt_sacf += 9;
    }

    if (vst->pt_sav0 == 27)
    {
        vst->pt_sav0 = 0;
    }
    else
    {
        vst->pt_sav0 += 9;
    }

    return;
}

/****************************************************************************
 *
 *     FUNCTION:  predictor_values
 *
 *     PURPOSE:   Computes the array rav[0..8] needed for the spectral
 *                comparison and the threshold adaptation.
 *
 *     INPUTS:    L_av1[0..8]   ACF averaged over previous four frames
 *
 *     OUTPUTS:   rav1[0..8]    ACF obtained from L_av1
 *                *scal_rav1    rav1[] scaling factor
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void predictor_values (
    Word32 L_av1[],
    Word16 rav1[],
    Word16 *scal_rav1
)
{
    Word16 vpar[8], aav1[9];

    schur_recursion (L_av1, vpar);
    step_up (8, vpar, aav1);
    compute_rav1 (aav1, rav1, scal_rav1);

    return;
}

/****************************************************************************
 *
 *     FUNCTION:  schur_recursion
 *
 *     PURPOSE:   Uses the Schur recursion to compute adaptive filter
 *                reflection coefficients from an autorrelation function.
 *
 *     INPUTS:    L_av1[0..8]    autocorrelation function
 *
 *     OUTPUTS:   vpar[0..7]     reflection coefficients
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void schur_recursion (
    Word32 L_av1[],
    Word16 vpar[]
)
{
    Word16 acf[9], pp[9], kk[9], temp;
    Word16 i, k, m, n;

    /*** Schur recursion with 16-bit arithmetic ***/

    test (); move32 (); 
    if (L_av1[0] == 0)
    {
        for (i = 0; i < 8; i++)
        {
            vpar[i] = 0;                                move16 (); 
        }
        return;
    }
    temp = norm_l (L_av1[0]);

    for (k = 0; k <= 8; k++)
    {
        acf[k] = extract_h (L_shl (L_av1[k], temp));    move16 (); 
    }

    /*** Initialize arrays pp[..] and kk[..] for the recursion: ***/

    for (i = 1; i <= 7; i++)
    {
        kk[9 - i] = acf[i];                             move16 (); 
    }

    for (i = 0; i <= 8; i++)
    {
        pp[i] = acf[i];                                 move16 (); 
    }

    /*** Compute Parcor coefficients: ***/

    for (n = 0; n < 8; n++)
    {
        test (); 
        if ((pp[0] == 0) ||
            (pp[0] < abs_s (pp[1])))
        {
            for (i = n; i < 8; i++)
            {
                vpar[i] = 0;                            move16 (); 
            }
            return;
        }
        vpar[n] = div_s (abs_s (pp[1]), pp[0]);         move16 (); 

        test ();                                        move16 (); 
        if (pp[1] > 0)
        {
            vpar[n] = negate (vpar[n]);                 move16 (); 
        }
        test (); 
        if (n == 7)
        {
            return;
        }
        /*** Schur recursion: ***/

        pp[0] = add (pp[0], mult_r (pp[1], vpar[n]));   move16 (); 

        for (m = 1; m <= 7 - n; m++)
        {
            pp[m] = add (pp[1 + m], mult_r (kk[9 - m], vpar[n]));
                                                        move16 (); 
            kk[9 - m] = add (kk[9 - m], mult_r (pp[1 + m], vpar[n]));
                                                        move16 (); 
        }
    }

    return;
}

/****************************************************************************
 *
 *     FUNCTION:  step_up
 *
 *     PURPOSE:   Computes the transversal filter coefficients from the
 *                reflection coefficients.
 *
 *     INPUTS:    np               filter order (2..8)
 *                vpar[0..np-1]    reflection coefficients
 *
 *     OUTPUTS:   aav1[0..np]      transversal filter coefficients
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void step_up (
    Word16 np,
    Word16 vpar[],
    Word16 aav1[]
)
{
    Word32 L_coef[9], L_work[9];
    Word16 temp;
    Word16 i, m;

    /*** Initialization of the step-up recursion ***/

    L_coef[0] = 0x20000000L;    move32 (); 
    L_coef[1] = L_shl (L_deposit_l (vpar[0]), 14);              move32 (); 

    /*** Loop on the LPC analysis order: ***/

    for (m = 2; m <= np; m++)
    {
        for (i = 1; i < m; i++)
        {
            temp = extract_h (L_coef[m - i]);
            L_work[i] = L_mac (L_coef[i], vpar[m - 1], temp);   move32 (); 
        }

        for (i = 1; i < m; i++)
        {
            L_coef[i] = L_work[i];                              move32 (); 
        }

        L_coef[m] = L_shl (L_deposit_l (vpar[m - 1]), 14);      move32 (); 
    }

    /*** Keep the aav1[0..np] in 15 bits ***/

    for (i = 0; i <= np; i++)
    {
        aav1[i] = extract_h (L_shr (L_coef[i], 3));             move32 (); 
    }

    return;
}

/****************************************************************************
 *
 *     FUNCTION:  compute_rav1
 *
 *     PURPOSE:   Computes the autocorrelation function of the adaptive
 *                filter coefficients.
 *
 *     INPUTS:    aav1[0..8]     adaptive filter coefficients
 *
 *     OUTPUTS:   rav1[0..8]     ACF of aav1
 *                *scal_rav1     rav1[] scaling factor
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void compute_rav1 (
    Word16 aav1[],
    Word16 rav1[],
    Word16 *scal_rav1
)
{
    Word32 L_work[9];
    Word16 i, k;

    /*** Computation of the rav1[0..8] ***/

    for (i = 0; i <= 8; i++)
    {
        L_work[i] = 0L;                                           move32 (); 

        for (k = 0; k <= 8 - i; k++)
        {
            L_work[i] = L_mac (L_work[i], aav1[k], aav1[k + i]);  move32 (); 
        }
    }

    test (); move32 (); 
    if (L_work[0] == 0L)
    {
        *scal_rav1 = 0;                                           move16 (); 
    }
    else
    {
        *scal_rav1 = norm_l (L_work[0]);
    }

    for (i = 0; i <= 8; i++)
    {
        rav1[i] = extract_h (L_shl (L_work[i], *scal_rav1));      move16 (); 
    }

    return;
}

/****************************************************************************
 *
 *     FUNCTION:  spectral_comparison
 *
 *     PURPOSE:   Computes the stat flag needed for the threshold
 *                adaptation decision.
 *
 *     INPUTS:    rav1[0..8]      ACF obtained from L_av1
 *                *scal_rav1      rav1[] scaling factor
 *                L_av0[0..8]     ACF averaged over last four frames
 *
 *     OUTPUTS:   none
 *
 *     RETURN VALUE: flag to indicate spectral stationarity
 *
 ***************************************************************************/

Word16 spectral_comparison (
    struct EFR_encoder_state *st,
    Word16 rav1[],
    Word16 scal_rav1,
    Word32 L_av0[]
)
{
    struct vad_state *vst = &st->vad;
    Word32 L_dm, L_sump, L_temp;
    Word16 stat, sav0[9], shift, divshift, temp;
    Word16 i;

    /*** Re-normalize L_av0[0..8] ***/

    test (); move32 (); 
    if (L_av0[0] == 0L)
    {
        for (i = 0; i <= 8; i++)
        {
            sav0[i] = 0x0fff;   /* 4095 */                      move16 (); 
        }
    }
    else
    {
        shift = sub (norm_l (L_av0[0]), 3);
        for (i = 0; i <= 8; i++)
        {
            sav0[i] = extract_h (L_shl (L_av0[i], shift));      move16 (); 
        }
    }

    /*** Compute partial sum of dm ***/

    L_sump = 0L;                                                move32 (); 
    for (i = 1; i <= 8; i++)
    {
        L_sump = L_mac (L_sump, rav1[i], sav0[i]);
    }

    /*** Compute the division of the partial sum by sav0[0] ***/

    test (); 
    if (L_sump < 0L)
    {
        L_temp = L_negate (L_sump);
    }
    else
    {
        L_temp = L_sump;                                        move32 (); 
    }

    test (); 
    if (L_temp == 0L)
    {
        L_dm = 0L;                                              move32 (); 
        shift = 0;                                              move16 (); 
    }
    else
    {
        sav0[0] = shl (sav0[0], 3);                             move16 (); 
        shift = norm_l (L_temp);
        temp = extract_h (L_shl (L_temp, shift));

        test (); 
        if (sav0[0] >= temp)
        {
            divshift = 0;                                       move16 (); 
            temp = div_s (temp, sav0[0]);
        }
        else
        {
            divshift = 1;                                       move16 (); 
            temp = sub (temp, sav0[0]);
            temp = div_s (temp, sav0[0]);
        }

        test (); 
        if (divshift == 1)
        {
            L_dm = 0x8000L;                                     move32 (); 
        }
        else
        {
            L_dm = 0L;                                          move32 (); 
        }

        L_dm = L_shl (L_add (L_dm, L_deposit_l (temp)), 1);

        test (); 
        if (L_sump < 0L)
        {
            L_dm = L_negate (L_dm);
        }
    }

    /*** Re-normalization and final computation of L_dm ***/

    L_dm = L_shl (L_dm, 14);
    L_dm = L_shr (L_dm, shift);
    L_dm = L_add (L_dm, L_shl (L_deposit_l (rav1[0]), 11));
    L_dm = L_shr (L_dm, scal_rav1);

    /*** Compute the difference and save L_dm ***/

    L_temp = L_sub (L_dm, vst->L_lastdm);
    vst->L_lastdm = L_dm;

    test (); 
    if (L_temp < 0L)
    {
        L_temp = L_negate (L_temp);
    }
    /*** Evaluation of the stat flag ***/

    L_temp = L_sub (L_temp, STAT_THRESH);

    test (); 
    if (L_temp < 0L)
    {
        stat = 1;                                               move16 (); 
    }
    else
    {
        stat = 0;                                               move16 (); 
    }

    return stat;
}

/****************************************************************************
 *
 *     FUNCTION:  threshold_adaptation
 *
 *     PURPOSE:   Evaluates the secondary VAD decision.  If speech is not
 *                present then the noise model rvad and adaptive threshold
 *                thvad are updated.
 *
 *     INPUTS:    stat          flag to indicate spectral stationarity
 *                ptch          flag to indicate a periodic signal component
 *                tone          flag to indicate a tone signal component
 *                rav1[0..8]    ACF obtained from L_av1
 *                scal_rav1     rav1[] scaling factor
 *                pvad          filtered signal energy (mantissa+exponent)
 *                acf0          signal frame energy (mantissa+exponent)
 *
 *     OUTPUTS:   rvad[0..8]    autocorrelated adaptive filter coefficients
 *                *scal_rvad    rvad[] scaling factor
 *                *thvad        decision threshold (mantissa+exponent)
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void threshold_adaptation (
    struct EFR_encoder_state *st,
    Word16 stat,
    Word16 ptch,
    Word16 tone,
    Word16 rav1[],
    Word16 scal_rav1,
    Pfloat pvad,
    Pfloat acf0,
    Word16 rvad[],
    Word16 *scal_rvad,
    Pfloat * thvad
)
{
    struct vad_state *vst = &st->vad;
    Word16 comp, comp2;
    Word32 L_temp;
    Word16 temp;
    Pfloat p_temp;
    Word16 i;

    comp = 0;                                           move16 (); 

    /*** Test if acf0 < pth; if yes set thvad to plev ***/

    test (); 
    if (acf0.e < E_PTH)
    {
        comp = 1;                                       move16 (); 
    }
    test (); test (); 
    if ((acf0.e == E_PTH) && (acf0.m < M_PTH))
    {
        comp = 1;                                       move16 (); 
    }
    test (); 
    if (comp == 1)
    {
        thvad->e = E_PLEV;                              move16 (); 
        thvad->m = M_PLEV;                              move16 (); 

        return;
    }
    /*** Test if an adaption is required ***/

    test (); 
    if (ptch == 1)
    {
        comp = 1;                                       move16 (); 
    }
    test (); 
    if (stat == 0)
    {
        comp = 1;                                       move16 (); 
    }
    test (); 
    if (tone == 1)
    {
        comp = 1;                                       move16 (); 
    }
    test (); 
    if (comp == 1)
    {
        vst->adaptcount = 0;
        return;
    }
    /*** Increment adaptcount ***/

    vst->adaptcount = add (vst->adaptcount, 1);
    if (vst->adaptcount <= 8)
    {
        return;
    }
    /*** computation of thvad-(thvad/dec) ***/

    thvad->m = sub (thvad->m, shr (thvad->m, 5));       move16 (); 

    test (); 
    if (thvad->m < 0x4000)
    {
        thvad->m = shl (thvad->m, 1);                   move16 (); 
        thvad->e = sub (thvad->e, 1);                   move16 (); 
    }
    /*** computation of pvad*fac ***/

    L_temp = L_mult (pvad.m, FAC);
    L_temp = L_shr (L_temp, 15);
    p_temp.e = add (pvad.e, 1);                         move16 (); 

    test (); 
    if (L_temp > 0x7fffL)
    {
        L_temp = L_shr (L_temp, 1);
        p_temp.e = add (p_temp.e, 1);                   move16 (); 
    }
    p_temp.m = extract_l (L_temp);                      move16 (); 

    /*** test if thvad < pvad*fac ***/

    test (); 
    if (thvad->e < p_temp.e)
    {
        comp = 1;                                       move16 (); 
    }
    test (); test (); 
    if ((thvad->e == p_temp.e) &&
        (thvad->m < p_temp.m))
    {
        comp = 1;                                       move16 (); 
    }
    /*** compute minimum(thvad+(thvad/inc), pvad*fac) when comp = 1 ***/

    test (); 
    if (comp == 1)
    {
        /*** compute thvad + (thvad/inc) ***/

        L_temp = L_add (L_deposit_l (thvad->m),
                        L_deposit_l (shr (thvad->m, 4)));

        test (); 
        if (L_temp > 0x7fffL)
        {
            thvad->m = extract_l (L_shr (L_temp, 1));   move16 (); 
            thvad->e = add (thvad->e, 1);               move16 (); 
        }
        else
        {
            thvad->m = extract_l (L_temp);              move16 (); 
        }

        comp2 = 0;                                      move16 (); 

        test (); 
        if (p_temp.e < thvad->e)
        {
            comp2 = 1;                                  move16 (); 
        }
        test (); test (); 
        if ((p_temp.e == thvad->e) &&
            (p_temp.m < thvad->m))
        {
            comp2 = 1;                                  move16 (); 
        }
        test (); 
        if (comp2 == 1)
        {
            thvad->e = p_temp.e;move16 (); 
            thvad->m = p_temp.m;move16 (); 
        }
    }
    /*** compute pvad + margin ***/

    test (); 
    if (pvad.e == E_MARGIN)
    {
        L_temp = L_add (L_deposit_l (pvad.m), L_deposit_l (M_MARGIN));
        p_temp.m = extract_l (L_shr (L_temp, 1));       move16 (); 
        p_temp.e = add (pvad.e, 1);     move16 (); 
    }
    else
    {
        test (); 
        if (pvad.e > E_MARGIN)
        {
            temp = sub (pvad.e, E_MARGIN);
            temp = shr (M_MARGIN, temp);
            L_temp = L_add (L_deposit_l (pvad.m), L_deposit_l (temp));

            test (); 
            if (L_temp > 0x7fffL)
            {
                p_temp.e = add (pvad.e, 1);             move16 (); 
                p_temp.m = extract_l (L_shr (L_temp, 1));
                                                        move16 (); 
            }
            else
            {
                p_temp.e = pvad.e;                      move16 (); 
                p_temp.m = extract_l (L_temp);          move16 (); 
            }
        }
        else
        {
            temp = sub (E_MARGIN, pvad.e);
            temp = shr (pvad.m, temp);
            L_temp = L_add (L_deposit_l (M_MARGIN), L_deposit_l (temp));

            test (); 
            if (L_temp > 0x7fffL)
            {
                p_temp.e = add (E_MARGIN, 1);           move16 (); 
                p_temp.m = extract_l (L_shr (L_temp, 1));
                                                        move16 (); 
            }
            else
            {
                p_temp.e = E_MARGIN;                    move16 (); 
                p_temp.m = extract_l (L_temp);          move16 (); 
            }
        }
    }

    /*** Test if thvad > pvad + margin ***/

    comp = 0;                                           move16 (); 

    test (); 
    if (thvad->e > p_temp.e)
    {
        comp = 1;                                       move16 (); 
    }
    test (); test (); 
    if ((thvad->e == p_temp.e) &&
        (thvad->m > p_temp.m))
    {
        comp = 1;                                       move16 (); 
    }
    test (); 
    if (comp == 1)
    {
        thvad->e = p_temp.e;                            move16 (); 
        thvad->m = p_temp.m;                            move16 (); 
    }
    /*** Normalise and retain rvad[0..8] in memory ***/

    *scal_rvad = scal_rav1;                             move16 (); 

    for (i = 0; i <= 8; i++)
    {
        rvad[i] = rav1[i];                              move16 (); 
    }

    /*** Set adaptcount to adp + 1 ***/

    vst->adaptcount = 9;

    return;
}

/****************************************************************************
 *
 *     FUNCTION:  tone_detection
 *
 *     PURPOSE:   Computes the tone flag needed for the threshold
 *                adaptation decision.
 *
 *     INPUTS:    rc[0..3]    reflection coefficients calculated in the
 *                            speech encoder short term predictor
 *
 *     OUTPUTS:   *tone       flag to indicate a periodic signal component
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void tone_detection (
    Word16 rc[],
    Word16 *tone
)
{
    Word32 L_num, L_den, L_temp;
    Word16 temp, prederr, a[3];
    Word16 i;

    *tone = 0;                  move16 (); 

    /*** Calculate filter coefficients ***/

    step_up (2, rc, a);

    /*** Calculate ( a[1] * a[1] ) ***/

    temp = shl (a[1], 3);
    L_den = L_mult (temp, temp);

    /*** Calculate ( 4*a[2] - a[1]*a[1] ) ***/

    L_temp = L_shl (L_deposit_h (a[2]), 3);
    L_num = L_sub (L_temp, L_den);

    /*** Check if pole frequency is less than 385 Hz ***/

    test (); 
    if (L_num <= 0)
    {
        return;
    }
    test (); move16 (); 
    if (a[1] < 0)
    {
        temp = extract_h (L_den);
        L_den = L_mult (temp, FREQTH);

        L_temp = L_sub (L_num, L_den);

        test (); 
        if (L_temp < 0)
        {
            return;
        }
    }
    /*** Calculate normalised prediction error ***/

    prederr = 0x7fff;           move16 (); 

    for (i = 0; i < 4; i++)
    {
        temp = mult (rc[i], rc[i]);
        temp = sub (0x7fff, temp);
        prederr = mult (prederr, temp);
    }

    /*** Test if prediction error is smaller than threshold ***/

    temp = sub (prederr, PREDTH);

    test (); 
    if (temp < 0)
    {
        *tone = 1;              move16 (); 
    }
    return;
}

/****************************************************************************
 *
 *     FUNCTION:  vad_decision
 *
 *     PURPOSE:   Computes the VAD decision based on the comparison of the
 *                floating point representations of pvad and thvad.
 *
 *     INPUTS:    pvad          filtered signal energy (mantissa+exponent)
 *                thvad         decision threshold (mantissa+exponent)
 *
 *     OUTPUTS:   none
 *
 *     RETURN VALUE: vad decision before hangover is added
 *
 ***************************************************************************/

Word16 vad_decision (
    Pfloat pvad,
    Pfloat thvad
)
{
    Word16 vvad;

    test (); test (); test (); 
    if (pvad.e > thvad.e)
    {
        vvad = 1;               move16 (); 
    }
    else if ((pvad.e == thvad.e) &&
             (pvad.m > thvad.m))
    {
        vvad = 1;               move16 (); 
    }
    else
    {
        vvad = 0;               move16 (); 
    }

    return vvad;
}

/****************************************************************************
 *
 *     FUNCTION:  vad_hangover
 *
 *     PURPOSE:   Computes the final VAD decision for the current frame
 *                being processed.
 *
 *     INPUTS:    vvad           vad decision before hangover is added
 *
 *     OUTPUTS:   none
 *
 *     RETURN VALUE: vad decision after hangover is added
 *
 ***************************************************************************/

Word16 vad_hangover (
    struct EFR_encoder_state *st,
    Word16 vvad
)
{
    struct vad_state *vst = &st->vad;

    if (vvad == 1)
    {
        vst->burstcount++;
    }
    else
    {
        vst->burstcount = 0;
    }

    if (vst->burstcount >= BURSTCONST)
    {
        vst->hangcount = HANGCONST;
        vst->burstcount = BURSTCONST;
    }
    if (vst->hangcount >= 0)
    {
        vst->hangcount--;
        return 1;               /* vad = 1 */
    }
    return vvad;                /* vad = vvad */
}

/****************************************************************************
 *
 *     FUNCTION:  periodicity_update
 *
 *     PURPOSE:   Computes the ptch flag needed for the threshold
 *                adaptation decision for the next frame.
 *
 *     INPUTS:    lags[0..1]       speech encoder long term predictor lags
 *
 *     OUTPUTS:   *ptch            Boolean voiced / unvoiced decision
 *
 *     RETURN VALUE: none
 *
 ***************************************************************************/

void periodicity_update (
    struct EFR_encoder_state *st,
    Word16 lags[]
)
{
    struct vad_state *vst = &st->vad;
    Word16 minlag, maxlag, lagcount, temp;
    Word16 i;

    /*** Run loop for the two halves in the frame ***/

    lagcount = 0;

    for (i = 0; i <= 1; i++)
    {
        /*** Search the maximum and minimum of consecutive lags ***/

        if (vst->oldlag > lags[i])
        {
            minlag = lags[i];
            maxlag = vst->oldlag;
        }
        else
        {
            minlag = vst->oldlag;
            maxlag = lags[i];
        }

        temp = sub (maxlag, minlag);

        if (temp < LTHRESH)
        {
            lagcount = add (lagcount, 1);
        }
        /*** Save the current LTP lag ***/

        vst->oldlag = lags[i];
    }

    /*** Update the veryoldlagcount and oldlagcount ***/

    vst->veryoldlagcount = vst->oldlagcount;
    vst->oldlagcount = lagcount;

    /*** Make ptch decision ready for next frame ***/

    temp = add (vst->oldlagcount, vst->veryoldlagcount);

    if (temp >= NTHRESH)
    {
        st->ptch = 1;
    }
    else
    {
        st->ptch = 0;
    }

    return;
}