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

       iLBC Speech Coder ANSI-C Source Code

       iLBC_encode.c

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

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

#include <math.h>
#include <stdlib.h>
#include <string.h>

#include "iLBC_define.h"
#include "LPCencode.h"
#include "FrameClassify.h"
#include "StateSearchW.h"
#include "StateConstructW.h"
#include "helpfun.h"
#include "constants.h"
#include "packing.h"
#include "iCBSearch.h"
#include "iCBConstruct.h"
#include "hpInput.h"
#include "anaFilter.h"
#include "syntFilter.h"

   /*----------------------------------------------------------------*
    *  Initiation of encoder instance.
    *---------------------------------------------------------------*/

short initEncode(               /* (o) Number of bytes
                                   encoded */
  iLBC_Enc_Inst_t * iLBCenc_inst,       /* (i/o) Encoder instance */
  int mode                      /* (i) frame size mode */
  )
{
  iLBCenc_inst->mode = mode;
  if (mode == 30) {
    iLBCenc_inst->blockl = BLOCKL_30MS;
    iLBCenc_inst->nsub = NSUB_30MS;
    iLBCenc_inst->nasub = NASUB_30MS;
    iLBCenc_inst->lpc_n = LPC_N_30MS;
    iLBCenc_inst->no_of_bytes = NO_OF_BYTES_30MS;
    iLBCenc_inst->no_of_words = NO_OF_WORDS_30MS;





    iLBCenc_inst->state_short_len = STATE_SHORT_LEN_30MS;
    /* ULP init */
    iLBCenc_inst->ULP_inst = &ULP_30msTbl;
  } else if (mode == 20) {
    iLBCenc_inst->blockl = BLOCKL_20MS;
    iLBCenc_inst->nsub = NSUB_20MS;
    iLBCenc_inst->nasub = NASUB_20MS;
    iLBCenc_inst->lpc_n = LPC_N_20MS;
    iLBCenc_inst->no_of_bytes = NO_OF_BYTES_20MS;
    iLBCenc_inst->no_of_words = NO_OF_WORDS_20MS;
    iLBCenc_inst->state_short_len = STATE_SHORT_LEN_20MS;
    /* ULP init */
    iLBCenc_inst->ULP_inst = &ULP_20msTbl;
  } else {
    exit(2);
  }

  memset((*iLBCenc_inst).anaMem, 0, LPC_FILTERORDER * sizeof(float));
  memcpy((*iLBCenc_inst).lsfold, lsfmeanTbl,
    LPC_FILTERORDER * sizeof(float));
  memcpy((*iLBCenc_inst).lsfdeqold, lsfmeanTbl,
    LPC_FILTERORDER * sizeof(float));
  memset((*iLBCenc_inst).lpc_buffer, 0,
    (LPC_LOOKBACK + BLOCKL_MAX) * sizeof(float));
  memset((*iLBCenc_inst).hpimem, 0, 4 * sizeof(float));

  return (iLBCenc_inst->no_of_bytes);
}

   /*----------------------------------------------------------------*
    *  main encoder function
    *---------------------------------------------------------------*/

