/*
 *  Copyright (c) 2012 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.
 */

/*
 * pitch_filter.c
 *
 * Pitch filter functions
 *
 */

#include "common_audio/signal_processing/include/signal_processing_library.h"
#include "modules/audio_coding/codecs/isac/fix/source/pitch_estimator.h"
#include "modules/audio_coding/codecs/isac/fix/source/settings.h"
#include "modules/audio_coding/codecs/isac/fix/source/structs.h"
#include "system_wrappers/interface/compile_assert.h"

// Number of segments in a pitch subframe.
static const int kSegments = 5;

// A division factor of 1/5 in Q15.
static const WebRtc_Word16 kDivFactor = 6553;

// Filter coefficicients in Q15.
static const WebRtc_Word16 kDampFilter[PITCH_DAMPORDER] = {
  -2294, 8192, 20972, 8192, -2294
};

// Interpolation coefficients; generated by design_pitch_filter.m.
// Coefficients are stored in Q14.
static const WebRtc_Word16 kIntrpCoef[PITCH_FRACS][PITCH_FRACORDER] = {
  {-367, 1090, -2706,  9945, 10596, -3318,  1626, -781,  287},
  {-325,  953, -2292,  7301, 12963, -3320,  1570, -743,  271},
  {-240,  693, -1622,  4634, 14809, -2782,  1262, -587,  212},
  {-125,  358,  -817,  2144, 15982, -1668,   721, -329,  118},
  {   0,    0,    -1,     1, 16380,     1,    -1,    0,    0},
  { 118, -329,   721, -1668, 15982,  2144,  -817,  358, -125},
  { 212, -587,  1262, -2782, 14809,  4634, -1622,  693, -240},
  { 271, -743,  1570, -3320, 12963,  7301, -2292,  953, -325}
};

// Function prototype for pitch filtering.
// TODO(Turaj): Add descriptions of input and output parameters.
void WebRtcIsacfix_PitchFilterCore(int loopNumber,
                                   WebRtc_Word16 gain,
                                   int index,
                                   WebRtc_Word16 sign,
                                   WebRtc_Word16* inputState,
                                   WebRtc_Word16* outputBuf2,
                                   const WebRtc_Word16* coefficient,
                                   WebRtc_Word16* inputBuf,
                                   WebRtc_Word16* outputBuf,
                                   int* index2);

static __inline WebRtc_Word32 CalcLrIntQ(WebRtc_Word32 fixVal,
                                         WebRtc_Word16 qDomain) {
  WebRtc_Word32 intgr;
  WebRtc_Word32 roundVal;

  roundVal = WEBRTC_SPL_LSHIFT_W32((WebRtc_Word32)1,  qDomain - 1);
  intgr = WEBRTC_SPL_RSHIFT_W32(fixVal + roundVal, qDomain);

  return intgr;
}

