C++ Bitstream类

从raknet上剥下来的

比较适用于前后端通讯，可以对BitStream进行二次封装，方便使用.

BitStream.h:

#ifndef __BITSTREAM_H
#define __BITSTREAM_H

#ifdef _WIN32
#if defined (_INTEGRAL_MAX_BITS) && _INTEGRAL_MAX_BITS >= 64
typedef signed __int64 int64_t;
typedef unsigned __int64 uint64_t;
#define HAS_INT64
#endif
#else
#include <stdint.h>
#define HAS_INT64

#endif

#include <string>
#include "share.h"

// Arbitrary size, just picking something likely to be larger than most packets
#define BITSTREAM_STACK_ALLOCATION_SIZE 1024*2

/** \note If you want the default network byte stream to be
in Network Byte Order (Big Endian) then #define __BITSTREAM_BIG_END
otherwise the default is 'Little Endian'. If your CPU has the same
Byte Order as your network stream, you can cut out some overheads
using #define __BITSTREAM_NATIVE_END --- if this is defined,
the __BITSTREAM_BIG_END flag becomes ineffective.
*/

namespace dhpnet
{
/**
* This macro transforms a bit in byte 
* @param x Transform a bit to a byte 
*/
#define BITS_TO_BYTES(x) (((x)+7)>>3)

#define BYTES_TO_BITS(x) (x<<3)

/**
* @brief Packets encoding and decoding facilities 
* 
* Helper class to encode and decode packets. 
* 
*/

class BitStream
{

public:
/**
* Default Constructor 
*/
BitStream();
/**
* Preallocate some memory for the construction of the packet 
* @param initialBytesToAllocate the amount of byte to pre-allocate. 
*/
BitStream( int initialBytesToAllocate );

/**
* Initialize the BitStream object using data from the network. 
* Set _copyData to true if you want to make an internal copy of
* the data you are passing. You can then Write and do all other
* operations Set it to false if you want to just use a pointer to
* the data you are passing, in order to save memory and speed.
* You should only then do read operations.
* @param _data An array of bytes.
* @param lengthInBytes Size of the @em _data.
* @param _copyData Does a copy of the input data. 
*/
BitStream( unsigned char* _data, unsigned int lengthInBytes, bool _copyData );
//
BitStream( unsigned char* _data, unsigned int lengthInBytes, unsigned int datasize);
/**
* Destructor 
*/
~BitStream();
/**
* Reset the bitstream for reuse
*/
void Reset( void );
void SetBuffer(char* _data, unsigned int lengthInBytes, unsigned int datasize);
void ClearBuffer();
//

/**
* Write the native types to the end of the buffer
* without any compression mecanism. 
* @param input The data 
*/
void WriteBool( const bool input );
/**
* Write the native types to the end of the buffer
* without any compression mecanism. 
* @param input The data 
*/
void WriteUInt8( const uint8 input );
//

/**
* Write the native types to the end of the buffer
* without any compression mecanism. 
* @param input The data 
*/
void WriteInt8( const int8 input );
/**
* Write the native types to the end of the buffer
* without any compression mecanism. 
* @param input The data 
*/
void WriteUInt16( const uint16 input );
/**
* Write the native types to the end of the buffer
* without any compression mecanism. 
* @param input The data 
*/
void WriteInt16( const int16 input );
/**
* Write the native types to the end of the buffer
* without any compression mecanism. 
* @param input The data 
*/
void WriteUInt32( const uint32 input );
/**
* Write the native types to the end of the buffer
* without any compression mecanism. 
* @param input The data 
*/
void WriteInt32( const int32 input );

#ifdef HAS_INT64
/**
* Write the native types to the end of the buffer
* without any compression mecanism. 
* @param input The data 
*/
void WriteUInt64( const uint64 input );
/**
* Write the native types to the end of the buffer
* without any compression mecanism. 
* @param input The data 
*/
void WriteInt64( const int64 input );
#endif

/**
* Write the native types to the end of the buffer
* without any compression mecanism. 
* @param input The data 
*/
void WriteFloat( const float input );
/**
* Write the native types to the end of the buffer
* without any compression mechanism. 
* @param input The data 
*/
void WriteDouble( const double input );
/**
* Write an array or casted stream. It is supposed to
* be raw data. It is also not possible to deal with endian problem 
* @param input a byte buffer 
* @param numberOfBytes the size of the byte buffer 
*/
void WriteBytes( const char* input, const int numberOfBytes );
/**
* write multi bytes string
* @param input
*/
void WriteStr(char input[]);
/**
* write standard string
* @param input
*/
void WriteStr( const std::string& input );
/**
* Copy from another bitstream
* @bitStream the bitstream to copy from
*/
void WriteBS( const BitStream *bitStream );

/**
* Write the native types with simple compression.
* Best used with negatives and positives close to 0
* @param input The data.
*/
void WriteCompUInt8( const uint8 input );
/**
* Write the native types with simple compression.
* Best used with negatives and positives close to 0
* @param input The data.
*/
void WriteCompInt8( const int8 input );
/**
* Write the native types with simple compression.
* Best used with negatives and positives close to 0
* @param input The data.
*/
void WriteCompUInt16( const uint16 input );
/**
* Write the native types with simple compression.
* Best used with negatives and positives close to 0
* @param input The data.
*/
void WriteCompInt16( const int16 input );
/**
* Write the native types with simple compression.
* Best used with negatives and positives close to 0
* @param input The data.
*/
void WriteCompUInt32( const uint32 input );
/**
* Write the native types with simple compression.
* Best used with negatives and positives close to 0
* @param input The data.
*/
void WriteCompInt32( const int32 input );

#ifdef HAS_INT64
/**
* Write the native types with simple compression.
* Best used with negatives and positives close to 0
* @param input The data.
*/
void WriteCompUInt64( const uint64 input );
/**
* Write the native types with simple compression.
* Best used with negatives and positives close to 0
* @param input The data.
*/
void WriteCompInt64( const int64 input );
#endif
/**
* Write the native types with simple compression.
* Best used with negatives and positives close to 0
* @param input The data.
*/
void WriteCompFloat( const float input );

/**
* Write the native types with simple compression.
* Best used with negatives and positives close to 0
* @param input The data.
*/
void WriteCompDouble( const double input );

/**
* Write a normalized 3D vector, using (at most) 4 bytes + 3 bits instead of 12 bytes. Will further compress y or z axis aligned vectors.
* Accurate to 1/32767.5.
* @param x x
* @param y y
* @param z z
*/
void WriteNormVector( float x, float y, float z );

/**
* Write a vector, using 10 bytes instead of 12.
* Loses accuracy to about 3/10ths and only saves 2 bytes, so only use if accuracy is not important.
* @param x x
* @param y y
* @param z z
*/
void WriteVector( float x, float y, float z );

/**
* Write a normalized quaternion in 6 bytes + 4 bits instead of 16 bytes. Slightly lossy.
* @param w w
* @param x x
* @param y y
* @param z z
*/
void WriteNormQuat( float w, float x, float y, float z);

/**
* Write an orthogonal matrix by creating a quaternion, and writing 3 components of the quaternion in 2 bytes each
* for 6 bytes instead of 36
*/
void WriteOrthMatrix( 
float m00, float m01, float m02,
float m10, float m11, float m12,
float m20, float m21, float m22 );
/**
* Read the native types from the front of the buffer
* @param output The readed value. 
* @return true on success false otherwise. The result of a reading 
* can only be wrong in the case we reach the end of the BitStream 
* with some missing bits. 
*/
bool ReadBool();
/**
* Read the native types from the front of the buffer
* @param output The readed value. 
* @return true on success false otherwise. The result of a reading 
* can only be wrong in the case we reach the end of the BitStream 
* with some missing bits. 
*/
uint8 ReadUInt8();
/**
* Read the native types from the front of the buffer
* @param output The readed value. 
* @return true on success false otherwise. The result of a reading 
* can only be wrong in the case we reach the end of the BitStream 
* with some missing bits. 
*/
int8 ReadInt8();
/**
* Read the native types from the front of the buffer
* @param output The readed value. 
* @return true on success false otherwise. The result of a reading 
* can only be wrong in the case we reach the end of the BitStream 
* with some missing bits. 
*/
uint16 ReadUInt16();
/**
* Read the native types from the front of the buffer
* @param output The readed value. 
* @return true on success false otherwise. The result of a reading 
* can only be wrong in the case we reach the end of the BitStream 
* with some missing bits. 
*/
int16 ReadInt16();
/**
* Read the native types from the front of the buffer
* @param output The readed value. 
* @return true on success false otherwise. The result of a reading 
* can only be wrong in the case we reach the end of the BitStream 
* with some missing bits. 
*/
uint32 ReadUInt32();
/**
* Read the native types from the front of the buffer
* @param output The readed value. 
* @return true on success false otherwise. The result of a reading 
* can only be wrong in the case we reach the end of the BitStream 
* with some missing bits. 
*/
int32 ReadInt32();


#ifdef HAS_INT64
/**
* Read the native types from the front of the buffer
* @param output The readed value. 
* @return true on success false otherwise. The result of a reading 
* can only be wrong in the case we reach the end of the BitStream 
* with some missing bits. 
*/
uint64 ReadUInt64();
/**
* Read the native types from the front of the buffer
* @param output The readed value. 
* @return true on success false otherwise. The result of a reading 
* can only be wrong in the case we reach the end of the BitStream 
* with some missing bits. 
*/
int64 ReadInt64();
#endif
/**
* Read the native types from the front of the buffer
* @param output The readed value. 
* @return true on success false otherwise. The result of a reading 
* can only be wrong in the case we reach the end of the BitStream 
* with some missing bits. 
*/
float ReadFloat();
/**
* Read the native types from the front of the buffer
* @param output The readed value. 
* @return true on success false otherwise. The result of a reading 
* can only be wrong in the case we reach the end of the BitStream 
* with some missing bits. 
*/
double ReadDouble();
/**
* Read an array or casted stream of byte. The array
* is raw data. There is no automatic conversion on
* big endian arch
* @param output The result byte array. It should be larger than @em numberOfBytes. 
* @param numberOfBytes The number of byte to read
* @return true on success false if there is some missing bytes. 
*/
bool ReadBytes( char* output, const int numberOfBytes );
/**
* Read multi bytes string
* @return 
*/
bool ReadStr(char output[], int size);
/**
* Read standard string
* @return
*/
std::string ReadStr();
/**
* Read the types you wrote with WriteCompressed
* @param output The read value
* @return true on success, false on not enough data to read
*/
uint8 ReadCompUInt8();
/**
* Read the types you wrote with WriteCompressed
* @param output The read value
* @return true on success, false on not enough data to read
*/
int8 ReadCompInt8();
/**
* Read the types you wrote with WriteCompressed
* @param output The read value
* @return true on success, false on not enough data to read
*/
uint16 ReadCompUInt16();
/**
* Read the types you wrote with WriteCompressed
* @param output The read value
* @return true on success, false on not enough data to read
*/
int16 ReadCompInt16();
/**
* Read the types you wrote with WriteCompressed
* @param output The read value
* @return true on success, false on not enough data to read
*/
uint32 ReadCompUInt32();
/**
* Read the types you wrote with WriteCompressed
* @param output The read value
* @return true on success, false on not enough data to read
*/
int32 ReadCompInt32();

#ifdef HAS_INT64
/**
* Read the types you wrote with WriteCompressed
* @param output The read value
* @return true on success, false on not enough data to read
*/
uint64 ReadCompUInt64();
/**
* Read the types you wrote with WriteCompressed
* @param output The read value
* @return true on success, false on not enough data to read
*/
int64 ReadCompInt64();
#endif
/**
* Read the types you wrote with WriteCompressed
* @param output The read value
* @return true on success, false on not enough data to read
*/
float ReadCompFloat();

/**
* Read the types you wrote with WriteCompressed
* @param output The read value
* @return true on success, false on not enough data to read
*/
double ReadCompDouble();

/**
* Read a normalized 3D vector, using (at most) 4 bytes + 3 bits instead of 12 bytes. Will further compress y or z axis aligned vectors.
* Accurate to 1/32767.5.
* @param x x
* @param y y
* @param z z
*/
bool ReadNormVector( float &x, float &y, float &z );

/**
* Read 3 floats, using 10 bytes, where those floats comprise a vector
* Loses accuracy to about 3/10ths and only saves 2 bytes, so only use if accuracy is not important.
* @param x x
* @param y y
* @param z z
*/
bool ReadVector( float &x, float &y, float &z );
/**
* Read a normalized quaternion in 6 bytes + 4 bits instead of 16 bytes. Slightly lossy.
* @param w w
* @param x x
* @param y y
* @param z z
*/
bool ReadNormQuat( float &w, float &x, float &y, float &z);
/**
* Read an orthogonal matrix from a quaternion, reading 3 components of the quaternion in 2 bytes each and extrapolatig the 4th.
* for 6 bytes instead of 36
*/
bool ReadOrthMatrix( 
float &m00, float &m01, float &m02,
float &m10, float &m11, float &m12,
float &m20, float &m21, float &m22 );

/**
* Sets the read pointer back to the beginning of your data.
*/
void ResetReadPointer( void );
/**
* Sets the write pointer back to the beginning of your data.
*/
void ResetWritePointer( void );
/**
* This is good to call when you are done with the stream to make
* sure you didn't leave any data left over void
*/
void AssertStreamEmpty( void );
/**
* print to the standard output the state of the stream bit by bit 
*/
void PrintBits( void ) const;

/**
* Ignore data we don't intend to read
* @param numberOfBits The number of bits to ignore
*/
void IgnoreBits( const int numberOfBits );

/**
* Move the write pointer to a position on the array. 
* @param offset the offset from the start of the array. 
* @attention 
* Dangerous if you don't know what you are doing!
*
*/
void SetWriteOffset( const int offset );
/**
* Returns the length in bits of the stream
*/
int GetWriteOffset( void ) const;
/**
* Returns the length in bytes of the stream
*/
int GetNumberOfBytesUsed( void ) const;

int GetNumberOfBytesRead(void)const;

/**
* Move the read pointer to a position on the array.
* @param offset
*/
void SetReadOffset( const int offset );

void SetByteReadOffSet(const int offset);
/**
* Returns the number of bits into the stream that we have read
*/
int GetReadOffset( void ) const;

/**
* Returns the number of bits left in the stream that haven't been read
*/
int GetNumberOfUnreadBits( void ) const;
/**
* Makes a copy of the internal data for you Data will point to
* the stream. Returns the length in bits of the stream. Partial
* bytes are left aligned 
* @param _data the resulting byte copy of the internal state. 
*/
int CopyData( unsigned char** _data ) const;
/**
* Set the stream to some initial data. For internal use
* Partial bytes are left aligned
* @param input The data
* @param numberOfBits the number of bits set in the data buffer 
*/
void SetData( const unsigned char* input, const int numberOfBits );
/**
* Exposes the internal data.
* Partial bytes are left aligned.
* @return A pointer to the internal state 
*/
unsigned char* GetData( void ) const;
/**
* Write numberToWrite bits from the input source Right aligned
* data means in the case of a partial byte, the bits are aligned
* from the right (bit 0) rather than the left (as in the normal
* internal representation) You would set this to true when
* writing user data, and false when copying bitstream data, such
* as writing one bitstream to another
* @param input The data 
* @param numberOfBitsToWrite The number of bits to write 
* @param rightAlignedBits if true data will be right aligned 
*/
void WriteBits( const unsigned char* input,
int numberOfBitsToWrite, const bool rightAlignedBits = true );
/**
* Align the bitstream to the byte boundary and then write the
* specified number of bits. This is faster than WriteBits but
* wastes the bits to do the alignment and requires you to call
* ReadAlignedBits at the corresponding read position.
* @param input The data
* @param numberOfBytesToWrite The size of data. 
*/
void WriteAlignedBytes( const unsigned char* input,
const int numberOfBytesToWrite );
/**
* Read bits, starting at the next aligned bits. Note that the
* modulus 8 starting offset of the sequence must be the same as
* was used with WriteBits. This will be a problem with packet
* coalescence unless you byte align the coalesced packets.
* @param output The byte array larger than @em numberOfBytesToRead
* @param numberOfBytesToRead The number of byte to read from the internal state 
* @return true if there is enough byte. 
*/
bool ReadAlignedBytes( unsigned char* output,
const int numberOfBytesToRead );
/**
* Align the next write and/or read to a byte boundary. This can
* be used to 'waste' bits to byte align for efficiency reasons It
* can also be used to force coalesced bitstreams to start on byte
* boundaries so so WriteAlignedBits and ReadAlignedBits both
* calculate the same offset when aligning.
*/
void AlignWriteToByteBoundary( void );
/**
* Align the next write and/or read to a byte boundary. This can
* be used to 'waste' bits to byte align for efficiency reasons It
* can also be used to force coalesced bitstreams to start on byte
* boundaries so so WriteAlignedBits and ReadAlignedBits both
* calculate the same offset when aligning.
*/
void AlignReadToByteBoundary( void );

/**
* Read numberOfBitsToRead bits to the output source
* alignBitsToRight should be set to true to convert internal
* bitstream data to userdata It should be false if you used
* WriteBits with rightAlignedBits false
* @param output The resulting bits array 
* @param numberOfBitsToRead The number of bits to read 
* @param alignsBitsToRight if true bits will be right aligned. 
* @return true if there is enough bits to read 
*/
bool ReadBits( unsigned char* output, int numberOfBitsToRead,
const bool alignBitsToRight = true );

/**
* --- Low level functions --- 
* These are for when you want to deal
* with bits and don't care about type checking 
* Write a 0 
*/
void Write0( void );
/**
* --- Low level functions --- 
* These are for when you want to deal
* with bits and don't care about type checking 
* Write a 1 
*/
void Write1( void );
/**
* --- Low level functions --- 
* These are for when you want to deal
* with bits and don't care about type checking 
* Reads 1 bit and returns true if that bit is 1 and false if it is 0
*/
bool ReadBit( void );
/**
* If we used the constructor version with copy data off, this
* makes sure it is set to on and the data pointed to is copied.
*/
void AssertCopyData( void );
/**
* Use this if you pass a pointer copy to the constructor
* (_copyData==false) and want to overallocate to prevent
* reallocation
*/
void SetNumberOfBitsAllocated( const unsigned int lengthInBits );

private:
/**
* Assume the input source points to a native type, compress and write it.
*/
void WriteCompressed( const unsigned char* input,
const int size, const bool unsignedData );

/**
* Assume the input source points to a compressed native type.
* Decompress and read it.
*/
bool ReadCompressed( unsigned char* output,
const int size, const bool unsignedData );

/**
* Reallocates (if necessary) in preparation of writing
* numberOfBitsToWrite 
*/
void AddBitsAndReallocate( const int numberOfBitsToWrite );

/**
* Number of bits currently used 
*/
int numberOfBitsUsed;
/**
* Number of bits currently allocated 
*/
int numberOfBitsAllocated;
/**
* Current readOffset 
*/
int readOffset;
/**
* array of byte storing the data. Points to stackData or if is bigger than that then is allocated
*/
unsigned char *data;
/**
* true if the internal buffer is copy of the data passed to the
* constructor
*/
bool copyData;

unsigned char stackData[BITSTREAM_STACK_ALLOCATION_SIZE];

};    
}

