/***************************************************************************
 *
 *   File Name:  err_conc.c
 *
 *   Purpose:
 *      Contains all functions for error concealment.
 *      Relevant specification: GSM 06.21
 *
 *     Below is a listing of all the functions appearing in the file.
 *     All functions are called within speechDecoder().
 *
 *     Error concealment on parameter level:
 *        para_conceal_speech_decoder()
 *
 *     Error concealment on signal level:
 *        signal_conceal_sub()
 *
 *     Additional functions to support concealment:
 *        level_estimator()
 *        level_calc()
 *
 **************************************************************************/

/*_________________________________________________________________________
 |                                                                         |
 |                              Include Files                              |
 |_________________________________________________________________________|
*/

#include "typedefs.h"
#include "tw_gsmhr.h"
#include "namespace.h"
#include "mathhalf.h"
#include "dec_func.h"
#include "dec_state.h"
#include "err_conc.h"

/*_________________________________________________________________________
 |                                                                         |
 |                              Local Defines                              |
 |_________________________________________________________________________|
*/
#define  MIN_MUTE_LEVEL -45


/****************************************************************************
 *
 *   FUNCTION NAME: level_estimator
 *
 *     This subroutine determines the mean level and the maximum level
 *     of the last four speech sub-frames. These parameters are the basis
 *     for the level estimation in signal_conceal_sub().
 *
 *     Input:     swUpdate      = 0: the levels are determined
 *                              = 1: the memory of the level estimator
 *                                   is updated
 *                pswDecodedSpeechFrame[]  synthesized speech signal
 *
 *     Output:    swLevelMean   mean level of the last 4 sub-frames
 *                swLevelMax    maximum level of the last 4 sub-frames
 *
 ***************************************************************************/

void level_estimator_det(struct gsmhr_decoder_state *st,
			 Shortword *pswLevelMean, Shortword *pswLevelMax)
{

/*_________________________________________________________________________
 |                                                                         |
 |                            Automatic Variables                          |
 |_________________________________________________________________________|
*/
  Shortword i;
  Longword L_sum;

/*_________________________________________________________________________
 |                                                                         |
 |                              Executable Code                            |
 |_________________________________________________________________________|
*/

    /* Determine mean level of the last 4 sub-frames: */
    /* ---------------------------------------------- */
    for (i = 0, L_sum = 0; i < 4; ++i)
    {
      L_sum = L_add(L_sum, st->plSubfrEnergyMem[i]);
    }
    *pswLevelMean = level_calc(1, &L_sum);

    /* Determine maximum level of the last 4 sub-frames: */
    /* ------------------------------------------------- */
    *pswLevelMax = -72;
    for (i = 0; i < 4; ++i)
    {
      if (sub(st->swLevelMem[i], *pswLevelMax) > 0)
        *pswLevelMax = st->swLevelMem[i];
    }
}

void level_estimator_upd(struct gsmhr_decoder_state *st,
			 Shortword pswDecodedSpeechFrame[])
{

/*_________________________________________________________________________
 |                                                                         |
 |                            Automatic Variables                          |
 |_________________________________________________________________________|
*/
  Shortword i,
         tmp,
         swLevelSub;
  Longword L_sum;

/*_________________________________________________________________________
 |                                                                         |
 |                              Executable Code                            |
 |_________________________________________________________________________|
*/

    /* Determine the energy of the synthesized speech signal: */
    /* ------------------------------------------------------ */
    for (i = 0, L_sum = 0; i < S_LEN; ++i)
    {
      tmp = shr(pswDecodedSpeechFrame[i], 3);
      L_sum = L_mac(L_sum, tmp, tmp);
    }
    swLevelSub = level_calc(0, &L_sum);

    /* Update memories of level estimator: */
    /* ----------------------------------- */
    for (i = 0; i < 3; ++i)
      st->plSubfrEnergyMem[i] = st->plSubfrEnergyMem[i + 1];
    st->plSubfrEnergyMem[3] = L_sum;

    for (i = 0; i < 3; ++i)
      st->swLevelMem[i] = st->swLevelMem[i + 1];
    st->swLevelMem[3] = swLevelSub;
}


