#include <sstream>
#include <iomanip>
#include <string.h>
#include <hwloc.h>
#include "Bluetooth.h"

namespace trygvis {
    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 &_0, uint8_t &_1, uint8_t &_2, uint8_t &_3, uint8_t &_4, uint8_t &_5) const {
        _0 = bytes[0];
        _1 = bytes[1];
        _2 = bytes[2];
        _3 = bytes[3];
        _4 = bytes[4];
        _5 = bytes[5];
    }

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

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

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

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

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

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

    void AttPdu::makeReadByGroupType(ByteBuffer &bytes, uint16_t startHandle, uint16_t endHandle, uint16_t uuid) {
        bytes.setCursor(0);
        bytes.add8(AttPduType::READ_BY_GROUP_TYPE_REQ);
        bytes.add16le(startHandle);
        bytes.add16le(endHandle);
        bytes.add16le(uuid);
    }

    void AttPdu::checkType(ByteBuffer &bytes, AttPduType type) {
        if (bytes.getSize() == 0) {
            throw BluetoothException("PDU is too small");
        }

        bytes.setCursor(0);
        AttPduType t = (AttPduType) bytes.get8();

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

    vector<AttributeData*>* AttPdu::parseReadByGroupType(ByteBuffer &bytes) {
        checkType(bytes, READ_BY_GROUP_TYPE_RES);

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

        uint8_t length = bytes.get8();
        DF << "length=" << (int) length;

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

        vector<AttributeData*> *values = new vector<AttributeData*>;
        for (int i = 0; i < count; i++) {
            values->push_back(AttributeData::fromByteBuffer(bytes, length));
        }

        return values;
    }

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

    AttributeData* AttributeData::fromByteBuffer(ByteBuffer &bytes, uint8_t length) {
        uint16_t handle = bytes.get16le();

        uint8_t* value = new uint8_t[length];
        bytes.copy(value, length);
        return new AttributeData(handle, value);
    }

    AttributeData::AttributeData(uint16_t handle, uint8_t *bytes) : handle(handle), bytes(bytes) {

    }

    AttributeData::~AttributeData() {
        delete bytes;
    }

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

    BluetoothAdapter::~BluetoothAdapter() {
    }
};