#endif

BitSteam.cpp:

// This should be on by default for speed.  Turn it off if you actually need endian swapping
#define __BITSTREAM_BIG_END

#include "BitStream.h"
#include <math.h>
#include <assert.h>
#include <string.h>
#ifdef _WIN32
#include <stdlib.h>
#include <memory.h>
#include <stdio.h>
#include <float.h>
#else
#define _copysign copysign
#endif



#if defined ( __APPLE__ ) || defined ( __APPLE_CC__ )
    #include <malloc/malloc.h>
#else
    #include <malloc.h>
#include <string>
#endif

#ifdef __BITSTREAM_BIG_END
// Set up the read/write routines to produce Big-End network streams.
#define B16_1 0
#define B16_0 1

#define B32_3 0
#define B32_2 1
#define B32_1 2
#define B32_0 3

#define B64_7 0
#define B64_6 1
#define B64_5 2
#define B64_4 3
#define B64_3 4
#define B64_2 5
#define B64_1 6
#define B64_0 7

#else
// Default to producing Little-End network streams.
#define B16_1 1
#define B16_0 0

#define B32_3 3
#define B32_2 2
#define B32_1 1
#define B32_0 0

#define B64_7 7
#define B64_6 6
#define B64_5 5
#define B64_4 4
#define B64_3 3
#define B64_2 2
#define B64_1 1
#define B64_0 0
#endif