/*****************************************************************************
 *
 *   FUNCTION NAME: signal_conceal_sub
 *
 *     This subroutine performs concealment on subframe signal level.
 *     A test synthesis is performed and the level of the synthesized speech
 *     signal is compared to the estimated level. Depending on the control
 *     flag "swMutePermit" a muting factor is determined.
 *     If muting is permitted (swMutePermit=1) and the actual sub-frame level
 *     exceeds the maximum level of the last four sub-frames "swLevelMax" plus
 *     an allowed increase "psrLevelMaxIncrease[]" then the synthesized speech
 *     signal together with the signal memories is muted.
 *     In table "psrLevelMaxIncrease[]" the maximum allowed increase
 *     of the maximum sub-frame level is stored. The table is controled by the
 *     mean level "swMeanLevel".
 *     If e.g. the level is in the range between -30 and -35 db
 *     the allowed maximum increase is 4 db (psrLevelMaxIncrease[6]).
 *     The figures in psrLevelMaxIncrease[] have been determined
 *     by measuring the level statistics of error free synthesized speech.
 *
 *     Input:     pswPPFExcit[]            excitation signal
 *                pswSynthFiltState[]      state of LPC synthesis filter
 *                ppswSynthAs[]            LPC coefficients
 *                pswLtpStateOut[]         state of long term predictor
 *                pswPPreState[]           state of pitch prefilter
 *                swLevelMean              mean level
 *                swLevelMax               maximum level
 *                swUFI                    unreliable frame flag
 *                swMuteFlagOld            last muting flag
 *                pswMuteFlag              actual muting flag
 *                swMutePermit             mute permission
 *
 *     Output:    pswPPFExcit[]            muted excitation signal
 *                pswSynthFiltState[]      muted state of LPC synthesis filter
 *                pswLtpStateOut[]         muted state of long term predictor
 *                pswPPreState[]           muted state of pitch prefilter
 *
 *     Constants: psrConceal[0:15]         muting factors
 *                psrLevelMaxIncrease[0:7] maximum allowed level increase
 *
 *
 ****************************************************************************/

void   signal_conceal_sub(Shortword pswPPFExcit[],
                     Shortword ppswSynthAs[], Shortword pswSynthFiltState[],
                       Shortword pswLtpStateOut[], Shortword pswPPreState[],
                                 Shortword swLevelMean, Shortword swLevelMax,
                                 Shortword swUFI, Shortword swMuteFlagOld,
                             Shortword *pswMuteFlag, Shortword swMutePermit)
{

/*_________________________________________________________________________
 |                                                                         |
 |                           Local Static Variables                        |
 |_________________________________________________________________________|
*/
  static const Shortword psrConceal[15] = {29205, 27571, 24573, 21900,
  19519, 17396, 15504, 13818, 12315, 10976, 9783, 8719, 7771, 6925, 6172};
  static const Shortword psrLevelMaxIncrease[16] =
  {0, 0, 1, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 12, 14, 16};

/*_________________________________________________________________________
 |                                                                         |
 |                            Automatic Variables                          |
 |_________________________________________________________________________|
*/
  Shortword swMute,
         swLevelSub,
         i,
         swIndex;
  Shortword swTmp,
         pswStateTmp[10],
         swOutTmp[40],
         swPermitMuteSub;
  Longword L_sum;


/*_________________________________________________________________________
 |                                                                         |
 |                              Executable Code                            |
 |_________________________________________________________________________|
*/

  /* Test synthesis filter: */
  /* ---------------------- */
  for (i = 0; i < 10; ++i)
    pswStateTmp[i] = pswSynthFiltState[i];

  lpcIir(pswPPFExcit, ppswSynthAs, pswStateTmp, swOutTmp);


  /* Determine level in db of synthesized signal: */
  /* -------------------------------------------- */
  L_sum = 0;
  for (i = 0; i < S_LEN; ++i)
  {
    swTmp = shr(swOutTmp[i], 2);
    L_sum = L_mac(L_sum, swTmp, swTmp);
  }
  swLevelSub = level_calc(0, &L_sum);


  /* Determine index to table, specifying the allowed level increase: */
  /* level [ 0 ..  -5] --> swIndex = 0       */
  /* level [-5 .. -10] --> swIndex = 1  etc. */
  /*---------------------------------------------*/
  swIndex = mult(negate(swLevelMean), 1638);
  if (sub(swIndex, 15) > 0)
    swIndex = 15;

  /* Muting is permitted, if it is signalled from the parameter concealment */
  /* unit or if muting has been performed in the last frame                */
  /*-----------------------------------------------------------------------*/
  swPermitMuteSub = swMutePermit;
  if (swMuteFlagOld > 0)
    swPermitMuteSub = 1;

  if (swPermitMuteSub > 0)
  {
    /* Muting is not permitted if the sub-frame level is less than */
    /* MIN_MUTE_LEVEL                                              */
    /* ------------------------------------------------------------ */
    if (sub(swLevelSub, MIN_MUTE_LEVEL) <= 0)
      swPermitMuteSub = 0;

    /* Muting is not permitted if the sub-frame level is less than */
    /* the maximum level of the last 4 sub-frames plus the allowed */
    /* increase                                                    */
    /* ------------------------------------------------------------ */
    swMute = sub(swLevelSub, add(swLevelMax, psrLevelMaxIncrease[swIndex]));
    if (swMute <= 0)
      swPermitMuteSub = 0;
  }


  /* Perform muting, if allowed */
  /* -------------------------- */
  if (swPermitMuteSub > 0)
  {

    if (sub(swMute, (Shortword) 15) > 0)
      swMute = 15;

    /* Keep information that muting occured for next frame */
    /* --------------------------------------------------- */
    if (swUFI > 0)
      *pswMuteFlag = 1;


    /* Mute excitation signal: */
    /* ----------------------- */
    for (i = 0; i < 10; ++i)
      pswSynthFiltState[i] =
              mult_r(pswSynthFiltState[i], psrConceal[swMute - 1]);
    for (i = 0; i < S_LEN; ++i)
      pswPPFExcit[i] = mult_r(pswPPFExcit[i], psrConceal[swMute - 1]);

    /* Mute pitch memory: */
    /* ------------------ */
    for (i = 0; i < S_LEN; ++i)
      pswLtpStateOut[i] =
              mult_r(pswLtpStateOut[i], psrConceal[swMute - 1]);


    /* Mute pitch prefilter memory: */
    /* ---------------------------- */
    for (i = 0; i < S_LEN; ++i)
      pswPPreState[i] = mult_r(pswPPreState[i], psrConceal[swMute - 1]);
  }
}


