/*************************************************************************
 *
 *  FILE NAME:   D_GAINS.C
 *
 *  FUNCTIONS DEFINED IN THIS FILE:
 *
 *        d_gain_pitch(), d_gain_code() and gmed5()
 *
 * MA prediction is performed on the innovation energy
 * ( in dB/(20*log10(2)) ) with mean removed.
 *************************************************************************/

#include "gsm_efr.h"
#include "typedef.h"
#include "namespace.h"
#include "basic_op.h"
#include "oper_32b.h"
#include "no_count.h"
#include "sig_proc.h"
#include "memops.h"
#include "codec.h"

#include "gains_tb.h"

#include "cnst.h"
#include "dec_state.h"
#include "dtx.h"

/*************************************************************************
 *
 *  FUNCTION:   gmed5
 *
 *  PURPOSE:    calculates 5-point median.
 *
 *  DESCRIPTION:
 *             
 *************************************************************************/

static Word16 gmed5 ( /* out      : index of the median value (0...4) */
    Word16 ind[]      /* in       : Past gain values                  */
)
{
    Word16 i, j, ix = 0, tmp[5];
    Word16 max, tmp2[5];

    Copy (ind, tmp2, 5);

    for (i = 0; i < 5; i++)
    {
        max = -8192;                                           move16 (); 
        for (j = 0; j < 5; j++)
        {
            if (tmp2[j] >= max)
            {
                max = tmp2[j];                                 move16 (); 
                ix = j;                                        move16 (); 
            }
        }
        tmp2[ix] = -16384;                                     move16 (); 
        tmp[i] = ix;                                           move16 (); 
    }

    return (ind[tmp[2]]);
}

/*************************************************************************
 *
 *  FUNCTION:   d_gain_pitch
 *
 *  PURPOSE:  decodes the pitch gain using the received index.
 *
 *  DESCRIPTION:
 *       In case of no frame erasure, the gain is obtained from the
 *       quantization table at the given index; otherwise, a downscaled
 *       past gain is used.
 *
 *************************************************************************/

Word16 d_gain_pitch ( /* out      : quantized pitch gain           */
    struct EFR_decoder_state *st,
    Word16 index,     /* in       : index of quantization          */
    Word16 bfi,       /* in       : bad frame indicator (good = 0) */
    Word16 state,
    Word16 prev_bf,
    Word16 rxdtx_ctrl
)
{
    static const Word16 pdown[7] =
    {
        32767, 32112, 32112, 26214,
        9830, 6553, 6553
    };

    Word16 gain, tmp, i;

    if (bfi == 0)
    {
        if ((rxdtx_ctrl & RX_SP_FLAG) != 0)
        {
            gain = shr (qua_gain_pitch[index], 2);

            if (prev_bf != 0)
            {
                if (gain > st->prev_gp)
                {
                    gain = st->prev_gp;
                }
            }
        }
        else
        {
            gain = 0;
        }
        st->prev_gp = gain;
    }
    else
    {
        if ((rxdtx_ctrl & RX_SP_FLAG) != 0)
        {
            tmp = gmed5 (st->pbuf);

            if (tmp < st->past_gain_pit)
            {
                st->past_gain_pit = tmp;
            }
            gain = mult (pdown[state], st->past_gain_pit);
        }
        else
        {
            gain = 0;
        }
    }

    st->past_gain_pit = gain;

    if (st->past_gain_pit > 4096)  /* if (past_gain_pit > 1.0) */
    {
        st->past_gain_pit = 4096;
    }
    for (i = 1; i < 5; i++)
    {
        st->pbuf[i - 1] = st->pbuf[i];
    }

    st->pbuf[4] = st->past_gain_pit;

    return gain;
}

/*************************************************************************
 *
 *  FUNCTION:  d_gain_code
 *
 *  PURPOSE:  decode the fixed codebook gain using the received index.
 *
 *  DESCRIPTION:
 *      The received index gives the gain correction factor gamma.
 *      The quantized gain is given by   g_q = g0 * gamma
 *      where g0 is the predicted gain.
 *      To find g0, 4th order MA prediction is applied to the mean-removed
 *      innovation energy in dB.
 *      In case of frame erasure, downscaled past gain is used.
 *
 *************************************************************************/

/* average innovation energy.                             */
/* MEAN_ENER = 36.0/constant, constant = 20*Log10(2)      */
#define MEAN_ENER  783741L      /* 36/(20*log10(2))       */