using namespace dhpnet;

BitStream::BitStream()
{
    numberOfBitsUsed = 0;
    //numberOfBitsAllocated = 32 * 8;
    numberOfBitsAllocated = BITSTREAM_STACK_ALLOCATION_SIZE * 8;
    readOffset = 0;
    //data = ( unsigned char* ) malloc( 32 );
    data = ( unsigned char* ) stackData;
    
#ifdef _DEBUG    
//    assert( data );
#endif
    //memset(data, 0, 32);
    copyData = true;
}

BitStream::BitStream( int initialBytesToAllocate )
{
    numberOfBitsUsed = 0;
    readOffset = 0;
    if (initialBytesToAllocate <= BITSTREAM_STACK_ALLOCATION_SIZE)
    {
        data = ( unsigned char* ) stackData;
        numberOfBitsAllocated = BITSTREAM_STACK_ALLOCATION_SIZE * 8;
    }
    else
    {
        data = ( unsigned char* ) malloc( initialBytesToAllocate );
        numberOfBitsAllocated = initialBytesToAllocate << 3;
    }
#ifdef _DEBUG
    assert( data );
#endif
    // memset(data, 0, initialBytesToAllocate);
    copyData = true;
}

BitStream::BitStream(unsigned char* _data, unsigned int lengthInBytes, bool _copyData)
{
    numberOfBitsUsed = lengthInBytes << 3;
    readOffset = 0;
    copyData = _copyData;
    numberOfBitsAllocated = lengthInBytes << 3;
    
    if ( copyData )
    {
        if ( lengthInBytes > 0 )
        {
            if (lengthInBytes < BITSTREAM_STACK_ALLOCATION_SIZE)
            {
                data = ( unsigned char* ) stackData;
                numberOfBitsAllocated = BITSTREAM_STACK_ALLOCATION_SIZE << 3;
            }
            else
            {
                data = ( unsigned char* ) malloc( lengthInBytes );
            }
#ifdef _DEBUG
            assert( data );
#endif
            memcpy( data, _data, lengthInBytes );
        }
        else
            data = 0;
    }
    else
        {
        data = ( unsigned char* ) _data;
                numberOfBitsUsed = 0;
        }
}
BitStream::BitStream(unsigned char* _data, unsigned int lengthInBytes, unsigned int datasize)
{
    numberOfBitsUsed = datasize << 3;
    readOffset = 0;
    numberOfBitsAllocated = lengthInBytes << 3;    
    data = ( unsigned char* ) _data;
    copyData = false;
}
void BitStream::SetBuffer(char* _data, unsigned int lengthInBytes, unsigned int datasize)
{
    numberOfBitsUsed = datasize << 3;
    readOffset = 0;
    numberOfBitsAllocated = lengthInBytes << 3;    
    data = ( unsigned char* ) _data;
    copyData = false;
}
void BitStream::ClearBuffer()
{
    numberOfBitsUsed = 0;
    readOffset = 0;
    numberOfBitsAllocated = 0;    
    data = NULL;    
}
// Use this if you pass a pointer copy to the constructor (_copyData==false) and want to overallocate to prevent reallocation
void BitStream::SetNumberOfBitsAllocated( const unsigned int lengthInBits )
{
#ifdef _DEBUG
    assert( lengthInBits >= ( unsigned int ) numberOfBitsAllocated );
#endif    
    numberOfBitsAllocated = lengthInBits;
}

BitStream::~BitStream()
{
    if ( copyData && numberOfBitsAllocated > BITSTREAM_STACK_ALLOCATION_SIZE << 3)
        free( data );  // Use realloc and free so we are more efficient than delete and new for resizing
}

void BitStream::Reset( void )
{
    // Note:  Do NOT reallocate memory because BitStream is used
    // in places to serialize/deserialize a buffer. Reallocation
    // is a dangerous operation (may result in leaks).
    
    if ( numberOfBitsUsed > 0 )
    {
        //  memset(data, 0, BITS_TO_BYTES(numberOfBitsUsed));
    }
    
    // Don't free memory here for speed efficiency
    //free(data);  // Use realloc and free so we are more efficient than delete and new for resizing
    numberOfBitsUsed = 0;
    
    //numberOfBitsAllocated=8;
    readOffset = 0;
    
    //data=(unsigned char*)malloc(1);
    // if (numberOfBitsAllocated>0)
    //  memset(data, 0, BITS_TO_BYTES(numberOfBitsAllocated));
}

// Write the native types to the end of the buffer
void BitStream::WriteBool( const bool input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 0;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif
    
    if ( input )
        WriteInt8(1);
    else
        WriteInt8(0);
}

void BitStream::WriteUInt8( const uint8 input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 1;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif
    
    WriteBits( ( unsigned char* ) & input, sizeof( input ) * 8, true );
}

void BitStream::WriteInt8( const int8 input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 2;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif
    
    WriteBits( ( unsigned char* ) & input, sizeof( input ) * 8, true );
}

void BitStream::WriteUInt16( const uint16 input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 3;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif

#ifdef __BITSTREAM_NATIVE_END
    WriteBits( ( unsigned char* ) & input, sizeof( input ) * 8, true );
#else    
    unsigned char uint16w[2];
    uint16w[B16_1] =  (input >> 8)&(0xff);
    uint16w[B16_0] = input&(0xff);

    WriteBits( uint16w, sizeof( input ) * 8, true );
#endif
}

void BitStream::WriteInt16( const int16 input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 4;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif
    
#ifdef __BITSTREAM_NATIVE_END
    WriteBits( ( unsigned char* ) & input, sizeof( input ) * 8, true );
#else
    unsigned char int16w[2];
    int16w[B16_1] =  (input >> 8)&(0xff);
    int16w[B16_0] = input&(0xff);
    
    WriteBits( int16w, sizeof( input ) * 8, true );
#endif
}

void BitStream::WriteUInt32( const uint32 input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 5;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif
    
#ifdef __BITSTREAM_NATIVE_END
    WriteBits( ( unsigned char* ) & input, sizeof( input ) * 8, true );
#else
     unsigned char uint32w[4];
    uint32w[B32_3] = (input >> 24)&(0x000000ff);
    uint32w[B32_2] = (input >> 16)&(0x000000ff);
    uint32w[B32_1] = (input >> 8)&(0x000000ff);
    uint32w[B32_0] = (input)&(0x000000ff);
    
    WriteBits( uint32w, sizeof( input ) * 8, true );
#endif
}

