#include "BluetoothImpl.h"

#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>
#include <bluetooth/l2cap.h>
#include <map>
#include <iomanip>

// Got to love magic constants. Taken from bluez.git/tools/btgatt-client.c
#define ATT_CID 4

#define MAX_MTU 256

namespace trygvis {
namespace bluetooth {
namespace linux {

using namespace uuids;

class LinuxBluetoothGatt;

class LinuxBluetoothDevice;

class LinuxBluetoothAdapter;

class LinuxBluetoothManager;

class LinuxBluetoothAdapter : public DefaultBluetoothAdapter {
public:
    LinuxBluetoothAdapter(int hciDeviceId);

    ~LinuxBluetoothAdapter();

    void runScan(void (*callback)(BluetoothDevice &device)) override;

    BluetoothDevice &getDevice(Mac &mac) override;

private:
    void startScan() override;

    void stopScan() override;

    int hciDeviceId;
    int hciSocket;
    struct hci_filter hciFilter;
    bool scanning;

    map<Mac, LinuxBluetoothDevice *> devices;
};

class LinuxBluetoothDevice : public DefaultBluetoothDevice<LinuxBluetoothAdapter> {
public:
    LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac &mac);

    ~LinuxBluetoothDevice();

    shared_ptr<BluetoothGatt> connectGatt() override;

private:
    weak_ptr<LinuxBluetoothGatt> gatt;
    int l2cap;
};

class LinuxBluetoothGatt : public DefaultBluetoothGatt<LinuxBluetoothDevice> {
public:
    LinuxBluetoothGatt(LinuxBluetoothDevice &device, int l2cap);

    ~LinuxBluetoothGatt();

    bool isConnected() const override;

    void discoverServices() override;

    void writeValue(const BluetoothGattCharacteristic &c, const ByteBuffer &bytes) override;

    ByteBuffer readValue(const BluetoothGattCharacteristic &c) override;

private:
    void connect();

    void disconnect();

    vector<AttributeData> discoverServices(uint16_t startHandle);

    vector<AttributeData> discoverCharacteristics(uint16_t startHandle, uint16_t endHandle);

    ByteBuffer writeAndRead(ByteBuffer &out, shared_ptr<uint8_t> buffer, size_t size);

    int l2cap;

