/*
 * This file is a product of splitting ETSI EFR dtx.c into parts;
 * the present module is the encoder-specific part.
 */

#include "gsm_efr.h"
#include "typedef.h"
#include "namespace.h"
#include "basic_op.h"
#include "cnst.h"
#include "sig_proc.h"
#include "memops.h"
#include "no_count.h"
#include "dtx.h"
#include "dtx_defs.h"
#include "enc_state.h"

/*************************************************************************
 *
 *   FUNCTION NAME: reset_tx_dtx
 *
 *   PURPOSE:  Resets the static variables of the TX DTX handler to their
 *             initial values
 *
 *************************************************************************/

void reset_tx_dtx (struct EFR_encoder_state *st)
{
    Word16 i;

    /* suppose infinitely long speech period before start */

    st->txdtx_hangover = DTX_HANGOVER;
    st->txdtx_N_elapsed = 0x7fff;
    st->txdtx_ctrl = TX_SP_FLAG | TX_VAD_FLAG;

    for (i = 0; i < 6; i++)
    {
        st->old_CN_mem_tx[i] = 0;
    }

    for (i = 0; i < DTX_HANGOVER; i++)
    {
        st->lsf_old_tx[i][0] = 1384;
        st->lsf_old_tx[i][1] = 2077;
        st->lsf_old_tx[i][2] = 3420;
        st->lsf_old_tx[i][3] = 5108;
        st->lsf_old_tx[i][4] = 6742;
        st->lsf_old_tx[i][5] = 8122;
        st->lsf_old_tx[i][6] = 9863;
        st->lsf_old_tx[i][7] = 11092;
        st->lsf_old_tx[i][8] = 12714;
        st->lsf_old_tx[i][9] = 13701;
    }

    for (i = 0; i < 4 * DTX_HANGOVER; i++)
    {
        st->gain_code_old_tx[i] = 0;
    }

    st->L_pn_seed_tx = PN_INITIAL_SEED;

    st->buf_p_tx = 0;
    return;
}

/*************************************************************************
 *
 *   FUNCTION NAME: tx_dtx
 *
 *   PURPOSE: DTX handler of the speech encoder. Determines when to add
 *            the hangover period to the end of the speech burst, and
 *            also determines when to use old SID parameters, and when
 *            to update the SID parameters. This function also initializes
 *            the pseudo noise generator shift register.
 *
 *            Operation of the TX DTX handler is based on the VAD flag
 *            given as input from the speech encoder.
 *
 *   INPUTS:      VAD_flag      Voice activity decision
 *                *txdtx_ctrl   Old encoder DTX control word
 *
 *   OUTPUTS:     *txdtx_ctrl   Updated encoder DTX control word
 *                L_pn_seed_tx  Initialized pseudo noise generator shift
 *                              register (global variable)
 *
 *   RETURN VALUE: none
 *
 *************************************************************************/

void tx_dtx (
    struct EFR_encoder_state *st,
    Word16 VAD_flag
)
{
    /* N_elapsed (frames since last SID update) is incremented. If SID
       is updated N_elapsed is cleared later in this function */

    st->txdtx_N_elapsed = add (st->txdtx_N_elapsed, 1);

    /* If voice activity was detected, reset hangover counter */

    if (sub (VAD_flag, 1) == 0)
    {
        st->txdtx_hangover = DTX_HANGOVER;
        st->txdtx_ctrl = TX_SP_FLAG | TX_VAD_FLAG;
    }
    else
    {
        if (st->txdtx_hangover == 0)
        {
            /* Hangover period is over, SID should be updated */

            st->txdtx_N_elapsed = 0;

            /* Check if this is the first frame after hangover period */
            if ((st->txdtx_ctrl & TX_HANGOVER_ACTIVE) != 0)
            {
                st->txdtx_ctrl = TX_PREV_HANGOVER_ACTIVE
                    | TX_SID_UPDATE;
                st->L_pn_seed_tx = PN_INITIAL_SEED;
            }
            else
            {
                st->txdtx_ctrl = TX_SID_UPDATE;
            }
        }
        else
        {
            /* Hangover period is not over, update hangover counter */
            st->txdtx_hangover = sub (st->txdtx_hangover, 1);

            /* Check if elapsed time from last SID update is greater than
               threshold. If not, set SP=0 (although hangover period is not
               over) and use old SID parameters for new SID frame.
               N_elapsed counter must be summed with hangover counter in order
               to avoid erroneus SP=1 decision in case when N_elapsed is grown
               bigger than threshold and hangover period is still active */

            if (sub (add (st->txdtx_N_elapsed, st->txdtx_hangover),
                     DTX_ELAPSED_THRESHOLD) < 0)
            {
                /* old SID frame should be used */
                st->txdtx_ctrl = TX_USE_OLD_SID;
            }
            else
            {
                if ((st->txdtx_ctrl & TX_HANGOVER_ACTIVE) != 0)
                {
                    st->txdtx_ctrl = TX_PREV_HANGOVER_ACTIVE
                        | TX_HANGOVER_ACTIVE
                        | TX_SP_FLAG;
                }
                else
                {
                    st->txdtx_ctrl = TX_HANGOVER_ACTIVE
                        | TX_SP_FLAG;
                }
            }
        }
    }

    return;
}

