/******************************************************************

       iLBC Speech Coder ANSI-C Source Code

       doCPLC.c

       Copyright (C) The Internet Society (2004).
       All Rights Reserved.

   ******************************************************************/

#include <math.h>
#include <string.h>
#include <stdio.h>





#include "iLBC_define.h"

   /*----------------------------------------------------------------*
    *  Compute cross correlation and pitch gain for pitch prediction
    *  of last subframe at given lag.
    *---------------------------------------------------------------*/

void compCorr(float *cc,        /* (o) cross correlation coefficient */
  float *gc,                    /* (o) gain */
  float *pm, float *buffer,     /* (i) signal buffer */
  int lag,                      /* (i) pitch lag */
  int bLen,                     /* (i) length of buffer */
  int sRange                    /* (i) correlation search length */
  )
{
  int i;
  float ftmp1, ftmp2, ftmp3;

  /* Guard against getting outside buffer */
  if ((bLen - sRange - lag) < 0) {
    sRange = bLen - lag;
  }

  ftmp1 = 0.0;
  ftmp2 = 0.0;
  ftmp3 = 0.0;
  for (i = 0; i < sRange; i++) {
    ftmp1 += buffer[bLen - sRange + i] *
      buffer[bLen - sRange + i - lag];
    ftmp2 += buffer[bLen - sRange + i - lag] *
      buffer[bLen - sRange + i - lag];
    ftmp3 += buffer[bLen - sRange + i] * buffer[bLen - sRange + i];
  }

  if (ftmp2 > 0.0) {
    *cc = ftmp1 * ftmp1 / ftmp2;
    *gc = (float) fabs(ftmp1 / ftmp2);
    *pm = (float) fabs(ftmp1) /
      ((float) sqrt(ftmp2) * (float) sqrt(ftmp3));
  } else {
    *cc = 0.0;
    *gc = 0.0;
    *pm = 0.0;
  }
}





   /*----------------------------------------------------------------*
    *  Packet loss concealment routine. Conceals a residual signal
    *  and LP parameters. If no packet loss, update state.
    *---------------------------------------------------------------*/

