// ConsoleApplication1.cpp : Defines the entry point for the console application.
//

#include "pch.h"
#include <process.h>

#include "..\..\..\..\Source\Bluetooth\wclBluetooth.h"

using namespace std;
using namespace wclCommon;
using namespace wclBluetooth;

// The class incapsulates a single GATT connection.
class CGattConnection
{
private:
	HANDLE				ConnectEvent;
	HANDLE				TerminateEvent;
	HANDLE				InitEvent;
	int					InitResult;
	HANDLE				GattThread;
	CwclGattClient*		Client;
	CwclBluetoothRadio*	BtRadio;

	void GetMaxPduSize()
	{
		unsigned short Size;
		int Res = Client->GetMaxPduSize(Size);
		if (Res != WCL_E_SUCCESS)
			cout << "[" << hex << Client->Address << "]: Get max PDU size error: 0x" << Res << dec << endl;
		else
			cout << "[" << hex << Client->Address << dec << "]: Max PDU size: " << Size << endl;
	}

	void GetConnectionParams()
	{
		wclBluetoothLeConnectionParameters Params;
		int Res = Client->GetConnectionParams(Params);
		if (Res != WCL_E_SUCCESS)
			cout << "[" << hex << Client->Address << "]: Get connection params error: 0x" << Res << dec << endl;
		else
		{
			cout << "[" << hex << Client->Address << dec << "]: Connection params: " << endl;
			cout << "  Interval     : " << Params.Interval << endl;
			cout << "  Latency      : " << Params.Latency << endl;
			cout << "  Link Timeout : " << Params.LinkTimeout << endl;
		}
	}

	void GetConnectionPhy()
	{
		wclBluetoothLeConnectionPhy Phy;
		int Res = Client->GetConnectionPhyInfo(Phy);
		if (Res != WCL_E_SUCCESS)
			cout << "[" << hex << Client->Address << "]: Get connection PHY error: 0x" << Res << dec << endl;
		else
		{
			cout << "[" << hex << Client->Address << dec << "]: Connection PHY: " << endl;
			cout << "  Receive" << endl;
			cout << "    IsCoded        : " << Phy.Receive.IsCoded << endl;
			cout << "    IsUncoded1MPhy : " << Phy.Receive.IsUncoded1MPhy << endl;
			cout << "    IsUncoded2MPhy : " << Phy.Receive.IsUncoded2MPhy << endl;
			cout << "  Transmit" << endl;
			cout << "    IsCoded        : " << Phy.Transmit.IsCoded << endl;
			cout << "    IsUncoded1MPhy : " << Phy.Transmit.IsUncoded1MPhy << endl;
			cout << "    IsUncoded2MPhy : " << Phy.Transmit.IsUncoded2MPhy << endl;
		}
	}

	string wtoa(const tstring& w) const
	{
		return string(w.begin(), w.end());
	}

	string UuidToString(const wclGattUuid& Uuid) const
	{
		if (Uuid.IsShortUuid)
		{
			char short_str[4];
			snprintf(short_str, sizeof(short_str), "%04x", Uuid.ShortUuid);
			return string(short_str);
		}
		
		char guid_cstr[39];
		GUID guid = Uuid.LongUuid;
		snprintf(guid_cstr, sizeof(guid_cstr),
			"{%08x-%04x-%04x-%02x%02x-%02x%02x%02x%02x%02x%02x}",
			guid.Data1, guid.Data2, guid.Data3,
			guid.Data4[0], guid.Data4[1], guid.Data4[2], guid.Data4[3],
			guid.Data4[4], guid.Data4[5], guid.Data4[6], guid.Data4[7]);

		return string(guid_cstr);
	}

	void DumpValue(const unsigned char* const Value, 
		const unsigned long Length) const
	{
		if (Value != NULL && Length > 0)
		{
			string s = "";
			for (unsigned long i = 0; i < Length; i++)
			{
				char c[2];
				snprintf(c, 2, "%02x", Value[i]);
				s = s + string(c);
			}
			cout << "[" << hex << Client->Address << dec << "]: value: " << s.c_str() << endl;
		}
	}

	void Client_OnCharacteristicChanged(void* Sender, const unsigned short Handle, 
		const unsigned char* const Value, const unsigned long Length)
	{
		UNREFERENCED_PARAMETER(Sender);
		UNREFERENCED_PARAMETER(Value);
		UNREFERENCED_PARAMETER(Length);

		cout << "[" << hex << Client->Address << "]: value " << Handle << " changed" << dec << endl;
	}
	