void WebRtcIsacfix_PitchFilter(WebRtc_Word16* indatQQ, // Q10 if type is 1 or 4,
                                                       // Q0 if type is 2.
                               WebRtc_Word16* outdatQQ,
                               PitchFiltstr* pfp,
                               WebRtc_Word16* lagsQ7,
                               WebRtc_Word16* gainsQ12,
                               WebRtc_Word16 type) {
  int    k, ind, cnt;
  WebRtc_Word16 sign = 1;
  WebRtc_Word16 inystateQQ[PITCH_DAMPORDER];
  WebRtc_Word16 ubufQQ[PITCH_INTBUFFSIZE + QLOOKAHEAD];
  const WebRtc_Word16 Gain = 21299;     // 1.3 in Q14
  WebRtc_Word16 oldLagQ7;
  WebRtc_Word16 oldGainQ12, lagdeltaQ7, curLagQ7, gaindeltaQ12, curGainQ12;
  int indW32 = 0, frcQQ = 0;
  WebRtc_Word32 tmpW32;
  const WebRtc_Word16* fracoeffQQ = NULL;

  // Assumptions in ARM assembly for WebRtcIsacfix_PitchFilterCoreARM().
  COMPILE_ASSERT(PITCH_FRACORDER == 9);
  COMPILE_ASSERT(PITCH_DAMPORDER == 5);

  // Set up buffer and states.
  memcpy(ubufQQ, pfp->ubufQQ, sizeof(pfp->ubufQQ));
  memcpy(inystateQQ, pfp->ystateQQ, sizeof(inystateQQ));

  // Get old lag and gain value from memory.
  oldLagQ7 = pfp->oldlagQ7;
  oldGainQ12 = pfp->oldgainQ12;

  if (type == 4) {
    sign = -1;

    // Make output more periodic.
    for (k = 0; k < PITCH_SUBFRAMES; k++) {
      gainsQ12[k] = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16_RSFT(
          gainsQ12[k], Gain, 14);
    }
  }

  // No interpolation if pitch lag step is big.
  if ((WEBRTC_SPL_MUL_16_16_RSFT(lagsQ7[0], 3, 1) < oldLagQ7) ||
      (lagsQ7[0] > WEBRTC_SPL_MUL_16_16_RSFT(oldLagQ7, 3, 1))) {
    oldLagQ7 = lagsQ7[0];
    oldGainQ12 = gainsQ12[0];
  }

  ind = 0;

  for (k = 0; k < PITCH_SUBFRAMES; k++) {
    // Calculate interpolation steps.
    lagdeltaQ7 = lagsQ7[k] - oldLagQ7;
    lagdeltaQ7 = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16_RSFT_WITH_ROUND(
                  lagdeltaQ7, kDivFactor, 15);
    curLagQ7 = oldLagQ7;
    gaindeltaQ12 = gainsQ12[k] - oldGainQ12;
    gaindeltaQ12 = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16_RSFT(
                    gaindeltaQ12, kDivFactor, 15);

    curGainQ12 = oldGainQ12;
    oldLagQ7 = lagsQ7[k];
    oldGainQ12 = gainsQ12[k];

    // Each frame has 4 60-sample pitch subframes, and each subframe has 5
    // 12-sample segments. Each segment need to be processed with
    // newly-updated parameters, so we break the pitch filtering into
    // two for-loops (5 x 12) below. It's also why kDivFactor = 0.2 (in Q15).
    for (cnt = 0; cnt < kSegments; cnt++) {
      // Update parameters for each segment.
      curGainQ12 += gaindeltaQ12;
      curLagQ7 += lagdeltaQ7;
      indW32 = CalcLrIntQ(curLagQ7, 7);
      tmpW32 = WEBRTC_SPL_LSHIFT_W32(indW32, 7);
      tmpW32 -= curLagQ7;
      frcQQ = WEBRTC_SPL_RSHIFT_W32(tmpW32, 4);
      frcQQ += 4;

      if (frcQQ == PITCH_FRACS) {
        frcQQ = 0;
      }
      fracoeffQQ = kIntrpCoef[frcQQ];

      // Pitch filtering.
      WebRtcIsacfix_PitchFilterCore(PITCH_SUBFRAME_LEN / kSegments, curGainQ12,
        indW32, sign, inystateQQ, ubufQQ, fracoeffQQ, indatQQ, outdatQQ, &ind);
    }
  }

  // Export buffer and states.
  memcpy(pfp->ubufQQ, ubufQQ + PITCH_FRAME_LEN, sizeof(pfp->ubufQQ));
  memcpy(pfp->ystateQQ, inystateQQ, sizeof(pfp->ystateQQ));

  pfp->oldlagQ7 = oldLagQ7;
  pfp->oldgainQ12 = oldGainQ12;

  if (type == 2) {
    // Filter look-ahead segment.
    WebRtcIsacfix_PitchFilterCore(QLOOKAHEAD, curGainQ12, indW32, 1, inystateQQ,
                ubufQQ, fracoeffQQ, indatQQ, outdatQQ, &ind);
  }
}


