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

       iLBC Speech Coder ANSI-C Source Code

       enhancer.c

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

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

#include <math.h>
#include <string.h>
#include "iLBC_define.h"
#include "constants.h"
#include "filter.h"

   /*----------------------------------------------------------------*
    * Find index in array such that the array element with said
    * index is the element of said array closest to "value"
    * according to the squared-error criterion
    *---------------------------------------------------------------*/

void NearestNeighbor(int *index,        /* (o) index of array element closest
                                           to value */
  float *array,                 /* (i) data array */
  float value,                  /* (i) value */
  int arlength                  /* (i) dimension of data array */
  )
{
  int i;
  float bestcrit, crit;

  crit = array[0] - value;
  bestcrit = crit * crit;
  *index = 0;
  for (i = 1; i < arlength; i++) {
    crit = array[i] - value;
    crit = crit * crit;

    if (crit < bestcrit) {
      bestcrit = crit;
      *index = i;
    }
  }
}

   /*----------------------------------------------------------------*
    * compute cross correlation between sequences
    *---------------------------------------------------------------*/

void mycorr1(float *corr,       /* (o) correlation of seq1 and seq2 */
  float *seq1,                  /* (i) first sequence */
  int dim1,                     /* (i) dimension first seq1 */
  const float *seq2,            /* (i) second sequence */
  int dim2                      /* (i) dimension seq2 */
  )
{
  int i, j;

  for (i = 0; i <= dim1 - dim2; i++) {
    corr[i] = 0.0;
    for (j = 0; j < dim2; j++) {
      corr[i] += seq1[i + j] * seq2[j];
    }
  }
}

   /*----------------------------------------------------------------*
    * upsample finite array assuming zeros outside bounds
    *---------------------------------------------------------------*/






void enh_upsample(float *useq1, /* (o) upsampled output sequence */
  float *seq1,                  /* (i) unupsampled sequence */
  int dim1,                     /* (i) dimension seq1 */
  int hfl                       /* (i) polyphase filter length=2*hfl+1 */
  )
{
  float *pu, *ps;
  int i, j, k, q, filterlength, hfl2;
  const float *polyp[ENH_UPS0]; /* pointers to
                                   polyphase columns */
  const float *pp;

  /* define pointers for filter */

  filterlength = 2 * hfl + 1;

  if (filterlength > dim1) {
    hfl2 = (int) (dim1 / 2);
    for (j = 0; j < ENH_UPS0; j++) {
      polyp[j] = polyphaserTbl + j * filterlength + hfl - hfl2;
    }
    hfl = hfl2;
    filterlength = 2 * hfl + 1;
  } else {
    for (j = 0; j < ENH_UPS0; j++) {
      polyp[j] = polyphaserTbl + j * filterlength;
    }
  }

  /* filtering: filter overhangs left side of sequence */

  pu = useq1;
  for (i = hfl; i < filterlength; i++) {
    for (j = 0; j < ENH_UPS0; j++) {
      *pu = 0.0;
      pp = polyp[j];
      ps = seq1 + i;
      for (k = 0; k <= i; k++) {
        *pu += *ps-- * *pp++;
      }
      pu++;
    }
  }

  /* filtering: simple convolution=inner products */

  for (i = filterlength; i < dim1; i++) {





    for (j = 0; j < ENH_UPS0; j++) {
      *pu = 0.0;
      pp = polyp[j];
      ps = seq1 + i;
      for (k = 0; k < filterlength; k++) {
        *pu += *ps-- * *pp++;
      }
      pu++;
    }
  }

  /* filtering: filter overhangs right side of sequence */

  for (q = 1; q <= hfl; q++) {
    for (j = 0; j < ENH_UPS0; j++) {
      *pu = 0.0;
      pp = polyp[j] + q;
      ps = seq1 + dim1 - 1;
      for (k = 0; k < filterlength - q; k++) {
        *pu += *ps-- * *pp++;
      }
      pu++;
    }
  }
}


   /*----------------------------------------------------------------*
    * find segment starting near idata+estSegPos that has highest
    * correlation with idata+centerStartPos through
    * idata+centerStartPos+ENH_BLOCKL-1 segment is found at a
    * resolution of ENH_UPSO times the original of the original
    * sampling rate
    *---------------------------------------------------------------*/

