#include "BitStream.h"

#include "ByteUtils.h"

#include <sstream>
#include <iostream>

BitStream::~BitStream()
{

}

unsigned char BitStream::getCurrentByte()
{
    if (mByteOffset < 0)
    {
        readNextByte();
    }
    return mCurrentByte;
}

void BitStream::write(uint32_t data)
{
    unsigned num_bytes = sizeof(uint32_t);
    for(unsigned idx=0; idx<num_bytes;idx++)
    {
        writeByte(ByteUtils::getByteN(data, idx));
    }
}

void BitStream::writeWord(uint16_t data)
{
    const auto byte0 = data >> 8;
    const auto byte1 = (data << 8) >> 8;
    writeByte(byte0);
    writeByte(byte1);
}

int BitStream::getCurrentByteOffset() const
{
    return mByteOffset;
}

unsigned BitStream::getCurrentBitOffset() const
{
    return mBitOffset;
}

std::string BitStream::logLocation()
{
    std::stringstream sstr;
    sstr << "Byte offset " << mByteOffset<< " | Bit offset " << mBitOffset;
    sstr << " | Working byte " << ByteUtils::toString(getCurrentByte()) << '\n';
    return sstr.str();
}

std::string BitStream::logNextNBytes(unsigned n) const
{
    std::stringstream sstr;
    unsigned count{0};
    for(auto byte : peekNextNBytes(n))
    {
        sstr << mByteOffset + count << " | " << ByteUtils::toString(byte) + '\n';
        count++;
    }
    return sstr.str();
}

void BitStream::writeNBits(uint32_t data, unsigned length)
{
    const auto num_left = 8 - mBitOffset;
    const int overshoot = length - num_left;

    if (overshoot > 0)
    {
        unsigned char lower_bits = ByteUtils::getLowerNBits(data, num_left);
        mCurrentByte |= lower_bits << mBitOffset;

        writeByte(mCurrentByte, false);

        auto num_bytes = overshoot / 8;
        for (unsigned idx=0; idx< num_bytes; idx++)
        {
            mCurrentByte = ByteUtils::getMBitsAtN(data, overshoot, idx*8 + num_left);
            writeByte(mCurrentByte, false);
        }

        if (const auto remainder = overshoot % 8; remainder > 0)
        {
            mCurrentByte = ByteUtils::getMBitsAtN(data, remainder, num_bytes*8 + num_left);
            mBitOffset = remainder;
        }
        else
        {
            mCurrentByte = 0;
            mBitOffset = 0;
        }
    }
    else
    {
        mCurrentByte |= (static_cast<unsigned char>(data) << mBitOffset);
        mBitOffset += length;
        if (mBitOffset == 8)
        {
            writeByte(mCurrentByte, false);
            mCurrentByte = 0;
            mBitOffset = 0;
        }
    }

}

bool BitStream::readNextNBits(unsigned n, unsigned char& buffer)
{
    if (mByteOffset < 0)
    {
        if (!readNextByte())
        {
            return false;
        }
    }

    int overshoot = n + mBitOffset - 8;
    if (overshoot > 0)
    {
        unsigned char last_byte = mCurrentByte;
        if (!readNextByte())
        {
            return false;
        }

        auto num_lower = 8 - mBitOffset;
        char lower_bits = ByteUtils::getHigherNBits(last_byte, num_lower);
        char higher_bits = ByteUtils::getLowerNBits(mCurrentByte, overshoot);

        buffer = (higher_bits << num_lower) | lower_bits;

        mBitOffset = overshoot;
        return true;
    }
    else
    {
        buffer = ByteUtils::getMBitsAtN(mCurrentByte, n, mBitOffset);
        mBitOffset += n;
        return true;
    }
}