void WebRtcIsacfix_PitchFilterGains(const WebRtc_Word16* indatQ0,
                                    PitchFiltstr* pfp,
                                    WebRtc_Word16* lagsQ7,
                                    WebRtc_Word16* gainsQ12) {
  int  k, n, m, ind, pos, pos3QQ;

  WebRtc_Word16 ubufQQ[PITCH_INTBUFFSIZE];
  WebRtc_Word16 oldLagQ7, lagdeltaQ7, curLagQ7;
  const WebRtc_Word16* fracoeffQQ = NULL;
  WebRtc_Word16 scale;
  WebRtc_Word16 cnt = 0, frcQQ, indW16 = 0, tmpW16;
  WebRtc_Word32 tmpW32, tmp2W32, csum1QQ, esumxQQ;

  // Set up buffer and states.
  memcpy(ubufQQ, pfp->ubufQQ, sizeof(pfp->ubufQQ));
  oldLagQ7 = pfp->oldlagQ7;

  // No interpolation if pitch lag step is big.
  if ((WEBRTC_SPL_MUL_16_16_RSFT(lagsQ7[0], 3, 1) < oldLagQ7) ||
      (lagsQ7[0] > WEBRTC_SPL_MUL_16_16_RSFT(oldLagQ7, 3, 1))) {
    oldLagQ7 = lagsQ7[0];
  }

  ind = 0;
  pos = ind + PITCH_BUFFSIZE;
  scale = 0;
  for (k = 0; k < PITCH_SUBFRAMES; k++) {

    // Calculate interpolation steps.
    lagdeltaQ7 = lagsQ7[k] - oldLagQ7;
    lagdeltaQ7 = (WebRtc_Word16)WEBRTC_SPL_MUL_16_16_RSFT_WITH_ROUND(
                   lagdeltaQ7, kDivFactor, 15);
    curLagQ7 = oldLagQ7;
    oldLagQ7 = lagsQ7[k];

    csum1QQ = 1;
    esumxQQ = 1;

    // Same as function WebRtcIsacfix_PitchFilter(), we break the pitch
    // filtering into two for-loops (5 x 12) below.
    for (cnt = 0; cnt < kSegments; cnt++) {
      // Update parameters for each segment.
      curLagQ7 += lagdeltaQ7;
      indW16 = (WebRtc_Word16)CalcLrIntQ(curLagQ7, 7);
      tmpW16 = WEBRTC_SPL_LSHIFT_W16(indW16, 7);
      tmpW16 -= curLagQ7;
      frcQQ = WEBRTC_SPL_RSHIFT_W16(tmpW16, 4);
      frcQQ += 4;

      if (frcQQ == PITCH_FRACS) {
        frcQQ = 0;
      }
      fracoeffQQ = kIntrpCoef[frcQQ];

      pos3QQ = pos - (indW16 + 4);

      for (n = 0; n < PITCH_SUBFRAME_LEN / kSegments; n++) {
        // Filter to get fractional pitch.

        tmpW32 = 0;
        for (m = 0; m < PITCH_FRACORDER; m++) {
          tmpW32 += WEBRTC_SPL_MUL_16_16(ubufQQ[pos3QQ + m], fracoeffQQ[m]);
        }

        // Subtract from input and update buffer.
        ubufQQ[pos] = indatQ0[ind];

        tmp2W32 = WEBRTC_SPL_MUL_16_32_RSFT14(indatQ0[ind], tmpW32);
        tmpW32 += 8192;
        tmpW16 = (WebRtc_Word16)WEBRTC_SPL_RSHIFT_W32(tmpW32, 14);
        tmpW32 = WEBRTC_SPL_MUL_16_16(tmpW16, tmpW16);

        if ((tmp2W32 > 1073700000) || (csum1QQ > 1073700000) ||
            (tmpW32 > 1073700000) || (esumxQQ > 1073700000)) {  // 2^30
          scale++;
          csum1QQ = WEBRTC_SPL_RSHIFT_W32(csum1QQ, 1);
          esumxQQ = WEBRTC_SPL_RSHIFT_W32(esumxQQ, 1);
        }
        tmp2W32 = WEBRTC_SPL_RSHIFT_W32(tmp2W32, scale);
        csum1QQ += tmp2W32;
        tmpW32 = WEBRTC_SPL_RSHIFT_W32(tmpW32, scale);
        esumxQQ += tmpW32;

        ind++;
        pos++;
        pos3QQ++;
      }
    }

    if (csum1QQ < esumxQQ) {
      tmp2W32 = WebRtcSpl_DivResultInQ31(csum1QQ, esumxQQ);

      // Gain should be half the correlation.
      tmpW32 = WEBRTC_SPL_RSHIFT_W32(tmp2W32, 20);
    } else {
      tmpW32 = 4096;
    }
    gainsQ12[k] = (WebRtc_Word16)WEBRTC_SPL_SAT(PITCH_MAX_GAIN_Q12, tmpW32, 0);
  }

  // Export buffer and states.
  memcpy(pfp->ubufQQ, ubufQQ + PITCH_FRAME_LEN, sizeof(pfp->ubufQQ));
  pfp->oldlagQ7 = lagsQ7[PITCH_SUBFRAMES - 1];
  pfp->oldgainQ12 = gainsQ12[PITCH_SUBFRAMES - 1];

}
