/*
 *  Copyright (c) 2011 The WebRTC project authors. All Rights Reserved.
 *
 *  Use of this source code is governed by a BSD-style license
 *  that can be found in the LICENSE file in the root of the source
 *  tree. An additional intellectual property rights grant can be found
 *  in the file PATENTS.  All contributing project authors may
 *  be found in the AUTHORS file in the root of the source tree.
 */

#include "pitch_estimator.h"

#include <math.h>
#include <memory.h>
#include <string.h>
#ifdef WEBRTC_ANDROID
#include <stdlib.h>
#endif

static const double kInterpolWin[8] = {-0.00067556028640,  0.02184247643159, -0.12203175715679,  0.60086484101160,
                                       0.60086484101160, -0.12203175715679,  0.02184247643159, -0.00067556028640};

/* interpolation filter */
__inline static void IntrepolFilter(double *data_ptr, double *intrp)
{
  *intrp = kInterpolWin[0] * data_ptr[-3];
  *intrp += kInterpolWin[1] * data_ptr[-2];
  *intrp += kInterpolWin[2] * data_ptr[-1];
  *intrp += kInterpolWin[3] * data_ptr[0];
  *intrp += kInterpolWin[4] * data_ptr[1];
  *intrp += kInterpolWin[5] * data_ptr[2];
  *intrp += kInterpolWin[6] * data_ptr[3];
  *intrp += kInterpolWin[7] * data_ptr[4];
}


/* 2D parabolic interpolation */
/* probably some 0.5 factors can be eliminated, and the square-roots can be removed from the Cholesky fact. */
__inline static void Intrpol2D(double T[3][3], double *x, double *y, double *peak_val)
{
  double c, b[2], A[2][2];
  double t1, t2, d;
  double delta1, delta2;


  // double T[3][3] = {{-1.25, -.25,-.25}, {-.25, .75, .75}, {-.25, .75, .75}};
  // should result in: delta1 = 0.5;  delta2 = 0.0;  peak_val = 1.0

  c = T[1][1];
  b[0] = 0.5 * (T[1][2] + T[2][1] - T[0][1] - T[1][0]);
  b[1] = 0.5 * (T[1][0] + T[2][1] - T[0][1] - T[1][2]);
  A[0][1] = -0.5 * (T[0][1] + T[2][1] - T[1][0] - T[1][2]);
  t1 = 0.5 * (T[0][0] + T[2][2]) - c;
  t2 = 0.5 * (T[2][0] + T[0][2]) - c;
  d = (T[0][1] + T[1][2] + T[1][0] + T[2][1]) - 4.0 * c - t1 - t2;
  A[0][0] = -t1 - 0.5 * d;
  A[1][1] = -t2 - 0.5 * d;

  /* deal with singularities or ill-conditioned cases */
  if ( (A[0][0] < 1e-7) || ((A[0][0] * A[1][1] - A[0][1] * A[0][1]) < 1e-7) ) {
    *peak_val = T[1][1];
    return;
  }

  /* Cholesky decomposition: replace A by upper-triangular factor */
  A[0][0] = sqrt(A[0][0]);
  A[0][1] = A[0][1] / A[0][0];
  A[1][1] = sqrt(A[1][1] - A[0][1] * A[0][1]);

  /* compute [x; y] = -0.5 * inv(A) * b */
  t1 = b[0] / A[0][0];
  t2 = (b[1] - t1 * A[0][1]) / A[1][1];
  delta2 = t2 / A[1][1];
  delta1 = 0.5 * (t1 - delta2 * A[0][1]) / A[0][0];
  delta2 *= 0.5;

  /* limit norm */
  t1 = delta1 * delta1 + delta2 * delta2;
  if (t1 > 1.0) {
    delta1 /= t1;
    delta2 /= t1;
  }

  *peak_val = 0.5 * (b[0] * delta1 + b[1] * delta2) + c;

  *x += delta1;
  *y += delta2;
}