	void Client_OnDisconnect(void* Sender, const int Reason)
	{
		UNREFERENCED_PARAMETER(Sender);

		cout << "[" << hex << Client->Address << "]: disconnected with reason: 0x" << Reason << dec << endl;
	}

	void Client_OnConnect(void* Sender, const int Error)
	{
		UNREFERENCED_PARAMETER(Sender);

		InitResult = Error;
		if (Error == WCL_E_SUCCESS)
		{
			cout << "[" << hex << Client->Address << dec << "]: connected" << endl;
			GetMaxPduSize();
			GetConnectionParams();
			GetConnectionPhy();
		}
		else
			cout << "[" << hex << Client->Address << "]: connect error: 0x" << Error << dec << endl;
		SetEvent(ConnectEvent);
	}

	void Client_OnMaxPduSizeChanged(void* Sender)
	{
		UNREFERENCED_PARAMETER(Sender);

		GetMaxPduSize();
	}

	void Client_OnConnectionParamsChanged(void* Sender)
	{
		UNREFERENCED_PARAMETER(Sender);

		GetConnectionParams();
	}

	void Client_OnConnectionPhyChanged(void* Sender)
	{
		UNREFERENCED_PARAMETER(Sender);

		GetConnectionPhy();
	}

	void ReadCharacteristics(const wclGattService& Service)
	{
		wclGattCharacteristics Characteristics;
		int Res = Client->ReadCharacteristics(Service, goNone, Characteristics);
		if (Res != WCL_E_SUCCESS)
			cout << "[" << hex << Client->Address << "]: read characteristics error: 0x" << Res << dec << endl;
		else
		{
			if (Characteristics.size() == 0)
				cout << "[" << hex << Client->Address << "]: no characteristics found" << dec << endl;
			else
			{
				for (wclGattCharacteristics::iterator i = Characteristics.begin(); i != Characteristics.end(); i++)
				{
					wclGattCharacteristic Characteristic = (*i);
					cout << "[" << hex << Client->Address << dec << "]: characteristic: " << UuidToString(Characteristic.Uuid).c_str() << endl;
					if (Characteristic.IsReadable)
					{
						cout << "[" << hex << Client->Address << dec << "]: readable" << endl;
						unsigned char* Value;
						unsigned long Length;
						Res = Client->ReadCharacteristicValue(Characteristic, goNone, Value, Length);
						if (Value != NULL && Length > 0)
							cout << "[" << hex << Client->Address << dec << "]: value is empty" << endl;
						else
							DumpValue(Value, Length);
					}

					if (Characteristic.IsIndicatable || Characteristic.IsNotifiable)
					{
						cout << "[" << hex << Client->Address << dec << "]: notifiable (or indicatable)" << endl;
						wclGattCharacteristic Chr = Characteristic;
						if (Characteristic.IsIndicatable && Characteristic.IsNotifiable)
							Chr.IsIndicatable = false;
						Res = Client->Subscribe(Chr);
						if (Res != WCL_E_SUCCESS)
							cout << "[" << hex << Client->Address << "]: subscribe error: 0x" << Res << dec << endl;
						else
						{
							cout << "[" << hex << Client->Address << "]: subscribed" << dec << endl;
							Res = Client->WriteClientConfiguration(Chr, true, goNone);
							if (Res != WCL_E_SUCCESS)
							{
								cout << "[" << hex << Client->Address << "]: write configuration error: 0x" << Res << dec << endl;
								Client->Unsubscribe(Chr);
							}
							else
								cout << "[" << hex << Client->Address << "]: write configuration completed" << dec << endl;
						}
					}
				}
			}
		}
	}

	void ReadServices()
	{
		wclGattServices Services;
		int Res = Client->ReadServices(goNone, Services);
		if (Res != WCL_E_SUCCESS)
			cout << "[" << hex << Client->Address << "]: read services error: 0x" << Res << dec << endl;
		else
		{
			if (Services.size() == 0)
				cout << "[" << hex << Client->Address << "]: no services found" << dec << endl;
			else
			{
				cout << "[" << hex << Client->Address << dec << "]: found " << Services.size() << " services" << endl;
				for (wclGattServices::iterator i = Services.begin(); i != Services.end(); i++)
				{
					wclGattService Service = (*i);
					cout << "[" << hex << Client->Address << dec << "]: service: " << UuidToString(Service.Uuid).c_str() << endl;
					ReadCharacteristics(Service);
				}
			}
		}
	}