void BitStream::WriteInt32( const int32 input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 6;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif
    
#ifdef __BITSTREAM_NATIVE_END
    WriteBits( ( unsigned char* ) & input, sizeof( input ) * 8, true );
#else
     unsigned char int32w[4];
    int32w[B32_3] = (input >> 24)&(0x000000ff);
    int32w[B32_2] = (input >> 16)&(0x000000ff);
    int32w[B32_1] = (input >> 8)&(0x000000ff);
    int32w[B32_0] = (input)&(0x000000ff);
    
    WriteBits( int32w, sizeof( input ) * 8, true );
#endif
}

#ifdef HAS_INT64
void BitStream::WriteUInt64( const uint64 input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 7;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif
    
#ifdef __BITSTREAM_NATIVE_END
    WriteBits( ( unsigned char* ) & input, sizeof( input ) * 8, true );
#else
     unsigned char uint64w[8];
    uint64w[B64_7] = (input >> 56) & 0xff;
    uint64w[B64_6] = (input >> 48) & 0xff;
    uint64w[B64_5] = (input >> 40) & 0xff;
    uint64w[B64_4] = (input >> 32) & 0xff;
    uint64w[B64_3] = (input >> 24) & 0xff;
    uint64w[B64_2] = (input >> 16) & 0xff;
    uint64w[B64_1] = (input >> 8) & 0xff;
    uint64w[B64_0] = input & 0xff;
    
    WriteBits( uint64w, sizeof( input ) * 8, true );
#endif
}

void BitStream::WriteInt64( const int64 input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 8;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif
    
#ifdef __BITSTREAM_NATIVE_END
    WriteBits( ( unsigned char* ) & input, sizeof( input ) * 8, true );
#else
     unsigned char int64w[8];
    int64w[B64_7] = (input >> 56) & 0xff;
    int64w[B64_6] = (input >> 48) & 0xff;
    int64w[B64_5] = (input >> 40) & 0xff;
    int64w[B64_4] = (input >> 32) & 0xff;
    int64w[B64_3] = (input >> 24) & 0xff;
    int64w[B64_2] = (input >> 16) & 0xff;
    int64w[B64_1] = (input >> 8) & 0xff;
    int64w[B64_0] = input & 0xff;
    
    WriteBits( int64w, sizeof( input ) * 8, true );
#endif
}

#endif

void BitStream::WriteFloat( const float input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 9;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif

#ifndef __BITSTREAM_NATIVE_END
    unsigned int intval = *((unsigned int *)(&input));
    WriteUInt32(intval);
#else
    WriteBits( ( unsigned char* ) & input, sizeof( input ) * 8, true );
#endif
}

void BitStream::WriteDouble( const double input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 10;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif

#if defined ( __BITSTREAM_NATIVE_END ) || ( ! defined (HAS_INT64) )
    WriteBits( ( unsigned char* ) & input, sizeof( input ) * 8, true );
#else
    uint64_t intval = *((uint64_t *)(&input));
    WriteUInt64(intval);
#endif
}
// Write an array or casted stream
void BitStream::WriteBytes( const char* input, const int numberOfBytes )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 11;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
    WriteBits( ( unsigned char* ) & numberOfBytes, sizeof( int ) * 8, true );
#endif
    
    WriteBits( ( unsigned char* ) input, numberOfBytes * 8, true );
}

void BitStream::WriteStr(char input[])
{
        unsigned short len = strlen(input);
        WriteUInt16(len);
        if(len > 0){
            WriteBytes(input,len);
        }
}

void BitStream::WriteStr(const std::string& input)
{
        unsigned short len = input.size();
        WriteUInt16(len);
        if(len > 0){
            WriteBytes(input.data(),len);
        }
}

void BitStream::WriteBS( const BitStream *bitStream )
{
    WriteBits(bitStream->GetData(), bitStream->GetWriteOffset(), false);
}

// Write the native types with simple compression.
// Best used with  negatives and positives close to 0
void BitStream::WriteCompUInt8( const uint8 input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 12;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif
    
    WriteCompressed( ( unsigned char* ) & input, sizeof( input ) * 8, true );
}

void BitStream::WriteCompInt8( const int8 input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 13;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif
    
    WriteCompressed( ( unsigned char* ) & input, sizeof( input ) * 8, false );
}

void BitStream::WriteCompUInt16( const uint16 input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 14;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif
    
#ifdef __BITSTREAM_NATIVE_END
    WriteCompressed( ( unsigned char* ) & input, sizeof( input ) * 8, true );
#else
    unsigned char uint16wc[2];
    uint16wc[B16_1] = (input >> 8)&(0xff);
    uint16wc[B16_0] = input&(0xff);
    
    WriteCompressed( uint16wc, sizeof( input ) * 8, true );
#endif
}

void BitStream::WriteCompInt16( const int16 input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 15;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif
    
#ifdef __BITSTREAM_NATIVE_END
    WriteCompressed( ( unsigned char* ) & input, sizeof( input ) * 8, true );
#else
    unsigned char int16wc[2];
    int16wc[B16_1] =  (input >> 8)&(0xff);
    int16wc[B16_0] = input&(0xff);
    
    WriteCompressed( int16wc, sizeof( input ) * 8, false );
#endif
}

void BitStream::WriteCompUInt32( const uint32 input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 16;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif
    
#ifdef __BITSTREAM_NATIVE_END
    WriteCompressed( ( unsigned char* ) & input, sizeof( input ) * 8, true );
#else
    unsigned char uint32wc[4];
    uint32wc[B32_3] = (input >> 24)&(0x000000ff);
    uint32wc[B32_2] = (input >> 16)&(0x000000ff);
    uint32wc[B32_1] = (input >> 8)&(0x000000ff);
    uint32wc[B32_0] = (input)&(0x000000ff);
    
    WriteCompressed( uint32wc, sizeof( input ) * 8, true );
#endif
}

void BitStream::WriteCompInt32( const int32 input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 17;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif
    
#ifdef __BITSTREAM_NATIVE_END
    WriteCompressed( ( unsigned char* ) & input, sizeof( input ) * 8, true );
#else
    unsigned char int32wc[4];
    int32wc[B32_3] = (input >> 24)&(0x000000ff);
    int32wc[B32_2] = (input >> 16)&(0x000000ff);
    int32wc[B32_1] = (input >> 8)&(0x000000ff);
    int32wc[B32_0] = (input)&(0x000000ff);
    
    WriteCompressed( int32wc, sizeof( input ) * 8, false );
#endif
}

#ifdef HAS_INT64
void BitStream::WriteCompUInt64( const uint64 input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 18;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif
    
#ifdef __BITSTREAM_NATIVE_END
    WriteCompressed( ( unsigned char* ) & input, sizeof( input ) * 8, true );
#else
    unsigned char uint64wc[8];
    uint64wc[B64_7] = (input >> 56) & 0xff;
    uint64wc[B64_6] = (input >> 48) & 0xff;
    uint64wc[B64_5] = (input >> 40) & 0xff;
    uint64wc[B64_4] = (input >> 32) & 0xff;
    uint64wc[B64_3] = (input >> 24) & 0xff;
    uint64wc[B64_2] = (input >> 16) & 0xff;
    uint64wc[B64_1] = (input >> 8) & 0xff;
    uint64wc[B64_0] = input & 0xff;
    
    WriteCompressed( uint64wc, sizeof( input ) * 8, true );
#endif
}

void BitStream::WriteCompInt64( const int64 input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 19;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif
    
#ifdef __BITSTREAM_NATIVE_END
    WriteCompressed( ( unsigned char* ) & input, sizeof( input ) * 8, true );
#else
    unsigned char int64wc[8];
    int64wc[B64_7] = (input >> 56) & 0xff;
    int64wc[B64_6] = (input >> 48) & 0xff;
    int64wc[B64_5] = (input >> 40) & 0xff;
    int64wc[B64_4] = (input >> 32) & 0xff;
    int64wc[B64_3] = (input >> 24) & 0xff;
    int64wc[B64_2] = (input >> 16) & 0xff;
    int64wc[B64_1] = (input >> 8) & 0xff;
    int64wc[B64_0] = input & 0xff;
    
    WriteCompressed( int64wc, sizeof( input ) * 8, false );
#endif
}
#endif


void BitStream::WriteCompFloat( const float input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 20;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif

// Not yet implemented (no compression)
#if defined ( __BITSTREAM_NATIVE_END )
    WriteBits( ( unsigned char* ) &input, sizeof( input ) * 8, true );
#else
    WriteFloat( input );
#endif
}

void BitStream::WriteCompDouble( const double input )
{
#ifdef TYPE_CHECKING
    unsigned char ID = 21;
    WriteBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8, true );
#endif

    // Not yet implemented (no compression)
#if defined ( __BITSTREAM_NATIVE_END )
    WriteBits( ( unsigned char* ) & input, sizeof( input ) * 8, true );
#else
    WriteDouble( input );
#endif
}