void refiner(float *seg,        /* (o) segment array */
  float *updStartPos,           /* (o) updated start point */
  float *idata,                 /* (i) original data buffer */
  int idatal,                   /* (i) dimension of idata */
  int centerStartPos,           /* (i) beginning center segment */
  float estSegPos,              /* (i) estimated beginning other segment */
  float period                  /* (i) estimated pitch period */
  )
{
  int estSegPosRounded, searchSegStartPos, searchSegEndPos, corrdim;
  int tloc, tloc2, i, st, en, fraction;
  float vect[ENH_VECTL], corrVec[ENH_CORRDIM], maxv;
  float corrVecUps[ENH_CORRDIM * ENH_UPS0];





  /* defining array bounds */

  estSegPosRounded = (int) (estSegPos - 0.5);

  searchSegStartPos = estSegPosRounded - ENH_SLOP;

  if (searchSegStartPos < 0) {
    searchSegStartPos = 0;
  }
  searchSegEndPos = estSegPosRounded + ENH_SLOP;

  if (searchSegEndPos + ENH_BLOCKL >= idatal) {
    searchSegEndPos = idatal - ENH_BLOCKL - 1;
  }
  corrdim = searchSegEndPos - searchSegStartPos + 1;

  /* compute upsampled correlation (corr33) and find
     location of max */

  mycorr1(corrVec, idata + searchSegStartPos,
    corrdim + ENH_BLOCKL - 1, idata + centerStartPos, ENH_BLOCKL);
  enh_upsample(corrVecUps, corrVec, corrdim, ENH_FL0);
  tloc = 0;
  maxv = corrVecUps[0];
  for (i = 1; i < ENH_UPS0 * corrdim; i++) {

    if (corrVecUps[i] > maxv) {
      tloc = i;
      maxv = corrVecUps[i];
    }
  }

  /* make vector can be upsampled without ever running outside
     bounds */

  *updStartPos = (float) searchSegStartPos +
    (float) tloc / (float) ENH_UPS0 + (float) 1.0;
  tloc2 = (int) (tloc / ENH_UPS0);

  if (tloc > tloc2 * ENH_UPS0) {
    tloc2++;
  }
  st = searchSegStartPos + tloc2 - ENH_FL0;

  if (st < 0) {
    memset(vect, 0, -st * sizeof(float));
    memcpy(&vect[-st], idata, (ENH_VECTL + st) * sizeof(float));
  } else {





    en = st + ENH_VECTL;

    if (en > idatal) {
      memcpy(vect, &idata[st],
        (ENH_VECTL - (en - idatal)) * sizeof(float));
      memset(&vect[ENH_VECTL - (en - idatal)], 0,
        (en - idatal) * sizeof(float));
    } else {
      memcpy(vect, &idata[st], ENH_VECTL * sizeof(float));
    }
  }
  fraction = tloc2 * ENH_UPS0 - tloc;

  /* compute the segment (this is actually a convolution) */

  mycorr1(seg, vect, ENH_VECTL,
    polyphaserTbl + (2 * ENH_FL0 + 1) * fraction, 2 * ENH_FL0 + 1);
}

   /*----------------------------------------------------------------*
    * find the smoothed output data
    *---------------------------------------------------------------*/

