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

       iLBC Speech Coder ANSI-C Source Code

       packing.c

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

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

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

   #include "iLBC_define.h"
   #include "constants.h"
   #include "helpfun.h"
   #include "string.h"

   /*----------------------------------------------------------------*
    *  splitting an integer into first most significant bits and
    *  remaining least significant bits
    *---------------------------------------------------------------*/

   void packsplit(
       int *index,                 /* (i) the value to split */
       int *firstpart,             /* (o) the value specified by most
                                          significant bits */
       int *rest,                  /* (o) the value specified by least
                                          significant bits */


       int bitno_firstpart,    /* (i) number of bits in most
                                          significant part */
       int bitno_total             /* (i) number of bits in full range
                                          of value */
   ){
       int bitno_rest = bitno_total-bitno_firstpart;

       *firstpart = *index>>(bitno_rest);
       *rest = *index-(*firstpart<<(bitno_rest));
   }

   /*----------------------------------------------------------------*
    *  combining a value corresponding to msb's with a value
    *  corresponding to lsb's
    *---------------------------------------------------------------*/

   void packcombine(
       int *index,                 /* (i/o) the msb value in the
                                          combined value out */
       int rest,                   /* (i) the lsb value */
       int bitno_rest              /* (i) the number of bits in the
                                          lsb part */
   ){
       *index = *index<<bitno_rest;
       *index += rest;
   }

   /*----------------------------------------------------------------*
    *  packing of bits into bitstream, i.e., vector of bytes
    *---------------------------------------------------------------*/

   void dopack(
       unsigned char **bitstream,  /* (i/o) on entrance pointer to
                                          place in bitstream to pack
                                          new data, on exit pointer
                                          to place in bitstream to
                                          pack future data */
       int index,                  /* (i) the value to pack */
       int bitno,                  /* (i) the number of bits that the
                                          value will fit within */
       int *pos                /* (i/o) write position in the
                                          current byte */
   ){
       int posLeft;

       /* Clear the bits before starting in a new byte */

       if ((*pos)==0) {


           **bitstream=0;
       }

       while (bitno>0) {

           /* Jump to the next byte if end of this byte is reached*/

           if (*pos==8) {
               *pos=0;
               (*bitstream)++;
               **bitstream=0;
           }

           posLeft=8-(*pos);

           /* Insert index into the bitstream */

           if (bitno <= posLeft) {
               **bitstream |= (unsigned char)(index<<(posLeft-bitno));
               *pos+=bitno;
               bitno=0;
           } else {
               **bitstream |= (unsigned char)(index>>(bitno-posLeft));

               *pos=8;
               index-=((index>>(bitno-posLeft))<<(bitno-posLeft));

               bitno-=posLeft;
           }
       }
   }

   /*----------------------------------------------------------------*
    *  unpacking of bits from bitstream, i.e., vector of bytes
    *---------------------------------------------------------------*/

   void unpack(
       unsigned char **bitstream,  /* (i/o) on entrance pointer to
                                          place in bitstream to
                                          unpack new data from, on
                                          exit pointer to place in
                                          bitstream to unpack future
                                          data from */
       int *index,                 /* (o) resulting value */
       int bitno,                  /* (i) number of bits used to
                                          represent the value */
       int *pos                /* (i/o) read position in the
                                          current byte */


   ){
       int BitsLeft;

       *index=0;

       while (bitno>0) {

           /* move forward in bitstream when the end of the
              byte is reached */

           if (*pos==8) {
               *pos=0;
               (*bitstream)++;
           }

           BitsLeft=8-(*pos);

           /* Extract bits to index */

           if (BitsLeft>=bitno) {
               *index+=((((**bitstream)<<(*pos)) & 0xFF)>>(8-bitno));

               *pos+=bitno;
               bitno=0;
           } else {

               if ((8-bitno)>0) {
                   *index+=((((**bitstream)<<(*pos)) & 0xFF)>>
                       (8-bitno));
                   *pos=8;
               } else {
                   *index+=(((int)(((**bitstream)<<(*pos)) & 0xFF))<<
                       (bitno-8));
                   *pos=8;
               }
               bitno-=BitsLeft;
           }
       }
   }