/***************************************************************************
 *
 *   File Name:  sp_frm.c
 *
 *   Purpose:  Contains all functions for frame-based processing in the
 *      speech encoder.  The frame-based processing yields the following:
 *      energy in the speech signal, LPC filter coefficients, perceptually-
 *      weighted filter coefficients (for H(z) and C(z)), perceptually-
 *      weighted speech, voicing level, and constrained adaptive-codebook
 *      (long-term predictor) choices.
 *
 *     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.
 *
 *     High pass filtering:
 *       filt4_2nd()
 *         iir_d()
 *
 *     AFLAT, vector quantization of LPC coefficients:
 *       aflat()
 *         aflatNewBarRecursionL()
 *         aflatRecursion()
 *         findBestInQuantList()
 *         getNextVec()
 *         initPBarVBarFullL()
 *         initPBarVBarL()
 *         setupPreQ()
 *         setupQuant()
 *
 *     FLAT:  derivation of the unquantized LPC coefficients:
 *       flat()
 *         cov32()
 *         r0Quant()
 *
 *
 *     Generation of LPC filters for each subframe:
 *       getSfrmLpcTx()
 *         compResidEnergy()
 *
 *     Perceptual weighting:
 *       weightSpeechFrame()
 *
 *     Generation of the noise weighting filter:
 *       getNWCoefs()
 *
 *     Open loop lag search:
 *       openLoopLagSearch()
 *         bestDelta()
 *           maxCCOverGWithSign()
 *         getCCThreshold()
 *           fnExp2()
 *           fnLog2()
 *         pitchLags()
 *           CGInterp()
 *           CGInterpValid()
 *           findPeak()
 *           fnBest_CG()
 *           quantLag()
 *
 **************************************************************************/

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

#include "mathhalf.h"
#include "mathdp31.h"
#include "sp_rom.h"
#include "sp_dec.h"
#include "sp_frm.h"
#include "sp_sfrm.h"
#include "vad.h"
#include "dtx.h"

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

#define ASCALE  0x0800
#define ASHIFT 4
#define CG_INT_MACS     6
#define CG_TERMS        (LSMAX - LSMIN + 1)
#define CVSHIFT 2                      /* Number of right shifts to be
                                        * applied to the normalized Phi
                                        * array in cov32, also used in flat
                                        * to shift down normalized F, B, C
                                        * matrices.                        */
#define C_FRAME_LEN     (N_SUB * CG_TERMS)
#define DELTA_LEVELS    16
#define G_FRAME_LEN     (LSMAX + (N_SUB-1) * S_LEN - LSMIN  + 1)
#define HIGH 1
#define INV_OS_FCTR     0x1555         /* 1.0/6.0 */
#define LAG_TABLE_LEN   (1 << L_BITS)
#define LMAX            142
#define LMAX_FR         (LMAX * OS_FCTR)
#define LMIN            21
#define LMIN_FR         (LMIN * OS_FCTR)
#define LOW 0
#define LPC_VQ_SEG 3
#define LSMAX           (LMAX + CG_INT_MACS/2)
#define LSMIN           (LMIN - CG_INT_MACS/2)
#define LSP_MASK  0xffff
#define L_BITS          8
#define L_ROUND (Longword)0x8000       /* Preload accumulator value for
                                        * rounding  */
#define NP_AFLAT     4
#define NUM_CLOSED      3
#define NUM_TRAJ_MAX    2
#define ONE_EIGHTH      0x1000
#define ONE_HALF        0x4000
#define ONE_QUARTER     0x2000
#define PEAK_VICINITY   3
#define PGAIN_CLAMP    0x0021          /* 0.001 */
#define PGAIN_SCALE    0x6000          /* 0.75 */
#define PW_FRAC         0x3333         /* 0.4 */
#define R0BITS 5
#define RSHIFT  2
#define S_SH    6                      /* Shift offset for computing frame
                                        * energy */
#define UV_SCALE0       -0x2976
#define UV_SCALE1       -0x46d3
#define UV_SCALE2       -0x6676
#define W_F_BUFF_LEN  (F_LEN + LSMAX)
#define high(x) (shr(x,8) & 0x00ff)
#define low(x) x & 0x00ff              /* This macro will return the low
                                        * byte of a word */
#define odd(x) (x & 0x0001)            /* This macro will determine if an
                                        * integer is odd */

