/***************************************************************************
 *
 *   File Name:  sp_dec.c
 *
 *   Purpose:
 *      Contains all functions for decoding speech.  It does not
 *      include those routines needed to decode channel information.
 *
 *      Since the GSM half-rate speech coder is an analysis-by-synthesis
 *      coder, many of the routines in this file are also called by the
 *      encoder.  Functions are included for coded-parameter lookup,
 *      LPC filter coefficient interpolation, excitation vector lookup
 *      and construction, vector quantized gain lookup, and LPC synthesis
 *      filtering.  In addition, some post-processing functions are
 *      included.
 *
 *     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.
 *
 *     The entire speech decoder, under which all these routines fall,
 *     except were noted:
 *     speechDecoder()
 *
 *     Spectral Smoothing of LPC:
 *       a_sst()
 *         aFlatRcDp()
 *         rcToCorrDpL()
 *         aToRc()
 *         rcToADp()
 *     VSELP codevector construction:
 *       b_con()
 *       v_con()
 *     LTP vector contruction:
 *       fp_ex()
 *         get_ipjj()
 *       lagDecode()
 *     LPC contruction
 *       getSfrmLpc()
 *         interpolateCheck()
 *         res_eng()
 *       lookupVq()
 *     Excitation scaling:
 *       rs_rr()
 *         g_corr1() (no scaling)
 *       rs_rrNs()
 *         g_corr1s() (g_corr1 with scaling)
 *       scaleExcite()
 *     Post filtering:
 *       pitchPreFilt()
 *         agcGain()
 *         lpcIir()
 *       r0BasedEnergyShft()
 *       spectralPostFilter()
 *         lpcFir()
 *
 *
 *     Routines not referenced by speechDecoder()
 *     Filtering routines:
 *       lpcIrZsIir()
 *       lpcZiIir()
 *       lpcZsFir()
 *       lpcZsIir()
 *       lpcZsIirP()
 *     Square root:
 *       sqroot()
 *
 **************************************************************************/

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

#include <stddef.h>
#include <stdint.h>
#include <string.h>
#include "typedefs.h"
#include "tw_gsmhr.h"
#include "namespace.h"
#include "mathhalf.h"
#include "dec_func.h"
#include "dec_state.h"
#include "dtx_const.h"
#include "dtx_dec.h"
#include "err_conc.h"
#include "rxfe.h"
#include "sp_rom.h"


/*_________________________________________________________________________
 |                                                                         |
 |            Local Functions (scope is limited to this file)              |
 |_________________________________________________________________________|
*/

static void a_sst(Shortword swAshift, Shortword swAscale,
                         Shortword pswDirectFormCoefIn[],
                         Shortword pswDirectFormCoefOut[]);

static Shortword lagDecode(Shortword swDeltaLag, Shortword *pswLastLag,
			   Shortword subfrm);

  static Shortword agcGain(Shortword pswStateCurr[],
                                  struct NormSw snsInSigEnergy,
                                  Shortword swEngyRShft);

  static void lookupVq(Shortword pswVqCodeWds[], Shortword pswRCOut[]);

  static void pitchPreFilt(Shortword pswExcite[],
                                  Shortword swRxGsp0,
                                  Shortword swRxLag,
                                  Shortword swUvCode,
                                  Shortword swSemiBeta,
                                  struct NormSw snsSqrtRs,
                                  Shortword pswExciteOut[],
                                  Shortword pswPPreState[]);

static void spectralPostFilter(struct gsmhr_decoder_state *st,
				Shortword swEngyRShift,
				Shortword pswSPFIn[], Shortword pswNumCoef[],
				Shortword pswDenomCoef[],
				Shortword pswSPFOut[]);

/*_________________________________________________________________________
 |                                                                         |
 |                              Local Defines                              |
 |_________________________________________________________________________|
*/

#define  P_INT_MACS   10
#define  ASCALE       0x0800
#define  ASHIFT       4
#define  DELTA_LEVELS 16
#define  GSP0_SCALE   1
#define  C_BITS_V     9                /* number of bits in any voiced VSELP
                                        * codeword */
#define  C_BITS_UV    7                /* number of bits in a unvoiced VSELP
                                        * codeword */
#define  MAXBITS      C_BITS_V         /* max number of bits in any VSELP
                                        * codeword */
#define  SQRT_ONEHALF 0x5a82           /* the 0.5 ** 0.5 */
#define  LPC_ROUND    0x00000800L      /* 0x8000 >> ASHIFT */
#define  AFSHIFT      2                /* number of right shifts to be
                                        * applied to the autocorrelation
                                        * sequence in aFlatRcDp     */

#define  PCM_MASK     0xfff8           /* 16 to 13 bit linear PCM mask */

/***************************************************************************
 *
 *   FUNCTION NAME: a_sst
 *
 *   PURPOSE:
 *
 *     The purpose of this function is to perform spectral smoothing of the
 *     direct form filter coefficients
 *
 *   INPUTS:
 *
 *     swAshift
 *                     number of shift for coefficients
 *
 *     swAscale
 *                     scaling factor for coefficients
 *
 *     pswDirectFormCoefIn[0:NP-1]
 *
 *                     array of input direct form coefficients
 *
 *   OUTPUTS:
 *
 *     pswDirectFormCoefOut[0:NP-1]
 *
 *                     array of output direct form coefficients
 *
 *   RETURN VALUE:
 *
 *     none
 *
 *   DESCRIPTION:
 *
 *     In a_sst() direct form coefficients are converted to
 *     autocorrelations, and smoothed in that domain.  The function is
 *     used in the spectral postfilter.  A description can be found in
 *     section 3.2.4 as well as in the reference by Y. Tohkura et al.
 *     "Spectral Smoothing Technique in PARCOR Speech
 *     Analysis-Synthesis", IEEE Trans. ASSP, vol. ASSP-26, pp. 591-596,
 *     Dec. 1978.
 *
 *     After smoothing is performed conversion back to direct form
 *     coefficients is done by calling aFlatRc(), followed by rcToADp().
 *
 *     The spectral smoothing filter coefficients with bandwidth set to 300
 *     and a sampling rate of 8000 be :
 *     static ShortwordRom psrSST[NP+1] = { 0x7FFF,
 *         0x7F5C, 0x7D76, 0x7A5B, 0x7622, 0x70EC,
 *         0x6ADD, 0x641F, 0x5CDD, 0x5546, 0x4D86
 *     }
 *
 *   REFERENCES: Sub_Clause 4.2.4 of GSM Recomendation 06.20
 *
 *   KEYWORDS: spectral smoothing, direct form coef, sst, atorc, atocor
 *   KEYWORDS: levinson
 *
 *************************************************************************/

