#include "BitStream.hpp"

namespace util {
    const uint8_t bitmasks[] = {0, 128, 192, 224, 240, 248, 252, 254};

    BitStreamReader::BitStreamReader(uint8_t *b, size_t s) : BitStream(b, s, 0, false) {}

    BitStreamReader::~BitStreamReader() {}

    void BitStreamReader::flush() {
        this->position = this->get_last_byte_position() * 8u;
    }

    uint8_t BitStreamReader::get_bit() {
        const size_t current_start_byte = this->position / 8u;

        if (current_start_byte >= this->get_size()) {
            // Prevent reading byte outside of array (-> Valgrind flagged)
            return 0u;
        }

        const size_t  bits_taken = this->position % 8;
        const uint8_t value      = this->buffer[current_start_byte];

        this->position++;

        return (value & (1 get_bit();
            value |= v position % 8 != 0) {
            this->buffer[this->position / 8] &= bitmasks[this->position % 8];
            this->position += 8 - (this->position % 8);
        }
    }

    void BitStreamWriter::put_bit(int8_t value) {
        const size_t bits_taken = this->position % 8;

        if (value) {
            this->buffer[this->position / 8] |= 1 buffer[this->position / 8] &= ~(1 position++;
    }

    void BitStreamWriter::put(size_t length, uint32_t value) {
        for (size_t p = 0; p < length; p++) {
            put_bit(1 & (value >> (length - 1 - p)));
        }
    }

    void write(FILE *f, const BitStreamWriter &b) {
        const size_t position = b.get_position();
        const uint8_t *buffer = b.get_buffer();

        fwrite(buffer, 1, position / 8, f);

        if (position % 8 != 0) {
            fwrite(buffer + position / 8, 1, 1, f);
        }
    }

    void write(std::ofstream &fs, const BitStreamWriter &b) {
        const size_t position = b.get_position();
        const uint8_t *buffer = b.get_buffer();

        fs.write(reinterpret_cast(buffer), position / 8);

        if (position % 8 != 0) {
            fs.write(reinterpret_cast(buffer + position / 8), 1);
        }
    }

}