	static UINT __stdcall _ConnectionThread(LPVOID Param)
	{
		((CGattConnection*)Param)->ConnectionThread();
		return 0;
	}

	void ConnectionThread()
	{
		InitResult = Client->Connect(BtRadio);
		if (InitResult == WCL_E_SUCCESS)
		{
			CwclMessageBroadcaster::Wait(ConnectEvent);
		}

		if (InitResult == WCL_E_SUCCESS)
			ReadServices();

		SetEvent(InitEvent);

		if (InitResult == WCL_E_SUCCESS)
		{
			CwclMessageBroadcaster::Wait(TerminateEvent);
			Client->Disconnect();
		}
	}

public:
	CGattConnection()
	{
		ConnectEvent = NULL;
		TerminateEvent = NULL;
		InitEvent = NULL;
		InitResult = WCL_E_SUCCESS;
		GattThread = NULL;
		Client = NULL;
		BtRadio = NULL;
	}

	bool Connect(CwclBluetoothRadio* const Radio, const __int64 Address)
	{
		if (GattThread != NULL)
			return false;

		bool Result = false;

		ConnectEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
		if (ConnectEvent == NULL)
			cout << "[" << hex << Address << dec << "]: Create connection event failed" << endl;
		else
		{
			TerminateEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
			if (TerminateEvent == NULL)
				cout << "[" << hex << Address << dec << "]: Create termination event failed" << endl;
			else
			{
				InitEvent = CreateEvent(NULL, TRUE, FALSE, NULL);
				if (InitEvent == NULL)
					cout << "[" << hex << Address << dec << "]: Create initialization event failed" << endl;
				else
				{
					GattThread = (HANDLE)_beginthreadex(NULL, 0, _ConnectionThread, this, CREATE_SUSPENDED, NULL);
					if (GattThread == NULL)
						cout << "[" << hex << Address << dec << "]: Create communication thread failed" << endl;
					else
					{
						BtRadio = Radio;

						Client = new CwclGattClient();
						Client->Address = Address;
						__hook(&CwclGattClient::OnConnect, Client, &CGattConnection::Client_OnConnect);
						__hook(&CwclGattClient::OnDisconnect, Client, &CGattConnection::Client_OnDisconnect);
						__hook(&CwclGattClient::OnCharacteristicChanged, Client, &CGattConnection::Client_OnCharacteristicChanged);
						__hook(&CwclGattClient::OnMaxPduSizeChanged, Client, &CGattConnection::Client_OnMaxPduSizeChanged);
						__hook(&CwclGattClient::OnConnectionParamsChanged, Client, &CGattConnection::Client_OnConnectionParamsChanged);
						__hook(&CwclGattClient::OnConnectionPhyChanged, Client, &CGattConnection::Client_OnConnectionPhyChanged);

						InitResult = WCL_E_SUCCESS;
						ResumeThread(GattThread);
						
						WaitForSingleObject(InitEvent, INFINITE);
						Result = (InitResult == WCL_E_SUCCESS);
						if (!Result)
						{
							WaitForSingleObject(GattThread, INFINITE);
							CloseHandle(GattThread);
							GattThread = NULL;
							__unhook(Client);
							delete Client;
							BtRadio = NULL;
						}
					}

					CloseHandle(InitEvent);
					InitEvent = NULL;
				}

				if (!Result)
				{
					CloseHandle(TerminateEvent);
					TerminateEvent = NULL;
				}
			}

			if (!Result)
			{
				CloseHandle(ConnectEvent);
				ConnectEvent = NULL;
			}
		}
		return Result;
	}

	void Disconnect()
	{
		if (GattThread != NULL)
		{
			SetEvent(TerminateEvent);
			WaitForSingleObject(GattThread, INFINITE);

			CloseHandle(ConnectEvent);
			ConnectEvent = NULL;

			CloseHandle(TerminateEvent);
			TerminateEvent = NULL;

			InitResult = WCL_E_SUCCESS;
			
			CloseHandle(GattThread);
			GattThread = NULL;

			__unhook(Client);
			delete Client;
			Client = NULL;

			BtRadio = NULL;
		}
	}
};

class CRunner
{
private:
	typedef list<CGattConnection*> CONNECTIONS;
	typedef list<__int64> DEVICES;

	CONNECTIONS	Connections;
	DEVICES		Devices;
	HANDLE		DiscoveryEvent;