static void a_sst(Shortword swAshift, Shortword swAscale,
                         Shortword pswDirectFormCoefIn[],
                         Shortword pswDirectFormCoefOut[])
{

/*_________________________________________________________________________
 |                                                                         |
 |                           Local Static Variables                        |
 |_________________________________________________________________________|
*/

  static const ShortwordRom psrSST[NP + 1] = {0x7FFF,
    0x7F5C, 0x7D76, 0x7A5B, 0x7622, 0x70EC,
    0x6ADD, 0x641F, 0x5CDD, 0x5546, 0x4D86,
  };

/*_________________________________________________________________________
 |                                                                         |
 |                            Automatic Variables                          |
 |_________________________________________________________________________|
*/

  Longword pL_CorrTemp[NP + 1];

  Shortword pswRCNum[NP],
         pswRCDenom[NP];

  short int siLoopCnt;

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

  /* convert direct form coefs to reflection coefs */
  /* --------------------------------------------- */

  aToRc(swAshift, pswDirectFormCoefIn, pswRCDenom);

  /* convert to autocorrelation coefficients */
  /* --------------------------------------- */

  rcToCorrDpL(swAshift, swAscale, pswRCDenom, pL_CorrTemp);

  /* do spectral smoothing technique */
  /* ------------------------------- */

  for (siLoopCnt = 1; siLoopCnt <= NP; siLoopCnt++)
  {
    pL_CorrTemp[siLoopCnt] = L_mpy_ls(pL_CorrTemp[siLoopCnt],
                                      psrSST[siLoopCnt]);
  }

  /* Compute the reflection coefficients via AFLAT */
  /*-----------------------------------------------*/

  aFlatRcDp(pL_CorrTemp, pswRCNum);


  /* Convert reflection coefficients to direct form filter coefficients */
  /*-------------------------------------------------------------------*/

  rcToADp(swAscale, pswRCNum, pswDirectFormCoefOut);
}

/**************************************************************************
 *
 *   FUNCTION NAME: agcGain
 *
 *   PURPOSE:
 *
 *     Figure out what the agc gain should be to make the energy in the
 *     output signal match that of the input signal.  Used in the post
 *     filters.
 *
 *   INPUT:
 *
 *      pswStateCurr[0:39]
 *                     Input signal into agc block whose energy is
 *                     to be modified using the gain returned. Signal is not
 *                     modified in this routine.
 *
 *      snsInSigEnergy
 *                     Normalized number with shift count - the energy in
 *                     the input signal.
 *
 *      swEngyRShft
 *                     Number of right shifts to apply to the vectors energy
 *                     to ensure that it remains less than 1.0
 *                     (swEngyRShft is always positive or zero)
 *
 *   OUTPUT:
 *
 *      none
 *
 *   RETURN:
 *
 *      the agc's gain/2 note DIVIDED by 2
 *
 *
 *   REFERENCES: Sub_Clause 4.2.2 and 4.2.4 of GSM Recomendation 06.20
 *
 *   KEYWORDS: postfilter, agc, automaticgaincontrol, leveladjust
 *
 *************************************************************************/

static Shortword agcGain(Shortword pswStateCurr[],
                        struct NormSw snsInSigEnergy, Shortword swEngyRShft)
{

/*_________________________________________________________________________
 |                                                                         |
 |                            Automatic Variables                          |
 |_________________________________________________________________________|
*/

  Longword L_OutEnergy,
         L_AgcGain;

  struct NormSw snsOutEnergy,
         snsAgc;

  Shortword swAgcOut,
         swAgcShftCnt;

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

  /* Calculate the energy in the output vector divided by 2 */
  /*--------------------------------------------------------*/

  snsOutEnergy.sh = g_corr1s(pswStateCurr, swEngyRShft, &L_OutEnergy);

  /* reduce energy by a factor of 2 */
  snsOutEnergy.sh = add(snsOutEnergy.sh, 1);

  /* if waveform has nonzero energy, find AGC gain */
  /*-----------------------------------------------*/

  if (L_OutEnergy == 0)
  {
    swAgcOut = 0;
  }
  else
  {

    snsOutEnergy.man = round(L_OutEnergy);

    /* divide input energy by 2 */
    snsInSigEnergy.man = shr(snsInSigEnergy.man, 1);


    /* Calculate AGC gain squared */
    /*----------------------------*/

    snsAgc.man = divide_s(snsInSigEnergy.man, snsOutEnergy.man);
    swAgcShftCnt = norm_s(snsAgc.man);
    snsAgc.man = shl(snsAgc.man, swAgcShftCnt);

    /* find shift count for G^2 */
    /*--------------------------*/

    snsAgc.sh = add(sub(snsInSigEnergy.sh, snsOutEnergy.sh),
                    swAgcShftCnt);
    L_AgcGain = L_deposit_h(snsAgc.man);


    /* Calculate AGC gain */
    /*--------------------*/

    snsAgc.man = sqroot(L_AgcGain);


    /* check if 1/2 sqrt(G^2) >= 1.0                      */
    /* This is equivalent to checking if shiftCnt/2+1 < 0 */
    /*----------------------------------------------------*/

    if (add(snsAgc.sh, 2) < 0)
    {
      swAgcOut = SW_MAX;
    }
    else
    {

      if (0x1 & snsAgc.sh)
      {
        snsAgc.man = mult(snsAgc.man, SQRT_ONEHALF);
      }

      snsAgc.sh = shr(snsAgc.sh, 1);   /* shiftCnt/2 */
      snsAgc.sh = add(snsAgc.sh, 1);   /* shiftCnt/2 + 1 */

      if (snsAgc.sh > 0)
      {
        snsAgc.man = shr(snsAgc.man, snsAgc.sh);
      }
      swAgcOut = snsAgc.man;
    }
  }

