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

    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 "iLBC_encode.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 */

            memmove(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 */

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