void BitStream::WriteNormVector( float x, float y, float z )
{
#ifdef _DEBUG
    assert(x <= 1.01f && y <= 1.01f && z <= 1.01f && x >= -1.01f && y >= -1.01f && z >= -1.01f);
#endif
    if (x>1.0f)
        x=1.0f;
    if (y>1.0f)
        y=1.0f;
    if (z>1.0f)
        z=1.0f;
    if (x<-1.0f)
        x=-1.0f;
    if (y<-1.0f)
        y=-1.0f;
    if (z<-1.0f)
        z=-1.0f;

    WriteBool((bool) (x < 0.0f));

    if (y==0.0f)
        WriteBool(true);
    else
    {
        WriteBool(false);
        WriteUInt16((unsigned short)((y+1.0f)*32767.5f));
    }
    if (z==0.0f)
        WriteBool(true);
    else
    {
        WriteBool(false);
        WriteUInt16((unsigned short)((z+1.0f)*32767.5f));
    }
}
void BitStream::WriteVector( float x, float y, float z )
{
    float magnitude = sqrtf(x * x + y * y + z * z);
    WriteFloat(magnitude);
    if (magnitude > 0.0f)
    {
        WriteUInt16((unsigned short)((x/magnitude+1.0f)*32767.5f));
        WriteUInt16((unsigned short)((y/magnitude+1.0f)*32767.5f));
        WriteUInt16((unsigned short)((z/magnitude+1.0f)*32767.5f));
    }
}
void BitStream::WriteNormQuat( float w, float x, float y, float z)
{
    WriteBool((bool)(w<0.0f));
    WriteBool((bool)(x<0.0f));
    WriteBool((bool)(y<0.0f));
    WriteBool((bool)(z<0.0f));
        WriteUInt16((unsigned short)(fabs(x)*65535.0));
    WriteUInt16((unsigned short)(fabs(y)*65535.0));
    WriteUInt16((unsigned short)(fabs(z)*65535.0));
    // Leave out w and calcuate it on the target
}
void BitStream::WriteOrthMatrix( 
                     float m00, float m01, float m02,
                     float m10, float m11, float m12,
                     float m20, float m21, float m22 )
{
    double qw;
    double qx;
    double qy;
    double qz;

    // Convert matrix to quat
    // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
    qw = sqrt( 1 + m00 + m11 + m22  ) / 2; 
    qx = sqrt( 1 + m00 - m11 - m22  ) / 2; 
    qy = sqrt( 1 - m00 + m11 - m22  ) / 2; 
    qz = sqrt( 1 - m00 - m11 + m22  ) / 2;
    if (qw < 0.0) qw=0.0;
    if (qx < 0.0) qx=0.0;
    if (qy < 0.0) qy=0.0;
    if (qz < 0.0) qz=0.0;
    qx = _copysign( qx, m21 - m12 );
    qy = _copysign( qy, m02 - m20 );
    qz = _copysign( qz, m20 - m02 );

    WriteNormQuat((float)qw,(float)qx,(float)qy,(float)qz);
}

// Read the native types from the front of the buffer
// Write the native types to the end of the buffer
bool BitStream::ReadBool()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return false;
        
#ifdef _DEBUG
        
    assert( ID == 0 );
    
#endif
        return true;
#endif
    
    //assert(readOffset+1 <=numberOfBitsUsed); // If this assert is hit the stream wasn't long enough to read from
    if ( readOffset + 1 > numberOfBitsUsed )
        return false;
        
    //if (ReadBit()) // Check that bit
    if ( data[ readOffset >> 3 ] & ( 0x80 >> ( readOffset++ % 8 ) ) )   // Is it faster to just write it out here?
        return true;
        
    return false;
}

uint8 BitStream::ReadUInt8()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 1 );
        return ID;
#endif
        uint8 uint8r;
    if(ReadBits( ( unsigned char* ) & uint8r, sizeof( uint8r ) * 8 )){
            return uint8r;
        }
        return 0;
}

int8 BitStream::ReadInt8()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 2 );
        return ID;
#endif
    int8 int8r;
    if(ReadBits( ( unsigned char* ) & int8r, sizeof( int8r ) * 8 )) {
            return int8r;
        }
        return 0;
}

uint16 BitStream::ReadUInt16()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 3 );
        return ID;
#endif
    unsigned char uint16r[2];
#ifdef __BITSTREAM_NATIVE_END
    if(ReadBits( uint16r, sizeof( uint16_t ) * 8 )){
            return *(uint16_t*)uint16r;
        }
        return 0;
#else
    if (ReadBits( uint16r, sizeof( uint16 ) * 8 ) != true) return 0;
    return (((uint16) uint16r[B16_1])<<8)|((uint16)uint16r[B16_0]);
#endif
}

int16 BitStream::ReadInt16()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 4 );
        return ID;
#endif
    unsigned char int16r[2];
#ifdef __BITSTREAM_NATIVE_END
    if(ReadBits( int16r, sizeof( int16_t ) * 8 )){
            return *(int16_t*)int16r;
        }
        return 0;
#else
    if (ReadBits( int16r, sizeof( int16 ) * 8 ) != true) return 0;
    return (((int16) int16r[B16_1])<<8)|((int16)int16r[B16_0]);
#endif
}

uint32 BitStream::ReadUInt32()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 5 );
        return ID;
#endif
    unsigned char uint32r[4];
#ifdef __BITSTREAM_NATIVE_END
    if(ReadBits( uint32r, sizeof( uint32_t ) * 8 )){
            return *(uint32_t*)uint32r;
        }
        return 0;
#else
    if(ReadBits( uint32r, sizeof( uint32 ) * 8 ) != true)
        return 0;
    return (((uint32) uint32r[B32_3])<<24)|
        (((uint32) uint32r[B32_2])<<16)|
        (((uint32) uint32r[B32_1])<<8)|
        ((uint32) uint32r[B32_0]);
#endif
}

int32 BitStream::ReadInt32()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 6 );
        return ID;
#endif
    unsigned char int32r[4];
#ifdef __BITSTREAM_NATIVE_END
    if(ReadBits( int32r, sizeof( int32_t ) * 8 )){
            return *(int32_t*)int32r;
        }
        return 0;
#else
    if(ReadBits( int32r, sizeof( int32 ) * 8 ) != true)
        return 0;
    return (((int32) int32r[B32_3])<<24)|
        (((int32) int32r[B32_2])<<16)|
        (((int32) int32r[B32_1])<<8)|
        ((int32) int32r[B32_0]);
#endif
}

#ifdef HAS_INT64
uint64 BitStream::ReadUInt64()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 7 );
        return ID;
#endif
    unsigned char uint64r[8];
#ifdef __BITSTREAM_NATIVE_END
    if(ReadBits( uint64r, sizeof( uint64_t ) * 8 )){
            return *(uint64_t*)uint64r;
        }
        return 0;
#else
    if(ReadBits( uint64r, sizeof( uint64 ) * 8 ) != true)
        return 0;
    return (((uint64) uint64r[B64_7])<<56)|(((uint64) uint64r[B64_6])<<48)|
        (((uint64) uint64r[B64_5])<<40)|(((uint64) uint64r[B64_4])<<32)|
        (((uint64) uint64r[B64_3])<<24)|(((uint64) uint64r[B64_2])<<16)|
        (((uint64) uint64r[B64_1])<<8)|((uint64) uint64r[B64_0]);
#endif
}

int64 BitStream::ReadInt64()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 8 );
        return ID;
#endif
    unsigned char int64r[8];
#ifdef __BITSTREAM_NATIVE_END
    if(ReadBits(int64r, sizeof( int64_t ) * 8 )){
            return *(int64_t*)int64r;
        }
        return 0;
#else
    if(ReadBits( int64r, sizeof( int64_t ) * 8 ) != true)
        return 0;
    return (((uint64) int64r[B64_7])<<56)|(((uint64) int64r[B64_6])<<48)|
        (((uint64) int64r[B64_5])<<40)|(((uint64) int64r[B64_4])<<32)|
        (((uint64) int64r[B64_3])<<24)|(((uint64) int64r[B64_2])<<16)|
        (((uint64) int64r[B64_1])<<8)|((uint64) int64r[B64_0]);
#endif
}
#endif

float BitStream::ReadFloat()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 9 );
        return ID;
#endif

#ifdef __BITSTREAM_NATIVE_END
        static float floatr;
    if(ReadBits( ( unsigned char* ) & floatr, sizeof( floatr ) * 8 )){
            return floatr;
        }
        return 0;
#else
    unsigned int val = ReadUInt32();
    return *((float *)(&val));
#endif
}

double BitStream::ReadDouble()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 10 );
        return ID;
#endif
    
#if defined ( __BITSTREAM_NATIVE_END ) || ( ! defined ( HAS_INT64 ) )
        static double doubler;
    if(ReadBits( ( unsigned char* ) & doubler, sizeof( doubler ) * 8 )){
            return doubler;
        }
        return 0;
#else
    uint64_t val = ReadUInt64();
    return *((double *)(&val));
#endif
}
// Read an array or casted stream
bool BitStream::ReadBytes( char* output, const int numberOfBytes )
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return false;
        
    assert( ID == 11 );
    
    int NOB;
    
    ReadBits( ( unsigned char* ) & NOB, sizeof( int ) * 8 );
    
    assert( NOB == numberOfBytes );
    