  return (swAgcOut);
}

/***************************************************************************
 *
 *   FUNCTION NAME: lagDecode
 *
 *   PURPOSE:
 *
 *     The purpose of this function is to decode the lag received from the
 *     speech encoder into a full resolution lag for the speech decoder
 *
 *   INPUTS:
 *
 *     swDeltaLag
 *
 *                     lag received from channel decoder
 *
 *     giSfrmCnt
 *
 *                     current sub-frame count
 *
 *     swLastLag
 *
 *                     previous lag to un-delta this sub-frame's lag
 *
 *     psrLagTbl[0:255]
 *
 *                     table used to look up full resolution lag
 *
 *   OUTPUTS:
 *
 *     swLastLag
 *
 *                     new previous lag for next sub-frame
 *
 *   RETURN VALUE:
 *
 *     swLag
 *
 *                     decoded full resolution lag
 *
 *   DESCRIPTION:
 *
 *     If first subframe, use lag as index to look up table directly.
 *
 *     If it is one of the other subframes, the codeword represents a
 *     delta offset.  The previously decoded lag is used as a starting
 *     point for decoding the current lag.
 *
 *   REFERENCES: Sub-clause 4.2.1 of GSM Recomendation 06.20
 *
 *   KEYWORDS: deltalags, lookup lag
 *
 *************************************************************************/

static Shortword lagDecode(Shortword swDeltaLag, Shortword *pswLastLag,
			   Shortword subfrm)
{

/*_________________________________________________________________________
 |                                                                         |
 |                              Local Constants                            |
 |_________________________________________________________________________|
*/

#define  DELTA_LEVELS_D2  DELTA_LEVELS/2
#define  MAX_LAG          0x00ff
#define  MIN_LAG          0x0000

/*_________________________________________________________________________
 |                                                                         |
 |                            Automatic Variables                          |
 |_________________________________________________________________________|
*/

  Shortword swLag;

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

  /* first sub-frame */
  /* --------------- */

  if (subfrm == 0)
  {
    *pswLastLag = swDeltaLag;
  }

  /* remaining sub-frames */
  /* -------------------- */

  else
  {

    /* get lag biased around 0 */
    /* ----------------------- */

    swLag = sub(swDeltaLag, DELTA_LEVELS_D2);

    /* get real lag relative to last */
    /* ----------------------------- */

    swLag = add(swLag, *pswLastLag);

    /* clip to max or min */
    /* ------------------ */

    if (sub(swLag, MAX_LAG) > 0)
    {
      *pswLastLag = MAX_LAG;
    }
    else if (sub(swLag, MIN_LAG) < 0)
    {
      *pswLastLag = MIN_LAG;
    }
    else
    {
      *pswLastLag = swLag;
    }
  }

  /* return lag after look up */
  /* ------------------------ */

  swLag = psrLagTbl[*pswLastLag];
  return (swLag);
}

/***************************************************************************
 *
 *   FUNCTION NAME: lookupVq
 *
 *   PURPOSE:
 *
 *     The purpose of this function is to recover the reflection coeffs from
 *     the received LPC codewords.
 *
 *   INPUTS:
 *
 *     pswVqCodeWds[0:2]
 *
 *                         the codewords for each of the segments
 *
 *   OUTPUTS:
 *
 *     pswRCOut[0:NP-1]
 *
 *                        the decoded reflection coefficients
 *
 *   RETURN VALUE:
 *
 *     none.
 *
 *   DESCRIPTION:
 *
 *     For each segment do the following:
 *       setup the retrieval pointers to the correct vector
 *       get that vector
 *
 *   REFERENCES: Sub-clause 4.2.3 of GSM Recomendation 06.20
 *
 *   KEYWORDS: vq, vectorquantizer, lpc
 *
 *************************************************************************/

static void lookupVq(Shortword pswVqCodeWds[], Shortword pswRCOut[])
{
/*_________________________________________________________________________
 |                                                                         |
 |                              Local Constants                            |
 |_________________________________________________________________________|
*/

#define  LSP_MASK  0x00ff

/*_________________________________________________________________________
 |                                                                         |
 |                            Automatic Variables                          |
 |_________________________________________________________________________|
*/

  short int siSeg,
         siIndex,
         siVector,
         siVector1,
         siVector2,
         siWordPtr;

  const ShortwordRom *psrQTable;

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

  /* for each segment */
  /* ---------------- */

  for (siSeg = 0; siSeg < QUANT_NUM_OF_TABLES; siSeg++)
  {

    siVector = pswVqCodeWds[siSeg];
    siIndex = psvqIndex[siSeg].l;

    if (sub(siSeg, 2) == 0)
    {                                  /* segment 3 */

      /* set table */
      /* --------- */

      psrQTable = psrQuant3;

      /* set offset into table */
      /* ---------------------- */

      siWordPtr = add(siVector, siVector);

      /* look up coeffs */
      /* -------------- */

      siVector1 = psrQTable[siWordPtr];
      siVector2 = psrQTable[siWordPtr + 1];

      pswRCOut[siIndex - 1] = psrSQuant[shr(siVector1, 8) & LSP_MASK];
      pswRCOut[siIndex] = psrSQuant[siVector1 & LSP_MASK];
      pswRCOut[siIndex + 1] = psrSQuant[shr(siVector2, 8) & LSP_MASK];
      pswRCOut[siIndex + 2] = psrSQuant[siVector2 & LSP_MASK];
    }
    else
    {                                  /* segments 1 and 2 */

      /* set tables */
      /* ---------- */

      if (siSeg == 0)
      {
        psrQTable = psrQuant1;
      }
      else
      {
        psrQTable = psrQuant2;

      }

      /* set offset into table */
      /* --------------------- */

      siWordPtr = add(siVector, siVector);
      siWordPtr = add(siWordPtr, siVector);
      siWordPtr = shr(siWordPtr, 1);

      /* look up coeffs */
      /* -------------- */

      siVector1 = psrQTable[siWordPtr];
      siVector2 = psrQTable[siWordPtr + 1];

      if ((siVector & 0x0001) == 0)
      {
        pswRCOut[siIndex - 1] = psrSQuant[shr(siVector1, 8) & LSP_MASK];
        pswRCOut[siIndex] = psrSQuant[siVector1 & LSP_MASK];
        pswRCOut[siIndex + 1] = psrSQuant[shr(siVector2, 8) & LSP_MASK];
      }
      else
      {
        pswRCOut[siIndex - 1] = psrSQuant[siVector1 & LSP_MASK];
        pswRCOut[siIndex] = psrSQuant[shr(siVector2, 8) & LSP_MASK];
        pswRCOut[siIndex + 1] = psrSQuant[siVector2 & LSP_MASK];
      }
    }
  }
}

