#include <string.h>
#include <stropts.h>
#include <sys/select.h>
#include <unistd.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/hci_lib.h>
#include <bluetooth/l2cap.h>
#include <map>
#include <sstream>
#include <iomanip>

#include "Bluetooth.h"

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

#define MAX_MTU 256

namespace trygvis {

    class LinuxBluetoothDevice;

    class LinuxBluetoothAdapter;

    class LinuxBluetoothManager;

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

        ~LinuxBluetoothAdapter();

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

        BluetoothDevice &getDevice(Mac &mac) override;

    private:
        void startScan();

        void stopScan();

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

    class LinuxBluetoothDevice : public BluetoothDevice {
    public:
        LinuxBluetoothDevice(LinuxBluetoothAdapter &adapter, Mac mac);

        Mac const &mac() override;

        LinuxBluetoothAdapter &adapter() override;

        void connect() override;

        void disconnect() override;

        void discoverServices() override;

    private:
        LinuxBluetoothAdapter &_adapter;
        Mac _mac;
        int l2cap;
    };

    // 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) :
            _adapter(adapter), _mac(mac) {
    }

    Mac const &LinuxBluetoothDevice::mac() {
        return _mac;
    }

    LinuxBluetoothAdapter &LinuxBluetoothDevice::adapter() {
        return _adapter;
    }

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

        D << "connect: mac=" << _mac.str();

        l2cap = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
        if (l2cap < 0) {
            throw BluetoothException(this, "LinuxBluetoothDevice::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(this, "LinuxBluetoothDevice::connect(): 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(this, "LinuxBluetoothDevice::connect(): 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;
        _mac.copy(addr.l2_bdaddr.b[0],
                addr.l2_bdaddr.b[1],
                addr.l2_bdaddr.b[2],
                addr.l2_bdaddr.b[3],
                addr.l2_bdaddr.b[4],
                addr.l2_bdaddr.b[5]);

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

    void LinuxBluetoothDevice::disconnect() {
        DF << "mac=" << _mac.str();
        close(l2cap);
    }

    void LinuxBluetoothDevice::discoverServices() {
        DF;

        uint8_t buffer[MAX_MTU];
        ByteBuffer out = ByteBuffer(buffer, MAX_MTU, 0);

        AttPdu::makeReadByGroupType(out, 0x0001, 0xffff, UUID_PRIMARY_SERVICE);

        D << "pdu.size()=" << out.getSize();
        ssize_t written = write(l2cap, buffer, out.getSize());

        D << "written=" << written;

        ssize_t r = read(l2cap, buffer, MAX_MTU);
        ByteBuffer in = ByteBuffer(buffer, MAX_MTU, r);

        D << "read: " << r << " bytes";

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

        D << "READ_BY_GROUP_TYPE response has " + to_string(values->size()) + " values";

        for (auto &data: *values) {
            D << "handle: " << data->handle << ", value: " << data->value.toString();
        }

        delete values;
    }

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


    LinuxBluetoothAdapter::LinuxBluetoothAdapter(int hciDeviceId) {
        DF << "hciDeviceId=" << hciDeviceId;

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

        D << "HCI socket: " << hciSocket;

        if (hciSocket == -1) {
            throw BluetoothException(this, "Could not open HCI device " + 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() {
        DF;

        stopScan();

        close(hciSocket);
    }

    void LinuxBluetoothAdapter::startScan() {
        DF;

        struct hci_dev_info di;

        if (hci_devinfo(hciDeviceId, &di) < 0) {
            throw BluetoothException(this, "hci_devinfo: " + hciDeviceId);
        }

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

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

        if (!up) {
            throw BluetoothException(this, "HCI adapter is not up: " + 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() {
        DF;

        if (!scanning) {
            return;
        }

        scanning = false;

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

    BluetoothDevice &LinuxBluetoothAdapter::getDevice(Mac &mac) {
        static map<Mac, LinuxBluetoothDevice *> devices;

        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) {
                D << "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);

            D << "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);
            }
        }
    }

    // -----------------------------------------------------------------------
    //
    // -----------------------------------------------------------------------

    /*
    map<int, LinuxBluetoothAdapter *> adapters;

    BluetoothAdapter &getAdapter(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;
    }
    */

    BluetoothAdapter *getAdapter(int hciDevice) {
        return new LinuxBluetoothAdapter(hciDevice);
    }
};