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

/// Takes a pointer, offset bitcount and data bitcount, returning the unsigned int read from the givens.
/// offsetBits may be > 7, in which case offsetBits / 8 is added to the pointer automatically.
/// This function assumes Most Significant Bits first.
/// If dataBits > 64, only the last 64 bits are returned.
unsigned long long Bit::getMSB(char * pointer, unsigned int offsetBits, unsigned int dataBits){
  //If the offset is a whole byte or more, add the whole bytes to the pointer instead.
  pointer += offsetBits >> 3;
  //The offset is now guaranteed less than a whole byte.
  offsetBits &= 0x07;
  unsigned long long retVal = 0;
  //Now we parse the remaining bytes
  while (dataBits){
    //Calculate how many bits we're reading from this byte
    //We assume all except for the offset
    unsigned int curBits = 8 - offsetBits;
    //If that is too much, we use the remainder instead
    if (curBits > dataBits){
      curBits = dataBits;
    }
    //First, shift the current return value by the amount of bits we're adding
    retVal <<= curBits;
    //Next, add those bits from the current pointer position at the correct offset, increasing the pointer by one
    retVal |= ((int)(*(pointer++)) << offsetBits) >> (8 - curBits);
    //Finally, set the offset to zero and remove curBits from dataBits.
    offsetBits = 0;
    dataBits -= curBits;
  }//Loop until we run out of dataBits, then return the result
  return retVal;
}

/// Takes a pointer, offset bitcount and data bitcount, setting to given value.
/// offsetBits may be > 7, in which case offsetBits / 8 is added to the pointer automatically.
/// This function assumes Most Significant Bits first.
/// WARNING: UNFINISHED. DO NOT USE.
/// \todo Finish writing this - untested atm.
void Bit::setMSB(char * pointer, unsigned int offsetBits, unsigned int dataBits, unsigned long long value){
  //Set the pointer to the last byte we need to be setting
  pointer += (offsetBits + dataBits) >> 3;
  //The offset is now guaranteed less than a whole byte.
  offsetBits = (offsetBits + dataBits) & 0x07;
  unsigned long long retVal = 0;
  //Now we set the remaining bytes
  while (dataBits){
    //Calculate how many bits we're setting in this byte
    //We assume all that will fit in the current byte
    unsigned int curBits = offsetBits;
    //If that is too much, we use the remainder instead
    if (curBits > dataBits){
      curBits = dataBits;
    }
    //Set the current pointer position at the correct offset, increasing the pointer by one
    retVal |= ((int)(*(pointer++)) << offsetBits) >> (8 - curBits);
    *pointer = (((*pointer) << offsetBits) >> offsetBits) | ((value & 0xFF) << (8 - offsetBits));
    --pointer;
    //Finally, shift the current value by the amount of bits we're adding
    value >>= offsetBits;
    //... and set the offset to eight and remove curBits from dataBits.
    offsetBits = 8;
    dataBits -= curBits;
  }//Loop until we run out of dataBits, then return the result
}

/// Parses a string reference to a boolean.
/// Returns true if the string, with whitespace removed and converted to lowercase, prefix-matches any of: "1", "yes", "true", "cont".
/// Returns false otherwise.
bool Util::stringToBool(std::string & str){
  std::string tmp;
  tmp.reserve(4);
  for (unsigned int i = 0; i < str.size() && tmp.size() < 4; ++i){
    if (!::isspace(str[i])){
      tmp.push_back((char)tolower(str[i]));
    }
  }
  return (strncmp(tmp.c_str(), "1", 1) == 0 || strncmp(tmp.c_str(), "yes", 3) == 0 || strncmp(tmp.c_str(), "true", 4) == 0 || strncmp(tmp.c_str(), "cont", 4) == 0);
}