/**************************************************************************
 *
 *   FUNCTION NAME: pitchPreFilt
 *
 *   PURPOSE:
 *
 *     Performs pitch pre-filter on excitation in speech decoder.
 *
 *   INPUTS:
 *
 *     pswExcite[0:39]
 *
 *                     Synthetic residual signal to be filtered, a subframe-
 *                     length vector.
 *
 *     ppsrPVecIntFilt[0:9][0:5] ([tap][phase])
 *
 *                     Interpolation filter coefficients.
 *
 *     ppsrSqtrP0[0:2][0:31] ([voicing level-1][gain code])
 *
 *                     Sqrt(P0) look-up table, used to determine pitch
 *                     pre-filtering coefficient.
 *
 *     swRxGsp0
 *
 *                     Coded value from gain quantizer, used to look up
 *                     sqrt(P0).
 *
 *     swRxLag
 *
 *                     Full-resolution lag value (fractional lag *
 *                     oversampling factor), used to index pitch pre-filter
 *                     state.
 *
 *     swUvCode
 *
 *                     Coded voicing level, used to distinguish between
 *                     voiced and unvoiced conditions, and to look up
 *                     sqrt(P0).
 *
 *     swSemiBeta
 *
 *                     The gain applied to the adaptive codebook excitation
 *                     (long-term predictor excitation) limited to a maximum
 *                     of 1.0, used to determine the pitch pre-filter
 *                     coefficient.
 *
 *     snsSqrtRs
 *
 *                     The estimate of the energy in the residual, used only
 *                     for scaling.
 *
 *   OUTPUTS:
 *
 *     pswExciteOut[0:39]
 *
 *                     The output pitch pre-filtered excitation.
 *
 *     pswPPreState[0:44]
 *
 *                     Contains the state of the pitch pre-filter
 *
 *   RETURN VALUE:
 *
 *     none
 *
 *   DESCRIPTION:
 *
 *     If the voicing mode for the frame is unvoiced, then the pitch pre-
 *     filter state is updated with the input excitation, and the input
 *     excitation is copied to the output.
 *
 *     If voiced: first the energy in the input excitation is calculated.
 *     Then, the coefficient of the pitch pre-filter is obtained:
 *
 *     PpfCoef = POST_EPSILON * min(beta, sqrt(P0)).
 *
 *     Then, the pitch pre-filter is performed:
 *
 *     ex_p(n) = ex(n)  +  PpfCoef * ex_p(n-L)
 *
 *     The ex_p(n-L) sample is interpolated from the surrounding samples,
 *     even for integer values of L.
 *
 *     Note: The coefficients of the interpolating filter are multiplied
 *     by PpfCoef, rather multiplying ex_p(n_L) after interpolation.
 *
 *     Finally, the energy in the output excitation is calculated, and
 *     automatic gain control is applied to the output signal so that
 *     its energy matches the original.
 *
 *     The pitch pre-filter is described in section 4.2.2.
 *
 *   REFERENCES: Sub-clause 4.2.2 of GSM Recomendation 06.20
 *
 *   KEYWORDS: prefilter, pitch, pitchprefilter, excitation, residual
 *
 *************************************************************************/

