#ifndef BT_H
#define BT_H

#include <QObject>
#include <QDebug>
#include <QTimer>

#include <QBluetoothDeviceDiscoveryAgent>
#include <QBluetoothUuid>
#include <QLowEnergyController>
#include <QLowEnergyConnectionParameters>

class BT : public QObject {

	Q_OBJECT

	QBluetoothDeviceDiscoveryAgent* m_deviceDiscoveryAgent = nullptr;
	QLowEnergyService* m_service = nullptr;

	QLowEnergyCharacteristic* charM1 = nullptr;
	QLowEnergyCharacteristic* charM2 = nullptr;

	QBluetoothDeviceInfo m_uwbDevice;

	enum State {
		DISCONNECTED,
		CONNECTING,
		CONNECTED
	};
	State state = State::DISCONNECTED;

public:


	void setChar(int idx, std::vector<uint8_t> data) {

		if (!m_service) {return;}
		if (m_service->characteristics().empty()) {return;}

		auto c = m_service->characteristics().at(idx);

		//qDebug() << "write char" << c.uuid() << " to " << data;

		QByteArray arr;
		for (uint8_t val : data) {arr.append(val);}
		m_service->writeCharacteristic(c, arr);


	}

	BT() {

		//findDevices();

	}

	void start() {
		findDevices();
	}

	void connectToDevice() {
		assert(m_uwbDevice.isValid());

		connectTo(m_uwbDevice);
	}

	void findDevices() {

		qDebug() << "findDevices()";

		// helper class to find nearby BLE devices
		m_deviceDiscoveryAgent = new QBluetoothDeviceDiscoveryAgent(this);
		m_deviceDiscoveryAgent->setLowEnergyDiscoveryTimeout(5000);

		// connect events
		assert(connect(m_deviceDiscoveryAgent, &QBluetoothDeviceDiscoveryAgent::deviceDiscovered, this, &BT::daDeviceDiscovered));
		assert(connect(m_deviceDiscoveryAgent, static_cast<void (QBluetoothDeviceDiscoveryAgent::*)(QBluetoothDeviceDiscoveryAgent::Error)>(&QBluetoothDeviceDiscoveryAgent::error), this, &BT::daError));
		assert(connect(m_deviceDiscoveryAgent, &QBluetoothDeviceDiscoveryAgent::finished, this, &BT::daScanFinished));
		assert(connect(m_deviceDiscoveryAgent, &QBluetoothDeviceDiscoveryAgent::canceled, this, &BT::daScanCanceled));

		// start scan
		m_deviceDiscoveryAgent->start(QBluetoothDeviceDiscoveryAgent::LowEnergyMethod);

	}

	void daDeviceDiscovered(const QBluetoothDeviceInfo& device) {

		qDebug() << "daDeviceDiscovered() " << device.name() << " " << device.address();

		if (device.coreConfigurations() & QBluetoothDeviceInfo::LowEnergyCoreConfiguration) {
			if (device.name() == "DWDA96" && device.address().toString() == "D1:6C:7A:99:57:71") {
				m_uwbDevice = device;

				//connectTo(device);
			}
		}

	}

	void daError(QBluetoothDeviceDiscoveryAgent::Error error) {
		qDebug() << "daError() " << error;
	}

	void daScanFinished() {
		qDebug() << "daScanFinished()";

		connectToDevice();
	}

	void daScanCanceled() {
		qDebug() << "daScanCanceled()";
	}







	QLowEnergyController* m_control = nullptr;

	void connectTo(const QBluetoothDeviceInfo& dev) {

		if (state != State::DISCONNECTED) {
			qDebug() << "connectTo() already called. skipping";
			return;
		}
		state = State::CONNECTING;

		qDebug() << "connectTo() " << dev.name() << " " << dev.address();

		// delte previous connection
		if (m_control) {
			m_control->disconnectFromDevice();
			delete m_control;
			m_control = nullptr;
		}

		/* 2 Step: QLowEnergyController */
		m_control = new QLowEnergyController(dev, this);
		m_control ->setRemoteAddressType(QLowEnergyController::RandomAddress);

		assert(connect(m_control, &QLowEnergyController::serviceDiscovered, this, &BT::lecServiceDiscovered));
		assert(connect(m_control, &QLowEnergyController::discoveryFinished, this, &BT::lecServiceScanDone));
		assert(connect(m_control, static_cast<void (QLowEnergyController::*)(QLowEnergyController::Error)>(&QLowEnergyController::error), this, &BT::lecError));
		assert(connect(m_control, &QLowEnergyController::connected, this, &BT::lecDeviceConnected));
		assert(connect(m_control, &QLowEnergyController::disconnected, this, &BT::lecDeviceDisconnected));
		assert(connect(m_control, &QLowEnergyController::connectionUpdated, this, &BT::lecConnectionUpdated));

		/* Start connecting to device */
		m_control->connectToDevice();
	//	setState(Connecting);

	}

	void lecServiceDiscovered(const QBluetoothUuid& gatt) {

		qDebug() << "lecServiceDiscovered() " << gatt;
		if (gatt.toString() == "{680c21d9-c946-4c1f-9c11-baa1c21329e7}") {
			qDebug() << "is our service";
			//_createService(gatt);
		}

	}