/*************************************************************************
 *
 *   FUNCTION NAME: CN_encoding
 *
 *   PURPOSE:  Encoding of the comfort noise parameters into a SID frame.
 *             Use old SID parameters if necessary. Set the parameter
 *             indices not used by comfort noise parameters to zero.
 *
 *   INPUTS:      params[0..56]  Comfort noise parameter frame from the
 *                               speech encoder
 *                txdtx_ctrl     TX DTX handler control word
 *
 *   OUTPUTS:     params[0..56]  Comfort noise encoded parameter frame
 *
 *   RETURN VALUE: none
 *
 *************************************************************************/

void CN_encoding (
    struct EFR_encoder_state *st,
    Word16 params[],
    Word16 txdtx_ctrl
)
{
    Word16 i;

    if ((txdtx_ctrl & TX_SID_UPDATE) != 0)
    {
        /* Store new CN parameters in memory to be used later as old
           CN parameters */

        /* LPC parameter indices */
        for (i = 0; i < 5; i++)
        {
            st->old_CN_mem_tx[i] = params[i];
        }
        /* Codebook index computed in last subframe */
        st->old_CN_mem_tx[5] = params[56];
    }
    if ((txdtx_ctrl & TX_USE_OLD_SID) != 0)
    {
        /* Use old CN parameters previously stored in memory */
        for (i = 0; i < 5; i++)
        {
            params[i] = st->old_CN_mem_tx[i];
        }
        params[17] = st->old_CN_mem_tx[5];
        params[30] = st->old_CN_mem_tx[5];
        params[43] = st->old_CN_mem_tx[5];
        params[56] = st->old_CN_mem_tx[5];
    }
    /* Set all the rest of the parameters to zero (SID codeword will
       be written later) */
    for (i = 0; i < 12; i++)
    {
        params[i + 5] = 0;
        params[i + 18] = 0;
        params[i + 31] = 0;
        params[i + 44] = 0;
    }

    return;
}

/*************************************************************************
 *
 *   FUNCTION NAME: aver_lsf_history
 *
 *   PURPOSE: Compute the averaged LSF parameter vector. Computation is
 *            performed by averaging the LSF parameter vectors which exist
 *            in the LSF parameter history, together with the LSF
 *            parameter vectors of the current frame.
 *
 *   INPUTS:      lsf_old[0..DTX_HANGOVER-1][0..M-1]
 *                                   LSF parameter history
 *                lsf1[0..M-1]       LSF vector of the 1st half of the frame
 *                lsf2[0..M-1]       LSF vector of the 2nd half of the frame
 *
 *   OUTPUTS:     lsf_aver[0..M-1]   Averaged LSF parameter vector
 *
 *   RETURN VALUE: none
 *
 *************************************************************************/

void aver_lsf_history (
    Word16 lsf_old[DTX_HANGOVER][M],
    Word16 lsf1[M],
    Word16 lsf2[M],
    Word16 lsf_aver[M]
)
{
    Word16 i, j;
    Word32 L_temp;

    for (j = 0; j < M; j++)
    {
        L_temp = L_mult (0x3fff, lsf1[j]);
        L_temp = L_mac (L_temp, 0x3fff, lsf2[j]);
        L_temp = L_mult (INV_DTX_HANGOVER_P1, extract_h (L_temp));

        for (i = 0; i < DTX_HANGOVER; i++)
        {
            L_temp = L_mac (L_temp, INV_DTX_HANGOVER_P1, lsf_old[i][j]);
        }

        lsf_aver[j] = extract_h (L_temp);              move16 (); 
    }

    return;
}