static void pitchPreFilt(Shortword pswExcite[],
                                Shortword swRxGsp0,
                                Shortword swRxLag, Shortword swUvCode,
                              Shortword swSemiBeta, struct NormSw snsSqrtRs,
                                Shortword pswExciteOut[],
                                Shortword pswPPreState[])
{

/*_________________________________________________________________________
 |                                                                         |
 |                              Local Constants                            |
 |_________________________________________________________________________|
*/

#define  POST_EPSILON  0x2666

/*_________________________________________________________________________
 |                                                                         |
 |                           Local Static Variables                        |
 |_________________________________________________________________________|
*/


/*_________________________________________________________________________
 |                                                                         |
 |                            Automatic Variables                          |
 |_________________________________________________________________________|
*/

  Longword L_1,
         L_OrigEnergy;

  Shortword swScale,
         swSqrtP0,
         swIntLag,
         swRemain,
         swEnergy,
         pswInterpCoefs[P_INT_MACS];

  short int i,
         j;

  struct NormSw snsOrigEnergy;

  Shortword *pswPPreCurr = &pswPPreState[LTP_LEN];

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

  /* Initialization */
  /*----------------*/

  swEnergy = 0;

  /* Check voicing level */
  /*---------------------*/

  if (swUvCode == 0)
  {

    /* Unvoiced: perform one subframe of delay on state, copy input to */
    /* state, copy input to output (if not same)                       */
    /*-----------------------------------------------------------------*/

    for (i = 0; i < LTP_LEN - S_LEN; i++)
      pswPPreState[i] = pswPPreState[i + S_LEN];

    for (i = 0; i < S_LEN; i++)
      pswPPreState[i + LTP_LEN - S_LEN] = pswExcite[i];

    if (pswExciteOut != pswExcite)
    {

      for (i = 0; i < S_LEN; i++)
        pswExciteOut[i] = pswExcite[i];
    }
  }
  else
  {

    /* Voiced: calculate energy in input, filter, calculate energy in */
    /* output, scale                                                  */
    /*----------------------------------------------------------------*/

    /* Get energy in input excitation vector */
    /*---------------------------------------*/

    swEnergy = add(negate(shl(snsSqrtRs.sh, 1)), 3);

    if (swEnergy > 0)
    {

      /* High-energy residual: scale input vector during energy */
      /* calculation.  The shift count + 1 of the energy of the */
      /* residual estimate is used as an estimate of the shift  */
      /* count needed for the excitation energy                 */
      /*--------------------------------------------------------*/


      snsOrigEnergy.sh = g_corr1s(pswExcite, swEnergy, &L_OrigEnergy);
      snsOrigEnergy.man = round(L_OrigEnergy);

    }
    else
    {

      /* set shift count to zero for AGC later */
      /*---------------------------------------*/

      swEnergy = 0;

      /* Lower-energy residual: no overflow protection needed */
      /*------------------------------------------------------*/

      L_OrigEnergy = 0;
      for (i = 0; i < S_LEN; i++)
      {

        L_OrigEnergy = L_mac(L_OrigEnergy, pswExcite[i], pswExcite[i]);
      }

      snsOrigEnergy.sh = norm_l(L_OrigEnergy);
      snsOrigEnergy.man = round(L_shl(L_OrigEnergy, snsOrigEnergy.sh));
    }

    /* Determine pitch pre-filter coefficient, and scale the appropriate */
    /* phase of the interpolating filter by it                           */
    /*-------------------------------------------------------------------*/

    swSqrtP0 = ppsrSqrtP0[swUvCode - 1][swRxGsp0];

    if (sub(swSqrtP0, swSemiBeta) > 0)
      swScale = swSemiBeta;
    else
      swScale = swSqrtP0;

    swScale = mult_r(POST_EPSILON, swScale);

    get_ipjj(swRxLag, &swIntLag, &swRemain);

    for (i = 0; i < P_INT_MACS; i++)
      pswInterpCoefs[i] = mult_r(ppsrPVecIntFilt[i][swRemain], swScale);

    /* Perform filter */
    /*----------------*/

    for (i = 0; i < S_LEN; i++)
    {

      L_1 = L_deposit_h(pswExcite[i]);

      for (j = 0; j < P_INT_MACS - 1; j++)
      {

        L_1 = L_mac(L_1, pswPPreCurr[i - swIntLag - P_INT_MACS / 2 + j],
                    pswInterpCoefs[j]);
      }

      pswPPreCurr[i] = mac_r(L_1,
                             pswPPreCurr[i - swIntLag + P_INT_MACS / 2 - 1],
                             pswInterpCoefs[P_INT_MACS - 1]);
    }

    /* Get energy in filtered vector, determine automatic-gain-control */
    /* scale factor                                                    */
    /*-----------------------------------------------------------------*/

    swScale = agcGain(pswPPreCurr, snsOrigEnergy, swEnergy);

    /* Scale filtered vector by AGC, put out.  NOTE: AGC scale returned */
    /* by routine above is divided by two, hence the shift below        */
    /*------------------------------------------------------------------*/

    for (i = 0; i < S_LEN; i++)
    {

      L_1 = L_mult(pswPPreCurr[i], swScale);
      L_1 = L_shl(L_1, 1);
      pswExciteOut[i] = round(L_1);
    }

    /* Update pitch pre-filter state */
    /*-------------------------------*/

    for (i = 0; i < LTP_LEN; i++)
      pswPPreState[i] = pswPPreState[i + S_LEN];
  }
}

/***************************************************************************
 *
 *   FUNCTION NAME: spectralPostFilter
 *
 *   PURPOSE:
 *
 *     Perform spectral post filter on the output of the
 *     synthesis filter.
 *
 *
 *   INPUT:
 *
 *      S_LEN         a global constant
 *
 *      pswSPFIn[0:S_LEN-1]
 *
 *                    input to the routine. Unmodified
 *                    pswSPFIn[0] is the oldest point (first to be filtered),
 *                    pswSPFIn[iLen-1] is the last pointer filtered,
 *                    the newest.
 *
 *      pswNumCoef[0:NP-1],pswDenomCoef[0:NP-1]
 *
 *                    numerator and denominator
 *                    direct form coeffs used by postfilter.
 *                    Exactly like lpc coefficients in format.  Shifted down
 *                    by iAShift to ensure that they are < 1.0.
 *
 *      gpswPostFiltStateNum[0:NP-1], gpswPostFiltStateDenom[0:NP-1]
 *
 *                    array of the filter state.
 *                    Same format as coefficients: *praState = state of
 *                    filter for delay n = -1 praState[NP] = state of
 *                    filter for delay n = -NP These numbers are not
 *                    shifted at all.  These states are static to this
 *                    file.
 *
 *   OUTPUT:
 *
 *      gpswPostFiltStateNum[0:NP-1], gpswPostFiltStateDenom[0:NP-1]
 *
 *                      See above for description.  These are updated.
 *
 *      pswSPFOut[0:S_LEN-1]
 *
 *                    same format as pswSPFIn,
 *                    *pswSPFOut is oldest point. The filtered output.
 *                    Note this routine can handle pswSPFOut = pswSPFIn.
 *                    output can be the same as input.  i.e. in place
 *                    calculation.
 *
 *   RETURN:
 *
 *      none
 *
 *   DESCRIPTION:
 *
 *      find energy in input,
 *      perform the numerator fir
 *      perform the denominator iir
 *      perform the post emphasis
 *      find energy in signal,
 *      perform the agc using energy in and energy in signam after
 *      post emphasis signal
 *
 *      The spectral postfilter is described in section 4.2.4.
 *
 *   REFERENCES: Sub-clause 4.2.4 of GSM Recomendation 06.20
 *
 *   KEYWORDS: postfilter, emphasis, postemphasis, brightness,
 *   KEYWORDS: numerator, deminator, filtering, lpc,
 *
 *************************************************************************/

