/****************************************************************************
* $Id: bitstm.c,v 1.1 2006-01-17 14:27:51+01 rolf Exp rolf $
*****************************************************************************/
#include <string.h>
#include "bitstm.h"

BitStream BS;

/* Setup bitstream memory pointer, size in bytes */
void bsInit (void *buf, size_t size)
{
	BS.bufPtr  = buf;
	BS.bufSize = size / bsCellSize;	/* Buffer size in CELLS ! */
	bsReset ();
	return;
}

/* Reset pointer to 1st Bit */
void bsReset (void)
{
	BS.curPtr = BS.bufPtr;		/* Pointer to first CELL */
	BS.curBit = bsMSB;			/* 1st Bit = MSB */
}

/* TRUE: End of bitstream */
int bsEnd (void)
{
	return (   (BS.curBit == bsMSB)
		&& ((BS.curPtr - BS.bufPtr) >= (signed)BS.bufSize) );	/* Offset compared in CELLS */
}

/* Seek bit position */
int bsSeek (unsigned n)
{
	if (n > (bsCellBits * BS.bufSize))
		return 0;
	BS.curPtr = BS.bufPtr + (n / bsCellBits);	/* CELL position */
	BS.curBit = bsMSB  >> (n % bsCellBits);		/* Bit position in CELL */
	return 1;
}

/* Retrieve current bit position */
unsigned bsPos (void)
{
	unsigned size = (BS.curPtr - BS.bufPtr) * bsCellBits;
	unsigned mask = BS.curBit;

	while( mask < bsMSB ) {	/* Count number of written bits in current byte */
		size++;
		mask <<= 1;
	}
	return size;
}


/* Clear BitStream from current position (0) */
void bsClear (void)
{
	bsCell x = BS.curBit;		/* 00010000 */
	int  n;

	if (bsEnd())				/* If end-of-buffer, do nothing */
		return;

	*BS.curPtr &= x ^(1+~x);	/* 11100000 */				/* clear last bits in current word */
	if ((n = BS.bufPtr + BS.bufSize - BS.curPtr - 1) > 0)	/* number of words (after curr. word) to clear */
		memset (BS.curPtr+1, 0, n * sizeof(bsCell));		/* clear last words */
}

/* internal: increment pointer/mask by 1 Bit */
void bsNextBit (void)
{
	if ((BS.curBit >>= 1) == 0) {	/* set mask to next bit, if end-of-CELL */
		BS.curPtr++;				/*   Skip to next word */
		BS.curBit  = bsMSB;			/*   Next bit is MSB */
    }
}

/* Write Bit=0 (w==0) or Bit=1 (w!=0) to BS */
int bsPutBit (bsWord  w)
{
	if (bsEnd())					/* Error if end-of-buffer */
		return 0;
	if (w)							/* set bit=1 ? */
		*(BS.curPtr) |= BS.curBit;
	else							/* reset bit=0 ? */
		*(BS.curPtr) &= ~BS.curBit;
	bsNextBit();                    /* Skip to next bit */
	return 1;
}

/* Write multiple bits, Mask points to the 1st Bit (MSB) */
int bsPutWord (bsWord  w, bsWord mask)
{
	while (mask != 0) {
		if (! bsPutBit( (bsWord)(w & mask) ) )
			return 0;
		(mask >>= 1);
    }
	return 1;
}

/* Read one Bit from BS, mask it to word w */
/* Without initializing *w !!! */
int bsGetBit(bsWord *w, bsWord mask)
{
	if( bsEnd() )						/* Error if end-of-buffer */
		return 0;
	if( BS.curBit & *(BS.curPtr) )		/* if current bit=1 ? */
		*w |= mask;                     /*   set it in w */
	else								/* if current bit=0 */
		*w &= ~mask;                    /*   reset it in w */
	bsNextBit();                    /* Skip to next bit */
	return 1;
}

/* Read multiple bits, Mask points to the 1st Bit (MSB) */
/* With initializing *w !!! */
int bsGetWord( bsWord *w, bsWord mask)
{
	*w = 0;
	while( mask != 0 ) {
		if( !bsGetBit( w, mask ) )
			return 1;
		mask >>= 1;
	}
	return 1;
}
