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

       iLBC Speech Coder ANSI-C Source Code

       helpfun.c

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

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

#include <math.h>

#include "iLBC_define.h"
#include "constants.h"

   /*----------------------------------------------------------------*
    *  calculation of auto correlation
    *---------------------------------------------------------------*/

void autocorr(float *r,         /* (o) autocorrelation vector */
  const float *x,               /* (i) data vector */
  int N,                        /* (i) length of data vector */
  int order                     /* largest lag for calculated
                                   autocorrelations */
  )
{
  int lag, n;
  float sum;

  for (lag = 0; lag <= order; lag++) {
    sum = 0;
    for (n = 0; n < N - lag; n++) {
      sum += x[n] * x[n + lag];
    }
    r[lag] = sum;
  }





}

   /*----------------------------------------------------------------*
    *  window multiplication
    *---------------------------------------------------------------*/

void window(float *z,           /* (o) the windowed data */
  const float *x,               /* (i) the original data vector */
  const float *y,               /* (i) the window */
  int N                         /* (i) length of all vectors */
  )
{
  int i;

  for (i = 0; i < N; i++) {
    z[i] = x[i] * y[i];
  }
}

   /*----------------------------------------------------------------*
    *  levinson-durbin solution for lpc coefficients
    *---------------------------------------------------------------*/

void levdurb(float *a,          /* (o) lpc coefficient vector starting
                                   with 1.0 */
  float *k,                     /* (o) reflection coefficients */
  float *r,                     /* (i) autocorrelation vector */
  int order                     /* (i) order of lpc filter */
  )
{
  float sum, alpha;
  int m, m_h, i;

  a[0] = 1.0;

  if (r[0] < EPS) {             /* if r[0] <= 0, set LPC coeff. to zero */
    for (i = 0; i < order; i++) {
      k[i] = 0;
      a[i + 1] = 0;
    }
  } else {
    a[1] = k[0] = -r[1] / r[0];
    alpha = r[0] + r[1] * k[0];
    for (m = 1; m < order; m++) {
      sum = r[m + 1];
      for (i = 0; i < m; i++) {
        sum += a[i + 1] * r[m - i];
      }





      k[m] = -sum / alpha;
      alpha += k[m] * sum;
      m_h = (m + 1) >> 1;
      for (i = 0; i < m_h; i++) {
        sum = a[i + 1] + k[m] * a[m - i];
        a[m - i] += k[m] * a[i + 1];
        a[i + 1] = sum;
      }
      a[m + 1] = k[m];
    }
  }
}

   /*----------------------------------------------------------------*
    *  interpolation between vectors
    *---------------------------------------------------------------*/

void interpolate(float *out,    /* (o) the interpolated vector */
  float *in1,                   /* (i) the first vector for the
                                   interpolation */
  float *in2,                   /* (i) the second vector for the
                                   interpolation */
  float coef,                   /* (i) interpolation weights */
  int length                    /* (i) length of all vectors */
  )
{
  int i;
  float invcoef;

  invcoef = (float) 1.0 - coef;
  for (i = 0; i < length; i++) {
    out[i] = coef * in1[i] + invcoef * in2[i];
  }
}

   /*----------------------------------------------------------------*
    *  lpc bandwidth expansion
    *---------------------------------------------------------------*/

void bwexpand(float *out,       /* (o) the bandwidth expanded lpc
                                   coefficients */
  float *in,                    /* (i) the lpc coefficients before bandwidth
                                   expansion */
  float coef,                   /* (i) the bandwidth expansion factor */
  int length                    /* (i) the length of lpc coefficient vectors */
  )
{
  int i;





  float chirp;

  chirp = coef;

  out[0] = in[0];
  for (i = 1; i < length; i++) {
    out[i] = chirp * in[i];
    chirp *= coef;
  }
}

   /*----------------------------------------------------------------*
    *  vector quantization
    *---------------------------------------------------------------*/