static void spectralPostFilter(struct gsmhr_decoder_state *st,
				Shortword swEngyRShift,
				Shortword pswSPFIn[], Shortword pswNumCoef[],
				Shortword pswDenomCoef[],
				Shortword pswSPFOut[])
{
/*_________________________________________________________________________
 |                                                                         |
 |                              Local Constants                            |
 |_________________________________________________________________________|
*/

#define  AGC_COEF       (Shortword)0x19a        /* (1.0 - POST_AGC_COEF)
                                                 * 1.0-.9875 */
#define  POST_EMPHASIS  (Shortword)0x199a       /* 0.2 */

/*_________________________________________________________________________
 |                                                                         |
 |                            Automatic Variables                          |
 |_________________________________________________________________________|
*/

  short int i;

  Longword L_OrigEnergy,
         L_runningGain,
         L_Output;

  Shortword swAgcGain,
         swRunningGain,
         swTemp;

  struct NormSw snsOrigEnergy;

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

  /* calculate the energy in the input and save it */
  /*-----------------------------------------------*/

  snsOrigEnergy.sh = g_corr1s(pswSPFIn, swEngyRShift, &L_OrigEnergy);
  snsOrigEnergy.man = round(L_OrigEnergy);

  /* numerator of the postfilter */
  /*-----------------------------*/

  lpcFir(pswSPFIn, pswNumCoef, st->gpswPostFiltStateNum, pswSPFOut);

  /* denominator of the postfilter */
  /*-------------------------------*/

  lpcIir(pswSPFOut, pswDenomCoef, st->gpswPostFiltStateDenom, pswSPFOut);

  /* postemphasis section of postfilter */
  /*------------------------------------*/

  for (i = 0; i < S_LEN; i++)
  {
    swTemp = msu_r(L_deposit_h(pswSPFOut[i]), st->swPostEmphasisState,
                   POST_EMPHASIS);
    st->swPostEmphasisState = pswSPFOut[i];
    pswSPFOut[i] = swTemp;
  }

  swAgcGain = agcGain(pswSPFOut, snsOrigEnergy, swEngyRShift);

  /* scale the speech vector */
  /*-----------------------------*/

  swRunningGain = st->gswPostFiltAgcGain;
  L_runningGain = L_deposit_h(st->gswPostFiltAgcGain);
  for (i = 0; i < S_LEN; i++)
  {
    L_runningGain = L_msu(L_runningGain, swRunningGain, AGC_COEF);
    L_runningGain = L_mac(L_runningGain, swAgcGain, AGC_COEF);
    swRunningGain = extract_h(L_runningGain);

    /* now scale input with gain */

    L_Output = L_mult(swRunningGain, pswSPFOut[i]);
    pswSPFOut[i] = extract_h(L_shl(L_Output, 2));
  }
  st->gswPostFiltAgcGain = swRunningGain;

}

