/******************************************************************
   
       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;
           }
       }
   }