#endif
    
    return ReadBits( ( unsigned char* ) output, numberOfBytes * 8 );
}

bool BitStream::ReadStr(char output[], int size){
        unsigned short len = ReadUInt16();
            len = (len+1) > size?size:len;
            if(len > 0){
                if(ReadBytes(output,len)){
                    output[len] = '\0';
                    return true;
                }
            }
        return false;
}

std::string BitStream::ReadStr()
{
    std::string strs ;
        unsigned short len = ReadUInt16();

            if(len > 0){
                char* str = new char[len+1];
                if(ReadBytes(str,len)){
                    str[len] = '\0';
                                    strs = str;
                                    delete[] str;
                    return strs;
                }
            }
    
        
    return std::string();
}

// Read the types you wrote with WriteCompressed
uint8 BitStream::ReadCompUInt8()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 12 );
        return ID;
#endif
        uint8 uint8rc;
    if(ReadCompressed( ( unsigned char* ) & uint8rc, sizeof( uint8rc ) * 8, true )){
            return uint8rc;
        }
        return 0;
}

int8 BitStream::ReadCompInt8()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 13 );
        return ID;
#endif
        int8 int8rc;
    if(ReadCompressed( ( unsigned char* ) & int8rc, sizeof( int8rc ) * 8, false )){
            return int8rc;
        }
        return 0;
}

uint16 BitStream::ReadCompUInt16()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 14 );
        return ID;
#endif
    unsigned char uint16rc[2];
#ifdef __BITSTREAM_NATIVE_END
    if(ReadCompressed( uint16rc, sizeof( uint16_t ) * 8, true )){
            return *(uint16_t*)uint16rc;
        }
        return 0;
#else
    if (ReadCompressed( uint16rc, sizeof( uint16 ) * 8, true ) != true) return 0;
    return (((uint16) uint16rc[B16_1])<<8)|
        ((uint16) uint16rc[B16_0]);
#endif
}

int16 BitStream::ReadCompInt16()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 15 );
        return ID;
#endif
    unsigned char int16rc[2];
#ifdef __BITSTREAM_NATIVE_END
    if(ReadCompressed( int16rc, sizeof( int16_t ) * 8, true )){
            return *(int16_t*)int16rc;
        }
        return 0;
#else
    if (ReadCompressed( int16rc, sizeof( int16 ) * 8, false ) != true) return 0;
    return (((uint16) int16rc[B16_1])<<8)|((uint16)int16rc[B16_0]);
#endif
}

uint32 BitStream::ReadCompUInt32()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 16 );
        return ID;
    
#endif
    unsigned char uint32rc[4];
#ifdef __BITSTREAM_NATIVE_END
    if(ReadCompressed( uint32rc, sizeof( uint32_t ) * 8, true )){
            return *(uint32_t*)uint32rc;
        }
        return 0;
#else
    if(ReadCompressed( uint32rc, sizeof( uint32 ) * 8, true ) != true)
        return 0;
    return (((uint32) uint32rc[B32_3])<<24)|
        (((uint32) uint32rc[B32_2])<<16)|
        (((uint32) uint32rc[B32_1])<<8)|
        ((uint32) uint32rc[B32_0]);
#endif
}

int32 BitStream::ReadCompInt32()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 17 );
        return ID;
#endif

    unsigned char int32rc[4];
#ifdef __BITSTREAM_NATIVE_END
    if(ReadCompressed(int32rc, sizeof( int32_t ) * 8, true )){
            return *(int32_t*)int32rc;
        }
        return 0;
#else
    if(ReadCompressed( int32rc, sizeof( int32 ) * 8, false ) != true)
        return 0;
    return (((uint32) int32rc[B32_3])<<24)|
        (((uint32) int32rc[B32_2])<<16)|
        (((uint32) int32rc[B32_1])<<8)|
        ((uint32) int32rc[B32_0]);

#endif
}

#ifdef HAS_INT64
uint64_t BitStream::ReadCompUInt64()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 18 );
        return ID;
#endif
        
    unsigned char uint64rc[8];
#ifdef __BITSTREAM_NATIVE_END
    if(ReadCompressed( uint64rc, sizeof( uint64_t ) * 8, true )){
            return *(uint64_t*)uint64rc;
        }
        return 0;
#else
    if(ReadCompressed( uint64rc, sizeof( uint64_t ) * 8, true ) != true)
        return 0;
    return (((uint64_t) uint64rc[B64_7])<<56)|(((uint64_t) uint64rc[B64_6])<<48)|
        (((uint64_t) uint64rc[B64_5])<<40)|(((uint64_t) uint64rc[B64_4])<<32)|
        (((uint64_t) uint64rc[B64_3])<<24)|(((uint64_t) uint64rc[B64_2])<<16)|
        (((uint64_t) uint64rc[B64_1])<<8)|((uint64_t) uint64rc[B64_0]);
#endif
}

int64_t BitStream::ReadCompInt64()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 19 );
        return ID;
#endif
    unsigned char int64rc[8];
#ifdef __BITSTREAM_NATIVE_END
    if(ReadCompressed( int64rc, sizeof( int64_t ) * 8, true )){
            return *(int64_t*)int64rc;
        }
        return 0;
#else
    if(ReadCompressed( int64rc, sizeof( int64_t ) * 8, false ) != true)
        return 0;
    return (((uint64_t) int64rc[B64_7])<<56)|(((uint64_t) int64rc[B64_6])<<48)|
        (((uint64_t) int64rc[B64_5])<<40)|(((uint64_t) int64rc[B64_4])<<32)|
        (((uint64_t) int64rc[B64_3])<<24)|(((uint64_t) int64rc[B64_2])<<16)|
        (((uint64_t) int64rc[B64_1])<<8)|((uint64_t) int64rc[B64_0]);
#endif
}
#endif

float BitStream::ReadCompFloat()
{
#ifdef TYPE_CHECKING
    unsigned char ID;
    
    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;
        
    assert( ID == 20 );
        return ID;
#endif
    
    return ReadFloat();
}

double BitStream::ReadCompDouble()
{
#ifdef TYPE_CHECKING
    unsigned char ID;

    if ( ReadBits( ( unsigned char* ) & ID, sizeof(unsigned char) * 8 ) == false )
        return 0;

    assert( ID == 21 );
        return ID;
#endif
        
    return ReadDouble();
}

bool BitStream::ReadNormVector( float &x, float &y, float &z )
{
    unsigned short sy, sz;

    bool xNeg = ReadBool();

    bool yZero = ReadBool();
    if (yZero)
        y=0.0f;
    else
    {
        sy = ReadUInt16();
        y=((float)sy / 32767.5f - 1.0f);
    }
        
        bool zZero = ReadBool();
    if (zZero)
        z=0.0f;
    else
    {
                sz = ReadUInt16();

        z=((float)sz / 32767.5f - 1.0f);
    }

    x = sqrtf(1.0f - y*y - z*z);
    if (xNeg)
        x=-x;
    return true;
}
bool BitStream::ReadVector( float &x, float &y, float &z )
{
    unsigned short sx,sy,sz;

        float magnitude = ReadFloat();
    
    if (magnitude!=0.0f)
    {
        sx = ReadUInt16();
        sy = ReadUInt16();
        sz = ReadUInt16();
                
        x=((float)sx / 32767.5f - 1.0f) * magnitude;
        y=((float)sy / 32767.5f - 1.0f) * magnitude;
        z=((float)sz / 32767.5f - 1.0f) * magnitude;
    }
    else
    {
        x=0.0f;
        y=0.0f;
        z=0.0f;
    }
    return true;
}
bool BitStream::ReadNormQuat( float &w, float &x, float &y, float &z)
{
    bool cwNeg = ReadBool();
    bool cxNeg = ReadBool();
    bool cyNeg = ReadBool();
    bool czNeg = ReadBool();
    unsigned short cx = ReadUInt16();
    unsigned short cy = ReadUInt16();
        unsigned short cz = ReadUInt16();

    // Calculate w from x,y,z
    x=cx/65535.0f;
    y=cy/65535.0f;
    z=cz/65535.0f;
    if (cxNeg) x=-x;
    if (cyNeg) y=-y;
    if (czNeg) z=-z;
    w = sqrt(1.0f - x*x - y*y - z*z);
    if (cwNeg)
        w=-w;
    return true;
}
bool BitStream::ReadOrthMatrix( 
                    float &m00, float &m01, float &m02,
                    float &m10, float &m11, float &m12,
                    float &m20, float &m21, float &m22 )
{
    float qw,qx,qy,qz;
    if (!ReadNormQuat(qw,qx,qy,qz))
        return false;

    // Quat to orthogonal rotation matrix
    // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
    double sqw = (double)qw*(double)qw;
    double sqx = (double)qx*(double)qx;
    double sqy = (double)qy*(double)qy;
    double sqz = (double)qz*(double)qz;
    m00 =  (float)(sqx - sqy - sqz + sqw); // since sqw + sqx + sqy + sqz =1
    m11 = (float)(-sqx + sqy - sqz + sqw);
    m22 = (float)(-sqx - sqy + sqz + sqw);

    double tmp1 = (double)qx*(double)qy;
    double tmp2 = (double)qz*(double)qw;
    m10 = (float)(2.0 * (tmp1 + tmp2));
    m01 = (float)(2.0 * (tmp1 - tmp2));

    tmp1 = (double)qx*(double)qz;
    tmp2 = (double)qy*(double)qw;
    m20 =(float)(2.0 * (tmp1 - tmp2));
    m02 = (float)(2.0 * (tmp1 + tmp2));
    tmp1 = (double)qy*(double)qz;
    tmp2 = (double)qx*(double)qw;
    m21 = (float)(2.0 * (tmp1 + tmp2));
    m12 = (float)(2.0 * (tmp1 - tmp2));

    return true;
}