void d_gain_code (
    struct EFR_decoder_state *st,
    Word16 index,      /* input : received quantization index */
    Word16 code[],     /* input : innovation codevector       */
    Word16 lcode,      /* input : codevector length           */
    Word16 *gain_code, /* output: decoded innovation gain     */
    Word16 bfi,        /* input : bad frame indicator         */
    Word16 state,
    Word16 prev_bf,
    Word16 rxdtx_ctrl,
    Word16 i_subfr,
    Word16 rx_dtx_state
)
{
    static const Word16 cdown[7] =
    {
        32767, 32112, 32112, 32112,
        32112, 32112, 22937
    };

    Word16 i, tmp;
    Word16 gcode0, exp, frac, av_pred_en;
    Word32 ener, ener_code;

    if (((rxdtx_ctrl & RX_UPD_SID_QUANT_MEM) != 0) && (i_subfr == 0))
    {
        st->gcode0_CN = update_gcode0_CN (st->gain_code_old_rx);
        st->gcode0_CN = shl (st->gcode0_CN, 4);
    }

    /* Handle cases of comfort noise fixed codebook gain decoding in
       which past valid SID frames are repeated */

    if (((rxdtx_ctrl & RX_NO_TRANSMISSION) != 0)
        || ((rxdtx_ctrl & RX_INVALID_SID_FRAME) != 0)
        || ((rxdtx_ctrl & RX_LOST_SID_FRAME) != 0))
    {

        if ((rxdtx_ctrl & RX_NO_TRANSMISSION) != 0)
        {
            /* DTX active: no transmission. Interpolate gain values
            in memory */
            if (i_subfr == 0)
            {
                *gain_code = interpolate_CN_param (st->gain_code_old_CN,
                                            st->gain_code_new_CN, rx_dtx_state);
            }
            else
            {
                *gain_code = st->prev_gc;
            }
        }
        else
        {                       /* Invalid or lost SID frame:
            use gain values from last good SID frame */
            st->gain_code_old_CN = st->gain_code_new_CN;
            *gain_code = st->gain_code_new_CN;

            /* reset table of past quantized energies */
            for (i = 0; i < 4; i++)
            {
                st->past_qua_en[i] = -2381;
            }
        }

        if ((rxdtx_ctrl & RX_DTX_MUTING) != 0)
        {
            /* attenuate the gain value by 0.75 dB in each subframe */
            /* (total of 3 dB per frame) */
            st->gain_code_muting_CN = mult (st->gain_code_muting_CN, 30057);
            *gain_code = st->gain_code_muting_CN;
        }
        else
        {
            /* Prepare for DTX muting by storing last good gain value */
            st->gain_code_muting_CN = st->gain_code_new_CN;
        }

        st->past_gain_code = *gain_code;

        for (i = 1; i < 5; i++)
        {
            st->gbuf[i - 1] = st->gbuf[i];
        }

        st->gbuf[4] = st->past_gain_code;
        st->prev_gc = st->past_gain_code;

        return;
    }

    /*----------------- Test erasure ---------------*/

    if (bfi != 0)
    {
        tmp = gmed5 (st->gbuf);
        if (tmp < st->past_gain_code)
        {
            st->past_gain_code = tmp;
        }
        st->past_gain_code = mult (st->past_gain_code, cdown[state]);
        *gain_code = st->past_gain_code;

        av_pred_en = 0;
        for (i = 0; i < 4; i++)
        {
            av_pred_en = add (av_pred_en, st->past_qua_en[i]);
        }

        /* av_pred_en = 0.25*av_pred_en - 4/(20Log10(2)) */
        av_pred_en = mult (av_pred_en, 8192);   /*  *= 0.25  */

        /* if (av_pred_en < -14/(20Log10(2))) av_pred_en = .. */
        if (av_pred_en < -2381)
        {
            av_pred_en = -2381;
        }
        for (i = 3; i > 0; i--)
        {
            st->past_qua_en[i] = st->past_qua_en[i - 1];
        }
        st->past_qua_en[0] = av_pred_en;
        for (i = 1; i < 5; i++)
        {
            st->gbuf[i - 1] = st->gbuf[i];
        }
        st->gbuf[4] = st->past_gain_code;

        /* Use the most recent comfort noise fixed codebook gain value
           for updating the fixed codebook gain history */
        if (st->gain_code_new_CN == 0)
        {
            tmp = st->prev_gc;
        }
        else
        {
            tmp = st->gain_code_new_CN;
        }

        update_gain_code_history_rx (st, tmp);

        if (sub (i_subfr, (3 * L_SUBFR)) == 0)
        {
            st->gain_code_old_CN = *gain_code;
        }
        return;
    }

    if ((rxdtx_ctrl & RX_SP_FLAG) != 0)
    {

        /*-------------- Decode codebook gain ---------------*/

        /*-------------------------------------------------------------------*
         *  energy of code:                                                   *
         *  ~~~~~~~~~~~~~~~                                                   *
         *  ener_code = 10 * Log10(energy/lcode) / constant                   *
         *            = 1/2 * Log2(energy/lcode)                              *
         *                                           constant = 20*Log10(2)   *
         *-------------------------------------------------------------------*/

        /* ener_code = log10(ener_code/lcode) / (20*log10(2)) */
        ener_code = 0;                                         move32 (); 
        for (i = 0; i < lcode; i++)
        {
            ener_code = L_mac (ener_code, code[i], code[i]);
        }
        /* ener_code = ener_code / lcode */
        ener_code = L_mult (round (ener_code), 26214);

        /* ener_code = 1/2 * Log2(ener_code) */
        Log2 (ener_code, &exp, &frac);
        ener_code = L_Comp (sub (exp, 30), frac);

        /* predicted energy */

        ener = MEAN_ENER;                                      move32 (); 
        for (i = 0; i < 4; i++)
        {
            ener = L_mac (ener, st->past_qua_en[i], st->pred[i]);
        }

        /*-------------------------------------------------------------------*
         *  predicted codebook gain                                           *
         *  ~~~~~~~~~~~~~~~~~~~~~~~                                           *
         *  gcode0     = Pow10( (ener*constant - ener_code*constant) / 20 )   *
         *             = Pow2(ener-ener_code)                                 *
         *                                           constant = 20*Log10(2)   *
         *-------------------------------------------------------------------*/

        ener = L_shr (L_sub (ener, ener_code), 1);
        L_Extract (ener, &exp, &frac);

        gcode0 = extract_l (Pow2 (exp, frac));  /* predicted gain */

        gcode0 = shl (gcode0, 4);

        *gain_code = mult (qua_gain_code[index], gcode0);      move16 (); 

        if (prev_bf != 0)
        {
            if (sub (*gain_code, st->prev_gc) > 0)
            {
                *gain_code = st->prev_gc;
            }
        }
        /*-------------------------------------------------------------------*
         *  update table of past quantized energies                           *
         *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~                           *
         *  past_qua_en      = 20 * Log10(qua_gain_code) / constant           *
         *                   = Log2(qua_gain_code)                            *
         *                                           constant = 20*Log10(2)   *
         *-------------------------------------------------------------------*/

        for (i = 3; i > 0; i--)
        {
            st->past_qua_en[i] = st->past_qua_en[i - 1];
        }
        Log2 (L_deposit_l (qua_gain_code[index]), &exp, &frac);

        st->past_qua_en[0] = shr (frac, 5);
        st->past_qua_en[0] = add (st->past_qua_en[0], shl (sub (exp, 11), 10));

        update_gain_code_history_rx (st, *gain_code);

        if (i_subfr == (3 * L_SUBFR))
        {
            st->gain_code_old_CN = *gain_code;
        }
    }
    else
    {
        if (((rxdtx_ctrl & RX_FIRST_SID_UPDATE) != 0) && (i_subfr == 0))
        {
            st->gain_code_new_CN = mult (st->gcode0_CN, qua_gain_code[index]);

            /*---------------------------------------------------------------*
             *  reset table of past quantized energies                        *
             *  ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~                        *
             *---------------------------------------------------------------*/

            for (i = 0; i < 4; i++)
            {
                st->past_qua_en[i] = -2381;
            }
        }
        if (((rxdtx_ctrl & RX_CONT_SID_UPDATE) != 0) && (i_subfr == 0))
        {
            st->gain_code_old_CN = st->gain_code_new_CN;
            st->gain_code_new_CN = mult (st->gcode0_CN, qua_gain_code[index]);
        }
        if (i_subfr == 0)
        {
            *gain_code = interpolate_CN_param (st->gain_code_old_CN,
                                               st->gain_code_new_CN,
                                               rx_dtx_state);
        }
        else
        {
            *gain_code = st->prev_gc;
        }
    }

    st->past_gain_code = *gain_code;

    for (i = 1; i < 5; i++)
    {
        st->gbuf[i - 1] = st->gbuf[i];
    }
    st->gbuf[4] = st->past_gain_code;
    st->prev_gc = st->past_gain_code;

    return;
}