#include <iostream>
#include <cmath>
#include <vector>

using namespace std;
using Blob = vector<uint8_t>;

// http://stackoverflow.com/questions/29547628/convert-uint8-t-hex-value-to-binary
void toBinary(uint8_t a) {
	int tmp = 0;
    for(uint8_t i=0x80;i!=0;i>>=1) {
    	if ( tmp++ % 4 == 0 ) 
    		cout << " ";

        cout << ((a&i)?'1':'0');
    }
}

void toBinary(Blob p) {
	for (size_t i = 0; i < p.size(); i++ )
		toBinary(p[i]);
}

// Unexpect behavior if a prefix without padding is given.
// No way to check if there is a prefix or if there is not
size_t realPrefix(Blob p) {
	for ( size_t i = p.size() - 1; i >= 0; i-- ) { 
		uint8_t j = 0x01;
		for ( size_t pos = 0; pos < 8; pos++, j <<= 1 )
			if ( (p[i] & j) != 0x00 )
				return pos + ( p.size() - 1 - i) * 8;
	}
	
	// Should never append
	return p.size() * 8;
}

int main() {
	
	Blob p; // Test Blob	
	p.push_back(0xBA); // 1011 1010
	p.push_back(0x80); // 1000 0000 -> Padding
	Blob out(2 * p.size()); // ceil(p.size_ * 2. / 8.);

    cout << "Prefix with unknown value :" ;
    toBinary(p);

	for ( size_t i = 0; i < p.size(); i++ ) {
    	for ( int b = 0, shift = 0; b < 8; b++, shift += 2) {
			if ( shift >= 8 ) 
				shift = 0;
			
			/* 1 -> 11 [0xC0], 0 -> 10 [0x80], shift in right place */
    		uint8_t mout = (( p[i] & (0x80 >> b ) ) ? 0xC0 : 0x80) >> shift;
			out[i * 2 + (b >= 4)] |= mout; 
    	}
	}

	cout << endl << "Output : " ;
    toBinary(out);
    cout << endl;
    
    exit(0);
}