#include "BitStream.h"
#include "mini-printf.h"

BitStream::BitStream(uint16_t init_storage)
{
  storage_size = init_storage;
  storage = new uint8_t[storage_size];
  processable = false;
  init();
}

BitStream::~BitStream(void)
{
  if(storage) delete storage;
}

int BitStream::putBit(bool bit)
{
  // at end of storage? Then add another block
  if(length>=(storage_size*8-1))
  {
    // if storage is not extendable any more, bail out
    if(!increaseStorage()) return -1;
  }

  uint16_t byte = length/8;            // byte in storage affected
  uint8_t mask = (1<<bitPos);          // Bit in that byte

  storage[byte] &= (~mask);            // clear bit
  if(bit) storage[byte] |= mask;       // set bit if required.
  bitPos++;                            // move to next bit in byte
  if(bitPos>=8) bitPos = 0;            // restart at 0 if byte was filled
  length++;                            // increase length of bit sequence
  return 0;
}

int BitStream::increaseStorage(void)
{
  // Extendable?
  if(storage && storage_size<MAX_BITSTREAM_SIZE)
  {
    // YES. get a larger buffer,
    uint8_t *p = new uint8_t[storage_size+32]; // add another block
    // copy the old storage into it,
    memcpy(p, storage, storage_size);
    // delete the old and take the new.
    delete storage;
    storage = p;
    storage_size += 32;
    return storage_size;
  }
  return 0;
}

int BitStream::getBit(uint16_t pos)
{
  // Invalid request?
  if(pos>=length) return -1;

  // No, will work. Set readPtr to the next bit following
  readPtr = pos+1;
  // if requested bit is set, return 1
  if(storage[pos>>3]&(1<<(pos&0x07))) return 1;
  // else return 0
  return 0;
}

int BitStream::dumpHex(char *buffer, uint16_t buffer_length, uint16_t start_pos)
{
  uint16_t offset = start_pos;
  uint16_t dump_length = length/8 + 1;
  char fmtbuf[10];

  if(offset>dump_length) offset = 0;

  mini_snprintf(fmtbuf, 10, "%%0%dB", dump_length);
  mini_snprintf(buffer, buffer_length, fmtbuf, storage+offset);
  if(buffer_length<dump_length*3) return dump_length*3;
  return 0;
}

int BitStream::dumpBin(char *buffer, uint16_t buffer_length, uint16_t start_pos, bool invert, bool reverse)
{
  char *cp = buffer;

  if(reverse)
  {
    uint16_t MyReadPtr = length-1;
    for(uint16_t i=0;i<buffer_length-2;i++)
    {
      int b = getBit(MyReadPtr);
      if(b==1) *cp++ = (invert?'0':'1');
      else if(b==0) *cp++ = (invert?'1':'0');
      if(MyReadPtr)
      {
        MyReadPtr--;
      }
      else break;
    }
    *cp = 0;
    if(MyReadPtr) return MyReadPtr;
  }
  else
  {
    readPtr = start_pos;
    for(uint16_t i=0;i<buffer_length-2;i++)
    {
      int b = getBit();
      if(b==1) *cp++ = (invert?'1':'0');
      else if(b==0) *cp++ = (invert?'0':'1');
      else break;
    }
    *cp = 0;
    if(readPtr<length) return readPtr;
  }
  return 0;
}