    bool connected;
};

// Utilities

string errnoAsString() {
    return string(strerror(errno));
};

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

Mac parseMac(bdaddr_t &a) {
    return Mac(a.b[0], a.b[1], a.b[2], a.b[3], a.b[4], a.b[5]);
}

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

LinuxBluetoothDevice::LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac &mac) :
        DefaultBluetoothDevice(adapter, mac) {
}

LinuxBluetoothDevice::~LinuxBluetoothDevice() {
//    if (gatt) {
//        delete gatt;
//    }
};

shared_ptr<BluetoothGatt> LinuxBluetoothDevice::connectGatt() {
    if (auto p = gatt.lock()) {
        return p;
    }
    auto ref = make_shared<LinuxBluetoothGatt>(*this, l2cap);

    gatt = ref;

    return ref;
}

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

LinuxBluetoothGatt::LinuxBluetoothGatt(LinuxBluetoothDevice &device, int l2cap) :
        DefaultBluetoothGatt(device), l2cap(l2cap) {
    connect();
}

LinuxBluetoothGatt::~LinuxBluetoothGatt() {
    disconnect();
}

bool LinuxBluetoothGatt::isConnected() const {
    return connected;
}

void LinuxBluetoothGatt::connect() {
    struct sockaddr_l2 addr;

    LOG_DEBUG("connect: mac=" << device.getMac().str());

    l2cap = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
    if (l2cap < 0) {
        throw BluetoothException(&device, "connect(): socket(): " + errnoAsString());
    }

    memset(&addr, 0, sizeof(addr));
    addr.l2_family = AF_BLUETOOTH;
    addr.l2_bdaddr = {{0, 0, 0, 0, 0, 0}};
    addr.l2_cid = htobs(ATT_CID);
    addr.l2_bdaddr_type = BDADDR_LE_PUBLIC;

    if (bind(l2cap, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
        close(l2cap);
        throw BluetoothException(&device, "bind(): " + errnoAsString());
    }

    struct bt_security btsec;
    memset(&btsec, 0, sizeof(btsec));
    btsec.level = BT_SECURITY_LOW;
    if (setsockopt(l2cap, SOL_BLUETOOTH, BT_SECURITY, &btsec, sizeof(btsec)) != 0) {
        close(l2cap);
        throw BluetoothException(&device, "setsockopt(): " + errnoAsString());
    }

    memset(&addr, 0, sizeof(addr));
    addr.l2_family = AF_BLUETOOTH;
    addr.l2_cid = htobs(ATT_CID);
    addr.l2_bdaddr_type = BDADDR_LE_RANDOM;
    device.getMac().copy(
            addr.l2_bdaddr.b[5],
            addr.l2_bdaddr.b[4],
            addr.l2_bdaddr.b[3],
            addr.l2_bdaddr.b[2],
            addr.l2_bdaddr.b[1],
            addr.l2_bdaddr.b[0]);

    if (::connect(l2cap, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
        close(l2cap);
        throw BluetoothException(&device, "connect(): " + errnoAsString());
    }
}

void LinuxBluetoothGatt::disconnect() {
    if (!connected) {
        return;
    }
    LOG_DEBUG("mac = " << device.getMac().str());
    close(l2cap);
}

uuid_t readUuid(BluetoothDevice *device, const ByteBuffer &bytes) {
    size_t bytesLeft = bytes.getBytesLeft();

    uuid_t u;

    if (bytesLeft == 2) {
        uint8_t bs[16] = BLUETOOTH_UUID_INITIALIZER;
        bs[2] = bytes.get8(1);
        bs[3] = bytes.get8(0);
        memcpy(&u, bs, 16);
    } else if (bytesLeft == 16) {
        uint8_t bs[16];
        bs[15] = bytes.get8(0);
        bs[14] = bytes.get8(1);
        bs[13] = bytes.get8(2);
        bs[12] = bytes.get8(3);
        bs[11] = bytes.get8(4);
        bs[10] = bytes.get8(5);
        bs[9] = bytes.get8(6);
        bs[8] = bytes.get8(7);
        bs[7] = bytes.get8(8);
        bs[6] = bytes.get8(9);
        bs[5] = bytes.get8(10);
        bs[4] = bytes.get8(11);
        bs[3] = bytes.get8(12);
        bs[2] = bytes.get8(13);
        bs[1] = bytes.get8(14);
        bs[0] = bytes.get8(15);
        memcpy(&u, bs, 16);
    } else {
        throw BluetoothException(device, "Unexpected bytes left: " + to_string(bytesLeft));
    }

    return u;
}

void LinuxBluetoothGatt::writeValue(const BluetoothGattCharacteristic &c, const ByteBuffer &bytes) {
    LOG_DEBUG("Writing to characteristic " << c.getUuid() << ": " << bytes.toString());

    shared_ptr<uint8_t> buffer(new uint8_t[MAX_MTU]);
    ByteBuffer out = ByteBuffer(buffer, MAX_MTU);

    AttPdu::makeWrite(out, c.getValueHandle(), bytes);

    ByteBuffer in = writeAndRead(out, buffer, MAX_MTU);

    AttPdu::parseWrite(in);
}

ByteBuffer LinuxBluetoothGatt::readValue(const BluetoothGattCharacteristic &c) {
    shared_ptr<uint8_t> buffer(new uint8_t[MAX_MTU]);
    ByteBuffer out = ByteBuffer(buffer, MAX_MTU);

    AttPdu::makeRead(out, c.getValueHandle());

    ByteBuffer in = writeAndRead(out, buffer, MAX_MTU);

    AttPdu::parseRead(in);

//    D << "READ response has " + to_string(in.getBytesLeft()) + " bytes";

    auto response = in.view();

    LOG_DEBUG("Value of characteristic " << c.getUuid() << "=" << response.toString());

    return response;
}

void LinuxBluetoothGatt::discoverServices() {
    uint16_t startHandle = 0x0001;

    removeServices();

    do {
        vector<AttributeData> values = discoverServices(startHandle);

        // Shouldn't happen, but you never know.
        if (values.size() == 0) {
            break;
        }

        uint16_t endGroupHandle = startHandle;

        for (auto &data: values) {
            endGroupHandle = data.value.read16le();

//            D << "handle: 0x" << hex << setw(4) << setfill('0') << data.handle <<
//                        ", endGroupHandle: 0x" << hex << setw(4) << setfill('0') << endGroupHandle <<
//                        ", value: " << data.value.toString();

            uuid_t u = readUuid(&device, data.value);

            addService(new DefaultBluetoothGattService(device, u, data.handle, endGroupHandle));
        }

//        auto last = values.back();

        startHandle = endGroupHandle;
    } while (startHandle != 0xffff);

//    for (auto &s : services) {
//        D << "service: " << to_string(s->getUuid()) << ", handle: " << s->getHandle() << ", end group handle: " << s->getEndGroupHandle();
//        D << "s= " << (uint64_t) s;
//    }

    auto it = services.begin(),
            end = services.end();

    if (it == end) {
        return;
    }

    startHandle = 0x0001;

    auto s = *it;

    do {
        vector<AttributeData> values = discoverCharacteristics(startHandle, 0xffff);

        if (values.size() == 0) {
            break;
        }

        uint16_t lastHandle = 0xffff;

        for (auto &c : values) {
            while (c.handle > s->getEndGroupHandle() && it != end) {
                s = *it++;
//                D << "service: " << s->getHandle();
            }

            lastHandle = c.handle;

            uint8_t properties = c.value.read8();
            uint16_t valueHandle = c.value.read16le();
            uuid_t uuid = readUuid(&device, c.value);

//            D << "characteristic: handle: " << setw(2) << setfill('0') << hex << (int) c.handle <<
//                        ", properties: " << setw(2) << setfill('0') << hex << (int) properties <<
//                        ", valueHandle: 0x" << setw(4) << setfill('0') << hex << (int) valueHandle <<
//                        ", uuid: " << uuid;

            s->addCharacteristic(new DefaultBluetoothGattCharacteristic(*s, c.handle, uuid, properties, valueHandle));
        }

        auto last = values.back();

        startHandle = lastHandle + (uint8_t) 2;
    } while (startHandle != 0xffff);
}

ByteBuffer LinuxBluetoothGatt::writeAndRead(ByteBuffer &out, shared_ptr<uint8_t> buffer, size_t size) {
//    LOG_DEBUG("pdu size=" << out.getCursor());
    ssize_t written = write(l2cap, buffer.get(), out.getCursor());

//    LOG_DEBUG("written=" << written);

    ssize_t r = read(l2cap, buffer.get(), size);

    if (r == -1) {
        throw BluetoothException(&device, "read(): " + errnoAsString());
    }

    auto in = ByteBuffer(buffer, (size_t) r, (size_t) r);

//    LOG_DEBUG("read: " << r << " bytes: " << in.toString());

    return in;
}

vector<AttributeData> LinuxBluetoothGatt::discoverServices(uint16_t startHandle) {
    shared_ptr<uint8_t> buffer(new uint8_t[MAX_MTU]);
    ByteBuffer out = ByteBuffer(buffer, MAX_MTU);

    AttPdu::makeReadByGroupType(out, startHandle, 0xffff, uuids::PRIMARY_SERVICE);

    ByteBuffer in = writeAndRead(out, buffer, MAX_MTU);

    vector<AttributeData> values = AttPdu::parseReadByGroupType(in);

    LOG_DEBUG("READ_BY_GROUP_TYPE response has " + to_string(values.size()) + " values");

    return values;
}

vector<AttributeData> LinuxBluetoothGatt::discoverCharacteristics(uint16_t startHandle, uint16_t endHandle) {
    shared_ptr<uint8_t> buffer(new uint8_t[MAX_MTU]);
    ByteBuffer out = ByteBuffer(buffer, MAX_MTU);

    AttPdu::makeReadByType(out, startHandle, endHandle, uuids::CHARACTERISTIC);

    ByteBuffer in = writeAndRead(out, buffer, MAX_MTU);

    vector<AttributeData> values = AttPdu::parseReadByType(in);

    LOG_DEBUG("READ_BY_TYPE response has " + to_string(values.size()) + " values");

    return values;
}

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

LinuxBluetoothAdapter::LinuxBluetoothAdapter(int hciDeviceId) :
        scanning(false) {
    LOG_DEBUG("hciDeviceId=" << hciDeviceId);

    this->hciDeviceId = hciDeviceId;
    hciSocket = ::hci_open_dev(hciDeviceId);

    LOG_DEBUG("HCI socket: " << hciSocket);

    if (hciSocket == -1) {
        throw BluetoothException(this, "Could not open HCI device " + to_string(hciDeviceId));
    }

    hci_filter_clear(&hciFilter);
    hci_filter_set_ptype(HCI_EVENT_PKT, &hciFilter);
    hci_filter_set_event(EVT_LE_META_EVENT, &hciFilter);
    hci_filter_set_event(EVT_LE_ADVERTISING_REPORT, &hciFilter);
    setsockopt(hciSocket, SOL_HCI, HCI_FILTER, &hciFilter, sizeof(hciFilter));
}

LinuxBluetoothAdapter::~LinuxBluetoothAdapter() {
    stopScan();

    close(hciSocket);

    for (auto &pair : devices) {
        delete pair.second;
    }
}

void LinuxBluetoothAdapter::startScan() {
    struct hci_dev_info di;

    if (hci_devinfo(hciDeviceId, &di) < 0) {
        throw BluetoothException(this, "HCI adapter is not up: " + to_string(hciDeviceId));
    }

    LOG_DEBUG("hciDeviceId.dev_id=" << di.dev_id);
    LOG_DEBUG("hciDeviceId.bdaddr=" << parseMac(di.bdaddr).str());
    LOG_DEBUG("hciDeviceId.flags=" << setw(8) << setfill('0') << hex << di.flags);
    LOG_DEBUG("hciDeviceId.flags RUNNING = " << hci_test_bit(HCI_RUNNING, &di.flags));
    LOG_DEBUG("hciDeviceId.flags UP      = " << hci_test_bit(HCI_UP, &di.flags));
    LOG_DEBUG("hciDeviceId.flags PSCAN   = " << hci_test_bit(HCI_PSCAN, &di.flags));
    LOG_DEBUG("hciDeviceId.flags ISCAN   = " << hci_test_bit(HCI_ISCAN, &di.flags));
    LOG_DEBUG("hciDeviceId.name=" << di.name);

    int up = hci_test_bit(HCI_UP, &di.flags);

    if (!up) {
        throw BluetoothException(this, "HCI adapter is not up: " + to_string(hciDeviceId));
    }

    if (hci_le_set_scan_parameters(hciSocket, 0x01, htobs(0x0010), htobs(0x0010), 0x00, 0, 1000) < 0) {
        throw BluetoothException(this, "hci_le_set_scan_parameters: " + errnoAsString());
    }

    if (hci_le_set_scan_enable(hciSocket, 1, 0, 1000)) {
        throw BluetoothException(this, "Could not start scanning: other" + errnoAsString());
    }

    scanning = true;
}

void LinuxBluetoothAdapter::stopScan() {
    if (!scanning) {
        return;
    }

    scanning = false;

    if (hci_le_set_scan_enable(hciSocket, 0, 0, 1000) < 0) {
        LOG_WARN("stopScan: hci_le_set_scan_enable: " << errnoAsString());
    }
}

BluetoothDevice &LinuxBluetoothAdapter::getDevice(Mac &mac) {
    map<Mac, LinuxBluetoothDevice *>::iterator it = devices.find(mac);

    if (it == devices.end()) {
        LinuxBluetoothDevice *device = new LinuxBluetoothDevice(*this, mac);
        devices[mac] = device;
        return *device;
    }

    return *it->second;
}

void LinuxBluetoothAdapter::runScan(void (*callback)(BluetoothDevice &device)) {
    fd_set rfds;
    FD_ZERO(&rfds);
    FD_SET(hciSocket, &rfds);

    startScan();

    while (scanning) {
        // Linux can change tv, so it has to be reinitialized
        struct timeval tv;
        tv.tv_sec = 1;
        tv.tv_usec = 0;

        int selected = select(hciSocket + 1, &rfds, NULL, NULL, &tv);

        if (selected == -1) {
            throw BluetoothException(this, "select() failed");
        }

        if (selected == 0) {
            LOG_DEBUG("timeout");
            // Timeout, just continue
            continue;
        }

        unsigned char hciEventBuf[HCI_MAX_EVENT_SIZE];

        ssize_t len = read(hciSocket, hciEventBuf, sizeof(hciEventBuf));
        evt_le_meta_event *metaEvent = (evt_le_meta_event *) (hciEventBuf + (1 + HCI_EVENT_HDR_SIZE));
        len -= (1 + HCI_EVENT_HDR_SIZE);

        LOG_DEBUG("metaEvent->subevent = " << std::hex << (int) metaEvent->subevent);

        if (metaEvent->subevent == EVT_LE_ADVERTISING_REPORT) {
            le_advertising_info *advertisingInfo = (le_advertising_info *) (metaEvent->data + 1);

            Mac mac = parseMac(advertisingInfo->bdaddr);

            BluetoothDevice &device = getDevice(mac);

            callback(device);
        }
    }
}

}
}
}

// -----------------------------------------------------------------------
// Implementation of platform-specific method.
// -----------------------------------------------------------------------

namespace trygvis {
namespace bluetooth {
using namespace trygvis::bluetooth::linux;

map<int, LinuxBluetoothAdapter *> adapters;

BluetoothAdapter &getAdapterImpl(int hciDevice) {
    map<int, LinuxBluetoothAdapter *>::iterator it = adapters.find(hciDevice);

    if (it == adapters.end()) {
        LinuxBluetoothAdapter *adapter = new LinuxBluetoothAdapter(hciDevice);
        adapters[hciDevice] = adapter;
        return *adapter;
    }

    return *it->second;
}

void shutdownImpl() {
    for (auto &pair: adapters) {
        delete pair.second;
    }
}

}
}