void smath(float *odata,        /* (o) smoothed output */
  float *sseq,                  /* (i) said second sequence of waveforms */
  int hl,                       /* (i) 2*hl+1 is sseq dimension */
  float alpha0                  /* (i) max smoothing energy fraction */
  )
{
  int i, k;
  float w00, w10, w11, A, B, C, *psseq, err, errs;
  float surround[BLOCKL_MAX];   /* shape contributed by other than
                                   current */
  float wt[2 * ENH_HL + 1];     /* waveform weighting to get
                                   surround shape */
  float denom;

  /* create shape of contribution from all waveforms except the
     current one */

  for (i = 1; i <= 2 * hl + 1; i++) {
    wt[i - 1] =
      (float) 0.5 *(1 - (float) cos(2 * PI * i / (2 * hl + 2)));
  }
  wt[hl] = 0.0;                 /* for clarity, not used */
  for (i = 0; i < ENH_BLOCKL; i++) {
    surround[i] = sseq[i] * wt[0];
  }





  for (k = 1; k < hl; k++) {
    psseq = sseq + k * ENH_BLOCKL;
    for (i = 0; i < ENH_BLOCKL; i++) {
      surround[i] += psseq[i] * wt[k];
    }
  }
  for (k = hl + 1; k <= 2 * hl; k++) {
    psseq = sseq + k * ENH_BLOCKL;
    for (i = 0; i < ENH_BLOCKL; i++) {
      surround[i] += psseq[i] * wt[k];
    }
  }

  /* compute some inner products */

  w00 = w10 = w11 = 0.0;
  psseq = sseq + hl * ENH_BLOCKL;       /* current block  */
  for (i = 0; i < ENH_BLOCKL; i++) {
    w00 += psseq[i] * psseq[i];
    w11 += surround[i] * surround[i];
    w10 += surround[i] * psseq[i];
  }

  if (fabs(w11) < 1.0) {
    w11 = 1.0;
  }
  C = (float) sqrt(w00 / w11);

  /* first try enhancement without power-constraint */

  errs = 0.0;
  psseq = sseq + hl * ENH_BLOCKL;
  for (i = 0; i < ENH_BLOCKL; i++) {
    odata[i] = C * surround[i];
    err = psseq[i] - odata[i];
    errs += err * err;
  }

  /* if constraint violated by first try, add constraint */

  if (errs > alpha0 * w00) {
    if (w00 < 1) {
      w00 = 1;
    }
    denom = (w11 * w00 - w10 * w10) / (w00 * w00);

    if (denom > 0.0001) {       /* eliminates numerical problems
                                   for if smooth */





      A = (float) sqrt((alpha0 - alpha0 * alpha0 / 4) / denom);
      B = -alpha0 / 2 - A * w10 / w00;
      B = B + 1;
    } else {                    /* essentially no difference between cycles;
                                   smoothing not needed */
      A = 0.0;
      B = 1.0;
    }

    /* create smoothed sequence */

    psseq = sseq + hl * ENH_BLOCKL;
    for (i = 0; i < ENH_BLOCKL; i++) {
      odata[i] = A * surround[i] + B * psseq[i];
    }
  }
}

   /*----------------------------------------------------------------*
    * get the pitch-synchronous sample sequence
    *---------------------------------------------------------------*/

void getsseq(float *sseq,       /* (o) the pitch-synchronous sequence */
  float *idata,                 /* (i) original data */
  int idatal,                   /* (i) dimension of data */
  int centerStartPos,           /* (i) where current block starts */
  float *period,                /* (i) rough-pitch-period array */
  float *plocs,                 /* (i) where periods of period array
                                   are taken */
  int periodl,                  /* (i) dimension period array */
  int hl                        /* (i) 2*hl+1 is the number of sequences */
  )
{
  int i, centerEndPos, q;
  float blockStartPos[2 * ENH_HL + 1];
  int lagBlock[2 * ENH_HL + 1];
  float plocs2[ENH_PLOCSL];
  float *psseq;

  centerEndPos = centerStartPos + ENH_BLOCKL - 1;

  /* present */

  NearestNeighbor(lagBlock + hl, plocs,
    (float) 0.5 * (centerStartPos + centerEndPos), periodl);

  blockStartPos[hl] = (float) centerStartPos;





  psseq = sseq + ENH_BLOCKL * hl;
  memcpy(psseq, idata + centerStartPos, ENH_BLOCKL * sizeof(float));

  /* past */

  for (q = hl - 1; q >= 0; q--) {
    blockStartPos[q] = blockStartPos[q + 1] - period[lagBlock[q + 1]];
    NearestNeighbor(lagBlock + q, plocs,
      blockStartPos[q] +
      ENH_BLOCKL_HALF - period[lagBlock[q + 1]], periodl);


    if (blockStartPos[q] - ENH_OVERHANG >= 0) {
      refiner(sseq + q * ENH_BLOCKL, blockStartPos + q, idata,
        idatal, centerStartPos, blockStartPos[q],
        period[lagBlock[q + 1]]);
    } else {
      psseq = sseq + q * ENH_BLOCKL;
      memset(psseq, 0, ENH_BLOCKL * sizeof(float));
    }
  }

  /* future */

  for (i = 0; i < periodl; i++) {
    plocs2[i] = plocs[i] - period[i];
  }
  for (q = hl + 1; q <= 2 * hl; q++) {
    NearestNeighbor(lagBlock + q, plocs2,
      blockStartPos[q - 1] + ENH_BLOCKL_HALF, periodl);

    blockStartPos[q] = blockStartPos[q - 1] + period[lagBlock[q]];
    if (blockStartPos[q] + ENH_BLOCKL + ENH_OVERHANG < idatal) {
      refiner(sseq + ENH_BLOCKL * q, blockStartPos + q, idata,
        idatal, centerStartPos, blockStartPos[q], period[lagBlock[q]]);
    } else {
      psseq = sseq + q * ENH_BLOCKL;
      memset(psseq, 0, ENH_BLOCKL * sizeof(float));
    }
  }
}

   /*----------------------------------------------------------------*
    * perform enhancement on idata+centerStartPos through
    * idata+centerStartPos+ENH_BLOCKL-1
    *---------------------------------------------------------------*/





