#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(uint8_t* bytes, size_t size) :
    zero(bytes), end_(&bytes[size]), cursor(bytes) {
}

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

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

ByteBuffer &ByteBuffer::write32le(uint32_t value) {
    assertCanAccessRelative(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) {
    assertCanAccessRelative(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);
        }

        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 zero[index];
}

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

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

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

uint32_t ByteBuffer::read32le() {
    assertCanAccessRelative(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::copy(uint8_t *bytes, size_t length) const {
    assertCanAccessRelative(length - 1);

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

void ByteBuffer::copy(ByteBuffer& other) const {
    other.assertCanAccessRelative(getBytesLeft());

    std::memcpy(other.cursor, cursor, getBytesLeft());
}

void ByteBuffer::reset() {
    cursor = const_cast<uint8_t *>(zero);
}

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

ByteBuffer ByteBuffer::view(size_t length) const {
    assertCanAccessRelative(length - 1);
    return {cursor, length};
}

void ByteBuffer::assertCanAccessRelative(size_t diff) const {
    assertCanAccessIndex(cursor + 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 (auto *i = zero; i < end_; i++) {
        s << hex << setfill('0') << setw(2) << (int) *i << " ";
    }

    return string(s.str());
}