void doThePLC(float *PLCresidual,       /* (o) concealed residual */
  float *PLClpc,                /* (o) concealed LP parameters */
  int PLI,                      /* (i) packet loss indicator
                                   0 - no PL, 1 = PL */
  float *decresidual,           /* (i) decoded residual */
  float *lpc,                   /* (i) decoded LPC (only used for no PL) */
  int inlag,                    /* (i) pitch lag */
  iLBC_Dec_Inst_t * iLBCdec_inst
  /* (i/o) decoder instance */
  )
{
  int lag = 20, randlag;
  float gain, maxcc;
  float use_gain;
  float gain_comp, maxcc_comp, per, max_per;
  int i, pick, use_lag;
  float ftmp, randvec[BLOCKL_MAX], pitchfact, energy;

  /* Packet Loss */

  if (PLI == 1) {

    iLBCdec_inst->consPLICount += 1;

    /* if previous frame not lost,
       determine pitch pred. gain */

    if (iLBCdec_inst->prevPLI != 1) {

      /* Search around the previous lag to find the
         best pitch period */

      lag = inlag - 3;
      compCorr(&maxcc, &gain, &max_per,
        iLBCdec_inst->prevResidual, lag, iLBCdec_inst->blockl, 60);
      for (i = inlag - 2; i <= inlag + 3; i++) {
        compCorr(&maxcc_comp, &gain_comp, &per,
          iLBCdec_inst->prevResidual, i, iLBCdec_inst->blockl, 60);

        if (maxcc_comp > maxcc) {
          maxcc = maxcc_comp;





          gain = gain_comp;
          lag = i;
          max_per = per;
        }
      }

    }

    /* previous frame lost, use recorded lag and periodicity */

    else {
      lag = iLBCdec_inst->prevLag;
      max_per = iLBCdec_inst->per;
    }

    /* downscaling */

    use_gain = 1.0;
    if (iLBCdec_inst->consPLICount * iLBCdec_inst->blockl > 320)
      use_gain = (float) 0.9;
    else if (iLBCdec_inst->consPLICount *
      iLBCdec_inst->blockl > 2 * 320)
      use_gain = (float) 0.7;
    else if (iLBCdec_inst->consPLICount *
      iLBCdec_inst->blockl > 3 * 320)
      use_gain = (float) 0.5;
    else if (iLBCdec_inst->consPLICount *
      iLBCdec_inst->blockl > 4 * 320)
      use_gain = (float) 0.0;

    /* mix noise and pitch repeatition */
    ftmp = (float) sqrt(max_per);
    if (ftmp > (float) 0.7)
      pitchfact = (float) 1.0;
    else if (ftmp > (float) 0.4)
      pitchfact = (ftmp - (float) 0.4) / ((float) 0.7 - (float) 0.4);
    else
      pitchfact = 0.0;


    /* avoid repetition of same pitch cycle */
    use_lag = lag;
    if (lag < 80) {
      use_lag = 2 * lag;
    }

    /* compute concealed residual */






    energy = 0.0;
    for (i = 0; i < iLBCdec_inst->blockl; i++) {

      /* noise component */

      iLBCdec_inst->seed = (iLBCdec_inst->seed * 69069L + 1) &
        (0x80000000L - 1);
      randlag = 50 + ((signed long) iLBCdec_inst->seed) % 70;
      pick = i - randlag;

      if (pick < 0) {
        randvec[i] =
          iLBCdec_inst->prevResidual[iLBCdec_inst->blockl + pick];
      } else {
        randvec[i] = randvec[pick];
      }

      /* pitch repeatition component */
      pick = i - use_lag;

      if (pick < 0) {
        PLCresidual[i] =
          iLBCdec_inst->prevResidual[iLBCdec_inst->blockl + pick];
      } else {
        PLCresidual[i] = PLCresidual[pick];
      }

      /* mix random and periodicity component */

      if (i < 80)
        PLCresidual[i] = use_gain * (pitchfact *
          PLCresidual[i] + ((float) 1.0 - pitchfact) * randvec[i]);
      else if (i < 160)
        PLCresidual[i] = (float) 0.95 *use_gain * (pitchfact *
        PLCresidual[i] + ((float) 1.0 - pitchfact) * randvec[i]);
      else
      PLCresidual[i] = (float) 0.9 *use_gain * (pitchfact *
        PLCresidual[i] + ((float) 1.0 - pitchfact) * randvec[i]);

      energy += PLCresidual[i] * PLCresidual[i];
    }

    /* less than 30 dB, use only noise */






    if (sqrt(energy / (float) iLBCdec_inst->blockl) < 30.0) {
      gain = 0.0;
      for (i = 0; i < iLBCdec_inst->blockl; i++) {
        PLCresidual[i] = randvec[i];
      }
    }

    /* use old LPC */

    memcpy(PLClpc, iLBCdec_inst->prevLpc,
      (LPC_FILTERORDER + 1) * sizeof(float));

  }

  /* no packet loss, copy input */

  else {
    memcpy(PLCresidual, decresidual,
      iLBCdec_inst->blockl * sizeof(float));
    memcpy(PLClpc, lpc, (LPC_FILTERORDER + 1) * sizeof(float));
    iLBCdec_inst->consPLICount = 0;
  }

  /* update state */

  if (PLI) {
    iLBCdec_inst->prevLag = lag;
    iLBCdec_inst->per = max_per;
  }

  iLBCdec_inst->prevPLI = PLI;
  memcpy(iLBCdec_inst->prevLpc, PLClpc,
    (LPC_FILTERORDER + 1) * sizeof(float));
  memcpy(iLBCdec_inst->prevResidual, PLCresidual,
    iLBCdec_inst->blockl * sizeof(float));
}