#include "ble/Bluetooth.h"
#include "BluetoothImpl.h"

#include <sstream>
#include <iomanip>

namespace trygvis {
namespace bluetooth {
using namespace std;

// -----------------------------------------------------------------------
// Mac
// -----------------------------------------------------------------------

string Mac::str() const {
    std::ostringstream buf;

    buf
            << setw(2) << hex << setfill('0') << (int) bytes[5] << ":"
            << setw(2) << hex << setfill('0') << (int) bytes[4] << ":"
            << setw(2) << hex << setfill('0') << (int) bytes[3] << ":"
            << setw(2) << hex << setfill('0') << (int) bytes[2] << ":"
            << setw(2) << hex << setfill('0') << (int) bytes[1] << ":"
            << setw(2) << hex << setfill('0') << (int) bytes[0];

    return buf.str();
}

bool Mac::operator==(Mac &other) const {
    const uint8_t *b = bytes;
    return memcmp(b, other.bytes, sizeof(bytes)) == 0;
}

bool Mac::operator!=(Mac &other) const {
    return !operator==(other);
}

bool operator<(const Mac &a, const Mac &b) {
    return memcmp(a.bytes, b.bytes, sizeof(a.bytes)) < 0;
}

void Mac::copy(uint8_t &_5, uint8_t &_4, uint8_t &_3, uint8_t &_2, uint8_t &_1, uint8_t &_0) const {
    _5 = bytes[5];
    _4 = bytes[4];
    _3 = bytes[3];
    _2 = bytes[2];
    _1 = bytes[1];
    _0 = bytes[0];
}

Mac Mac::parseMac(string s) {
    unsigned int bytes[6];
    int count = sscanf(s.c_str(), "%02x:%02x:%02x:%02x:%02x:%02x",
            &bytes[5], &bytes[4], &bytes[3], &bytes[2], &bytes[1], &bytes[0]);

    if (count != 6) {
        throw BluetoothException("Unable to parse mac: " + s);
    }

    return Mac((uint8_t) bytes[5], (uint8_t) bytes[4], (uint8_t) bytes[3], (uint8_t) bytes[2], (uint8_t) bytes[1], (uint8_t) bytes[0]);
}

AttPdu::AttPdu(ByteBuffer &bytes) : bytes(bytes) {
}

AttPdu::AttPdu(ByteBuffer &bytes, AttPduType type) : bytes(bytes) {
    bytes.write8(type);
}

AttPduType AttPdu::getType() {
    return (AttPduType) bytes.get8(0);
}

void AttPdu::makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, ShortUuid uuid) {
    bytes.write8(AttPduType::READ_BY_GROUP_TYPE_REQ);
    bytes.write16le(startHandle);
    bytes.write16le(endHandle);
    bytes.write16le(uuid.value);
}

void AttPdu::makeReadByType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, ShortUuid uuid) {
    bytes.write8(AttPduType::READ_BY_TYPE_REQ);
    bytes.write16le(startHandle);
    bytes.write16le(endHandle);
    bytes.write16le(uuid.value);
}

void AttPdu::makeRead(ByteBuffer &bytes, uint16_t handle) {
    bytes.write8(AttPduType::READ_REQ);
    bytes.write16le(handle);
}

void AttPdu::makeWrite(ByteBuffer &req, uint16_t handle, const ByteBuffer &bytes) {
    req.write8(AttPduType::WRITE_REQ);
    req.write16le(handle);
    req.write(bytes);
}

vector<AttributeData> AttPdu::parse(ByteBuffer &bytes, AttPduType type) {
    // cout << "bytes: " << bytes.toString();

    auto t = static_cast<AttPduType>(bytes.read8());

    if (t == ERROR) {
        return vector<AttributeData>();
    }

    if (t != type) {
        throw BluetoothException("Unexpected type: " + to_string(t) + ", expected " + to_string(type));
    }

    if (bytes.getSize() < 4) {
        throw BluetoothException("Bad READ_BY_GROUP_TYPE_RES packet, expected at least 4 octets, got " + to_string(bytes.getSize()));
    }

    uint8_t length = bytes.read8();
    // cout << "length=" << (int) length;

    size_t count = (bytes.getSize() - 2) / length;
    // cout << "count=" << count;

    vector<AttributeData> values;
    for (int i = 0; i < count; i++) {
        auto data = bytes.view(length);
        // cout << "data, size=" << data.getSize() << ", bytes=" << data.toString();
        bytes.skip(length);
        values.push_back(AttributeData::fromByteBuffer(data));
    }

    return values;
}

vector<AttributeData> AttPdu::parseReadByGroupType(ByteBuffer &bytes) {
    return parse(bytes, READ_BY_GROUP_TYPE_RES);
}

vector<AttributeData> AttPdu::parseReadByType(ByteBuffer &bytes) {
    return parse(bytes, READ_BY_TYPE_RES);
}

static uint16_t parseExchangeMtuReq(ByteBuffer &bytes);

void AttPdu::parseRead(ByteBuffer &bytes) {
    AttPduType t = (AttPduType) bytes.read8();

    if (t != READ_RES) {
        throw BluetoothException("Unexpected type: " + to_string(t) + ", expected " + to_string(READ_RES));
    }
}

void AttPdu::parseWrite(ByteBuffer &bytes) {
    AttPduType t = (AttPduType) bytes.read8();

    if (t != WRITE_RES) {
        throw BluetoothException("Unexpected type: " + to_string(t) + ", expected " + to_string(WRITE_RES));
    }
}

// -----------------------------------------------------------------------
// AttributeData
// -----------------------------------------------------------------------

AttributeData AttributeData::fromByteBuffer(ByteBuffer &bytes) {
    uint16_t handle = bytes.read16le();

    return AttributeData(handle, bytes.view());
}

AttributeData::AttributeData(uint16_t handle, ByteBuffer value) :
        handle(handle), value(value) {
}

AttributeData::~AttributeData() = default;

// -----------------------------------------------------------------------
// Gatt
// -----------------------------------------------------------------------

BluetoothGatt::BluetoothGatt() = default;

BluetoothGatt::~BluetoothGatt() = default;

// -----------------------------------------------------------------------
// Device
// -----------------------------------------------------------------------

BluetoothDevice::BluetoothDevice() = default;

BluetoothDevice::~BluetoothDevice() = default;

// -----------------------------------------------------------------------
// Adapter
// -----------------------------------------------------------------------

BluetoothAdapter::BluetoothAdapter() = default;

BluetoothAdapter::~BluetoothAdapter() = default;
// -----------------------------------------------------------------------
// Bluetooth System. This is not sub-classed by implementations.
// -----------------------------------------------------------------------

BluetoothSystem::BluetoothSystem() = default;

BluetoothSystem::~BluetoothSystem() {
    adapters.clear();
}

shared_ptr<BluetoothAdapter> BluetoothSystem::getAdapter(string name) {
    auto it = adapters.find(name);

    if (it == adapters.end()) {
        auto adapter = adapters[name] = getAdapterImpl(name);
        return adapter;
    }

    return it->second;
}

Uuid makeUuid(const Uuid &base, uint8_t a, uint8_t b) {
    uint8_t value[16];
    memcpy(value, base.value, 16);
    value[2] = a;
    value[3] = b;
    return Uuid{value};
}

}
};