static void PCorr(const double *in, double *outcorr)
{
  double sum, ysum, prod;
  const double *x, *inptr;
  int k, n;

  //ysum = 1e-6;          /* use this with float (i.s.o. double)! */
  ysum = 1e-13;
  sum = 0.0;
  x = in + PITCH_MAX_LAG/2 + 2;
  for (n = 0; n < PITCH_CORR_LEN2; n++) {
    ysum += in[n] * in[n];
    sum += x[n] * in[n];
  }

  outcorr += PITCH_LAG_SPAN2 - 1;     /* index of last element in array */
  *outcorr = sum / sqrt(ysum);

  for (k = 1; k < PITCH_LAG_SPAN2; k++) {
    ysum -= in[k-1] * in[k-1];
    ysum += in[PITCH_CORR_LEN2 + k - 1] * in[PITCH_CORR_LEN2 + k - 1];
    sum = 0.0;
    inptr = &in[k];
    prod = x[0] * inptr[0];
    for (n = 1; n < PITCH_CORR_LEN2; n++) {
      sum += prod;
      prod = x[n] * inptr[n];
    }
    sum += prod;
    outcorr--;
    *outcorr = sum / sqrt(ysum);
  }
}


void WebRtcIsac_InitializePitch(const double *in,
                                const double old_lag,
                                const double old_gain,
                                PitchAnalysisStruct *State,
                                double *lags)
{
  double buf_dec[PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2+2];
  double ratio, log_lag, gain_bias;
  double bias;
  double corrvec1[PITCH_LAG_SPAN2];
  double corrvec2[PITCH_LAG_SPAN2];
  int m, k;
  // Allocating 10 extra entries at the begining of the CorrSurf
  double corrSurfBuff[10 + (2*PITCH_BW+3)*(PITCH_LAG_SPAN2+4)];
  double* CorrSurf[2*PITCH_BW+3];
  double *CorrSurfPtr1, *CorrSurfPtr2;
  double LagWin[3] = {0.2, 0.5, 0.98};
  int ind1, ind2, peaks_ind, peak, max_ind;
  int peaks[PITCH_MAX_NUM_PEAKS];
  double adj, gain_tmp;
  double corr, corr_max;
  double intrp_a, intrp_b, intrp_c, intrp_d;
  double peak_vals[PITCH_MAX_NUM_PEAKS];
  double lags1[PITCH_MAX_NUM_PEAKS];
  double lags2[PITCH_MAX_NUM_PEAKS];
  double T[3][3];
  int row;

  for(k = 0; k < 2*PITCH_BW+3; k++)
  {
    CorrSurf[k] = &corrSurfBuff[10 + k * (PITCH_LAG_SPAN2+4)];
  }
  /* reset CorrSurf matrix */
  memset(corrSurfBuff, 0, sizeof(double) * (10 + (2*PITCH_BW+3) * (PITCH_LAG_SPAN2+4)));

  //warnings -DH
  max_ind = 0;
  peak = 0;

  /* copy old values from state buffer */
  memcpy(buf_dec, State->dec_buffer, sizeof(double) * (PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2));

  /* decimation; put result after the old values */
  WebRtcIsac_DecimateAllpass(in, State->decimator_state, PITCH_FRAME_LEN,
                             &buf_dec[PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2]);

  /* low-pass filtering */
  for (k = PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2; k < PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2+2; k++)
    buf_dec[k] += 0.75 * buf_dec[k-1] - 0.25 * buf_dec[k-2];

  /* copy end part back into state buffer */
  memcpy(State->dec_buffer, buf_dec+PITCH_FRAME_LEN/2, sizeof(double) * (PITCH_CORR_LEN2+PITCH_CORR_STEP2+PITCH_MAX_LAG/2-PITCH_FRAME_LEN/2+2));

  /* compute correlation for first and second half of the frame */
  PCorr(buf_dec, corrvec1);
  PCorr(buf_dec + PITCH_CORR_STEP2, corrvec2);

  /* bias towards pitch lag of previous frame */
  log_lag = log(0.5 * old_lag);
  gain_bias = 4.0 * old_gain * old_gain;
  if (gain_bias > 0.8) gain_bias = 0.8;
  for (k = 0; k < PITCH_LAG_SPAN2; k++)
  {
    ratio = log((double) (k + (PITCH_MIN_LAG/2-2))) - log_lag;
    bias = 1.0 + gain_bias * exp(-5.0 * ratio * ratio);
    corrvec1[k] *= bias;
  }

  /* taper correlation functions */
  for (k = 0; k < 3; k++) {
    gain_tmp = LagWin[k];
    corrvec1[k] *= gain_tmp;
    corrvec2[k] *= gain_tmp;
    corrvec1[PITCH_LAG_SPAN2-1-k] *= gain_tmp;
    corrvec2[PITCH_LAG_SPAN2-1-k] *= gain_tmp;
  }

  corr_max = 0.0;
  /* fill middle row of correlation surface */
  ind1 = 0;
  ind2 = 0;
  CorrSurfPtr1 = &CorrSurf[PITCH_BW][2];
  for (k = 0; k < PITCH_LAG_SPAN2; k++) {
    corr = corrvec1[ind1++] + corrvec2[ind2++];
    CorrSurfPtr1[k] = corr;
    if (corr > corr_max) {
      corr_max = corr;  /* update maximum */
      max_ind = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]);
    }
  }
  /* fill first and last rows of correlation surface */
  ind1 = 0;
  ind2 = PITCH_BW;
  CorrSurfPtr1 = &CorrSurf[0][2];
  CorrSurfPtr2 = &CorrSurf[2*PITCH_BW][PITCH_BW+2];
  for (k = 0; k < PITCH_LAG_SPAN2-PITCH_BW; k++) {
    ratio = ((double) (ind1 + 12)) / ((double) (ind2 + 12));
    adj = 0.2 * ratio * (2.0 - ratio);   /* adjustment factor; inverse parabola as a function of ratio */
    corr = adj * (corrvec1[ind1] + corrvec2[ind2]);
    CorrSurfPtr1[k] = corr;
    if (corr > corr_max) {
      corr_max = corr;  /* update maximum */
      max_ind = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]);
    }
    corr = adj * (corrvec1[ind2++] + corrvec2[ind1++]);
    CorrSurfPtr2[k] = corr;
    if (corr > corr_max) {
      corr_max = corr;  /* update maximum */
      max_ind = (int)(&CorrSurfPtr2[k] - &CorrSurf[0][0]);
    }
  }
  /* fill second and next to last rows of correlation surface */
  ind1 = 0;
  ind2 = PITCH_BW-1;
  CorrSurfPtr1 = &CorrSurf[1][2];
  CorrSurfPtr2 = &CorrSurf[2*PITCH_BW-1][PITCH_BW+1];
  for (k = 0; k < PITCH_LAG_SPAN2-PITCH_BW+1; k++) {
    ratio = ((double) (ind1 + 12)) / ((double) (ind2 + 12));
    adj = 0.9 * ratio * (2.0 - ratio);   /* adjustment factor; inverse parabola as a function of ratio */
    corr = adj * (corrvec1[ind1] + corrvec2[ind2]);
    CorrSurfPtr1[k] = corr;
    if (corr > corr_max) {
      corr_max = corr;  /* update maximum */
      max_ind = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]);
    }
    corr = adj * (corrvec1[ind2++] + corrvec2[ind1++]);
    CorrSurfPtr2[k] = corr;
    if (corr > corr_max) {
      corr_max = corr;  /* update maximum */
      max_ind = (int)(&CorrSurfPtr2[k] - &CorrSurf[0][0]);
    }
  }
  /* fill remainder of correlation surface */
  for (m = 2; m < PITCH_BW; m++) {
    ind1 = 0;
    ind2 = PITCH_BW - m;         /* always larger than ind1 */
    CorrSurfPtr1 = &CorrSurf[m][2];
    CorrSurfPtr2 = &CorrSurf[2*PITCH_BW-m][PITCH_BW+2-m];
    for (k = 0; k < PITCH_LAG_SPAN2-PITCH_BW+m; k++) {
      ratio = ((double) (ind1 + 12)) / ((double) (ind2 + 12));
      adj = ratio * (2.0 - ratio);    /* adjustment factor; inverse parabola as a function of ratio */
      corr = adj * (corrvec1[ind1] + corrvec2[ind2]);
      CorrSurfPtr1[k] = corr;
      if (corr > corr_max) {
        corr_max = corr;  /* update maximum */
        max_ind = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]);
      }
      corr = adj * (corrvec1[ind2++] + corrvec2[ind1++]);
      CorrSurfPtr2[k] = corr;
      if (corr > corr_max) {
        corr_max = corr;  /* update maximum */
        max_ind = (int)(&CorrSurfPtr2[k] - &CorrSurf[0][0]);
      }
    }
  }

  /* threshold value to qualify as a peak */
  corr_max *= 0.6;

  peaks_ind = 0;
  /* find peaks */
  for (m = 1; m < PITCH_BW+1; m++) {
    if (peaks_ind == PITCH_MAX_NUM_PEAKS) break;
    CorrSurfPtr1 = &CorrSurf[m][2];
    for (k = 2; k < PITCH_LAG_SPAN2-PITCH_BW-2+m; k++) {
      corr = CorrSurfPtr1[k];
      if (corr > corr_max) {
        if ( (corr > CorrSurfPtr1[k - (PITCH_LAG_SPAN2+5)]) && (corr > CorrSurfPtr1[k - (PITCH_LAG_SPAN2+4)]) ) {
          if ( (corr > CorrSurfPtr1[k + (PITCH_LAG_SPAN2+4)]) && (corr > CorrSurfPtr1[k + (PITCH_LAG_SPAN2+5)]) ) {
            /* found a peak; store index into matrix */
            peaks[peaks_ind++] = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]);
            if (peaks_ind == PITCH_MAX_NUM_PEAKS) break;
          }
        }
      }
    }
  }
  for (m = PITCH_BW+1; m < 2*PITCH_BW; m++) {
    if (peaks_ind == PITCH_MAX_NUM_PEAKS) break;
    CorrSurfPtr1 = &CorrSurf[m][2];
    for (k = 2+m-PITCH_BW; k < PITCH_LAG_SPAN2-2; k++) {
      corr = CorrSurfPtr1[k];
      if (corr > corr_max) {
        if ( (corr > CorrSurfPtr1[k - (PITCH_LAG_SPAN2+5)]) && (corr > CorrSurfPtr1[k - (PITCH_LAG_SPAN2+4)]) ) {
          if ( (corr > CorrSurfPtr1[k + (PITCH_LAG_SPAN2+4)]) && (corr > CorrSurfPtr1[k + (PITCH_LAG_SPAN2+5)]) ) {
            /* found a peak; store index into matrix */
            peaks[peaks_ind++] = (int)(&CorrSurfPtr1[k] - &CorrSurf[0][0]);
            if (peaks_ind == PITCH_MAX_NUM_PEAKS) break;
          }
        }
      }
    }
  }

  if (peaks_ind > 0) {
    /* examine each peak */
    CorrSurfPtr1 = &CorrSurf[0][0];
    for (k = 0; k < peaks_ind; k++) {
      peak = peaks[k];

      /* compute four interpolated values around current peak */
      IntrepolFilter(&CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+5)], &intrp_a);
      IntrepolFilter(&CorrSurfPtr1[peak - 1            ], &intrp_b);
      IntrepolFilter(&CorrSurfPtr1[peak                ], &intrp_c);
      IntrepolFilter(&CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+4)], &intrp_d);

      /* determine maximum of the interpolated values */
      corr = CorrSurfPtr1[peak];
      corr_max = intrp_a;
      if (intrp_b > corr_max) corr_max = intrp_b;
      if (intrp_c > corr_max) corr_max = intrp_c;
      if (intrp_d > corr_max) corr_max = intrp_d;

      /* determine where the peak sits and fill a 3x3 matrix around it */
      row = peak / (PITCH_LAG_SPAN2+4);
      lags1[k] = (double) ((peak - row * (PITCH_LAG_SPAN2+4)) + PITCH_MIN_LAG/2 - 4);
      lags2[k] = (double) (lags1[k] + PITCH_BW - row);
      if ( corr > corr_max ) {
        T[0][0] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+5)];
        T[2][0] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+4)];
        T[1][1] = corr;
        T[0][2] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+4)];
        T[2][2] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+5)];
        T[1][0] = intrp_a;
        T[0][1] = intrp_b;
        T[2][1] = intrp_c;
        T[1][2] = intrp_d;
      } else {
        if (intrp_a == corr_max) {
          lags1[k] -= 0.5;
          lags2[k] += 0.5;
          IntrepolFilter(&CorrSurfPtr1[peak - 2*(PITCH_LAG_SPAN2+5)], &T[0][0]);
          IntrepolFilter(&CorrSurfPtr1[peak - (2*PITCH_LAG_SPAN2+9)], &T[2][0]);
          T[1][1] = intrp_a;
          T[0][2] = intrp_b;
          T[2][2] = intrp_c;
          T[1][0] = CorrSurfPtr1[peak - (2*PITCH_LAG_SPAN2+9)];
          T[0][1] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+5)];
          T[2][1] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+4)];
          T[1][2] = corr;
        } else if (intrp_b == corr_max) {
          lags1[k] -= 0.5;
          lags2[k] -= 0.5;
          IntrepolFilter(&CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+6)], &T[0][0]);
          T[2][0] = intrp_a;
          T[1][1] = intrp_b;
          IntrepolFilter(&CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+3)], &T[0][2]);
          T[2][2] = intrp_d;
          T[1][0] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+5)];
          T[0][1] = CorrSurfPtr1[peak - 1];
          T[2][1] = corr;
          T[1][2] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+4)];
        } else if (intrp_c == corr_max) {
          lags1[k] += 0.5;
          lags2[k] += 0.5;
          T[0][0] = intrp_a;
          IntrepolFilter(&CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+4)], &T[2][0]);
          T[1][1] = intrp_c;
          T[0][2] = intrp_d;
          IntrepolFilter(&CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+5)], &T[2][2]);
          T[1][0] = CorrSurfPtr1[peak - (PITCH_LAG_SPAN2+4)];
          T[0][1] = corr;
          T[2][1] = CorrSurfPtr1[peak + 1];
          T[1][2] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+5)];
        } else {
          lags1[k] += 0.5;
          lags2[k] -= 0.5;
          T[0][0] = intrp_b;
          T[2][0] = intrp_c;
          T[1][1] = intrp_d;
          IntrepolFilter(&CorrSurfPtr1[peak + 2*(PITCH_LAG_SPAN2+4)], &T[0][2]);
          IntrepolFilter(&CorrSurfPtr1[peak + (2*PITCH_LAG_SPAN2+9)], &T[2][2]);
          T[1][0] = corr;
          T[0][1] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+4)];
          T[2][1] = CorrSurfPtr1[peak + (PITCH_LAG_SPAN2+5)];
          T[1][2] = CorrSurfPtr1[peak + (2*PITCH_LAG_SPAN2+9)];
        }
      }

      /* 2D parabolic interpolation gives more accurate lags and peak value */
      Intrpol2D(T, &lags1[k], &lags2[k], &peak_vals[k]);
    }

    /* determine the highest peak, after applying a bias towards short lags */
    corr_max = 0.0;
    for (k = 0; k < peaks_ind; k++) {
      corr = peak_vals[k] * pow(PITCH_PEAK_DECAY, log(lags1[k] + lags2[k]));
      if (corr > corr_max) {
        corr_max = corr;
        peak = k;
      }
    }

    lags1[peak] *= 2.0;
    lags2[peak] *= 2.0;

    if (lags1[peak] < (double) PITCH_MIN_LAG) lags1[peak] = (double) PITCH_MIN_LAG;
    if (lags2[peak] < (double) PITCH_MIN_LAG) lags2[peak] = (double) PITCH_MIN_LAG;
    if (lags1[peak] > (double) PITCH_MAX_LAG) lags1[peak] = (double) PITCH_MAX_LAG;
    if (lags2[peak] > (double) PITCH_MAX_LAG) lags2[peak] = (double) PITCH_MAX_LAG;

    /* store lags of highest peak in output array */
    lags[0] = lags1[peak];
    lags[1] = lags1[peak];
    lags[2] = lags2[peak];
    lags[3] = lags2[peak];
  }
  else
  {
    row = max_ind / (PITCH_LAG_SPAN2+4);
    lags1[0] = (double) ((max_ind - row * (PITCH_LAG_SPAN2+4)) + PITCH_MIN_LAG/2 - 4);
    lags2[0] = (double) (lags1[0] + PITCH_BW - row);

    if (lags1[0] < (double) PITCH_MIN_LAG) lags1[0] = (double) PITCH_MIN_LAG;
    if (lags2[0] < (double) PITCH_MIN_LAG) lags2[0] = (double) PITCH_MIN_LAG;
    if (lags1[0] > (double) PITCH_MAX_LAG) lags1[0] = (double) PITCH_MAX_LAG;
    if (lags2[0] > (double) PITCH_MAX_LAG) lags2[0] = (double) PITCH_MAX_LAG;

    /* store lags of highest peak in output array */
    lags[0] = lags1[0];
    lags[1] = lags1[0];
    lags[2] = lags2[0];
    lags[3] = lags2[0];
  }
}


