--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/intercom/ilbc/iCBSearch.c	Fri Jun 25 09:57:52 2010 +0200
@@ -0,0 +1,487 @@
+
+   /******************************************************************
+
+       iLBC Speech Coder ANSI-C Source Code
+
+       iCBSearch.c
+
+       Copyright (C) The Internet Society (2004).
+       All Rights Reserved.
+
+   ******************************************************************/
+
+#include <math.h>
+#include <string.h>
+
+#include "iLBC_define.h"
+#include "gainquant.h"
+#include "createCB.h"
+#include "filter.h"
+#include "constants.h"
+
+   /*----------------------------------------------------------------*
+    *  Search routine for codebook encoding and gain quantization.
+    *---------------------------------------------------------------*/
+
+void iCBSearch(iLBC_Enc_Inst_t * iLBCenc_inst,
+  /* (i) the encoder state structure */
+  int *index,                   /* (o) Codebook indices */
+  int *gain_index,              /* (o) Gain quantization indices */
+  float *intarget,              /* (i) Target vector for encoding */
+  float *mem,                   /* (i) Buffer for codebook construction */
+  int lMem,                     /* (i) Length of buffer */
+  int lTarget,                  /* (i) Length of vector */
+  int nStages,                  /* (i) Number of codebook stages */
+  float *weightDenum,           /* (i) weighting filter coefficients */
+  float *weightState,           /* (i) weighting filter state */
+  int block                     /* (i) the sub-block number */
+  )
+{
+  int i, j, icount, stage, best_index, range, counter;
+  float max_measure, gain, measure, crossDot, ftmp;
+  float gains[CB_NSTAGES];
+  float target[SUBL];
+  int base_index, sInd, eInd, base_size;
+  int sIndAug = 0, eIndAug = 0;
+  float buf[CB_MEML + SUBL + 2 * LPC_FILTERORDER];
+  float invenergy[CB_EXPAND * 128], energy[CB_EXPAND * 128];
+  float *pp, *ppi = 0, *ppo = 0, *ppe = 0;
+  float cbvectors[CB_MEML];
+  float tene, cene, cvec[SUBL];
+  float aug_vec[SUBL];
+
+  memset(cvec, 0, SUBL * sizeof(float));
+
+  /* Determine size of codebook sections */
+
+  base_size = lMem - lTarget + 1;
+
+  if (lTarget == SUBL) {
+    base_size = lMem - lTarget + 1 + lTarget / 2;
+  }
+
+  /* setup buffer for weighting */
+
+  memcpy(buf, weightState, sizeof(float) * LPC_FILTERORDER);
+  memcpy(buf + LPC_FILTERORDER, mem, lMem * sizeof(float));
+  memcpy(buf + LPC_FILTERORDER + lMem, intarget,
+    lTarget * sizeof(float));
+
+  /* weighting */
+
+  AllPoleFilter(buf + LPC_FILTERORDER, weightDenum,
+    lMem + lTarget, LPC_FILTERORDER);
+
+  /* Construct the codebook and target needed */
+
+  memcpy(target, buf + LPC_FILTERORDER + lMem, lTarget * sizeof(float));
+
+  tene = 0.0;
+
+
+
+
+
+  for (i = 0; i < lTarget; i++) {
+    tene += target[i] * target[i];
+  }
+
+  /* Prepare search over one more codebook section. This section
+     is created by filtering the original buffer with a filter. */
+
+  filteredCBvecs(cbvectors, buf + LPC_FILTERORDER, lMem);
+
+  /* The Main Loop over stages */
+
+  for (stage = 0; stage < nStages; stage++) {
+
+    range = search_rangeTbl[block][stage];
+
+    /* initialize search measure */
+
+    max_measure = (float) -10000000.0;
+    gain = (float) 0.0;
+    best_index = 0;
+
+    /* Compute cross dot product between the target
+       and the CB memory */
+
+    crossDot = 0.0;
+    pp = buf + LPC_FILTERORDER + lMem - lTarget;
+    for (j = 0; j < lTarget; j++) {
+      crossDot += target[j] * (*pp++);
+    }
+
+    if (stage == 0) {
+
+      /* Calculate energy in the first block of
+         'lTarget' samples. */
+      ppe = energy;
+      ppi = buf + LPC_FILTERORDER + lMem - lTarget - 1;
+      ppo = buf + LPC_FILTERORDER + lMem - 1;
+
+      *ppe = 0.0;
+      pp = buf + LPC_FILTERORDER + lMem - lTarget;
+      for (j = 0; j < lTarget; j++, pp++) {
+        *ppe += (*pp) * (*pp);
+      }
+
+      if (*ppe > 0.0) {
+        invenergy[0] = (float) 1.0 / (*ppe + EPS);
+      } else {
+        invenergy[0] = (float) 0.0;
+
+
+
+
+
+      }
+      ppe++;
+
+      measure = (float) -10000000.0;
+
+      if (crossDot > 0.0) {
+        measure = crossDot * crossDot * invenergy[0];
+      }
+    } else {
+      measure = crossDot * crossDot * invenergy[0];
+    }
+
+    /* check if measure is better */
+    ftmp = crossDot * invenergy[0];
+
+    if ((measure > max_measure) && (fabs(ftmp) < CB_MAXGAIN)) {
+      best_index = 0;
+      max_measure = measure;
+      gain = ftmp;
+    }
+
+    /* loop over the main first codebook section,
+       full search */
+
+    for (icount = 1; icount < range; icount++) {
+
+      /* calculate measure */
+
+      crossDot = 0.0;
+      pp = buf + LPC_FILTERORDER + lMem - lTarget - icount;
+
+      for (j = 0; j < lTarget; j++) {
+        crossDot += target[j] * (*pp++);
+      }
+
+      if (stage == 0) {
+        *ppe++ = energy[icount - 1] + (*ppi) * (*ppi) - (*ppo) * (*ppo);
+        ppo--;
+        ppi--;
+
+        if (energy[icount] > 0.0) {
+          invenergy[icount] = (float) 1.0 / (energy[icount] + EPS);
+        } else {
+          invenergy[icount] = (float) 0.0;
+        }
+
+
+
+
+
+        measure = (float) -10000000.0;
+
+        if (crossDot > 0.0) {
+          measure = crossDot * crossDot * invenergy[icount];
+        }
+      } else {
+        measure = crossDot * crossDot * invenergy[icount];
+      }
+
+      /* check if measure is better */
+      ftmp = crossDot * invenergy[icount];
+
+      if ((measure > max_measure) && (fabs(ftmp) < CB_MAXGAIN)) {
+        best_index = icount;
+        max_measure = measure;
+        gain = ftmp;
+      }
+    }
+
+    /* Loop over augmented part in the first codebook
+     * section, full search.
+     * The vectors are interpolated.
+     */
+
+    if (lTarget == SUBL) {
+
+      /* Search for best possible cb vector and
+         compute the CB-vectors' energy. */
+      searchAugmentedCB(20, 39, stage, base_size - lTarget / 2,
+        target, buf + LPC_FILTERORDER + lMem,
+        &max_measure, &best_index, &gain, energy, invenergy);
+    }
+
+    /* set search range for following codebook sections */
+
+    base_index = best_index;
+
+    /* unrestricted search */
+
+    if (CB_RESRANGE == -1) {
+      sInd = 0;
+      eInd = range - 1;
+      sIndAug = 20;
+      eIndAug = 39;
+    }
+
+
+
+
+
+
+    /* restricted search around best index from first
+       codebook section */
+
+    else {
+      /* Initialize search indices */
+      sIndAug = 0;
+      eIndAug = 0;
+      sInd = base_index - CB_RESRANGE / 2;
+      eInd = sInd + CB_RESRANGE;
+
+      if (lTarget == SUBL) {
+
+        if (sInd < 0) {
+
+          sIndAug = 40 + sInd;
+          eIndAug = 39;
+          sInd = 0;
+
+        } else if (base_index < (base_size - 20)) {
+
+          if (eInd > range) {
+            sInd -= (eInd - range);
+            eInd = range;
+          }
+        } else {                /* base_index >= (base_size-20) */
+
+          if (sInd < (base_size - 20)) {
+            sIndAug = 20;
+            sInd = 0;
+            eInd = 0;
+            eIndAug = 19 + CB_RESRANGE;
+
+            if (eIndAug > 39) {
+              eInd = eIndAug - 39;
+              eIndAug = 39;
+            }
+          } else {
+            sIndAug = 20 + sInd - (base_size - 20);
+            eIndAug = 39;
+            sInd = 0;
+            eInd = CB_RESRANGE - (eIndAug - sIndAug + 1);
+          }
+        }
+
+      } else {                  /* lTarget = 22 or 23 */
+
+        if (sInd < 0) {
+          eInd -= sInd;
+
+
+
+
+
+          sInd = 0;
+        }
+
+        if (eInd > range) {
+          sInd -= (eInd - range);
+          eInd = range;
+        }
+      }
+    }
+
+    /* search of higher codebook section */
+
+    /* index search range */
+    counter = sInd;
+    sInd += base_size;
+    eInd += base_size;
+
+
+    if (stage == 0) {
+      ppe = energy + base_size;
+      *ppe = 0.0;
+
+      pp = cbvectors + lMem - lTarget;
+      for (j = 0; j < lTarget; j++, pp++) {
+        *ppe += (*pp) * (*pp);
+      }
+
+      ppi = cbvectors + lMem - 1 - lTarget;
+      ppo = cbvectors + lMem - 1;
+
+      for (j = 0; j < (range - 1); j++) {
+        *(ppe + 1) = *ppe + (*ppi) * (*ppi) - (*ppo) * (*ppo);
+        ppo--;
+        ppi--;
+        ppe++;
+      }
+    }
+
+    /* loop over search range */
+
+    for (icount = sInd; icount < eInd; icount++) {
+
+      /* calculate measure */
+
+      crossDot = 0.0;
+      pp = cbvectors + lMem - (counter++) - lTarget;
+
+      for (j = 0; j < lTarget; j++) {
+
+
+
+
+
+        crossDot += target[j] * (*pp++);
+      }
+
+      if (energy[icount] > 0.0) {
+        invenergy[icount] = (float) 1.0 / (energy[icount] + EPS);
+      } else {
+        invenergy[icount] = (float) 0.0;
+      }
+
+      if (stage == 0) {
+
+        measure = (float) -10000000.0;
+
+        if (crossDot > 0.0) {
+          measure = crossDot * crossDot * invenergy[icount];
+        }
+      } else {
+        measure = crossDot * crossDot * invenergy[icount];
+      }
+
+      /* check if measure is better */
+      ftmp = crossDot * invenergy[icount];
+
+      if ((measure > max_measure) && (fabs(ftmp) < CB_MAXGAIN)) {
+        best_index = icount;
+        max_measure = measure;
+        gain = ftmp;
+      }
+    }
+
+    /* Search the augmented CB inside the limited range. */
+
+    if ((lTarget == SUBL) && (sIndAug != 0)) {
+      searchAugmentedCB(sIndAug, eIndAug, stage,
+        2 * base_size - 20, target, cbvectors + lMem,
+        &max_measure, &best_index, &gain, energy, invenergy);
+    }
+
+    /* record best index */
+
+    index[stage] = best_index;
+
+    /* gain quantization */
+
+    if (stage == 0) {
+
+
+
+
+
+
+      if (gain < 0.0) {
+        gain = 0.0;
+      }
+
+      if (gain > CB_MAXGAIN) {
+        gain = (float) CB_MAXGAIN;
+      }
+      gain = gainquant(gain, 1.0, 32, &gain_index[stage]);
+    } else {
+      if (stage == 1) {
+        gain = gainquant(gain, (float) fabs(gains[stage - 1]),
+          16, &gain_index[stage]);
+      } else {
+        gain = gainquant(gain, (float) fabs(gains[stage - 1]),
+          8, &gain_index[stage]);
+      }
+    }
+
+    /* Extract the best (according to measure)
+       codebook vector */
+
+    if (lTarget == (STATE_LEN - iLBCenc_inst->state_short_len)) {
+
+      if (index[stage] < base_size) {
+        pp = buf + LPC_FILTERORDER + lMem - lTarget - index[stage];
+      } else {
+        pp = cbvectors + lMem - lTarget - index[stage] + base_size;
+      }
+    } else {
+
+      if (index[stage] < base_size) {
+        if (index[stage] < (base_size - 20)) {
+          pp = buf + LPC_FILTERORDER + lMem - lTarget - index[stage];
+        } else {
+          createAugmentedVec(index[stage] - base_size + 40,
+            buf + LPC_FILTERORDER + lMem, aug_vec);
+          pp = aug_vec;
+        }
+      } else {
+        int filterno, position;
+
+        filterno = index[stage] / base_size;
+        position = index[stage] - filterno * base_size;
+
+
+
+
+
+
+
+        if (position < (base_size - 20)) {
+          pp = cbvectors + filterno * lMem - lTarget -
+            index[stage] + filterno * base_size;
+        } else {
+          createAugmentedVec(index[stage] - (filterno + 1) * base_size +
+            40, cbvectors + filterno * lMem, aug_vec);
+          pp = aug_vec;
+        }
+      }
+    }
+
+    /* Subtract the best codebook vector, according
+       to measure, from the target vector */
+
+    for (j = 0; j < lTarget; j++) {
+      cvec[j] += gain * (*pp);
+      target[j] -= gain * (*pp++);
+    }
+
+    /* record quantized gain */
+
+    gains[stage] = gain;
+
+  }                             /* end of Main Loop. for (stage=0;... */
+
+  /* Gain adjustment for energy matching */
+  cene = 0.0;
+  for (i = 0; i < lTarget; i++) {
+    cene += cvec[i] * cvec[i];
+  }
+  j = gain_index[0];
+
+  for (i = gain_index[0]; i < 32; i++) {
+    ftmp = cene * gain_sq5Tbl[i] * gain_sq5Tbl[i];
+
+    if ((ftmp < (tene * gains[0] * gains[0])) &&
+      (gain_sq5Tbl[j] < (2.0 * gains[0]))) {
+      j = i;
+    }
+  }
+  gain_index[0] = j;
+}