void vq(float *Xq,              /* (o) the quantized vector */
  int *index,                   /* (o) the quantization index */
  const float *CB,              /* (i) the vector quantization codebook */
  float *X,                     /* (i) the vector to quantize */
  int n_cb,                     /* (i) the number of vectors in the codebook */
  int dim                       /* (i) the dimension of all vectors */
  )
{
  int i, j;
  int pos, minindex;
  float dist, tmp, mindist;

  pos = 0;
  mindist = FLOAT_MAX;
  minindex = 0;
  for (j = 0; j < n_cb; j++) {
    dist = X[0] - CB[pos];
    dist *= dist;
    for (i = 1; i < dim; i++) {
      tmp = X[i] - CB[pos + i];
      dist += tmp * tmp;
    }

    if (dist < mindist) {
      mindist = dist;
      minindex = j;
    }
    pos += dim;
  }
  for (i = 0; i < dim; i++) {
    Xq[i] = CB[minindex * dim + i];
  }
  *index = minindex;





}

   /*----------------------------------------------------------------*
    *  split vector quantization
    *---------------------------------------------------------------*/

void SplitVQ(float *qX,         /* (o) the quantized vector */
  int *index,                   /* (o) a vector of indexes for all vector
                                   codebooks in the split */
  float *X,                     /* (i) the vector to quantize */
  const float *CB,              /* (i) the quantizer codebook */
  int nsplit,                   /* the number of vector splits */
  const int *dim,               /* the dimension of X and qX */
  const int *cbsize             /* the number of vectors in the codebook */
  )
{
  int cb_pos, X_pos, i;

  cb_pos = 0;
  X_pos = 0;
  for (i = 0; i < nsplit; i++) {
    vq(qX + X_pos, index + i, CB + cb_pos, X + X_pos,
      cbsize[i], dim[i]);
    X_pos += dim[i];
    cb_pos += dim[i] * cbsize[i];
  }
}

   /*----------------------------------------------------------------*
    *  scalar quantization
    *---------------------------------------------------------------*/

void sort_sq(float *xq,         /* (o) the quantized value */
  int *index,                   /* (o) the quantization index */
  float x,                      /* (i) the value to quantize */
  const float *cb,              /* (i) the quantization codebook */
  int cb_size                   /* (i) the size of the quantization codebook */
  )
{
  int i;

  if (x <= cb[0]) {
    *index = 0;
    *xq = cb[0];
  } else {
    i = 0;
    while ((x > cb[i]) && i < cb_size - 1) {
      i++;





    }

    if (x > ((cb[i] + cb[i - 1]) / 2)) {
      *index = i;
      *xq = cb[i];
    } else {
      *index = i - 1;
      *xq = cb[i - 1];
    }
  }
}

   /*----------------------------------------------------------------*
    *  check for stability of lsf coefficients
    *---------------------------------------------------------------*/

int LSF_check(                  /* (o) 1 for stable lsf vectors and 0 for
                                   nonstable ones */
  float *lsf,                   /* (i) a table of lsf vectors */
  int dim,                      /* (i) the dimension of each lsf vector */
  int NoAn                      /* (i) the number of lsf vectors in the
                                   table */
  )
{
  int k, n, m, Nit = 2, change = 0, pos;
  float tmp;
  static float eps = (float) 0.039;     /* 50 Hz */
  static float eps2 = (float) 0.0195;
  static float maxlsf = (float) 3.14;   /* 4000 Hz */
  static float minlsf = (float) 0.01;   /* 0 Hz */

  /* LSF separation check */

  for (n = 0; n < Nit; n++) {   /* Run through a couple of times */
    for (m = 0; m < NoAn; m++) {        /* Number of analyses per frame */
      for (k = 0; k < (dim - 1); k++) {
        pos = m * dim + k;

        if ((lsf[pos + 1] - lsf[pos]) < eps) {

          if (lsf[pos + 1] < lsf[pos]) {
            tmp = lsf[pos + 1];
            lsf[pos + 1] = lsf[pos] + eps2;
            lsf[pos] = lsf[pos + 1] - eps2;
          } else {
            lsf[pos] -= eps2;
            lsf[pos + 1] += eps2;
          }
          change = 1;





        }

        if (lsf[pos] < minlsf) {
          lsf[pos] = minlsf;
          change = 1;
        }

        if (lsf[pos] > maxlsf) {
          lsf[pos] = maxlsf;
          change = 1;
        }
      }
    }
  }

  return change;
}