void iLBC_encode(unsigned char *bytes,  /* (o) encoded data bits iLBC */
  float *block,                 /* (o) speech vector to
                                   encode */
  iLBC_Enc_Inst_t * iLBCenc_inst        /* (i/o) the general encoder
                                           state */
  )
{

  float data[BLOCKL_MAX];
  float residual[BLOCKL_MAX], reverseResidual[BLOCKL_MAX];

  int start, idxForMax, idxVec[STATE_LEN];





  float reverseDecresidual[BLOCKL_MAX], mem[CB_MEML];
  int n, k, meml_gotten, Nfor, Nback, i, pos;
  int gain_index[CB_NSTAGES * NASUB_MAX], extra_gain_index[CB_NSTAGES];
  int cb_index[CB_NSTAGES * NASUB_MAX], extra_cb_index[CB_NSTAGES];
  int lsf_i[LSF_NSPLIT * LPC_N_MAX];
  unsigned char *pbytes;
  int diff, start_pos, state_first;
  float en1, en2;
  int index, ulp, firstpart;
  int subcount, subframe;
  float weightState[LPC_FILTERORDER];
  float syntdenum[NSUB_MAX * (LPC_FILTERORDER + 1)];
  float weightdenum[NSUB_MAX * (LPC_FILTERORDER + 1)];
  float decresidual[BLOCKL_MAX];

  /* high pass filtering of input signal if such is not done
     prior to calling this function */

  hpInput(block, iLBCenc_inst->blockl, data, (*iLBCenc_inst).hpimem);

  /* otherwise simply copy */

  /*memcpy(data,block,iLBCenc_inst->blockl*sizeof(float)); */

  /* LPC of hp filtered input data */

  LPCencode(syntdenum, weightdenum, lsf_i, data, iLBCenc_inst);


  /* inverse filter to get residual */

  for (n = 0; n < iLBCenc_inst->nsub; n++) {
    anaFilter(&data[n * SUBL], &syntdenum[n * (LPC_FILTERORDER + 1)],
      SUBL, &residual[n * SUBL], iLBCenc_inst->anaMem);
  }

  /* find state location */

  start = FrameClassify(iLBCenc_inst, residual);

  /* check if state should be in first or last part of the
     two subframes */

  diff = STATE_LEN - iLBCenc_inst->state_short_len;
  en1 = 0;
  index = (start - 1) * SUBL;





  for (i = 0; i < iLBCenc_inst->state_short_len; i++) {
    en1 += residual[index + i] * residual[index + i];
  }
  en2 = 0;
  index = (start - 1) * SUBL + diff;
  for (i = 0; i < iLBCenc_inst->state_short_len; i++) {
    en2 += residual[index + i] * residual[index + i];
  }


  if (en1 > en2) {
    state_first = 1;
    start_pos = (start - 1) * SUBL;
  } else {
    state_first = 0;
    start_pos = (start - 1) * SUBL + diff;
  }

  /* scalar quantization of state */

  StateSearchW(iLBCenc_inst, &residual[start_pos],
    &syntdenum[(start - 1) * (LPC_FILTERORDER + 1)],
    &weightdenum[(start - 1) * (LPC_FILTERORDER + 1)], &idxForMax,
    idxVec, iLBCenc_inst->state_short_len, state_first);

  StateConstructW(idxForMax, idxVec,
    &syntdenum[(start - 1) * (LPC_FILTERORDER + 1)],
    &decresidual[start_pos], iLBCenc_inst->state_short_len);

  /* predictive quantization in state */

  if (state_first) {            /* put adaptive part in the end */

    /* setup memory */

    memset(mem, 0,
      (CB_MEML - iLBCenc_inst->state_short_len) * sizeof(float));
    memcpy(mem + CB_MEML - iLBCenc_inst->state_short_len,
      decresidual + start_pos,
      iLBCenc_inst->state_short_len * sizeof(float));
    memset(weightState, 0, LPC_FILTERORDER * sizeof(float));

    /* encode sub-frames */

    iCBSearch(iLBCenc_inst, extra_cb_index, extra_gain_index,
      &residual[start_pos + iLBCenc_inst->state_short_len],
      mem + CB_MEML - stMemLTbl,
      stMemLTbl, diff, CB_NSTAGES,
      &weightdenum[start * (LPC_FILTERORDER + 1)], weightState, 0);

    /* construct decoded vector */

    iCBConstruct(&decresidual[start_pos +
        iLBCenc_inst->state_short_len], extra_cb_index,
      extra_gain_index, mem + CB_MEML - stMemLTbl, stMemLTbl, diff,
      CB_NSTAGES);

  } else {                      /* put adaptive part in the beginning */

    /* create reversed vectors for prediction */

    for (k = 0; k < diff; k++) {
      reverseResidual[k] = residual[(start + 1) * SUBL - 1
        - (k + iLBCenc_inst->state_short_len)];
    }

    /* setup memory */

    meml_gotten = iLBCenc_inst->state_short_len;
    for (k = 0; k < meml_gotten; k++) {
      mem[CB_MEML - 1 - k] = decresidual[start_pos + k];
    }
    memset(mem, 0, (CB_MEML - k) * sizeof(float));
    memset(weightState, 0, LPC_FILTERORDER * sizeof(float));

    /* encode sub-frames */

    iCBSearch(iLBCenc_inst, extra_cb_index, extra_gain_index,
      reverseResidual, mem + CB_MEML - stMemLTbl, stMemLTbl,
      diff, CB_NSTAGES,
      &weightdenum[(start - 1) * (LPC_FILTERORDER + 1)],
      weightState, 0);

    /* construct decoded vector */

    iCBConstruct(reverseDecresidual, extra_cb_index,
      extra_gain_index, mem + CB_MEML - stMemLTbl, stMemLTbl,
      diff, CB_NSTAGES);

    /* get decoded residual from reversed vector */

    for (k = 0; k < diff; k++) {
      decresidual[start_pos - 1 - k] = reverseDecresidual[k];





    }
  }

  /* counter for predicted sub-frames */

  subcount = 0;

  /* forward prediction of sub-frames */

  Nfor = iLBCenc_inst->nsub - start - 1;


  if (Nfor > 0) {

    /* setup memory */

    memset(mem, 0, (CB_MEML - STATE_LEN) * sizeof(float));
    memcpy(mem + CB_MEML - STATE_LEN, decresidual + (start - 1) * SUBL,
      STATE_LEN * sizeof(float));
    memset(weightState, 0, LPC_FILTERORDER * sizeof(float));

    /* loop over sub-frames to encode */

    for (subframe = 0; subframe < Nfor; subframe++) {

      /* encode sub-frame */

      iCBSearch(iLBCenc_inst, cb_index + subcount * CB_NSTAGES,
        gain_index + subcount * CB_NSTAGES,
        &residual[(start + 1 + subframe) * SUBL],
        mem + CB_MEML - memLfTbl[subcount],
        memLfTbl[subcount], SUBL, CB_NSTAGES,
        &weightdenum[(start + 1 + subframe) *
          (LPC_FILTERORDER + 1)], weightState, subcount + 1);

      /* construct decoded vector */

      iCBConstruct(&decresidual[(start + 1 + subframe) * SUBL],
        cb_index + subcount * CB_NSTAGES,
        gain_index + subcount * CB_NSTAGES,
        mem + CB_MEML - memLfTbl[subcount],
        memLfTbl[subcount], SUBL, CB_NSTAGES);

      /* update memory */

      memcpy(mem, mem + SUBL, (CB_MEML - SUBL) * sizeof(float));
      memcpy(mem + CB_MEML - SUBL,
        &decresidual[(start + 1 + subframe) * SUBL],
        SUBL * sizeof(float));
      memset(weightState, 0, LPC_FILTERORDER * sizeof(float));

      subcount++;
    }
  }


  /* backward prediction of sub-frames */

  Nback = start - 1;


  if (Nback > 0) {

    /* create reverse order vectors */

    for (n = 0; n < Nback; n++) {
      for (k = 0; k < SUBL; k++) {
        reverseResidual[n * SUBL + k] =
          residual[(start - 1) * SUBL - 1 - n * SUBL - k];
        reverseDecresidual[n * SUBL + k] =
          decresidual[(start - 1) * SUBL - 1 - n * SUBL - k];
      }
    }

    /* setup memory */

    meml_gotten = SUBL * (iLBCenc_inst->nsub + 1 - start);


    if (meml_gotten > CB_MEML) {
      meml_gotten = CB_MEML;
    }
    for (k = 0; k < meml_gotten; k++) {
      mem[CB_MEML - 1 - k] = decresidual[(start - 1) * SUBL + k];
    }
    memset(mem, 0, (CB_MEML - k) * sizeof(float));
    memset(weightState, 0, LPC_FILTERORDER * sizeof(float));

    /* loop over sub-frames to encode */

    for (subframe = 0; subframe < Nback; subframe++) {

      /* encode sub-frame */

      iCBSearch(iLBCenc_inst, cb_index + subcount * CB_NSTAGES,
        gain_index + subcount * CB_NSTAGES,
        &reverseResidual[subframe * SUBL],
        mem + CB_MEML - memLfTbl[subcount],
        memLfTbl[subcount], SUBL, CB_NSTAGES,
        &weightdenum[(start - 2 - subframe) *
          (LPC_FILTERORDER + 1)], weightState, subcount + 1);

      /* construct decoded vector */

      iCBConstruct(&reverseDecresidual[subframe * SUBL],
        cb_index + subcount * CB_NSTAGES,
        gain_index + subcount * CB_NSTAGES,
        mem + CB_MEML - memLfTbl[subcount],
        memLfTbl[subcount], SUBL, CB_NSTAGES);

      /* update memory */

      memcpy(mem, mem + SUBL, (CB_MEML - SUBL) * sizeof(float));
      memcpy(mem + CB_MEML - SUBL,
        &reverseDecresidual[subframe * SUBL], SUBL * sizeof(float));
      memset(weightState, 0, LPC_FILTERORDER * sizeof(float));

      subcount++;

    }

    /* get decoded residual from reversed vector */

    for (i = 0; i < SUBL * Nback; i++) {
      decresidual[SUBL * Nback - i - 1] = reverseDecresidual[i];
    }
  }
  /* end encoding part */

  /* adjust index */
  index_conv_enc(cb_index);

  /* pack bytes */

  pbytes = bytes;
  pos = 0;

  /* loop over the 3 ULP classes */

  for (ulp = 0; ulp < 3; ulp++) {






    /* LSF */
    for (k = 0; k < LSF_NSPLIT * iLBCenc_inst->lpc_n; k++) {
      packsplit(&lsf_i[k], &firstpart, &lsf_i[k],
        iLBCenc_inst->ULP_inst->lsf_bits[k][ulp],
        iLBCenc_inst->ULP_inst->lsf_bits[k][ulp] +
        iLBCenc_inst->ULP_inst->lsf_bits[k][ulp + 1] +
        iLBCenc_inst->ULP_inst->lsf_bits[k][ulp + 2]);
      dopack(&pbytes, firstpart,
        iLBCenc_inst->ULP_inst->lsf_bits[k][ulp], &pos);
    }

    /* Start block info */

    packsplit(&start, &firstpart, &start,
      iLBCenc_inst->ULP_inst->start_bits[ulp],
      iLBCenc_inst->ULP_inst->start_bits[ulp] +
      iLBCenc_inst->ULP_inst->start_bits[ulp + 1] +
      iLBCenc_inst->ULP_inst->start_bits[ulp + 2]);
    dopack(&pbytes, firstpart,
      iLBCenc_inst->ULP_inst->start_bits[ulp], &pos);

    packsplit(&state_first, &firstpart, &state_first,
      iLBCenc_inst->ULP_inst->startfirst_bits[ulp],
      iLBCenc_inst->ULP_inst->startfirst_bits[ulp] +
      iLBCenc_inst->ULP_inst->startfirst_bits[ulp + 1] +
      iLBCenc_inst->ULP_inst->startfirst_bits[ulp + 2]);
    dopack(&pbytes, firstpart,
      iLBCenc_inst->ULP_inst->startfirst_bits[ulp], &pos);

    packsplit(&idxForMax, &firstpart, &idxForMax,
      iLBCenc_inst->ULP_inst->scale_bits[ulp],
      iLBCenc_inst->ULP_inst->scale_bits[ulp] +
      iLBCenc_inst->ULP_inst->scale_bits[ulp + 1] +
      iLBCenc_inst->ULP_inst->scale_bits[ulp + 2]);
    dopack(&pbytes, firstpart,
      iLBCenc_inst->ULP_inst->scale_bits[ulp], &pos);

    for (k = 0; k < iLBCenc_inst->state_short_len; k++) {
      packsplit(idxVec + k, &firstpart, idxVec + k,
        iLBCenc_inst->ULP_inst->state_bits[ulp],
        iLBCenc_inst->ULP_inst->state_bits[ulp] +
        iLBCenc_inst->ULP_inst->state_bits[ulp + 1] +
        iLBCenc_inst->ULP_inst->state_bits[ulp + 2]);
      dopack(&pbytes, firstpart,
        iLBCenc_inst->ULP_inst->state_bits[ulp], &pos);
    }






    /* 23/22 (20ms/30ms) sample block */

    for (k = 0; k < CB_NSTAGES; k++) {
      packsplit(extra_cb_index + k, &firstpart,
        extra_cb_index + k,
        iLBCenc_inst->ULP_inst->extra_cb_index[k][ulp],
        iLBCenc_inst->ULP_inst->extra_cb_index[k][ulp] +
        iLBCenc_inst->ULP_inst->extra_cb_index[k][ulp + 1] +
        iLBCenc_inst->ULP_inst->extra_cb_index[k][ulp + 2]);
      dopack(&pbytes, firstpart,
        iLBCenc_inst->ULP_inst->extra_cb_index[k][ulp], &pos);
    }

    for (k = 0; k < CB_NSTAGES; k++) {
      packsplit(extra_gain_index + k, &firstpart,
        extra_gain_index + k,
        iLBCenc_inst->ULP_inst->extra_cb_gain[k][ulp],
        iLBCenc_inst->ULP_inst->extra_cb_gain[k][ulp] +
        iLBCenc_inst->ULP_inst->extra_cb_gain[k][ulp + 1] +
        iLBCenc_inst->ULP_inst->extra_cb_gain[k][ulp + 2]);
      dopack(&pbytes, firstpart,
        iLBCenc_inst->ULP_inst->extra_cb_gain[k][ulp], &pos);
    }

    /* The two/four (20ms/30ms) 40 sample sub-blocks */

    for (i = 0; i < iLBCenc_inst->nasub; i++) {
      for (k = 0; k < CB_NSTAGES; k++) {
        packsplit(cb_index + i * CB_NSTAGES + k, &firstpart,
          cb_index + i * CB_NSTAGES + k,
          iLBCenc_inst->ULP_inst->cb_index[i][k][ulp],
          iLBCenc_inst->ULP_inst->cb_index[i][k][ulp] +
          iLBCenc_inst->ULP_inst->cb_index[i][k][ulp + 1] +
          iLBCenc_inst->ULP_inst->cb_index[i][k][ulp + 2]);
        dopack(&pbytes, firstpart,
          iLBCenc_inst->ULP_inst->cb_index[i][k][ulp], &pos);
      }
    }

    for (i = 0; i < iLBCenc_inst->nasub; i++) {
      for (k = 0; k < CB_NSTAGES; k++) {
        packsplit(gain_index + i * CB_NSTAGES + k, &firstpart,
          gain_index + i * CB_NSTAGES + k,
          iLBCenc_inst->ULP_inst->cb_gain[i][k][ulp],
          iLBCenc_inst->ULP_inst->cb_gain[i][k][ulp] +
          iLBCenc_inst->ULP_inst->cb_gain[i][k][ulp + 1] +
          iLBCenc_inst->ULP_inst->cb_gain[i][k][ulp + 2]);
        dopack(&pbytes, firstpart,
          iLBCenc_inst->ULP_inst->cb_gain[i][k][ulp], &pos);
      }
    }
  }

  /* set the last bit to zero (otherwise the decoder
     will treat it as a lost frame) */
  dopack(&pbytes, 0, 1, &pos);
}