/****************************************************************************
 *
 *   FUNCTION NAME: level_calc
 *
 *     This subroutine calculates the level (db) from the energy
 *     of a speech sub-frame (swInd=0) or a speech frame (swInd=1):
 *     The level of a speech subframe is:
 *       swLevel =  10 * lg(EN/(40.*4096*4096))
 *               =   3 * ld(EN) - 88.27
 *               = (3*4*ld(EN) - 353)/4
 *               = (3*(4*POS(MSB(EN)) + 2*BIT(MSB-1) + BIT(MSB-2)) - 353)/4
 *
 *     Input:     pl_en      energy of the speech subframe or frame
 *                           The energy is multiplied by 2 because of the
 *                           MAC routines !!
 *                swInd      = 0: EN is the energy of one subframe
 *                           = 1: EN is the energy of one frame
 *
 *     Output:    swLevel    level in db
 *
 *
 ***************************************************************************/

Shortword level_calc(Shortword swInd, Longword *pl_en)
{

/*_________________________________________________________________________
 |                                                                         |
 |                            Automatic Variables                          |
 |_________________________________________________________________________|
*/
  Shortword swPos,
         swLevel;
  Longword L_tmp;

/*_________________________________________________________________________
 |                                                                         |
 |                              Executable Code                            |
 |_________________________________________________________________________|
*/

  if (*pl_en != 0)
    swPos = sub((Shortword) 29, norm_l(*pl_en));
  else
    swPos = 0;

  /* Determine the term: 4*POS(MSB(EN)): */
  /* ----------------------------------- */
  swLevel = shl(swPos, 2);

  /* Determine the term: 2*BIT(MSB-1): */
  /* --------------------------------- */
  if (swPos >= 0)
  {
    L_tmp = L_shl((Longword) 1, swPos);
    if ((*pl_en & L_tmp) != 0)
      swLevel += 2;
  }

  /* Determine the term: BIT(MSB-2): */
  /* ------------------------------- */
  if (--swPos >= 0)
  {
    L_tmp = L_shl((Longword) 1, swPos);
    if ((*pl_en & L_tmp) != 0)
      ++swLevel;
  }

  /* Multiply by 3: */
  /* -------------- */
  swLevel += shl(swLevel, 1);
  swLevel -= (swInd == 0) ? 353 : 377;
  swLevel = mult_r(swLevel, 0X2000);   /* >> 2 */

  if (sub(swLevel, -72) < 0)
  {
    swLevel = -72;
    *pl_en = (swInd == 0) ? 80 : 320;
  }

  return (swLevel);
}