	void _createService(const QBluetoothUuid& uuid) {

		// cleanup previous service (if any)
		if (m_service) {
			delete m_service;
			m_service = nullptr;
		}

		// create new service
		m_service = m_control->createServiceObject(uuid, this);

		// connect events
		assert(connect(m_service, &QLowEnergyService::stateChanged         , this, &BT::lesServiceStateChanged));
		assert(connect(m_service, &QLowEnergyService::characteristicChanged, this, &BT::lesUpdateData));
		assert(connect(m_service, &QLowEnergyService::descriptorWritten    , this, &BT::lesConfirmedDescriptorWrite));
		assert(connect(m_service, &QLowEnergyService::characteristicRead   , this, &BT::lesCharacteristicRead));
		// start discovering all characteristics and descriptors on this service
		qDebug() << "discoverDetails()";
		m_service->characteristics().clear();
		//m_service->discoverDetails();

		auto c = m_service->characteristic(QBluetoothUuid(QString("{003bbdf2-c634-4b3d-ab56-7ec889b89a37}")));

		assert(c.isValid());

		QLowEnergyDescriptor m_notificationDesc = c.descriptor(QBluetoothUuid::ClientCharacteristicConfiguration);
		m_service->writeDescriptor(m_notificationDesc, QByteArray::fromHex("0100"));

	}

	void lecServiceScanDone() {
		qDebug() << "lecServiceScanDone()";

		_createService(QBluetoothUuid::fromString(QString("{680c21d9-c946-4c1f-9c11-baa1c21329e7}")));
	}

	void lecError(QLowEnergyController::Error err) {
		qDebug() << "lecError() " << err;
	}

	void _faster() {
		QLowEnergyConnectionParameters param;
		//param.setIntervalRange(100,100);
		param.setIntervalRange(7.5, 7.5);
		//param.setSupervisionTimeout(1000);
		param.setLatency(100);
		m_control->requestConnectionUpdate(param);
	}

	void lecDeviceConnected() {

		state = State::CONNECTED;
		qDebug() << "lecDeviceConnected()";

		// faster!!
		//_faster();



		// trigger service discovery;
		m_control->discoverServices();

	}

	void lecConnectionUpdated(const QLowEnergyConnectionParameters& params) {
		qDebug() << "lecConnectionUpdated(" << params.minimumInterval() << "," << params.maximumInterval() << ")";
	}

	void lecDeviceDisconnected() {
		state = State::DISCONNECTED;
		qDebug() << "lecDeviceDisconnected()";
	}


	void lesServiceStateChanged(QLowEnergyService::ServiceState s) {
		qDebug() << "lesServiceStateChanged()" << s;

		// A descriptoc can only be written if the service is in the ServiceDiscovered state
		switch (s) {
			case QLowEnergyService::ServiceDiscovered:
				_gotServiceCharacteristics();
				break;
			default:
				break;
		}

	}

	QLowEnergyDescriptor m_notificationDesc;
	QLowEnergyCharacteristic m_DeviceInfoChar;
	QTimer* m_readTimer;

	void _gotServiceCharacteristics() {
		for (const auto& c : m_service->characteristics()) {
			char read   = c.properties().testFlag(QLowEnergyCharacteristic::Read)         ? 'R' : ' ';
			char write  = c.properties().testFlag(QLowEnergyCharacteristic::Write)        ? 'W' : ' ';
			char notify = c.properties().testFlag(QLowEnergyCharacteristic::Notify)       ? 'N' : ' ';
			char broad  = c.properties().testFlag(QLowEnergyCharacteristic::Broadcasting) ? 'B' : ' ';

			qDebug() << "char: " << c.uuid() << read << write << notify << broad;
//			QByteArray arr; arr.append(30);
//			m_service->writeCharacteristic(c, arr);

			if (c.uuid().toString() == "{1e63b1eb-d4ed-444e-af54-c1e965192501}") {
//				m_DeviceInfoChar = c;
				//m_service->readCharacteristic(m_DeviceInfoChar);

//				QTimer::singleShot(100, this, SLOT(doRead()));
//				qDebug() << "Test";

//				if(!m_readTimer){
//					m_readTimer = new QTimer(this);
//					connect(m_readTimer, &QTimer::timeout, this, &BT::doRead);
//					m_readTimer->start(100); // in ms
//				}
			}

			if (c.uuid().toString() == "{003bbdf2-c634-4b3d-ab56-7ec889b89a37}") {
				QLowEnergyDescriptor m_notificationDesc = c.descriptor(QBluetoothUuid::ClientCharacteristicConfiguration);
				m_service->writeDescriptor(m_notificationDesc, QByteArray::fromHex("0100"));
			}
		}
	}

public slots:
	void doRead()
	{
		qDebug() << "DoRead";
		m_service->readCharacteristic(m_DeviceInfoChar);
	}

public:

	void lesCharacteristicRead(const QLowEnergyCharacteristic& info, const QByteArray& value)
	{
		qDebug() << "lesCharacteristicRead() " << info.uuid() << value;
	}

	void lesUpdateData(const QLowEnergyCharacteristic& c,const QByteArray& value) {
		qDebug() << "lesUpdateData() " << c.uuid() << value;
	}

	void lesConfirmedDescriptorWrite(const QLowEnergyDescriptor& d, const QByteArray &value) {
		qDebug() << "lesConfirmedDescriptorWrite()" << d.uuid() << value;
	}


};

#endif // BT_H