/*
 * bits_util.cpp
 *
 *  Created on: 18 сент. 2017 г.
 *      Author: alexrayne
 */

#include <assert.h>


#include <bits_util.hpp>
uint8_t bits_reverse(uint8_t x){
    uint8_t tmp = ((x & 0x55)<<1) | ((x>>1)& 0x55);
    return bits2_reverse(tmp);
}

uint16_t bits_reversew(uint16_t x){
    return (bits_reverse(x) <<8) | bits_reverse(x>>8);
}

//* swap bit pairs in byte
uint8_t bits2_reverse(uint8_t x){
    uint8_t tmp =  ((x & 0x33)<<2) | ((x>>2)& 0x33);
    x = tmp;
    tmp =  ((x & 0xf)<<4) | ((x>>4)& 0xf);
    return tmp;
}

uint16_t bits2byte_reversew(uint16_t x){
    uint16_t tmp =  ((x & 0x3333)<<2) | ((x>>2)& 0x3333);
    x = tmp;
    tmp =  ((x & 0x0f0f)<<4) | ((x>>4)& 0x0f0f);
    return tmp;
}

//* \return - expands bits of x to 2bits groups
uint16_t bits2byte_expand(uint8_t x){
    uint16_t tmp = (x | (x <<4)) & 0xf0f;
    uint16_t t = tmp;
    tmp = (t | (t<<2)) & 0x3333;
    t = tmp;
    tmp = (t | (t<<1)) & 0x5555;
    return tmp;
}

void str_rshift(void* dst, const void* src, unsigned size, unsigned shift){
    uint8_t* pd = (uint8_t*)dst;
    const uint8_t* ps = (const uint8_t*)src;
    ps += shift / 8;
    shift = shift % 8;

    unsigned c = *ps++;
    for (; size > 0; size--){
        c |= (*ps++) << 8;
        *pd++ = (c >> shift) ;
        c = (c >> 8);
    }
}

#pragma pack(push, 1)
union long_bytes{
    unsigned    u;
    uint8_t     b[sizeof(unsigned)];
};
#pragma pack(pop)
typedef union long_bytes long_bytes;

//static
void str_scale_bits(void* dst, unsigned shiftd
                             , const void* src, unsigned shifts, unsigned bits
                             , unsigned scale)
{
    assert(scale > 0);
    uint8_t*        db = (uint8_t*)dst;
    const uint8_t*  sb = (const uint8_t*)src;
    const long_bytes* sl = (const long_bytes*)sb;
    unsigned sdata  = sl->u >>shifts;
    const unsigned ubits = sizeof(unsigned)*8;
    unsigned dmask = ((1 << scale)-1);
    for(; bits > 0; bits--, shifts++, sdata = sdata >>1){
        if (shifts >= ubits){
            sl++;
            shifts = 0;
            sdata  = sl->u;
        }
        unsigned sbit = sdata&1;
        if (sbit != 0){
            unsigned dbits = dmask << shiftd;
            long_bytes* dl = (long_bytes*)db;
            dl->u |= dbits;
        }
        shiftd  += scale;
        db     += shiftd / 8;
        shiftd  = shiftd %8;
    }
}
