#include "ble/ByteBuffer.h"
#include <sstream>
#include <cassert>
#include <cstring>
#include <iomanip>
#include <cmath>
#include <iostream>
#include <ble/ByteBuffer.h>

using namespace std;

ByteBuffer &ByteBuffer::write8(uint8_t value) {
    assertCanAccessPtr(cursor + 1);
    *cursor++ = value;
    return *this;
}

ByteBuffer &ByteBuffer::write16le(uint16_t value) {
    assertCanAccessPtr(cursor + 2);
    *cursor++ = (uint8_t) (value & 0xff);
    *cursor++ = (uint8_t) ((value >> 8) & 0xff);
    return *this;
}

ByteBuffer &ByteBuffer::write32le(uint32_t value) {
    assertCanAccessPtr(cursor + 4);
    *cursor++ = (uint8_t) (value & 0xff);
    *cursor++ = (uint8_t) ((value >> 8) & 0xff);
    *cursor++ = (uint8_t) ((value >> 16) & 0xff);
    *cursor++ = (uint8_t) ((value >> 24) & 0xff);
    return *this;
}

ByteBuffer &ByteBuffer::write(const ByteBuffer &value) {
    return write(value.zero, value.getSize());
}

ByteBuffer &ByteBuffer::write(const uint8_t *bytes, size_t len) {
    assertCanAccessPtr(cursor + len);

    std::memcpy(cursor, bytes, len);

    cursor += len;

    return *this;
}

ByteBuffer &ByteBuffer::writeFLOAT(double d) {
    uint32_t result;

    if (std::isnan(d)) {
        result = static_cast<uint32_t>(FLOAT::NaN);
    } else if (d > FLOAT::max) {
        result = static_cast<uint32_t>(FLOAT::positive_infinity);
    } else if (d < FLOAT::min) {
        result = static_cast<uint32_t>(FLOAT::negative_infinity);
    } else if (d >= -FLOAT::epsilon && d <= FLOAT::epsilon) {
        result = 0;
    } else {
        double sgn = d > 0 ? 1 : -1;
        double mantissa = fabs(d);
        int exponent = 0;

        if (mantissa > FLOAT::mantissa_max) {
            while (mantissa > FLOAT::mantissa_max) {
                mantissa /= 10.0;
                ++exponent;

//                if (exponent > FLOAT::exponent_max) {
//                    result = sgn ? FLOAT::positive_infinity : FLOAT::negative_infinity;
//                    return write32le(result);
//                }
            }
        } else if (mantissa < 1) {
            while (mantissa < 1) {
                mantissa *= 10;
                --exponent;

//                if (exponent < FLOAT::exponent_min) {
//                    result = 0;
//                    return write32le(result);
//                }
            }
        }

        // scale down if number needs more precision
        double smantissa = round(mantissa * FLOAT::precision);
        double rmantissa = round(mantissa) * FLOAT::precision;
        double mdiff = abs(smantissa - rmantissa);
        while (mdiff > 0.5 && exponent > FLOAT::exponent_min &&
               (mantissa * 10) <= FLOAT::mantissa_max) {
            mantissa *= 10;
            --exponent;
            smantissa = round(mantissa * FLOAT::precision);
            rmantissa = round(mantissa) * FLOAT::precision;
            mdiff = abs(smantissa - rmantissa);
        }

        auto int_mantissa = uint32_t(round(sgn * mantissa));
        result = (exponent << 24) | (int_mantissa & 0xFFFFFF);
    }

    return write32le(result);
}

uint8_t ByteBuffer::get8(size_t index) const {
    assertCanAccessPosition(index);
    return zero[index];
}

uint8_t ByteBuffer::peek8(size_t relative_index) const {
    assertCanAccessPtr(cursor + relative_index);
    return cursor[relative_index];
}

uint8_t ByteBuffer::read8() {
    assertCanAccessPtr(cursor);
    return *cursor++;
}

uint16_t ByteBuffer::read16le() {
    assertCanAccessPtr(cursor + 1);
    uint16_t value;
    value = *cursor++;
    value |= ((uint16_t) *cursor++) << 8;
    return value;
}

uint32_t ByteBuffer::read32le() {
    assertCanAccessPtr(cursor + 3);
    uint32_t value;
    value = *cursor++;
    value |= ((uint32_t) *cursor++) << 8;
    value |= ((uint32_t) *cursor++) << 16;
    value |= ((uint32_t) *cursor++) << 24;
    return value;
}

double ByteBuffer::readFLOAT() {
    uint32_t data = read32le();

    int32_t mantissa = data & 0xFFFFFF;
    auto exponent = static_cast<int8_t>(data >> 24);
    double output = 0;

    if (mantissa >= FLOAT::positive_infinity &&
        mantissa <= FLOAT::negative_infinity) {
        if (mantissa == FLOAT::positive_infinity) {
            output = INFINITY;
        } else if (mantissa == FLOAT::negative_infinity) {
            output = -INFINITY;
        } else {
            output = NAN;
        }
    } else {
        if (mantissa >= 0x800000) {
            mantissa = -((0xFFFFFF + 1) - mantissa);
        }
        output = mantissa * pow(10.0f, exponent);
    }

    return output;
}

void ByteBuffer::copyFromEntire(const ByteBuffer &other) const {
    size_t length = other.getPosition();

    assertCanAccessPosition(length - 1);
    std::memcpy(cursor, other.zero, length);
}

void ByteBuffer::copyTo(uint8_t *bytes, size_t length) const {
    assertCanAccessPtr(&cursor[length - 1]);

    std::memcpy(bytes, cursor, length);
}

ByteBuffer ByteBuffer::viewCursorToEnd() const {
//    LOG_DEBUG("cursor=" << getCursor() << ", size=" << getSize() << ", new size=" << end_ - cursor << ", cursor=" << (uint64_t) cursor << ", zero=" << (uint64_t) zero);
    return {cursor, getBytesLeft()};
}

ByteBuffer ByteBuffer::viewBeginningToCursor() const {
    return {zero, getPosition()};
}

ByteBuffer ByteBuffer::viewForward(size_t length) const {
    assertCanAccessPtr(&cursor[length - 1]);
    return {cursor, length};
}

ByteBuffer ByteBuffer::viewForwardAndSkip(size_t length){
    auto v = viewForward(length);
    skip(length);
    return v;
}

void ByteBuffer::assertCanAccessPosition(size_t position) const {
    assertCanAccessPtr(&zero[position]);
}

void ByteBuffer::assertCanAccessPtr(uint8_t *ptr) const {
    if (ptr >= end_ || ptr < zero) {
        throw ByteBufferException("Out of bounds! size=" + to_string(getSize()) + ", index=" + to_string(ptr - zero));
    }
}

std::string ByteBuffer::toString() const {
    std::stringstream s;

    for (auto *i = zero; i < end_; i++) {
        s << hex << setfill('0') << setw(2) << (int) *i << " ";
    }

    return string(s.str());
}

const ByteBuffer ByteBuffer::wrap(const uint8_t *bytes, size_t capacity, size_t cursor) noexcept {
    return {const_cast<uint8_t *>(bytes), capacity, cursor};
}