// Sets the read pointer back to the beginning of your data.
void BitStream::ResetReadPointer( void )
{
    readOffset = 0;
}

// Sets the write pointer back to the beginning of your data.
void BitStream::ResetWritePointer( void )
{
    numberOfBitsUsed = 0;
}

// Write a 0
void BitStream::Write0( void )
{
    AddBitsAndReallocate( 1 );
    
    // New bytes need to be zeroed
    
    if ( ( numberOfBitsUsed % 8 ) == 0 )
        data[ numberOfBitsUsed >> 3 ] = 0;
        
    numberOfBitsUsed++;
}

// Write a 1
void BitStream::Write1( void )
{
    AddBitsAndReallocate( 1 );
    
    int numberOfBitsMod8 = numberOfBitsUsed % 8;
    
    if ( numberOfBitsMod8 == 0 )
        data[ numberOfBitsUsed >> 3 ] = 0x80;
    else
        data[ numberOfBitsUsed >> 3 ] |= 0x80 >> ( numberOfBitsMod8 ); // Set the bit to 1
        
    numberOfBitsUsed++;
}

// Returns true if the next data read is a 1, false if it is a 0
bool BitStream::ReadBit( void )
{
#pragma warning( disable : 4800 )
    return ( bool ) ( data[ readOffset >> 3 ] & ( 0x80 >> ( readOffset++ % 8 ) ) );
#pragma warning( default : 4800 )
}

// Align the bitstream to the byte boundary and then write the specified number of bits.
// This is faster than WriteBits but wastes the bits to do the alignment and requires you to call
// SetReadToByteAlignment at the corresponding read position
void BitStream::WriteAlignedBytes( const unsigned char* input,
    const int numberOfBytesToWrite )
{
#ifdef _DEBUG
    assert( numberOfBytesToWrite > 0 );
#endif
    
    AlignWriteToByteBoundary();
    // Allocate enough memory to hold everything
    AddBitsAndReallocate( numberOfBytesToWrite << 3 );
    
    // Write the data
    memcpy( data + ( numberOfBitsUsed >> 3 ), input, numberOfBytesToWrite );
    
    numberOfBitsUsed += numberOfBytesToWrite << 3;
}

// Read bits, starting at the next aligned bits. Note that the modulus 8 starting offset of the
// sequence must be the same as was used with WriteBits. This will be a problem with packet coalescence
// unless you byte align the coalesced packets.
bool BitStream::ReadAlignedBytes( unsigned char* output,
    const int numberOfBytesToRead )
{
#ifdef _DEBUG
    assert( numberOfBytesToRead > 0 );
#endif
    
    if ( numberOfBytesToRead <= 0 )
        return false;
        
    // Byte align
    AlignReadToByteBoundary();
    
    if ( readOffset + ( numberOfBytesToRead << 3 ) > numberOfBitsUsed )
        return false;
        
    // Write the data
    memcpy( output, data + ( readOffset >> 3 ), numberOfBytesToRead );
    
    readOffset += numberOfBytesToRead << 3;
    
    return true;
}

// Align the next write and/or read to a byte boundary.  This can be used to 'waste' bits to byte align for efficiency reasons
void BitStream::AlignWriteToByteBoundary( void )
{
    if ( numberOfBitsUsed )
        numberOfBitsUsed += 8 - ( ( numberOfBitsUsed - 1 ) % 8 + 1 );
}

// Align the next write and/or read to a byte boundary.  This can be used to 'waste' bits to byte align for efficiency reasons
void BitStream::AlignReadToByteBoundary( void )
{
    if ( readOffset )
        readOffset += 8 - ( ( readOffset - 1 ) % 8 + 1 );
}

// Write numberToWrite bits from the input source
void BitStream::WriteBits( const unsigned char *input,
    int numberOfBitsToWrite, const bool rightAlignedBits )
{
    // if (numberOfBitsToWrite<=0)
    //  return;
    
    AddBitsAndReallocate( numberOfBitsToWrite );
    int offset = 0;
    unsigned char dataByte;
    int numberOfBitsUsedMod8;
    
    numberOfBitsUsedMod8 = numberOfBitsUsed % 8;
    
    // Faster to put the while at the top surprisingly enough
    while ( numberOfBitsToWrite > 0 )
        //do
    {
        dataByte = *( input + offset );
        
        if ( numberOfBitsToWrite < 8 && rightAlignedBits )   // rightAlignedBits means in the case of a partial byte, the bits are aligned from the right (bit 0) rather than the left (as in the normal internal representation)
            dataByte <<= 8 - numberOfBitsToWrite;  // shift left to get the bits on the left, as in our internal representation
            
        // Writing to a new byte each time
        if ( numberOfBitsUsedMod8 == 0 )
            * ( data + ( numberOfBitsUsed >> 3 ) ) = dataByte;
        else
        {
            // Copy over the new data.
            *( data + ( numberOfBitsUsed >> 3 ) ) |= dataByte >> ( numberOfBitsUsedMod8 ); // First half
            
            if ( 8 - ( numberOfBitsUsedMod8 ) < 8 && 8 - ( numberOfBitsUsedMod8 ) < numberOfBitsToWrite )   // If we didn't write it all out in the first half (8 - (numberOfBitsUsed%8) is the number we wrote in the first half)
            {
                *( data + ( numberOfBitsUsed >> 3 ) + 1 ) = (unsigned char) ( dataByte << ( 8 - ( numberOfBitsUsedMod8 ) ) ); // Second half (overlaps byte boundary)
            }
        }
        
        if ( numberOfBitsToWrite >= 8 )
            numberOfBitsUsed += 8;
        else
            numberOfBitsUsed += numberOfBitsToWrite;
        
        numberOfBitsToWrite -= 8;
        
        offset++;
    }
    // } while(numberOfBitsToWrite>0);
}

// Set the stream to some initial data.  For internal use
void BitStream::SetData( const unsigned char* input, const int numberOfBits )
{
#ifdef _DEBUG
    assert( numberOfBitsUsed == 0 ); // Make sure the stream is clear
#endif
    
    if ( numberOfBits <= 0 )
        return ;
        
    AddBitsAndReallocate( numberOfBits );
    
    memcpy( data, input, BITS_TO_BYTES( numberOfBits ) );
    
    numberOfBitsUsed = numberOfBits;
}

// Assume the input source points to a native type, compress and write it
void BitStream::WriteCompressed( const unsigned char* input,
    const int size, const bool unsignedData )
{
    int currentByte = ( size >> 3 ) - 1; // PCs
    
    unsigned char byteMatch;
    
    if ( unsignedData )
    {
        byteMatch = 0;
    }
    
    else
    {
        byteMatch = 0xFF;
    }
    
    // Write upper bytes with a single 1
    // From high byte to low byte, if high byte is a byteMatch then write a 1 bit. Otherwise write a 0 bit and then write the remaining bytes
    while ( currentByte > 0 )
    {
        if ( input[ currentByte ] == byteMatch )   // If high byte is byteMatch (0 of 0xff) then it would have the same value shifted
        {
            bool b = true;
            WriteBool( b );
        }
        else
        {
            // Write the remainder of the data after writing 0
            bool b = false;
            WriteBool( b );
            
            WriteBits( input, ( currentByte + 1 ) << 3, true );
            //  currentByte--;
            
            
            return ;
        }
        
        currentByte--;
    }
    
    // If the upper half of the last byte is a 0 (positive) or 16 (negative) then write a 1 and the remaining 4 bits.  Otherwise write a 0 and the 8 bites.
    if ( ( unsignedData && ( ( *( input + currentByte ) ) & 0xF0 ) == 0x00 ) ||
        ( unsignedData == false && ( ( *( input + currentByte ) ) & 0xF0 ) == 0xF0 ) )
    {
        bool b = true;
        WriteBool( b );
        WriteBits( input + currentByte, 4, true );
    }
    
    else
    {
        bool b = false;
        WriteBool( b );
        WriteBits( input + currentByte, 8, true );
    }
}

