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

using namespace std;

ByteBuffer ByteBuffer::alloc(std::size_t capacity) {
    auto bytes = shared_ptr<uint8_t>(new uint8_t[capacity], [](uint8_t* p) {
        delete[] p;
    });
    return ByteBuffer(bytes, capacity, (size_t) 0, (size_t) 0);
}

ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity) :
    bytes(bytes), capacity(capacity), zero(bytes.get()), end(bytes.get()) {
    ptr = const_cast<uint8_t *>(zero);
}

ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, size_t size) :
        bytes(bytes), capacity(capacity), zero(bytes.get()), end(&bytes.get()[size]) {
    assert(size <= capacity);
    ptr = const_cast<uint8_t *>(this->zero);
}

ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, size_t size, size_t zero) :
        bytes(bytes), capacity(capacity), zero(&bytes.get()[zero]), end(&bytes.get()[size]) {
    assert(zero <= size);
    assert(size <= capacity);
    ptr = const_cast<uint8_t *>(this->zero);
}

ByteBuffer::ByteBuffer(const std::shared_ptr<uint8_t> bytes, size_t capacity, const uint8_t *zero, const uint8_t *end) :
        bytes(bytes), capacity(capacity), zero(zero), end(end), ptr((uint8_t *) zero) {
}

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

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

ByteBuffer &ByteBuffer::write32le(uint32_t value) {
    checkAndUpdateEnd(4);
    (*ptr++) = (uint8_t) (value & 0xff);
    (*ptr++) = (uint8_t) ((value >> 8) & 0xff);
    (*ptr++) = (uint8_t) ((value >> 16) & 0xff);
    (*ptr++) = (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) {
    checkAndUpdateEnd(len);

    memcpy(ptr, bytes, len);

    ptr += 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);
        }

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

    return write32le(result);
}

uint8_t ByteBuffer::get8(size_t index) const {
    assertCanAccessRelative(index);
    return ptr[index];
}

uint8_t ByteBuffer::read8() {
    assertCanAccessRelative(0);
    return *ptr++;
}

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

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

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

    int32_t mantissa = data & 0xFFFFFF;
    int8_t exponent = (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::copy(uint8_t *bytes, size_t length) const {
    assertCanAccessRelative(length - 1);

    memcpy(bytes, ptr, length);
}

ByteBuffer ByteBuffer::view() const {
//    DF << "cursor=" << getCursor() << ", size=" << getSize() << ", new size=" << end - ptr << ", ptr=" << (uint64_t) ptr << ", zero=" << (uint64_t) zero;
    return view(ptr, end);
}

ByteBuffer ByteBuffer::view(size_t length) const {
    return ByteBuffer(bytes, length, ptr, ptr + length);
}

ByteBuffer ByteBuffer::view(uint8_t *ptr, const uint8_t *end) const {
    return ByteBuffer(bytes, end - ptr, ptr, end);
}

void ByteBuffer::checkAndUpdateEnd(size_t newBytes) {
    uint8_t *newEnd = ptr + newBytes;
    if (newEnd >= end) {
        if (newEnd >= &zero[capacity]) {
            throw ByteBufferException("New size is too large! cursor=" + to_string(getCursor()) + ", size=" + to_string(getSize()) + ", capacity=" + to_string(capacity) + ", new bytes=" + to_string(newBytes));
        }
        end = newEnd;
    }
}

void ByteBuffer::assertCanAccessRelative(size_t diff) const {
    assertCanAccessIndex(ptr + diff);
}

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

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

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

    return string(s.str());
}