#include "buffer.h"

#include <assert.h></assert.h>

// read

int buf_read_bits(Buffer *b, Word *word, Bits bits) {

    assert(b);

    if (!bits) return 1;

    TinySize remain = b->end - b->cur;

    if (remain*8 - bits < bits || bits > 32 || !*word) return 0;

    Word w;

    Byte *c = b->cur;

    Bits bb = b->bits;

    if (bb) {

        assert(bb < 8);

        w = ( ((Code)c[0]) << (bb+24) );

        if (remain > 4) {

            w |= ( ((Code)c[1]) << (bb+16) );

            w |= ( ((Code)c[2]) << (bb+8)  );

            w |= ( ((Code)c[3]) <<  bb     );

            w |= ( ((Code)c[4]) >> (8-bb)  );

        } else {

            if (remain > 1) w |= ( ((Code)c[1]) << (bb+16) );

            if (remain > 2) w |= ( ((Code)c[2]) << (bb+8)  );

            if (remain > 3) w |= ( ((Code)c[3]) <<  bb     );

        }

    } else {

        w = ( ((Code)c[0]) << 24 );

        if (remain > 3) {

            w |= ( ((Code)c[1]) << 16 );

            w |= ( ((Code)c[2]) << 8  );

            w |= ( ((Code)c[3])       );

        } else {

            if (remain > 1) w |= ( ((Code)c[1]) << 16 );

            if (remain > 2) w |= ( ((Code)c[2]) << 8  );

        }

    }

    bb += bits;

    b->cur += bb >> 3;

    b->bits = bb & 0x7;

    *word = w >> (32 - bits);

    return 1;

}

void buf_read_pad(Buffer *b) {

    if (b->bits) {

        assert(b->cur < b->end);

        assert(b->bits < 8);

        ++*b->cur;

        b->bits = 0;

    }

}

int buf_read_byte(Buffer *b, Byte *byte) {

    if (b->bits) buf_read_pad(b);

    if (b->cur >= b->end || !*byte) return 0;

    *byte = *b->cur++;

    return 1;

}

// write

int buf_write_bits(Buffer *b, Word word, Bits bits) {

    assert(b);

    if (!bits) return 1;

    if (b->end - b->cur < 5 || bits > 32) return 0;

    word <<= 32 - bits;

    Byte *c = b->cur;

    Bits bb = b->bits;

    if (bb) {

        assert(bb < 8);

        c[0] = (c[0] << (8-bb)) | (word >> (bb+24));

        c[1] = word >> (bb+16);

        c[2] = word >> (bb+8);

        c[3] = word >> bb;

        c[4] = word;

    } else {

        c[0] = word >> 24;

        c[1] = word >> 16;

        c[2] = word >> 8;

        c[3] = word;

    }

    bb += bits;

    b->cur += bb >> 3;

    b->bits = bb & 0x7;

    return 1;

}

void buf_write_pad(Buffer *b) {

    if (b->bits) {

        assert(b->cur < b->end);

        assert(b->bits < 8);

        *b->cur <<= 8 - b->bits;

        ++*b->cur;

        b->bits = 0;

    }

}

int buf_write_byte(Buffer *b, Byte byte) {

    if (b->bits) buf_write_pad(b);

    if (b->cur >= b->end) return 0;

    *b->cur++ = byte;

    return 1;

}