void enhancer(float *odata,     /* (o) smoothed block, dimension blockl */
  float *idata,                 /* (i) data buffer used for enhancing */
  int idatal,                   /* (i) dimension idata */
  int centerStartPos,           /* (i) first sample current block
                                   within idata */
  float alpha0,                 /* (i) max correction-energy-fraction
                                   (in [0,1]) */
  float *period,                /* (i) pitch period array */
  float *plocs,                 /* (i) locations where period array
                                   values valid */
  int periodl                   /* (i) dimension of period and plocs */
  )
{
  float sseq[(2 * ENH_HL + 1) * ENH_BLOCKL];

  /* get said second sequence of segments */

  getsseq(sseq, idata, idatal, centerStartPos, period,
    plocs, periodl, ENH_HL);

  /* compute the smoothed output from said second sequence */

  smath(odata, sseq, ENH_HL, alpha0);

}

   /*----------------------------------------------------------------*
    * cross correlation
    *---------------------------------------------------------------*/

float xCorrCoef(float *target,  /* (i) first array */
  float *regressor,             /* (i) second array */
  int subl                      /* (i) dimension arrays */
  )
{
  int i;
  float ftmp1, ftmp2;

  ftmp1 = 0.0;
  ftmp2 = 0.0;
  for (i = 0; i < subl; i++) {
    ftmp1 += target[i] * regressor[i];
    ftmp2 += regressor[i] * regressor[i];
  }

  if (ftmp1 > 0.0) {
    return (float) (ftmp1 * ftmp1 / ftmp2);
  }





  else {
    return (float) 0.0;
  }
}

   /*----------------------------------------------------------------*
    * interface for enhancer
    *---------------------------------------------------------------*/

