#include "grib2.h"

#include "limits.h"

int gbit(unsigned char *in,g2int *iout,g2int iskip,g2int nbyte)

{

      return gbits(in,G2_UNKNOWN_SIZE,iout,iskip,nbyte,(g2int)0,(g2int)1);

}

int gbit2(unsigned char *in,g2int in_length,g2int *iout,g2int iskip,g2int nbyte)

{

      return gbits(in,in_length,iout,iskip,nbyte,(g2int)0,(g2int)1);

}

void sbit(unsigned char *out,const g2int *in,g2int iskip,g2int nbyte)

{

      sbits(out,in,iskip,nbyte,(g2int)0,(g2int)1);

}

int gbits(unsigned char *in,g2int in_length,g2int *iout,g2int iskip,g2int nbyte,g2int nskip,

           g2int n)

/*          Get bits - unpack bits:  Extract arbitrary size values from a

/          packed bit string, right justifying each value in the unpacked

/          iout array.

/           *in    = pointer to character array input

/           *iout  = pointer to unpacked array output

/            iskip = initial number of bits to skip

/            nbyte = number of bits to take

/            nskip = additional number of bits to skip on each iteration

/            n     = number of iterations

/ v1.1

*/

{

      g2int i,tbit,bitcnt,ibit,itmp;

      g2int nbit,l_index;

      static const g2int ones[]={1,3,7,15,31,63,127,255};

//     nbit is the start position of the field in bits

      nbit = iskip;

      if( n> 0 && (nbyte + nskip > INT_MAX / n ||

                   iskip > INT_MAX - n*(nbyte + nskip)) )

          return -1;

      for (i=0;i<n;i++) {

         bitcnt = nbyte;

         l_index=nbit/8;

         ibit=nbit%8;

         nbit = nbit + nbyte + nskip;

//        first byte

         tbit= ( bitcnt < (8-ibit) ) ? bitcnt : 8-ibit;  // find min

         if( in_length != G2_UNKNOWN_SIZE && l_index >= in_length )

             return -1;

         itmp = (int)*(in+l_index) & ones[7-ibit];

         if (tbit != 8-ibit) itmp >>= (8-ibit-tbit);

         l_index++;

         bitcnt = bitcnt - tbit;

//        now transfer whole bytes

         while (bitcnt >= 8) {

            if( in_length != G2_UNKNOWN_SIZE && l_index >= in_length )

                return -1;

             itmp = (int)(((unsigned)itmp)<<8 | (int)*(in+l_index));

             bitcnt = bitcnt - 8;

             l_index++;

         }

//        get data from last byte

         if (bitcnt > 0) {

            if( in_length != G2_UNKNOWN_SIZE && l_index >= in_length )

                return -1;

             itmp = (int)( (unsigned)itmp << bitcnt ) | ( ((int)*(in+l_index) >> (8-bitcnt)) & ones[bitcnt-1] );

         }

         *(iout+i) = itmp;

      }

      return 0;

}

void sbits(unsigned char *out,const g2int *in,g2int iskip,g2int nbyte,g2int nskip,

           g2int n)

/*C          Store bits - pack bits:  Put arbitrary size values into a

/          packed bit string, taking the low order bits from each value

/          in the unpacked array.

/           *iout  = pointer to packed array output

/           *in    = pointer to unpacked array input

/            iskip = initial number of bits to skip

/            nbyte = number of bits to pack

/            nskip = additional number of bits to skip on each iteration

/            n     = number of iterations

/ v1.1

*/

{

      g2int i;

      unsigned bitcnt,tbit,ibit,itmp,imask,itmp2,itmp3;

      unsigned nbit,l_index;

      static const unsigned ones[]={1,3,7,15,31,63,127,255};

      const unsigned* u_in = (const unsigned*)in;

//     number bits from zero to ...

//     nbit is the last bit of the field to be filled

      nbit = iskip + nbyte - 1;

      for (i=0;i<n;i++) {

         itmp = *(u_in+i);

         bitcnt = nbyte;

         l_index=nbit/8;

         ibit=nbit%8;

         nbit = nbit + nbyte + nskip;

//        make byte aligned

         if (ibit != 7) {

             tbit= ( bitcnt < (ibit+1) ) ? bitcnt : ibit+1;  // find min

             imask = ones[tbit-1] << (7-ibit);

             itmp2 = (itmp << (7-ibit)) & imask;

             itmp3 = (int)*(out+l_index) & (255-imask);

             out[l_index] = (unsigned char)(itmp2 | itmp3);

             bitcnt = bitcnt - tbit;

             itmp = itmp >> tbit;

             if( bitcnt > 0 )

                l_index--;

         }

//        now byte aligned

//        do by bytes

         while (bitcnt >= 8) {

             out[l_index] = (unsigned char)(itmp & 255);

             itmp = itmp >> 8;

             bitcnt = bitcnt - 8;

             if( bitcnt > 0 )

                l_index--;

         }

//        do last byte

         if (bitcnt > 0) {

             itmp2 = itmp & ones[bitcnt-1];

             itmp3 = (int)*(out+l_index) & (255-ones[bitcnt-1]);

             out[l_index] = (unsigned char)(itmp2 | itmp3);

         }

      }

}