#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);
}