#include "util.h"

namespace utils {

    void Util::stringUpper(string& string) {
        transform(string.begin(), string.end(), string.begin(), toupper);
    }

    string Util::int2str(int n) {
        std::stringstream ss;

        ss << n;
        return ss.str();
    }

    int Util::stringtoint(const char* string) {
        return atoi(string);
    }

     /* 
         -- get bits out of buffer (max 32 bit!!!)
         -- return: value
     */

    unsigned long long Util::getBits(unsigned char* buf, int byte_offset, int startbit, int bitlen) {
        unsigned char *b;
        unsigned long v;
        unsigned long mask;
        unsigned long tmp_long;
        int bitHigh;


        b = &buf[byte_offset + (startbit >> 3)];
        startbit %= 8;

        switch ((bitlen - 1) >> 3) {
            case -1 : // -- <=0 bits: always 0
                return 0L;
                break;

            case 0: // -- 1..8 bit
                tmp_long = (unsigned long) (
                        (*(b) << 8) + *(b + 1));
                bitHigh = 16;
                break;

            case 1: // -- 9..16 bit
                tmp_long = (unsigned long) (
                        (*(b) << 16) + (*(b + 1) << 8) + *(b + 2));
                bitHigh = 24;
                break;

            case 2: // -- 17..24 bit
                tmp_long = (unsigned long) (
                        (*(b) << 24) + (*(b + 1) << 16) +
                        (*(b + 2) << 8) + *(b + 3));
                bitHigh = 32;
                break;

            case 3: // -- 25..32 bit
                // -- to be safe, we need 32+8 bit as shift range 
                return (unsigned long) Util::getBits48(b, 0, startbit, bitlen);
                break;

            default: // -- 33.. bits: fail, deliver constant fail value
                //printf("\nGET BITS: OUT OF BOUNDS\n");
                return getBits48(buf, byte_offset, startbit, bitlen);
                break;
        }

        startbit = bitHigh - startbit - bitlen;
        tmp_long = tmp_long >> startbit;
        mask = (1ULL << bitlen) - 1; // 1ULL !!!
        v = tmp_long & mask;

        return v;
    }

    /*
      -- get bits out of buffer  (max 48 bit)
      -- extended bitrange, so it's slower
      -- return: value
     */

    long long Util::getBits48(unsigned char *buf, int byte_offset, int startbit, int bitlen) {
        unsigned char *b;
        unsigned long long v;
        unsigned long long mask;
        unsigned long long tmp;

        if (bitlen > 48) {
            printf("\ngetBits48() OUT OF BOUNDS\n");
            return getBits64(buf, byte_offset, startbit, bitlen);
        }


        b = &buf[byte_offset + (startbit / 8)];
        startbit %= 8;


        // -- safe is 48 bitlen
        tmp = (unsigned long long) (
                ((unsigned long long) *(b) << 48) + ((unsigned long long) *(b + 1) << 40) +
                ((unsigned long long) *(b + 2) << 32) + ((unsigned long long) *(b + 3) << 24) +
                (*(b + 4) << 16) + (*(b + 5) << 8) + *(b + 6));

        startbit = 56 - startbit - bitlen;
        tmp = tmp >> startbit;
        mask = (1ULL << bitlen) - 1; // 1ULL !!!
        v = tmp & mask;

        return v;
    }

    /*
      -- get bits out of buffer   (max 64 bit)
      -- extended bitrange, so it's slower 
      -- return: value
     */

    unsigned long long Util::getBits64(unsigned char *buf, int byte_offset, int startbit, int bitlen) {
        unsigned long long x1, x2, x3;

        if (bitlen <= 32) {
            x3 = Util::getBits(buf, byte_offset, startbit, bitlen);
        } else {
            x1 = Util::getBits(buf, byte_offset, startbit, 32);
            x2 = Util::getBits(buf, byte_offset, startbit + 32, bitlen - 32);
            x3 = (x1 << (bitlen - 32)) + x2;
        }
        return x3;
    }

}