/* create weighting matrix by orthogonalizing a basis of polynomials of increasing order
 * t = (0:4)';
 * A = [t.^0, t.^1, t.^2, t.^3, t.^4];
 * [Q, dummy] = qr(A);
 * P.Weight = Q * diag([0, .1, .5, 1, 1]) * Q'; */
static const double kWeight[5][5] = {
  { 0.29714285714286,  -0.30857142857143,  -0.05714285714286,   0.05142857142857,  0.01714285714286},
  {-0.30857142857143,   0.67428571428571,  -0.27142857142857,  -0.14571428571429,  0.05142857142857},
  {-0.05714285714286,  -0.27142857142857,   0.65714285714286,  -0.27142857142857, -0.05714285714286},
  { 0.05142857142857,  -0.14571428571429,  -0.27142857142857,   0.67428571428571, -0.30857142857143},
  { 0.01714285714286,   0.05142857142857,  -0.05714285714286,  -0.30857142857143,  0.29714285714286}
};


void WebRtcIsac_PitchAnalysis(const double *in,               /* PITCH_FRAME_LEN samples */
                              double *out,                    /* PITCH_FRAME_LEN+QLOOKAHEAD samples */
                              PitchAnalysisStruct *State,
                              double *lags,
                              double *gains)
{
  double HPin[PITCH_FRAME_LEN];
  double Weighted[PITCH_FRAME_LEN];
  double Whitened[PITCH_FRAME_LEN + QLOOKAHEAD];
  double inbuf[PITCH_FRAME_LEN + QLOOKAHEAD];
  double out_G[PITCH_FRAME_LEN + QLOOKAHEAD];          // could be removed by using out instead
  double out_dG[4][PITCH_FRAME_LEN + QLOOKAHEAD];
  double old_lag, old_gain;
  double nrg_wht, tmp;
  double Wnrg, Wfluct, Wgain;
  double H[4][4];
  double grad[4];
  double dG[4];
  int k, m, n, iter;

  /* high pass filtering using second order pole-zero filter */
  WebRtcIsac_Highpass(in, HPin, State->hp_state, PITCH_FRAME_LEN);

  /* copy from state into buffer */
  memcpy(Whitened, State->whitened_buf, sizeof(double) * QLOOKAHEAD);

  /* compute weighted and whitened signals */
  WebRtcIsac_WeightingFilter(HPin, &Weighted[0], &Whitened[QLOOKAHEAD], &(State->Wghtstr));

  /* copy from buffer into state */
  memcpy(State->whitened_buf, Whitened+PITCH_FRAME_LEN, sizeof(double) * QLOOKAHEAD);

  old_lag = State->PFstr_wght.oldlagp[0];
  old_gain = State->PFstr_wght.oldgainp[0];

  /* inital pitch estimate */
  WebRtcIsac_InitializePitch(Weighted, old_lag, old_gain, State, lags);


  /* Iterative optimization of lags - to be done */

  /* compute energy of whitened signal */
  nrg_wht = 0.0;
  for (k = 0; k < PITCH_FRAME_LEN + QLOOKAHEAD; k++)
    nrg_wht += Whitened[k] * Whitened[k];


  /* Iterative optimization of gains */

  /* set weights for energy, gain fluctiation, and spectral gain penalty functions */
  Wnrg = 1.0 / nrg_wht;
  Wgain = 0.005;
  Wfluct = 3.0;

  /* set initial gains */
  for (k = 0; k < 4; k++)
    gains[k] = PITCH_MAX_GAIN_06;

  /* two iterations should be enough */
  for (iter = 0; iter < 2; iter++) {
    /* compute Jacobian of pre-filter output towards gains */
    WebRtcIsac_PitchfilterPre_gains(Whitened, out_G, out_dG, &(State->PFstr_wght), lags, gains);

    /* gradient and approximate Hessian (lower triangle) for minimizing the filter's output power */
    for (k = 0; k < 4; k++) {
      tmp = 0.0;
      for (n = 0; n < PITCH_FRAME_LEN + QLOOKAHEAD; n++)
        tmp += out_G[n] * out_dG[k][n];
      grad[k] = tmp * Wnrg;
    }
    for (k = 0; k < 4; k++) {
      for (m = 0; m <= k; m++) {
        tmp = 0.0;
        for (n = 0; n < PITCH_FRAME_LEN + QLOOKAHEAD; n++)
          tmp += out_dG[m][n] * out_dG[k][n];
        H[k][m] = tmp * Wnrg;
      }
    }

    /* add gradient and Hessian (lower triangle) for dampening fast gain changes */
    for (k = 0; k < 4; k++) {
      tmp = kWeight[k+1][0] * old_gain;
      for (m = 0; m < 4; m++)
        tmp += kWeight[k+1][m+1] * gains[m];
      grad[k] += tmp * Wfluct;
    }
    for (k = 0; k < 4; k++) {
      for (m = 0; m <= k; m++) {
        H[k][m] += kWeight[k+1][m+1] * Wfluct;
      }
    }

    /* add gradient and Hessian for dampening gain */
    for (k = 0; k < 3; k++) {
      tmp = 1.0 / (1 - gains[k]);
      grad[k] += tmp * tmp * Wgain;
      H[k][k] += 2.0 * tmp * (tmp * tmp * Wgain);
    }
    tmp = 1.0 / (1 - gains[3]);
    grad[3] += 1.33 * (tmp * tmp * Wgain);
    H[3][3] += 2.66 * tmp * (tmp * tmp * Wgain);


    /* compute Cholesky factorization of Hessian
     * by overwritting the upper triangle; scale factors on diagonal
     * (for non pc-platforms store the inverse of the diagonals seperately to minimize divisions) */
    H[0][1] = H[1][0] / H[0][0];
    H[0][2] = H[2][0] / H[0][0];
    H[0][3] = H[3][0] / H[0][0];
    H[1][1] -= H[0][0] * H[0][1] * H[0][1];
    H[1][2] = (H[2][1] - H[0][1] * H[2][0]) / H[1][1];
    H[1][3] = (H[3][1] - H[0][1] * H[3][0]) / H[1][1];
    H[2][2] -= H[0][0] * H[0][2] * H[0][2] + H[1][1] * H[1][2] * H[1][2];
    H[2][3] = (H[3][2] - H[0][2] * H[3][0] - H[1][2] * H[1][1] * H[1][3]) / H[2][2];
    H[3][3] -= H[0][0] * H[0][3] * H[0][3] + H[1][1] * H[1][3] * H[1][3] + H[2][2] * H[2][3] * H[2][3];

    /* Compute update as  delta_gains = -inv(H) * grad */
    /* copy and negate */
    for (k = 0; k < 4; k++)
      dG[k] = -grad[k];
    /* back substitution */
    dG[1] -= dG[0] * H[0][1];
    dG[2] -= dG[0] * H[0][2] + dG[1] * H[1][2];
    dG[3] -= dG[0] * H[0][3] + dG[1] * H[1][3] + dG[2] * H[2][3];
    /* scale */
    for (k = 0; k < 4; k++)
      dG[k] /= H[k][k];
    /* back substitution */
    dG[2] -= dG[3] * H[2][3];
    dG[1] -= dG[3] * H[1][3] + dG[2] * H[1][2];
    dG[0] -= dG[3] * H[0][3] + dG[2] * H[0][2] + dG[1] * H[0][1];

    /* update gains and check range */
    for (k = 0; k < 4; k++) {
      gains[k] += dG[k];
      if (gains[k] > PITCH_MAX_GAIN)
        gains[k] = PITCH_MAX_GAIN;
      else if (gains[k] < 0.0)
        gains[k] = 0.0;
    }
  }

  /* update state for next frame */
  WebRtcIsac_PitchfilterPre(Whitened, out, &(State->PFstr_wght), lags, gains);

  /* concatenate previous input's end and current input */
  memcpy(inbuf, State->inbuf, sizeof(double) * QLOOKAHEAD);
  memcpy(inbuf+QLOOKAHEAD, in, sizeof(double) * PITCH_FRAME_LEN);

  /* lookahead pitch filtering for masking analysis */
  WebRtcIsac_PitchfilterPre_la(inbuf, out, &(State->PFstr), lags, gains);

  /* store last part of input */
  for (k = 0; k < QLOOKAHEAD; k++)
    State->inbuf[k] = inbuf[k + PITCH_FRAME_LEN];
}