/*_________________________________________________________________________
 |                                                                         |
 |                         State Variables (globals)                       |
 |_________________________________________________________________________|
*/

Shortword pswAnalysisState[NP];

Shortword pswWStateNum[NP],
       pswWStateDenom[NP];

/*_________________________________________________________________________
 |                                                                         |
 |                         Other External Variables                        |
 |_________________________________________________________________________|
*/

static ShortwordRom *psrTable;         /* points to correct table of
                                        * vectors */
int iLimit;                            /* accessible to all in this file
                                        * and to lpcCorrQntz() in dtx.c */
static int iLow;                       /* the low element in this segment */
static int iThree;                     /* boolean, is this a three element
                                        * vector */
static int iWordHalfPtr;               /* points to the next byte */
static int iWordPtr;                   /* points to the next word to be
                                        * read */

extern Shortword pswCNVSCode1[],       /* comfort noise parameters */
                 pswCNVSCode2[],
                 pswCNGsp0Code[],
                 pswCNLpc[],
                 swCNR0;

/***************************************************************************
 *
 *    FUNCTION NAME: aflat
 *
 *    PURPOSE:  Given a vector of high-pass filtered input speech samples
 *              (A_LEN samples), function aflat computes the NP unquantized
 *              reflection coefficients using the FLAT algorithm, searches
 *              the three segment Rc-VQ based on the AFLAT recursion, and
 *              outputs a quantized set of NP reflection coefficients, along
 *              with the three indices specifying the selected vectors
 *              from the Rc-VQ. The index of the quantized frame energy R0
 *              is also output.
 *
 *
 *    INPUT:
 *
 *        pswSpeechToLpc[0:A_LEN-1]
 *                     A vector of high-pass filtered input speech, from
 *                     which the unquantized reflection coefficients and
 *                     the index of the quantized frame energy are
 *                     computed.
 *
 *    OUTPUTS:
 *
 *        piR0Index[0:0]
 *                     An index into a 5 bit table of quantized frame
 *                     energies.
 *
 *        pswFinalRc[0:NP-1]
 *                     A quantized set of NP reflection coefficients.
 *
 *        piVQCodewds[0:2]
 *                     An array containing the indices of the 3 reflection
 *                     coefficient vectors selected from the three segment
 *                     Rc-VQ.
 *
 *        swPtch
 *                     Flag to indicate a periodic signal component
 *
 *        pswVadFlag
 *                     Voice activity decision flag
 *                      = 1: voice activity
 *                      = 0: no voice activity
 *
 *        pswSP
 *                     Speech flag
 *                      = 1: encoder generates speech frames
 *                      = 0: encoder generate SID frames
 *
 *
 *    RETURN:
 *        None.
 *
 *    REFERENCE:  Sub-clauses 4.1.3, 4.1.4, and 4.1.4.1
 *        of GSM Recommendation 06.20
 *
 *    KEYWORDS: AFLAT,aflat,flat,vectorquantization, reflectioncoefficients
 *
 *************************************************************************/

  void   aflat(Shortword pswSpeechToLPC[],
                      int piR0Index[],
                      Shortword pswFinalRc[],
                      int piVQCodewds[],
                      Shortword swPtch,
                      Shortword *pswVadFlag,
                      Shortword *pswSP)
{

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

  Shortword pswPOldSpace[NP_AFLAT],
         pswPNewSpace[NP_AFLAT],
         pswVOldSpace[2 * NP_AFLAT - 1],
         pswVNewSpace[2 * NP_AFLAT - 1],
        *ppswPAddrs[2],
        *ppswVAddrs[2],
        *pswVBar,
         pswPBar[NP_AFLAT],
         pswVBarSpace[2 * NP_AFLAT - 1],
         pswFlatsRc[NP],               /* Unquantized Rc's computed by FLAT */
         pswRc[NP + 1];                /* Temp list for the converted RC's */
  Longword pL_CorrelSeq[NP + 1],
        *pL_VBarFull,
         pL_PBarFull[NP],
         pL_VBarFullSpace[2 * NP - 1];

  int    i,
         iVec,
         iSeg,
         iCnt;                         /* Loop counter */
  struct QuantList quantList,          /* A list of vectors */
         bestPql[4];                   /* The four best vectors from the
                                        * PreQ */
  struct QuantList bestQl[LPC_VQ_SEG + 1];      /* Best vectors for each of
                                                 * the three segments */
  Shortword swVadScalAuto;
  Shortword pswVadRc[4];
  Longword pL_VadAcf[9];

  Longword L_R0;                       /* Normalized R0 (use swRShifts to 
                                        * unnormalize). This is done prior
                                        * to r0quant(). After this, its is
                                        * a unnormalized number */

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

  /* Setup pointers temporary space */
  /*--------------------------------*/

  pswVBar = pswVBarSpace + NP_AFLAT - 1;
  pL_VBarFull = pL_VBarFullSpace + NP - 1;
  ppswPAddrs[0] = pswPOldSpace;
  ppswPAddrs[1] = pswPNewSpace;
  ppswVAddrs[0] = pswVOldSpace + NP_AFLAT - 1;
  ppswVAddrs[1] = pswVNewSpace + NP_AFLAT - 1;

  /* Given the input speech, compute the optimal reflection coefficients */
  /* using the FLAT algorithm.                                           */
  /*---------------------------------------------------------------------*/

  L_R0 = flat(pswSpeechToLPC, pswFlatsRc, piR0Index, pL_VadAcf, 
              &swVadScalAuto);

  /* Get unquantized reflection coefficients for VAD */      /* DTX mode */
  /* algorithm                                       */      /* DTX mode */
  /* ----------------------------------------------- */      /* DTX mode */

  for (i = 0; i < 4; i++)                                    /* DTX mode */
    pswVadRc[i] = pswFlatsRc[i];                             /* DTX mode */


  /* convert reflection coefficients to correlation */       /* DTX mode */
  /* sequence                                       */       /* DTX mode */
  /* ---------------------------------------------- */       /* DTX mode */

  rcToCorrDpL(ASHIFT, ASCALE, pswFlatsRc, pL_CorrelSeq);     /* DTX mode */


  /* Make the voice activity detection. Only swVadFlag is */ /* DTX mode */
  /*  modified.                                           */ /* DTX mode */
  /* ---------------------------------------------------- */ /* DTX mode */

  vad_algorithm(pL_VadAcf, swVadScalAuto, pswVadRc, swPtch,  /* DTX mode */
                pswVadFlag);


  /* if DTX mode off, then always voice activity */          /* DTX mode */
  /* ------------------------------------------- */          /* DTX mode */
  if (!giDTXon) *pswVadFlag = 1;                             /* DTX mode */


  /* determination of comfort noise parameters */            /* DTX mode */
  /* ----------------------------------------- */            /* DTX mode */

  *pswSP = swComfortNoise(*pswVadFlag,                       /* DTX mode */
                          L_R0,                              /* DTX mode */
                          pL_CorrelSeq);                     /* DTX mode */

  if (*pswSP == 0)                                           /* DTX mode */
  {   /* SID frame generation */                             /* DTX mode */

    /* use unquantized reflection coefficients in the */     /* DTX mode */
    /* encoder, when SID frames are generated         */     /* DTX mode */
    /* ---------------------------------------------- */     /* DTX mode */

    for (i = 0; i < NP; i++)                                 /* DTX mode */
      pswFinalRc[i] = pswFlatsRc[i];                         /* DTX mode */

  }                                                          /* DTX mode */
  else                                                       /* DTX mode */
  { /* speech frame generation */

    /* Set up pL_PBarFull and pL_VBarFull initial conditions, using the   */
    /* autocorrelation sequence derived from the optimal reflection       */
    /* coefficients computed by FLAT. The initial conditions are shifted  */
    /* right by RSHIFT bits. These initial conditions, stored as          */
    /* Longwords, are used to initialize PBar and VBar arrays for the     */
    /* next VQ segment.                                                   */
    /*--------------------------------------------------------------------*/

    initPBarFullVBarFullL(pL_CorrelSeq, pL_PBarFull, pL_VBarFull);

    /* Set up initial PBar and VBar initial conditions, using pL_PBarFull */
    /* and pL_VBarFull arrays initialized above. These are the initial    */
    /* PBar and VBar conditions to be used by the AFLAT recursion at the  */
    /* 1-st Rc-VQ segment.                                                */
    /*--------------------------------------------------------------------*/

    initPBarVBarL(pL_PBarFull, pswPBar, pswVBar);

    for (iSeg = 1; iSeg <= LPC_VQ_SEG; iSeg++)
    {