void gsmhr_decode_frame(struct gsmhr_decoder_state *st,
			const int16_t *param_in, int16_t *pcm_out)
{
  int16_t param_preened[GSMHR_NUM_PARAMS];

  Shortword *pswLtpStateOut = &st->pswLtpStateBaseDec[LTP_LEN];
  Shortword pswSythAsSpace[NP * N_SUB];
  Shortword pswPFNumAsSpace[NP * N_SUB];
  Shortword pswPFDenomAsSpace[NP * N_SUB];

  Shortword *ppswSynthAs[N_SUB] = {
    &pswSythAsSpace[0],
    &pswSythAsSpace[10],
    &pswSythAsSpace[20],
    &pswSythAsSpace[30],
  };

  Shortword *ppswPFNumAs[N_SUB] = {
    &pswPFNumAsSpace[0],
    &pswPFNumAsSpace[10],
    &pswPFNumAsSpace[20],
    &pswPFNumAsSpace[30],
  };

  Shortword *ppswPFDenomAs[N_SUB] = {
    &pswPFDenomAsSpace[0],
    &pswPFDenomAsSpace[10],
    &pswPFDenomAsSpace[20],
    &pswPFDenomAsSpace[30],
  };

  static const ShortwordRom psrSPFDenomWidenCf[NP] = {
    0x6000, 0x4800, 0x3600, 0x2880, 0x1E60,
    0x16C8, 0x1116, 0x0CD0, 0x099C, 0x0735,
  };

  short int i, j,
         giSfrmCnt,
         siLagCode,
         siGsp0Code,
         psiVselpCw[2],
         siVselpCw,
         siNumBits,
         siCodeBook;

  Shortword pswFrmKs[NP],
         pswFrmAs[NP],
         pswFrmPFNum[NP],
         pswFrmPFDenom[NP],
         pswPVec[S_LEN],
         ppswVselpEx[2][S_LEN],
         pswExcite[S_LEN],
         pswPPFExcit[S_LEN],
         pswSynthFiltOut[S_LEN],
         swDecoMode,
         swR0Index,
         swR0Dec,
         swLag,
         swLastLag,
         swSemiBeta,
         pswBitArray[MAXBITS];

  struct NormSw psnsSqrtRs[N_SUB],
         snsRs00,
         snsRs11,
         snsRs22;

  Shortword swMutePermit,
         swLevelMean,
         swLevelMax,                   /* error concealment */
         swMuteFlag;                   /* error concealment */

  Shortword swVoicingMode,             /* MODE */
         swSi,                         /* INT_LPC */
         swEngyRShift;                 /* for use by spectral postfilter */

  /* initial home state processing must be done first */
  if (st->is_homed) {
    /*
     * If we got a BFI, there is no point in taking the decoder out of the
     * initial state.  Emit all zeros and stay homed.  Most forms of
     * invalid SID will also be caught here.
     */
    if (param_in[18]) {
      memset(pcm_out, 0, sizeof(int16_t) * F_LEN);
      return;
    }
    /*
     * Do the spec-mandated check for partial DHF.  Note how we check for
     * non-SID, after having weeded out BFI above.
     */
    if (param_in[20] == 0 &&
        memcmp(param_in, gsmhr_dhf_params, sizeof(int16_t) * 9) == 0) {
      /* EHF output */
      for (i = 0; i < F_LEN; i++)
        pcm_out[i] = 0x0008;
      return;
    }
  }

  /* integration with factored-out RxFE - different from ETSI original code */
  rxfe_main(&st->rxfe, param_in, param_preened, 0, &swDecoMode, &swMutePermit,
            NULL);
  swR0Index = param_preened[0];      /* R0 Index */
  swR0Dec = psrR0DecTbl[swR0Index * 2];       /* R0 */
  if (swDecoMode != CNIBFI)
    lookupVq(param_preened + 1, pswFrmKs);
  swSi = param_preened[4];           /* INT_LPC */
  swVoicingMode = param_preened[5];  /* MODE */

  switch (swDecoMode) {
  case SPEECH:
    st->swRxDTXState = CNINTPER - 1;
    break;
  case CNIFIRSTSID:
    st->swRxDTXState = CNINTPER - 1;
    Init_CN_interpolation(st, swDecoMode, pswFrmKs);
    swR0Dec = CN_Interpolate_R0(st, swR0Dec);
    CN_Interpolate_LPC(st, pswFrmKs);
    break;
  case CNICONT:
    st->swRxDTXState = 0;
    Init_CN_interpolation(st, swDecoMode, pswFrmKs);
    swR0Dec = CN_Interpolate_R0(st, swR0Dec);
    CN_Interpolate_LPC(st, pswFrmKs);
    break;
  case CNIBFI:
    if (st->swRxDTXState < (CNINTPER - 1))
      st->swRxDTXState++;
    swR0Dec = CN_Interpolate_R0(st, swR0Dec);
    CN_Interpolate_LPC(st, pswFrmKs);
    break;
  }

  /* ------------------- */
  /* do frame processing */
  /* ------------------- */

  /* This flag indicates whether muting is performed in the actual frame */
  /* ------------------------------------------------------------------- */
  swMuteFlag = 0;

  /* generate the direct form coefs */
  /* ------------------------------ */

  if (!rcToADp(ASCALE, pswFrmKs, pswFrmAs))
  {
    /* widen direct form coefficients using the widening coefs */
    /* ------------------------------------------------------- */

    for (i = 0; i < NP; i++)
    {
      pswFrmPFDenom[i] = mult_r(pswFrmAs[i], psrSPFDenomWidenCf[i]);
    }

    a_sst(ASHIFT, ASCALE, pswFrmPFDenom, pswFrmPFNum);
  }
  else
  {
    for (i = 0; i < NP; i++)
    {
      pswFrmKs[i] = st->pswOldFrmKsDec[i];
      pswFrmAs[i] = st->pswOldFrmAsDec[i];
      pswFrmPFDenom[i] = st->pswOldFrmPFDenom[i];
      pswFrmPFNum[i] = st->pswOldFrmPFNum[i];
    }
  }

  /* interpolate, or otherwise get sfrm reflection coefs */
  /* --------------------------------------------------- */

  getSfrmLpc(swSi, st->swOldR0Dec, swR0Dec, st->pswOldFrmKsDec,
             st->pswOldFrmAsDec, st->pswOldFrmPFNum, st->pswOldFrmPFDenom,
             pswFrmKs, pswFrmAs,
             pswFrmPFNum, pswFrmPFDenom, psnsSqrtRs, ppswSynthAs,
             ppswPFNumAs, ppswPFDenomAs);

  /* calculate shift for spectral postfilter */
  /* --------------------------------------- */

  swEngyRShift = r0BasedEnergyShft(swR0Index);


  /* ----------------------- */
  /* do sub-frame processing */
  /* ----------------------- */

  for (giSfrmCnt = 0; giSfrmCnt < 4; giSfrmCnt++)
  {

    /* copy this sub-frame's parameters */
    /* -------------------------------- */

    if (swVoicingMode == 0)
    {                                  /* unvoiced */
      psiVselpCw[0] = param_preened[(giSfrmCnt * 3) + 6];       /* CODE_1 */
      psiVselpCw[1] = param_preened[(giSfrmCnt * 3) + 7];       /* CODE_2 */
      siGsp0Code = param_preened[(giSfrmCnt * 3) + 8];  /* GSP0 */
    }
    else
    {                                  /* voiced */
      siLagCode = param_preened[(giSfrmCnt * 3) + 6];   /* LAG */
      psiVselpCw[0] = param_preened[(giSfrmCnt * 3) + 7];       /* CODE */
      siGsp0Code = param_preened[(giSfrmCnt * 3) + 8];  /* GSP0 */
    }

    /* for voiced mode, reconstruct the pitch vector */
    /* --------------------------------------------- */

    if (swVoicingMode)
    {

      /* convert delta lag to lag and convert to fractional lag */
      /* ------------------------------------------------------ */

      swLag = lagDecode(siLagCode, &swLastLag, giSfrmCnt);

      /* state followed by out */
      /* --------------------- */

      fp_ex(swLag, pswLtpStateOut);

      /* extract a piece of pswLtpStateOut into newly named vector pswPVec */
      /* ----------------------------------------------------------------- */

      for (i = 0; i < S_LEN; i++)
      {
        pswPVec[i] = pswLtpStateOut[i];
      }
    }

    /* for unvoiced, do not reconstruct a pitch vector */
    /* ----------------------------------------------- */

    else
    {
      swLag = 0;                       /* indicates invalid lag
                                        * and unvoiced */
    }

    /* now work on the VSELP codebook excitation output */
    /* x_vec, x_a_vec here named ppswVselpEx[0] and [1] */
    /* ------------------------------------------------ */

    if (swVoicingMode)
    {                                  /* voiced */

      siNumBits = C_BITS_V;
      siVselpCw = psiVselpCw[0];

      b_con(siVselpCw, siNumBits, pswBitArray);

      v_con(pppsrVcdCodeVec[0][0], ppswVselpEx[0], pswBitArray, siNumBits);
    }

    else
    {                                  /* unvoiced */

      siNumBits = C_BITS_UV;

      for (siCodeBook = 0; siCodeBook < 2; siCodeBook++)
      {

        siVselpCw = psiVselpCw[siCodeBook];

        b_con(siVselpCw, siNumBits, (Shortword *) pswBitArray);

        v_con(pppsrUvCodeVec[siCodeBook][0], ppswVselpEx[siCodeBook],
              pswBitArray, siNumBits);
      }
    }

    /* all excitation vectors have been created: ppswVselpEx and pswPVec  */
    /* if voiced compute rs00 and rs11; if unvoiced cmpute rs11 and rs22  */
    /* ------------------------------------------------------------------ */

    if (swLag)
    {
      rs_rr(pswPVec, psnsSqrtRs[giSfrmCnt], &snsRs00);
    }

    rs_rrNs(ppswVselpEx[0], psnsSqrtRs[giSfrmCnt], &snsRs11);

    if (!swVoicingMode)
    {
      rs_rrNs(ppswVselpEx[1], psnsSqrtRs[giSfrmCnt], &snsRs22);
    }

    /* now implement synthesis - combine the excitations */
    /* ------------------------------------------------- */

    if (swVoicingMode)
    {                                  /* voiced */

      /* scale pitch and codebook excitations and get beta */
      /* ------------------------------------------------- */
      swSemiBeta = scaleExcite(pswPVec,
                               pppsrGsp0[swVoicingMode][siGsp0Code][0],
                               snsRs00, pswPVec);
      scaleExcite(ppswVselpEx[0],
                  pppsrGsp0[swVoicingMode][siGsp0Code][1],
                  snsRs11, ppswVselpEx[0]);

      /* combine the two scaled excitations */
      /* ---------------------------------- */
      for (i = 0; i < S_LEN; i++)
      {
        pswExcite[i] = add(pswPVec[i], ppswVselpEx[0][i]);
      }
    }
    else
    {                                  /* unvoiced */

      /* scale codebook excitations and set beta to 0 as not applicable */
      /* -------------------------------------------------------------- */
      swSemiBeta = 0;
      scaleExcite(ppswVselpEx[0],
                  pppsrGsp0[swVoicingMode][siGsp0Code][0],
                  snsRs11, ppswVselpEx[0]);
      scaleExcite(ppswVselpEx[1],
                  pppsrGsp0[swVoicingMode][siGsp0Code][1],
                  snsRs22, ppswVselpEx[1]);

      /* combine the two scaled excitations */
      /* ---------------------------------- */
      for (i = 0; i < S_LEN; i++)
      {
        pswExcite[i] = add(ppswVselpEx[1][i], ppswVselpEx[0][i]);
      }
    }

    /* now update the pitch state using the combined/scaled excitation */
    /* --------------------------------------------------------------- */

    for (i = 0; i < LTP_LEN; i++)
    {
      st->pswLtpStateBaseDec[i] = st->pswLtpStateBaseDec[i + S_LEN];
    }

    /* add the current sub-frames data to the state */
    /* -------------------------------------------- */

    for (i = -S_LEN, j = 0; j < S_LEN; i++, j++)
    {
      pswLtpStateOut[i] = pswExcite[j];/* add new frame at t = -S_LEN */
    }

    /* given the excitation perform pitch prefiltering */
    /* ----------------------------------------------- */

    pitchPreFilt(pswExcite, siGsp0Code, swLag,
                 swVoicingMode, swSemiBeta, psnsSqrtRs[giSfrmCnt],
                 pswPPFExcit, st->pswPPreState);


    /* Concealment on subframe signal level: */
    /* ------------------------------------- */
    level_estimator_det(st, &swLevelMean, &swLevelMax);

    signal_conceal_sub(pswPPFExcit, ppswSynthAs[giSfrmCnt],
                       st->pswSynthFiltState, &pswLtpStateOut[-S_LEN],
                       &st->pswPPreState[LTP_LEN - S_LEN],
                       swLevelMean, swLevelMax,
                       param_in[19], st->swMuteFlagOld,
                       &swMuteFlag, swMutePermit);


    /* synthesize the speech through the synthesis filter */
    /* -------------------------------------------------- */

    lpcIir(pswPPFExcit, ppswSynthAs[giSfrmCnt], st->pswSynthFiltState,
           pswSynthFiltOut);

    /* pass reconstructed speech through adaptive spectral postfilter */
    /* -------------------------------------------------------------- */

    spectralPostFilter(st, swEngyRShift,
                       pswSynthFiltOut, ppswPFNumAs[giSfrmCnt],
                       ppswPFDenomAs[giSfrmCnt],
                       &pcm_out[giSfrmCnt * S_LEN]);

    level_estimator_upd(st, &pcm_out[giSfrmCnt * S_LEN]);

  }

  /* truncate the 16 bit linear pcm to 13 bits */
  /* ----------------------------------------- */

  for (i = 0; i < F_LEN; i++)
  {
    pcm_out[i] &= PCM_MASK;
  }

  /* Nothing but state updates left - do homing logic here. */
  if (param_in[18] == 0 && param_in[20] == 0 &&
      memcmp(param_in, gsmhr_dhf_params, sizeof(int16_t) * 18) == 0) {
	gsmhr_decoder_reset(st);
	return;
  }
  st->is_homed = 0;

  /* Save muting information for next frame */
  /* -------------------------------------- */
  st->swMuteFlagOld = swMuteFlag;

  /* end of frame processing - save this frame's frame energy,  */
  /* reflection coefs, direct form coefs, and post filter coefs */
  /* ---------------------------------------------------------- */

  st->swOldR0Dec = swR0Dec;

  for (i = 0; i < NP; i++)
  {
    st->pswOldFrmKsDec[i] = pswFrmKs[i];
    st->pswOldFrmAsDec[i] = pswFrmAs[i];
    st->pswOldFrmPFNum[i] = pswFrmPFNum[i];
    st->pswOldFrmPFDenom[i] = pswFrmPFDenom[i];
  }
}