// Read numberOfBitsToRead bits to the output source
// alignBitsToRight should be set to true to convert internal bitstream data to userdata
// It should be false if you used WriteBits with rightAlignedBits false
bool BitStream::ReadBits( unsigned char* output,
    int numberOfBitsToRead, const bool alignBitsToRight )
{
#ifdef _DEBUG
    assert( numberOfBitsToRead > 0 );
#endif
    // if (numberOfBitsToRead<=0)
    //  return false;
    
    if ( readOffset + numberOfBitsToRead > numberOfBitsUsed )
        return false;
        
    int readOffsetMod8;
    
    int offset = 0;
    
    memset( output, 0, BITS_TO_BYTES( numberOfBitsToRead ) );
    
    readOffsetMod8 = readOffset % 8;
    
    // do
    // Faster to put the while at the top surprisingly enough
    while ( numberOfBitsToRead > 0 )
    {
        *( output + offset ) |= *( data + ( readOffset >> 3 ) ) << ( readOffsetMod8 ); // First half
        
        if ( readOffsetMod8 > 0 && numberOfBitsToRead > 8 - ( readOffsetMod8 ) )   // If we have a second half, we didn't read enough bytes in the first half
            *( output + offset ) |= *( data + ( readOffset >> 3 ) + 1 ) >> ( 8 - ( readOffsetMod8 ) ); // Second half (overlaps byte boundary)
            
        numberOfBitsToRead -= 8;
        
        if ( numberOfBitsToRead < 0 )   // Reading a partial byte for the last byte, shift right so the data is aligned on the right
        {
        
            if ( alignBitsToRight )
                * ( output + offset ) >>= -numberOfBitsToRead;
                
            readOffset += 8 + numberOfBitsToRead;
        }
        else
            readOffset += 8;
            
        offset++;
        
    }
    
    //} while(numberOfBitsToRead>0);
    
    return true;
}

// Assume the input source points to a compressed native type. Decompress and read it
bool BitStream::ReadCompressed( unsigned char* output,
    const int size, const bool unsignedData )
{
    int currentByte = ( size >> 3 ) - 1;
    
    
    unsigned char byteMatch, halfByteMatch;
    
    if ( unsignedData )
    {
        byteMatch = 0;
        halfByteMatch = 0;
    }
    
    else
    {
        byteMatch = 0xFF;
        halfByteMatch = 0xF0;
    }
    
    // Upper bytes are specified with a single 1 if they match byteMatch
    // From high byte to low byte, if high byte is a byteMatch then write a 1 bit. Otherwise write a 0 bit and then write the remaining bytes
    while ( currentByte > 0 )
    {
        // If we read a 1 then the data is byteMatch.
        
        bool b = ReadBool();
            
        if ( b )   // Check that bit
        {
            output[ currentByte ] = byteMatch;
            currentByte--;
        }
        else
        {
            // Read the rest of the bytes
            
            if ( ReadBits( output, ( currentByte + 1 ) << 3 ) == false )
                return false;
                
            return true;
        }
    }
    
    // All but the first bytes are byteMatch.  If the upper half of the last byte is a 0 (positive) or 16 (negative) then what we read will be a 1 and the remaining 4 bits.
    // Otherwise we read a 0 and the 8 bytes
    //assert(readOffset+1 <=numberOfBitsUsed); // If this assert is hit the stream wasn't long enough to read from
    if ( readOffset + 1 > numberOfBitsUsed )
        return false;
        
    bool b = ReadBool();
        
    if ( b )   // Check that bit
    {
    
        if ( ReadBits( output + currentByte, 4 ) == false )
            return false;
            
        output[ currentByte ] |= halfByteMatch; // We have to set the high 4 bits since these are set to 0 by ReadBits
    }
    else
    {
        if ( ReadBits( output + currentByte, 8 ) == false )
            return false;
    }
    
    return true;
}

// Reallocates (if necessary) in preparation of writing numberOfBitsToWrite
void BitStream::AddBitsAndReallocate( const int numberOfBitsToWrite )
{
    if ( numberOfBitsToWrite <= 0 )
        return;

    int newNumberOfBitsAllocated = numberOfBitsToWrite + numberOfBitsUsed;
    
    if ( numberOfBitsToWrite + numberOfBitsUsed > 0 && ( ( numberOfBitsAllocated - 1 ) >> 3 ) < ( ( newNumberOfBitsAllocated - 1 ) >> 3 ) )   // If we need to allocate 1 or more new bytes
    {
#ifdef _DEBUG
        // If this assert hits then we need to specify true for the third parameter in the constructor
        // It needs to reallocate to hold all the data and can't do it unless we allocated to begin with
        assert( copyData == true );
#endif

        // Less memory efficient but saves on news and deletes
        newNumberOfBitsAllocated = ( numberOfBitsToWrite + numberOfBitsUsed ) * 2;
//        int newByteOffset = BITS_TO_BYTES( numberOfBitsAllocated );
        // Use realloc and free so we are more efficient than delete and new for resizing
        int amountToAllocate = BITS_TO_BYTES( newNumberOfBitsAllocated );
        if (data==(unsigned char*)stackData)
        {
             if (amountToAllocate > BITSTREAM_STACK_ALLOCATION_SIZE)
             {
                 data = ( unsigned char* ) malloc( amountToAllocate );

                 // need to copy the stack data over to our new memory area too
                 memcpy ((void *)data, (void *)stackData, BITS_TO_BYTES( numberOfBitsAllocated )); 
             }
        }
        else
        {
            data = ( unsigned char* ) realloc( data, amountToAllocate );
        }

#ifdef _DEBUG
        assert( data ); // Make sure realloc succeeded
#endif
        //  memset(data+newByteOffset, 0,  ((newNumberOfBitsAllocated-1)>>3) - ((numberOfBitsAllocated-1)>>3)); // Set the new data block to 0
    }
    
    if ( newNumberOfBitsAllocated > numberOfBitsAllocated )
        numberOfBitsAllocated = newNumberOfBitsAllocated;
}

// Should hit if reads didn't match writes
void BitStream::AssertStreamEmpty( void )
{
    assert( readOffset == numberOfBitsUsed );
}

void BitStream::PrintBits( void ) const
{
    if ( numberOfBitsUsed <= 0 )
    {
//        printf( "No bits\n" );
        return ;
    }
    
    for ( int counter = 0; counter < BITS_TO_BYTES( numberOfBitsUsed ); counter++ )
    {
        int stop;
        
        if ( counter == ( numberOfBitsUsed - 1 ) >> 3 )
            stop = 8 - ( ( ( numberOfBitsUsed - 1 ) % 8 ) + 1 );
        else
            stop = 0;
            
        for ( int counter2 = 7; counter2 >= stop; counter2-- )
        {
            if ( ( data[ counter ] >> counter2 ) & 1 )
                putchar( '1' );
            else
                putchar( '0' );
        }
        
        putchar( ' ' );
    }
    
    putchar( '\n' );
}


// Exposes the data for you to look at, like PrintBits does.
// Data will point to the stream.  Returns the length in bits of the stream.
int BitStream::CopyData( unsigned char** _data ) const
{
#ifdef _DEBUG
    assert( numberOfBitsUsed > 0 );
#endif
    
    *_data = new unsigned char [ BITS_TO_BYTES( numberOfBitsUsed ) ];
    memcpy( *_data, data, sizeof(unsigned char) * ( BITS_TO_BYTES( numberOfBitsUsed ) ) );
    return numberOfBitsUsed;
}

// Ignore data we don't intend to read
void BitStream::IgnoreBits( const int numberOfBits )
{
    readOffset += numberOfBits;
}

// Move the write pointer to a position on the array.  Dangerous if you don't know what you are doing!
void BitStream::SetWriteOffset( const int offset )
{
    numberOfBitsUsed = offset;
}

// Returns the length in bits of the stream
int BitStream::GetWriteOffset( void ) const
{
    return numberOfBitsUsed;
}

// Returns the length in bytes of the stream
int BitStream::GetNumberOfBytesUsed( void ) const
{
    return BITS_TO_BYTES( numberOfBitsUsed );
}
int BitStream::GetNumberOfBytesRead(void)const
{
    return BITS_TO_BYTES(readOffset);
}

// Move the read pointer to a position on the array.
void BitStream::SetReadOffset( const int offset )
{
        readOffset = offset;
}
void BitStream::SetByteReadOffSet(const int offset)
{
    readOffset = BYTES_TO_BITS(offset);
}
// Returns the number of bits into the stream that we have read
int BitStream::GetReadOffset( void ) const
{
    return readOffset;
}

// Returns the number of bits left in the stream that haven't been read
int BitStream::GetNumberOfUnreadBits( void ) const
{
    return numberOfBitsUsed - readOffset;
}

// Exposes the internal data
unsigned char* BitStream::GetData( void ) const
{
    return data;
}

// If we used the constructor version with copy data off, this makes sure it is set to on and the data pointed to is copied.
void BitStream::AssertCopyData( void )
{
    if ( copyData == false )
    {
        copyData = true;
        
        if ( numberOfBitsAllocated > 0 )
        {
            unsigned char * newdata = ( unsigned char* ) malloc( BITS_TO_BYTES( numberOfBitsAllocated ) );
#ifdef _DEBUG
            
            assert( data );
#endif
            
            memcpy( newdata, data, BITS_TO_BYTES( numberOfBitsAllocated ) );
            data = newdata;
        }
        
        else
            data = 0;
    }
}