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

       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 (short)(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);
   }