	void Manager_OnDiscoveringCompleted(void* Sender, CwclBluetoothRadio* const Radio, 
		const int Error)
	{
		UNREFERENCED_PARAMETER(Sender);
		UNREFERENCED_PARAMETER(Radio);

		cout << "Discovering compleetd with result: 0x" << hex << Error << dec << endl;
		SetEvent(DiscoveryEvent);
	}

	void Manager_OnDeviceFound(void* Sender, CwclBluetoothRadio* const Radio, 
		const __int64 Address)
	{
		UNREFERENCED_PARAMETER(Sender);
		UNREFERENCED_PARAMETER(Radio);

		cout << "Device found: " << hex << Address << dec << endl;
		Devices.push_back(Address);
	}

	void Manager_OnDiscoveringStarted(void* Sender, CwclBluetoothRadio* const Radio)
	{
		UNREFERENCED_PARAMETER(Sender);
		UNREFERENCED_PARAMETER(Radio);

		cout << "Discovering started" << endl;
		Devices.clear();
	}

public:
	CRunner()
	{
		DiscoveryEvent = NULL;
	}

	void Start()
	{
		// Set synchronization.
		CwclMessageBroadcaster::SetSyncMethod(skApc);

		system("cls");

		cout << "GATT CLIENT DEMO" << endl;
		cout << "================" << endl;
		cout << "This demo discovers all BLE devices and connects to each of them." << endl;
		cout << "After connection established the demo trys to read services and characteristics." << endl;
		cout << "It also subscribes for characteristic changes notification." << endl;
		cout << "" << endl;

		DiscoveryEvent = CreateEvent(NULL, TRUE, FALSE, NULL);

		// Create and initialize Bluetooth Manager.
		CwclBluetoothManager* Manager = new CwclBluetoothManager();
		__hook(&CwclBluetoothManager::OnDiscoveringStarted, Manager, &CRunner::Manager_OnDiscoveringStarted);
		__hook(&CwclBluetoothManager::OnDeviceFound, Manager, &CRunner::Manager_OnDeviceFound);
		__hook(&CwclBluetoothManager::OnDiscoveringCompleted, Manager, &CRunner::Manager_OnDiscoveringCompleted);

		// Try to open Bluetooth Manager.
		int Res = Manager->Open();
		if (Res != WCL_E_SUCCESS)
			cout << "Bluetooth Manager open failed. Error: 0x" << hex << Res << dec << endl;
		else
		{
			if (Manager->Count == 0)
				cout << "No Bluetooth hardware was found" << endl;
			else
			{
				CwclBluetoothRadio* Radio = NULL;
				// Try to find available Bluetooth Radio.
				for (size_t i = 0; i < Manager->Count; i++)
				{
					if (Manager->Radios[i]->Available)
					{
						Radio = Manager->Radios[i];
						break;
					}
				}

				if (Radio == NULL)
					cout << "No Bluetooth Radio was found" << endl;
				else
				{
					cout << "All ready" << endl;
					cout << "Press any key to start discovering Bluetooth LE devices" << endl;
					cout << "Press ENTER to close the application." << endl;
					system("pause");

					// Start discovering.
					Res = Radio->Discover(10, dkBle);
					if (Res != WCL_E_SUCCESS)
						cout << "Failed to start discovering. Error: 0x" << hex << Res << dec << endl;
					else
					{
						// Wait for completion. We must use WaitEx to be able to process APC.
						CwclMessageBroadcaster::Wait(DiscoveryEvent);
						
						if (Devices.size() == 0)
							cout << "No Bluetooth LE devices were found" << endl;
						else
						{
							// Try to connect to each found device.
							for (DEVICES::iterator i = Devices.begin(); i != Devices.end(); i++)
							{
								__int64 Address = (*i);
								cout << "Connect to " << hex << Address << dec << endl;
								CGattConnection* Connection = new CGattConnection();
								if (!Connection->Connect(Radio, Address))
									cout << "Connect to failed" << endl;
								else
									Connections.push_back(Connection);
							}

							system("pause");
							for (CONNECTIONS::iterator i = Connections.begin(); i != Connections.end(); i++)
							{
								CGattConnection* Connection = (*i);
								Connection->Disconnect();
								delete Connection;
							}
						}
					}
				}
			}
			Manager->Close();
		}
		__unhook(Manager);
		delete Manager;

		Devices.clear();
		Connections.clear();

		CloseHandle(DiscoveryEvent);
	}
};

int main(int argc, char **argv)
{
	UNREFERENCED_PARAMETER(argc);
	UNREFERENCED_PARAMETER(argv);

	CRunner* Runner = new CRunner();
	Runner->Start();
	delete Runner;
}