+/* Thanks to
+ * http://stackoverflow.com/questions/3534535/whats-a-time-efficient-algorithm-to-copy-unaligned-bit-arrays
+ */
+static void bitarray_copy(const uint8_t* source_origin, int source_offset,
+ int source_length, uint8_t* destination_origin, int destination_offset) {
+ static const uint8_t reverse_mask[] =
+ { 0x55, 0x80, 0xc0, 0xe0, 0xf0, 0xf8, 0xfc, 0xfe, 0xff };
+ static const uint8_t reverse_mask_xor[] =
+ { 0xff, 0x7f, 0x3f, 0x1f, 0x0f, 0x07, 0x03, 0x01, 0x00 };
+
+ if(source_length < 1) {
+ return;
+ }
+
+ const uint8_t* source = source_origin + byteForBit(source_offset);
+ uint8_t* destination = destination_origin + byteForBit(destination_offset);
+ int source_offset_modulo = source_offset % CHAR_BIT;
+ int destination_offset_modulo = destination_offset % CHAR_BIT;
+
+ if(source_offset_modulo == destination_offset_modulo) {
+ if(source_offset_modulo > 0) {
+ uint8_t c = reverse_mask_xor[destination_offset_modulo] & *source++;
+ PREPARE_FIRST_COPY();
+ *destination++ |= c;
+ }
+
+ int byte_len = source_length / CHAR_BIT;
+ int source_length_modulo = source_length % CHAR_BIT;
+
+ if(byte_len > 0) {
+ memcpy(destination, source, byte_len);
+ source += byte_len;
+ destination += byte_len;
+ }
+
+ if(source_length_modulo > 0) {
+ *destination &= reverse_mask_xor[source_length_modulo];
+ *destination |= reverse_mask[source_length_modulo] & *source;
+ }
+ } else {
+ int bit_diff_left_shift;
+ int bit_diff_right_shift;
+ uint8_t c;
+ /*
+ * Begin: Line things up on destination.
+ */
+ if(source_offset_modulo > destination_offset_modulo) {
+ bit_diff_left_shift = source_offset_modulo - destination_offset_modulo;
+ bit_diff_right_shift = CHAR_BIT - bit_diff_left_shift;
+
+ c = *source++ << bit_diff_left_shift;
+ c |= *source >> bit_diff_right_shift;
+ c &= reverse_mask_xor[destination_offset_modulo];
+ } else {
+ bit_diff_right_shift = destination_offset_modulo - source_offset_modulo;
+ bit_diff_left_shift = CHAR_BIT - bit_diff_right_shift;
+
+ c = *source >> bit_diff_right_shift &
+ reverse_mask_xor[destination_offset_modulo];
+ }
+ PREPARE_FIRST_COPY();
+ *destination++ |= c;
+
+ /*
+ * Middle: copy with only shifting the source.
+ */
+ int byte_len = source_length / CHAR_BIT;
+ while(--byte_len >= 0) {
+ c = *source++ << bit_diff_left_shift;
+ c |= *source >> bit_diff_right_shift;
+ *destination++ = c;
+ }
+
+ /*
+ * End: copy the remaing bits;
+ */
+ int source_length_modulo = source_length % CHAR_BIT;
+ if(source_length_modulo > 0) {
+ c = *source++ << bit_diff_left_shift;
+ c |= *source >> bit_diff_right_shift;
+ c &= reverse_mask[source_length_modulo];
+
+ *destination &= reverse_mask_xor[source_length_modulo];
+ *destination |= c;
+ }
+ }