#include "DST_BitBuffer.h"

#define DHL_FAIL_OUT_OF_RESOURCE 0x0A

struct bitBuffer {
	DS_U8*	buffer;
	DS_U32	bufSize;
	DS_U32	bitOffset;
	bool	overrunError;
}bitBuffer;

/******************************************************************************
 * Local variables declaration
 ******************************************************************************/
static const DS_U32 bitMaskTable[] = {
	0x00000000,0x00000001,0x00000003,0x00000007,0x0000000F,0x0000001F,0x0000003F,0x0000007F,
	0x000000FF,0x000001FF,0x000003FF,0x000007FF,0x00000FFF,0x00001FFF,0x00003FFF,0x00007FFF,
	0x0000FFFF,0x0001FFFF,0x0003FFFF,0x0007FFFF,0x000FFFFF,0x001FFFFF,0x003FFFFF,0x007FFFFF,
	0x00FFFFFF,0x01FFFFFF,0x03FFFFFF,0x07FFFFFF,0x0FFFFFFF,0x1FFFFFFF,0x3FFFFFFF,0x7FFFFFFF,
	0xFFFFFFFF
};
static const DS_U8 shiftTable1[] = {8,1,2,3,4,5,6,7};
static const DS_U8 shiftTable2[] = {0,7,6,5,4,3,2,1};

/******************************************************************************
 * Local function prototypes
 ******************************************************************************/


/*=========================================================================
ErrCode bitBufferCreate (bitBufferPtr_t *instance, DS_U8 *buffer, DS_U32 bufSize)

	*instance:	bitBuffer instance passed back to caller.
	*buffer:	pointer to a byte buffer.
	bufSize:	size of the buffer (in bytes).

Creates a bitBuffer.  After creation, the bitBufferGetBits() function can
be called to extract bits.
=========================================================================*/
int bitBufferCreate (bitBufferPtr_t *instance, DS_U8 *buffer, DS_U32 bufSize)
{
	bitBufferPtr_t	bitBufferPtr;
	

	/* create bitBuffer structure */
	bitBufferPtr = (bitBufferPtr_t)DST_OS_MallocDirect(sizeof(bitBuffer_t));
	if (bitBufferPtr == NULL) {
		return DHL_FAIL_OUT_OF_RESOURCE;
	}

	memset( bitBufferPtr , 0 , sizeof(bitBuffer_t) );
	
	/* Initialize */
	bitBufferPtr->buffer = buffer;
	bitBufferPtr->bufSize = bufSize;
	bitBufferPtr->bitOffset = 0;

	/* pass instance to user */
	*instance = bitBufferPtr;

	return (0);
}


/*=========================================================================
int bitBufferDestroy (bitBufferPtr_t instance)

	instance:	bitBuffer instance.

Deletes the bitBuffer.  DOES NOT free the buffer passed by the user in the
function bitBufferCreate()!!
=========================================================================*/
int bitBufferDestroy (bitBufferPtr_t instance)
{
	//AtiCore_MemFree(instance);
	DST_OS_FreeDirect(instance);
	return (0);
}


/*=========================================================================
DS_U32 bitBufferGetBits (bitBufferPtr_t bitBufferPtr, DS_U8 numberOfBits)

	bitBufferPtr:	bitBuffer instance.
	numberOfBits:	number of bits to be read from bitBuffer.

Extracts the specified number of bits from the bitBuffer.  The number of
bits must be less than or equal to 32.  If more bits are requested than
are present in the bitBuffer, the overrunError flag is set.
=========================================================================*/
DS_U32 bitBufferGetBits (bitBufferPtr_t bitBufferPtr, DS_U8 numberOfBits)
{
	DS_U32			returnBits = 0;
	DS_U8			bitIndex;
	DS_U8			newBitIndexDiv8,newBitIndexMod8;
	DS_U8			readByteCount;
	DS_U8			*p;
	DS_U8			i;


	if (numberOfBits > (8*bitBufferPtr->bufSize - bitBufferPtr->bitOffset)) {
		
		bitBufferPtr->overrunError = true;
		bitBufferPtr->bitOffset = 8*bitBufferPtr->bufSize;
		returnBits = 0;
		goto GetBitsReturn;
	}

	bitIndex = bitBufferPtr->bitOffset & 0x07;
	newBitIndexDiv8 = (bitIndex+numberOfBits) >> 3;
	newBitIndexMod8 = (bitIndex+numberOfBits) & 0x07;
	readByteCount = newBitIndexDiv8 + (newBitIndexMod8 ? 1:0);

	p = bitBufferPtr->buffer + (bitBufferPtr->bitOffset >> 3);
	
	/* copy first byte */
	returnBits = *p++;
	if (readByteCount < 2) {
		/* shift bits right */
		returnBits = returnBits >> shiftTable2[newBitIndexMod8];
	}
	else { /* more bytes */
		for (i=0; i < readByteCount-2; i++) {
			/* just copying 'middle' bytes */
			returnBits = (returnBits << 8) | *p++;
		}		
		/* copy last byte */
		returnBits = (returnBits << shiftTable1[newBitIndexMod8]) | ((*p) >> shiftTable2[newBitIndexMod8]);
	}

	returnBits &= bitMaskTable[numberOfBits];
	bitBufferPtr->bitOffset += numberOfBits;

GetBitsReturn:
	return (returnBits);
}


/*=========================================================================
void bitBufferSkipBits (bitBufferPtr_t bitBufferPtr, DS_U16 numberOfBits)

	bitBufferPtr:	bitBuffer instance.
	numberOfBits:	number of bits to be skipped.

Skips the specified number of bits in the bitBuffer.  This operation is
faster than extracting bits and can be used when a bit field is ignored.
=========================================================================*/
void bitBufferSkipBits (bitBufferPtr_t bitBufferPtr, DS_U16 numberOfBits)
{
	bitBufferPtr->bitOffset += numberOfBits;
	if (8*bitBufferPtr->bufSize < bitBufferPtr->bitOffset) {
		bitBufferPtr->overrunError = true;
	}
	return;
}


/*=========================================================================
DS_U8* bitBufferGetBytePointer (INT32 instance)

	bitBufferPtr:		bitBuffer instance.

Returns a pointer to the current byte offset.
=========================================================================*/
DS_U8* bitBufferGetBytePointer (bitBufferPtr_t bitBufferPtr)
{
	return (bitBufferPtr->buffer + (bitBufferPtr->bitOffset >> 3));
}


/*=========================================================================
bool bitBufferCheckError (INT32 instance)

	bitBufferPtr_t:		bitBuffer instance.

Returns '_TRUE_' if the bitBuffer is in an overrun error state.
=========================================================================*/
bool bitBufferCheckError (bitBufferPtr_t bitBufferPtr)
{
	return (bitBufferPtr->overrunError);
}