#include "bitstream.h"

#define BITS_IN_BYTE 8
#define EXI_ERROR_INPUT_STREAM_EOF -10
#define EXI_ERROR_OUTPUT_STREAM_EOF -11

int bitstream_putNBits(bitstream_t* stream, int nbits, uint32_t value)
{
    int errn;
	if (nbits <= stream->xbits) {
		/* all bits fit into the current temp */
		stream->tmp = (uint8_t)(stream->tmp << (nbits)) | (uint8_t)(value & (uint32_t)(0xff >> (uint32_t)(BITS_IN_BYTE - nbits)));
		stream->xbits = stream->xbits - nbits;
		/* if the temp is full write byte */
		if (stream->xbits == 0) {

			if (stream->pos >= stream->size) {
				errn = EXI_ERROR_OUTPUT_STREAM_EOF;
			} else {
				stream->data[stream->pos++] = stream->tmp;
			}

			stream->xbits = BITS_IN_BYTE;
			stream->tmp = 0;
		}
	} else {
		/* the temp is not enough * fill the temp */
		stream->tmp = (uint8_t)(stream->tmp << stream->xbits) | ( (uint8_t)(value >> (nbits - stream->xbits)) & (uint8_t)(0xff >> (BITS_IN_BYTE - stream->xbits)) );

		nbits = (nbits - stream->xbits);

		if (stream->pos >= stream->size) {
			errn = EXI_ERROR_OUTPUT_STREAM_EOF;
		} else {
			stream->data[stream->pos++] = stream->tmp;
		}
		stream->tmp = 0;

		/* write whole bytes */
		while (errn == 0 && nbits >= BITS_IN_BYTE) {
			nbits = (nbits - BITS_IN_BYTE);
			if (stream->pos >= stream->size) {
				errn = EXI_ERROR_OUTPUT_STREAM_EOF;
			} else {
				stream->data[stream->pos++] = (uint8_t)(value >> (nbits));
			}
		}

		/* spared bits are kept in the temp */
		stream->tmp = (uint8_t)value; /* Note: the high bits will be shifted out during further filling */
		stream->xbits = (uint8_t)(BITS_IN_BYTE - (nbits));
	}

	return errn;    
}

int bitstream_getNBits(bitstream_t* stream, int nbits, uint32_t *value)
{
	int errn = 0;
	*value = 0;

	if (nbits <= 0) return errn;
	
	if(stream->xbits == 0)
	{
		if ( stream->pos < stream->size ) {
			stream->tmp = stream->data[stream->pos++];
			stream->xbits = BITS_IN_BYTE;
		} else {
			errn = EXI_ERROR_INPUT_STREAM_EOF;
		}
	}

	if (errn == 0) {
		/* read the bits in one step */
		if(nbits <= stream->xbits) {
			stream->xbits = (uint8_t)(stream->xbits - nbits);
			*value = (uint32_t)((stream->tmp >> stream->xbits) & (0xff >> (BITS_IN_BYTE - nbits)));
		} else {
			/* read bits as much as possible */
			*value = (uint32_t)(stream->tmp & (0xff >> (BITS_IN_BYTE - stream->xbits)));
			nbits = (nbits - stream->xbits);
			stream->xbits = 0;

			/* read whole bytes */
			while(errn == 0 && nbits >= 8)
			{
				errn = readBuffer(stream);
				*value = ((*value) << BITS_IN_BYTE) | stream->tmp;
				nbits = (nbits - BITS_IN_BYTE);
				stream->xbits = 0;
			}

			/* read the spare bits in the buffer */
			if(errn == 0 && nbits > 0)
			{
				errn = readBuffer(stream);
				if (errn == 0) {
					*value = ( (*value) << nbits) | (uint8_t)(stream->tmp  >> (BITS_IN_BYTE - nbits)) ;
					stream->xbits = (uint8_t)(BITS_IN_BYTE - nbits);
				}
			}
		}
	}

	return errn;
}

int bitstream_getBool(bitstream_t* stream, int *value)
{
	uint32_t out;
	int ret = bitstream_getNBits(stream, 1, &out);
	*value = (out==0x0? 0 : 1);
	return ret;
}