/*
 * BitOps.c
 *
 *  Created on: Apr 13, 2021
 *      Author: zemon
 */
#include "BitBuffer.h"


void vBitBufferAppendDataByteBitsCountWithBitsOffset(
        uint8_t *destinationBitSequence,
        uint16_t offsetOfFirstBit,
        uint8_t appendableByte,
        uint8_t countOfAppendableBits
) {

    uint16_t fullBytesOffset = offsetOfFirstBit / 8;
    uint8_t bitsOffsetInFirstByte = offsetOfFirstBit % 8;
    uint8_t bitsWritedToSecondByte = bitsOffsetInFirstByte + countOfAppendableBits - 8;

    uint8_t firstByteCut = appendableByte << (8 - countOfAppendableBits);

    destinationBitSequence[fullBytesOffset] |= firstByteCut >> bitsOffsetInFirstByte;

    if (bitsWritedToSecondByte > 0) {
        destinationBitSequence[fullBytesOffset + 1] |= appendableByte << (8 - bitsWritedToSecondByte);
    }
}

uint8_t vBitBufferAppendByte(
        uint8_t *destinationBitSequence,
        uint16_t offsetOfFirstBit,
        uint8_t appendableByte,
        uint8_t countOfAppendableBits
) {
    vBitBufferAppendDataByteBitsCountWithBitsOffset(
            destinationBitSequence,
            offsetOfFirstBit,
            appendableByte,
            countOfAppendableBits
    );
    return countOfAppendableBits;
}


uint8_t vBitBufferAppendBits(
        uint8_t *destinationBitSequence,
        uint16_t offsetOfFirstBit,
        uint8_t *appendableData,
        uint8_t countOfAppendableBits
) {

    uint16_t fullBytes = countOfAppendableBits / 8;
    uint16_t notFullByteBits = countOfAppendableBits % 8;

    uint16_t i = 0;
    for (; i < fullBytes; ++i) {
        vBitBufferAppendDataByteBitsCountWithBitsOffset(
                destinationBitSequence,
                offsetOfFirstBit,
                appendableData[i], 8
        );
        offsetOfFirstBit += 8;
    }

    if (notFullByteBits > 0) {
        vBitBufferAppendDataByteBitsCountWithBitsOffset(
                destinationBitSequence,
                offsetOfFirstBit,
                appendableData[i],
                notFullByteBits
        );
    }
    return countOfAppendableBits;
}

uint8_t vBitBufferAppendBitsReverseBytes(
        uint8_t *destinationBitSequence,
        uint16_t offsetOfFirstBit,
        uint8_t *appendableData,
        uint8_t countOfAppendableBits
) {

    uint16_t fullBytes = countOfAppendableBits / 8;
    uint16_t notFullByteBits = countOfAppendableBits % 8;

    uint16_t totalBytes = fullBytes + (notFullByteBits > 0 ? 1 : 0);

    uint16_t i = 0;
    for (; i < fullBytes; ++i) {
        vBitBufferAppendDataByteBitsCountWithBitsOffset(
                destinationBitSequence,
                offsetOfFirstBit,
                appendableData[(totalBytes - 1) - i],
                8
        );
        offsetOfFirstBit += 8;
    }

    if (notFullByteBits > 0) {
        vBitBufferAppendDataByteBitsCountWithBitsOffset(
                destinationBitSequence,
                offsetOfFirstBit,
                appendableData[(totalBytes - 1) - i],
                notFullByteBits
        );
    }
    return countOfAppendableBits;
}

uint8_t vBitBufferAppendBytesReverse(
        uint8_t *destinationBitSequence,
        uint16_t offsetOfFirstBit,
        uint8_t *appendableData,
        uint8_t countOfAppendableBytes
) {
    for (int i = countOfAppendableBytes - 1; i > -1; --i) {
        vBitBufferAppendDataByteBitsCountWithBitsOffset(
                destinationBitSequence,
                offsetOfFirstBit,
                appendableData[i], 8
        );
        offsetOfFirstBit += 8;
    }

    return countOfAppendableBytes * 8;
}