#include "bitstream.h"
#include "defines.h"
#include <stdlib.h>
#include <string.h>

namespace Utils{
  bitstream::bitstream(){
    data = NULL;
    offset = 0;
    dataSize = 0;
    bufferSize = 0;
  }
  
  bool bitstream::checkBufferSize(unsigned int size){
    if (size > bufferSize){
      void* temp = realloc(data, size);
      if (temp){
        data = (char*) temp;
        bufferSize = size;
        return true;
      }else{
        return false;
      }
    }else{
      return true;
    }
  }
  
  void bitstream::append(char* input, size_t bytes){
    if (checkBufferSize(dataSize+bytes)){
      memcpy(data+dataSize, input, bytes);
      dataSize += bytes;
    }
  }

  void bitstream::append(std::string input){
    append((char*)input.c_str(), input.size());
  }
  
  bool bitstream::peekOffset(size_t peekOffset){
    peekOffset += offset;
    return ((data[peekOffset >> 3]) >> (7 - (peekOffset & 7))) & 1;
  }
  
  long long unsigned int bitstream::peek(size_t count){
    if (count  > 64){
      DEBUG_MSG(DLVL_WARN, "Can not read %d bits into a long long unsigned int!", (int)count);
      //return 0;
    }
    if (count > size()){
      DEBUG_MSG(DLVL_ERROR, "Not enough bits left in stream. Left: %d requested: %d", (int)size(), (int)count);
      return 0;
    }
    long long unsigned int retval = 0;
    size_t curPlace = 0;
    size_t readSize;
    size_t readOff;
    char readBuff;
    while (curPlace < count){
      readBuff = data[(int)((offset+curPlace)/8)];
      readSize = 8;
      readOff = (offset + curPlace) % 8; //the reading offset within the byte
      if (readOff != 0){
        //if we start our read not on the start of a byte
        //curplace and retval should both be 0
        //this should be the first read that aligns reading to bytes, if we read over the end of read byte
        //we cut the MSb off of the buffer by bit mask
        readSize -= readOff;//defining starting bit
        readBuff = readBuff & ((1 << readSize) - 1);//bitmasking
      }
      //up until here we assume we read to the end of the byte
      if (count - curPlace < readSize){//if we do not read to the end of the byte
        //we cut off the LSb off of the read buffer by bitshift
        readSize = count - curPlace;
        readBuff = readBuff >> (8 - readSize - readOff);
      }
      retval = (retval << readSize) + readBuff;
      curPlace += readSize;
    }
    return retval;
  }
  
  long long unsigned int bitstream::get(size_t count){
    if(count <= size()){
      long long unsigned int retVal;
      retVal = peek(count);
      skip(count);
      return retVal;
    }else{
      return 0;
    }
  }
  
  void bitstream::skip(size_t count){
    if(count <= size()){
      offset += count;
    }else{
      offset = dataSize*8;
    }
    
  }
  
  long long unsigned int bitstream::size(){
    return (dataSize * 8) - offset;
  }
  
  void bitstream::clear(){
    dataSize = 0;
    offset = 0;
  }
  
  void bitstream::flush(){
    memmove(data, data + (offset/8), dataSize - (offset/8));
    dataSize -= offset / 8;
    offset %= 8;
  }
  
  long long unsigned int bitstream::golombPeeker(){
    for (size_t i = 0; i < 64 && i < size(); i++){
      if (peekOffset(i)){
        return peek((i * 2) + 1 );
      }
    }
    return 0;
  }
  
  long long unsigned int bitstream::golombGetter(){
    for (size_t i = 0; i < 64 && i < size(); i++){
      if (peekOffset(i)){
        return get((i * 2) + 1);
      }
    }
    return 0;
  }
  
  long long int bitstream::getExpGolomb(){
    long long unsigned int temp = golombGetter();
    return (temp >> 1) * (1 - ((temp & 1) << 1)); //Is actually return (temp / 2) * (1 - (temp & 1) * 2);
  }
  
  long long unsigned int bitstream::getUExpGolomb(){
    return golombGetter() - 1;
  }
  
  long long int bitstream::peekExpGolomb(){
    long long unsigned int temp = golombPeeker();
    return (temp >> 1) * (1 - ((temp & 1) << 1)); //Is actually return (temp / 2) * (1 - (temp & 1) * 2);
  }
  
  long long unsigned int bitstream::peekUExpGolomb(){
    return golombPeeker() - 1;
  }
  
//Note: other bitstream here  
  bitstreamLSBF::bitstreamLSBF(){
    readBufferOffset = 0;
    readBuffer = 0;
  }
  
  void bitstreamLSBF::append (char* input, size_t bytes){
    append(std::string(input,bytes));
  }
  
  void bitstreamLSBF::append (std::string input){
    data += input;
    fixData();
  }
  
  long long unsigned int bitstreamLSBF::size(){
    return data.size() * 8 + readBufferOffset;
  }
  
  long long unsigned int bitstreamLSBF::get(size_t count){
    if (count <= 32 && count <= readBufferOffset){
      long long unsigned int retval = readBuffer & (((long long unsigned int)1 << count) - 1);
      readBuffer = readBuffer >> count;
      readBufferOffset -= count;
      fixData();
      return retval;
    }
    return 42;
  }

  void bitstreamLSBF::skip(size_t count){
    if (count <= 32 && count <= readBufferOffset){
      readBuffer = readBuffer >> count;
      readBufferOffset -= count;
      fixData();
    }
  }
  
  long long unsigned int bitstreamLSBF::peek(size_t count){
    if (count <= 32 && count <= readBufferOffset){
      return readBuffer & ((1 << count) - 1);
    }
    return 0;
  }
  
  void bitstreamLSBF::clear(){
    data = "";
    readBufferOffset = 0;
    readBuffer = 0;
  }
  
  void bitstreamLSBF::fixData(){
    while (readBufferOffset <= 32 && data.size() != 0){
      //readBuffer = readBuffer & ((1 << readBufferOffset) - 1) | (data[0] << readBufferOffset);
      readBuffer |= (((long long unsigned int)data[0]) << readBufferOffset);
      data = data.substr(1);
      readBufferOffset += 8;
    }
  }
}