int enhancerInterface(float *out,       /* (o) enhanced signal */
  float *in,                    /* (i) unenhanced signal */
  iLBC_Dec_Inst_t * iLBCdec_inst        /* (i) buffers etc */
  )
{
  float *enh_buf, *enh_period;
  int iblock, isample;
  int lag = 0, ilag, i, ioffset;
  float cc, maxcc;
  float ftmp1, ftmp2;
  float *inPtr, *enh_bufPtr1, *enh_bufPtr2;
  float plc_pred[ENH_BLOCKL];

  float lpState[6], downsampled[(ENH_NBLOCKS * ENH_BLOCKL + 120) / 2];
  int inLen = ENH_NBLOCKS * ENH_BLOCKL + 120;
  int start, plc_blockl, inlag;

  enh_buf = iLBCdec_inst->enh_buf;
  enh_period = iLBCdec_inst->enh_period;

  memmove(enh_buf, &enh_buf[iLBCdec_inst->blockl],
    (ENH_BUFL - iLBCdec_inst->blockl) * sizeof(float));

  memcpy(&enh_buf[ENH_BUFL - iLBCdec_inst->blockl], in,
    iLBCdec_inst->blockl * sizeof(float));

  if (iLBCdec_inst->mode == 30)
    plc_blockl = ENH_BLOCKL;
  else
    plc_blockl = 40;

  /* when 20 ms frame, move processing one block */
  ioffset = 0;
  if (iLBCdec_inst->mode == 20)
    ioffset = 1;

  i = 3 - ioffset;
  memmove(enh_period, &enh_period[i],
    (ENH_NBLOCKS_TOT - i) * sizeof(float));






  /* Set state information to the 6 samples right before
     the samples to be downsampled. */

  memcpy(lpState,
    enh_buf + (ENH_NBLOCKS_EXTRA + ioffset) * ENH_BLOCKL - 126,
    6 * sizeof(float));

  /* Down sample a factor 2 to save computations */

  DownSample(enh_buf + (ENH_NBLOCKS_EXTRA + ioffset) * ENH_BLOCKL - 120,
    lpFilt_coefsTbl, inLen - ioffset * ENH_BLOCKL,
    lpState, downsampled);

  /* Estimate the pitch in the down sampled domain. */
  for (iblock = 0; iblock < ENH_NBLOCKS - ioffset; iblock++) {

    lag = 10;
    maxcc = xCorrCoef(downsampled + 60 + iblock *
      ENH_BLOCKL_HALF, downsampled + 60 + iblock *
      ENH_BLOCKL_HALF - lag, ENH_BLOCKL_HALF);
    for (ilag = 11; ilag < 60; ilag++) {
      cc = xCorrCoef(downsampled + 60 + iblock *
        ENH_BLOCKL_HALF, downsampled + 60 + iblock *
        ENH_BLOCKL_HALF - ilag, ENH_BLOCKL_HALF);

      if (cc > maxcc) {
        maxcc = cc;
        lag = ilag;
      }
    }

    /* Store the estimated lag in the non-downsampled domain */
    enh_period[iblock + ENH_NBLOCKS_EXTRA + ioffset] = (float) lag *2;


  }


  /* PLC was performed on the previous packet */
  if (iLBCdec_inst->prev_enh_pl == 1) {

    inlag = (int) enh_period[ENH_NBLOCKS_EXTRA + ioffset];

    lag = inlag - 1;
    maxcc = xCorrCoef(in, in + lag, plc_blockl);
    for (ilag = inlag; ilag <= inlag + 1; ilag++) {
      cc = xCorrCoef(in, in + ilag, plc_blockl);






      if (cc > maxcc) {
        maxcc = cc;
        lag = ilag;
      }
    }

    enh_period[ENH_NBLOCKS_EXTRA + ioffset - 1] = (float) lag;

    /* compute new concealed residual for the old lookahead,
       mix the forward PLC with a backward PLC from
       the new frame */

    inPtr = &in[lag - 1];

    enh_bufPtr1 = &plc_pred[plc_blockl - 1];

    if (lag > plc_blockl) {
      start = plc_blockl;
    } else {
      start = lag;
    }

    for (isample = start; isample > 0; isample--) {
      *enh_bufPtr1-- = *inPtr--;
    }

    enh_bufPtr2 = &enh_buf[ENH_BUFL - 1 - iLBCdec_inst->blockl];
    for (isample = (plc_blockl - 1 - lag); isample >= 0; isample--) {
      *enh_bufPtr1-- = *enh_bufPtr2--;
    }

    /* limit energy change */
    ftmp2 = 0.0;
    ftmp1 = 0.0;
    for (i = 0; i < plc_blockl; i++) {
      ftmp2 += enh_buf[ENH_BUFL - 1 - iLBCdec_inst->blockl - i] *
        enh_buf[ENH_BUFL - 1 - iLBCdec_inst->blockl - i];
      ftmp1 += plc_pred[i] * plc_pred[i];
    }
    ftmp1 = (float) sqrt(ftmp1 / (float) plc_blockl);
    ftmp2 = (float) sqrt(ftmp2 / (float) plc_blockl);
    if (ftmp1 > (float) 2.0 * ftmp2 && ftmp1 > 0.0) {
      for (i = 0; i < plc_blockl - 10; i++) {
        plc_pred[i] *= (float) 2.0 *ftmp2 / ftmp1;
      }
      for (i = plc_blockl - 10; i < plc_blockl; i++) {
        plc_pred[i] *= (float) (i - plc_blockl + 10) *
          ((float) 1.0 - (float) 2.0 * ftmp2 / ftmp1) / (float) (10) +
          (float) 2.0 *ftmp2 / ftmp1;
      }
    }

    enh_bufPtr1 = &enh_buf[ENH_BUFL - 1 - iLBCdec_inst->blockl];
    for (i = 0; i < plc_blockl; i++) {
      ftmp1 = (float) (i + 1) / (float) (plc_blockl + 1);
      *enh_bufPtr1 *= ftmp1;
      *enh_bufPtr1 += ((float) 1.0 - ftmp1) *
        plc_pred[plc_blockl - 1 - i];
      enh_bufPtr1--;
    }
  }

  if (iLBCdec_inst->mode == 20) {
    /* Enhancer with 40 samples delay */
    for (iblock = 0; iblock < 2; iblock++) {
      enhancer(out + iblock * ENH_BLOCKL, enh_buf,
        ENH_BUFL, (5 + iblock) * ENH_BLOCKL + 40,
        ENH_ALPHA0, enh_period, enh_plocsTbl, ENH_NBLOCKS_TOT);
    }
  } else if (iLBCdec_inst->mode == 30) {
    /* Enhancer with 80 samples delay */
    for (iblock = 0; iblock < 3; iblock++) {
      enhancer(out + iblock * ENH_BLOCKL, enh_buf,
        ENH_BUFL, (4 + iblock) * ENH_BLOCKL,
        ENH_ALPHA0, enh_period, enh_plocsTbl, ENH_NBLOCKS_TOT);
    }
  }

  return (lag * 2);
}