/*************************************************************************
 *
 *   FUNCTION NAME: update_gain_code_history_tx
 *
 *   PURPOSE: Update the fixed codebook gain parameter history of the
 *            encoder. The fixed codebook gain parameters kept in the buffer
 *            are used later for computing the reference fixed codebook
 *            gain parameter value and the averaged fixed codebook gain
 *            parameter value.
 *
 *   INPUTS:      new_gain_code   New fixed codebook gain value
 *
 *                gain_code_old_tx[0..4*DTX_HANGOVER-1]
 *                                Old fixed codebook gain history of encoder
 *
 *   OUTPUTS:     gain_code_old_tx[0..4*DTX_HANGOVER-1]
 *                                Updated fixed codebook gain history of encoder
 *
 *   RETURN VALUE: none
 *
 *************************************************************************/

void update_gain_code_history_tx (
    struct EFR_encoder_state *st,
    Word16 new_gain_code
)
{
    /* Circular buffer */
    st->gain_code_old_tx[st->buf_p_tx] = new_gain_code;

    if (sub (st->buf_p_tx, (4 * DTX_HANGOVER - 1)) == 0)
    {
        st->buf_p_tx = 0;
    }
    else
    {
        st->buf_p_tx = add (st->buf_p_tx, 1);
    }

    return;
}

/*************************************************************************
 *
 *   FUNCTION NAME: compute_CN_excitation_gain
 *
 *   PURPOSE: Compute the unquantized fixed codebook gain. Computation is
 *            based on the energy of the Linear Prediction residual signal.
 *
 *   INPUTS:      res2[0..39]   Linear Prediction residual signal
 *
 *   OUTPUTS:     none
 *
 *   RETURN VALUE: Unquantized fixed codebook gain
 *
 *************************************************************************/

Word16 compute_CN_excitation_gain (
    Word16 res2[L_SUBFR]
)
{
    Word16 i, norm, norm1, temp, overfl;
    Word32 L_temp;

    /* Compute the energy of the LP residual signal */

    norm = 0;                                          move16 (); 
    do
    {
        overfl = 0;                                    move16 ();

        L_temp = 0L;                                   move32 (); 
        for (i = 0; i < L_SUBFR; i++)
        {
            temp = shr (res2[i], norm);
            L_temp = L_mac (L_temp, temp, temp);
        }

        test (); 
        if (L_sub (L_temp, MAX_32) == 0)
        {
            norm = add (norm, 1);
            overfl = 1;                 move16 (); /* Set the overflow flag */
        }
        test (); 
    }
    while (overfl != 0);

    L_temp = L_add (L_temp, 1L);             /* Avoid the case of all zeros */

    /* Take the square root of the obtained energy value (sqroot is a 2nd
       order Taylor series approximation) */

    norm1 = norm_l (L_temp);
    temp = extract_h (L_shl (L_temp, norm1));
    L_temp = L_mult (temp, temp);
    L_temp = L_sub (805306368L, L_shr (L_temp, 3));
    L_temp = L_add (L_temp, L_mult (24576, temp));

    temp = extract_h (L_temp);
    test (); logic16 (); 
    if ((norm1 & 0x0001) != 0)
    {
        temp = mult_r (temp, 23170);
        norm1 = sub (norm1, 1);
    }
    /* Divide the result of sqroot operation by sqroot(10) */

    temp = mult_r (temp, 10362);

    /* Re-scale to get the final value */

    norm1 = shr (norm1, 1);
    norm1 = sub (norm1, norm);

    test (); 
    if (norm1 >= 0)
    {
        temp = shr (temp, norm1);
    }
    else
    {
        temp = shl (temp, abs_s (norm1));
    }

    return temp;
}

/*************************************************************************
 *
 *   FUNCTION NAME: aver_gain_code_history
 *
 *   PURPOSE: Compute the averaged fixed codebook gain parameter value.
 *            Computation is performed by averaging the fixed codebook
 *            gain parameter values which exist in the fixed codebook
 *            gain parameter history, together with the fixed codebook
 *            gain parameter value of the current subframe.
 *
 *   INPUTS:      CN_excitation_gain
 *                              Unquantized fixed codebook gain value
 *                              of the current subframe
 *                gain_code_old[0..4*DTX_HANGOVER-1]
 *                              fixed codebook gain parameter history
 *
 *   OUTPUTS:     none
 *
 *   RETURN VALUE: Averaged fixed codebook gain value
 *
 *************************************************************************/

Word16 aver_gain_code_history (
    Word16 CN_excitation_gain,
    Word16 gain_code_old[4 * DTX_HANGOVER]
)
{
    Word16 i;
    Word32 L_ret;

    L_ret = L_mult (0x470, CN_excitation_gain);

    for (i = 0; i < (4 * DTX_HANGOVER); i++)
    {
        L_ret = L_mac (L_ret, 0x470, gain_code_old[i]);
    }
    return extract_h (L_ret);
}