diff --git a/.gitignore b/.gitignore
index 32ccd08..daf41ad 100644
--- a/.gitignore
+++ b/.gitignore
@@ -530,3 +530,7 @@ Client/imgui.ini
 
 # linux build
 Client/linux/sonyheadphonesclient.build/
+
+# build folders
+build-win/
+build-linux/
diff --git a/Client/BluetoothWrapper.cpp b/Client/BluetoothWrapper.cpp
index 6c25711..217e699 100644
--- a/Client/BluetoothWrapper.cpp
+++ b/Client/BluetoothWrapper.cpp
@@ -22,17 +22,25 @@ BluetoothWrapper& BluetoothWrapper::operator=(BluetoothWrapper&& other) noexcept
 	return *this;
 }
 
-int BluetoothWrapper::sendCommand(const std::vector<char>& bytes)
+int BluetoothWrapper::sendCommand(const std::vector<char>& bytes, DATA_TYPE dtype)
 {
+	int bytesSent;
 	std::lock_guard guard(this->_connectorMtx);
-	auto data = CommandSerializer::packageDataForBt(bytes, DATA_TYPE::DATA_MDR, this->_seqNumber++);
-	auto bytesSent = this->_connector->send(data.data(), data.size());
+	auto data = CommandSerializer::packageDataForBt(bytes, dtype, this->_seqNumber);
+	bytesSent = this->_connector->send(data.data(), data.size());
 
-	this->_waitForAck();
+	if (dtype != DATA_TYPE::ACK)
+		this->_waitForAck();
 
 	return bytesSent;
 }
 
+void BluetoothWrapper::sendAck(unsigned int seqNumber)
+{
+	auto data = CommandSerializer::packageDataForBt({}, DATA_TYPE::ACK, seqNumber ^ 0x01);
+	this->_connector->send(data.data(), data.size());
+}
+
 bool BluetoothWrapper::isConnected() noexcept
 {
 	return this->_connector->isConnected();
@@ -51,13 +59,29 @@ void BluetoothWrapper::disconnect() noexcept
 	this->_connector->disconnect();
 }
 
-
 std::vector<BluetoothDevice> BluetoothWrapper::getConnectedDevices()
 {
 	return this->_connector->getConnectedDevices();
 }
 
 void BluetoothWrapper::_waitForAck()
+{
+	std::unique_lock<std::mutex> guard(this->_dataMtx);
+
+	while (!(this->_ackBuffer > 0)){
+		this->_ack.wait(guard);
+	}
+
+	this->_ackBuffer--;
+}
+
+void BluetoothWrapper::postAck()
+{
+	std::lock_guard guard(this->_dataMtx);
+	this->_ackBuffer++;
+}
+
+Buffer BluetoothWrapper::readReplies()
 {
 	bool ongoingMessage = false;
 	bool messageFinished = false;
@@ -66,11 +90,25 @@ void BluetoothWrapper::_waitForAck()
 
 	do
 	{
-		auto numRecvd = this->_connector->recv(buf, sizeof(buf));
+		int i = 0;
+		while(!messageFinished)
+		{
+			this->_connector->recv(buf+i, 1);
+			i++;
+			if (buf[i-1] == 0x3c) 
+				messageFinished = true;
+				// break;
+		}
+		auto numRecvd = i;
 		size_t messageStart = 0;
 		size_t messageEnd = numRecvd;
+		// for (int i = 0; i < numRecvd; i++)
+		// {
+		// 	std::cout << std::hex << (0xff & (unsigned int)buf[i]) << " ";
+		// }
+		// std::cout << std::endl;
 
-		for (size_t i = 0; i < numRecvd; i++)
+		for (int i = 0; i < numRecvd; i++)
 		{
 			if (buf[i] == START_MARKER)
 			{
@@ -92,7 +130,14 @@ void BluetoothWrapper::_waitForAck()
 		msgBytes.insert(msgBytes.end(), buf + messageStart, buf + messageEnd);
 	} while (!messageFinished);
 
+	return msgBytes;
+
 	auto msg = CommandSerializer::unpackBtMessage(msgBytes);
 	this->_seqNumber = msg.seqNumber;
 }
 
+void BluetoothWrapper::setSeqNumber(unsigned char seqNumber)
+{
+	std::lock_guard guard(this->_dataMtx);
+	this->_seqNumber = seqNumber;
+}
diff --git a/Client/BluetoothWrapper.h b/Client/BluetoothWrapper.h
index 7c7c044..0bf9fac 100644
--- a/Client/BluetoothWrapper.h
+++ b/Client/BluetoothWrapper.h
@@ -7,7 +7,8 @@
 #include <vector>
 #include <string>
 #include <mutex>
-
+#include <future>
+#include <iostream>
 
 //Thread-safety: This class is thread-safe.
 class BluetoothWrapper
@@ -21,7 +22,10 @@ class BluetoothWrapper
 	BluetoothWrapper(BluetoothWrapper&& other) noexcept;
 	BluetoothWrapper& operator=(BluetoothWrapper&& other) noexcept;
 
-	int sendCommand(const std::vector<char>& bytes);
+	int sendCommand(const std::vector<char>& bytes, DATA_TYPE dtype = DATA_TYPE::DATA_MDR);
+	void sendAck(unsigned int seqNumber);
+
+	Buffer readReplies();
 
 	bool isConnected() noexcept;
 	//Try to connect to the headphones
@@ -29,11 +33,25 @@ class BluetoothWrapper
 	void disconnect() noexcept;
 
 	std::vector<BluetoothDevice> getConnectedDevices();
+	void setSeqNumber(unsigned char seqNumber);
+	void postAck();
 
 private:
 	void _waitForAck();
 
 	std::unique_ptr<IBluetoothConnector> _connector;
 	std::mutex _connectorMtx;
-	unsigned int _seqNumber = 0;
-};
\ No newline at end of file
+	std::mutex _dataMtx;
+	
+	/*
+		seqNumber logic:
+			every command that client sends has the inverse seqNumber of the last ACK packet sent by the headphones
+			every ACK packet sent by the client has the inverse seqNumber as the response being ACK'd (this is passed as a parameter to sendACK)
+			both seqNumbers are independent of each other
+	*/
+	unsigned char _seqNumber = 0;
+	unsigned int _ackBuffer = 0;
+
+public:
+	std::condition_variable _ack;
+};
diff --git a/Client/CMakeLists.txt b/Client/CMakeLists.txt
index ae6c029..e0a8ac2 100644
--- a/Client/CMakeLists.txt
+++ b/Client/CMakeLists.txt
@@ -1,5 +1,5 @@
-project(SonyHeadphonesClient)
 cmake_minimum_required(VERSION 3.1 FATAL_ERROR)
+project(SonyHeadphonesClient)
 
 set(CMAKE_CXX_STANDARD 20)
 
@@ -14,6 +14,7 @@ target_sources(SonyHeadphonesClient
     ByteMagic.cpp
     CommandSerializer.cpp
     TimedMessageQueue.cpp
+    Listener.cpp
     CrossPlatformGUI.cpp "Headphones.cpp")
 
 target_include_directories (SonyHeadphonesClient
diff --git a/Client/CommandSerializer.cpp b/Client/CommandSerializer.cpp
index 6dd30fe..fc61590 100644
--- a/Client/CommandSerializer.cpp
+++ b/Client/CommandSerializer.cpp
@@ -1,9 +1,17 @@
 #include "CommandSerializer.h"
 
-constexpr unsigned char ESCAPED_BYTE_SENTRY = 61;
-constexpr unsigned char ESCAPED_60 = 44;
-constexpr unsigned char ESCAPED_61 = 45;
-constexpr unsigned char ESCAPED_62 = 46;
+/* 
+ * Because 
+ * 	0x3E represents beginning of packet
+ * 	0x3C represents end of packet
+ * we need to escape these in the packet payload
+*/
+constexpr unsigned char ESCAPED_BYTE_SENTRY = 61;   // 0x3D
+constexpr unsigned char ESCAPED_60 = 44;	    // 0x2C
+constexpr unsigned char ESCAPED_61 = 45;	    // 0x2D
+constexpr unsigned char ESCAPED_62 = 46;   	    // 0x2E
+
+
 constexpr int MAX_STEPS_WH_1000_XM3 = 19;
 
 namespace CommandSerializer
@@ -133,23 +141,25 @@ namespace CommandSerializer
 		return ret;
 	}
 
-	Message unpackBtMessage(const Buffer& src)
+	BtMessage unpackBtMessage(const Buffer& src)
 	{
 		//Message data format: ESCAPE_SPECIALS(<DATA_TYPE><SEQ_NUMBER><BIG ENDIAN 4 BYTE SIZE OF UNESCAPED DATA><DATA><1 BYTE CHECKSUM>)
 		auto unescaped = _unescapeSpecials(src);
 
-		if (src.size() < 7)
+		if (unescaped.size() < 7)
 		{
 			throw std::runtime_error("Invalid message: Smaller than the minimum message size");
 		}
 
-		Message ret;
-		ret.dataType = static_cast<DATA_TYPE>(src[0]);
-		ret.seqNumber = src[1];
-		if ((unsigned char)src[src.size() - 1] != _sumChecksum(src.data(), src.size() - 1))
+		BtMessage ret;
+		ret.dataType = static_cast<DATA_TYPE>(unescaped[0]);
+		ret.seqNumber = unescaped[1];
+		if ((unsigned char)unescaped[unescaped.size() - 1] != _sumChecksum(unescaped.data(), unescaped.size() - 1))
 		{
 			throw RecoverableException("Invalid checksum!", true);
 		}
+		unsigned char numMsgBytes = static_cast<unsigned char>(unescaped[5]);
+		ret.messageBytes.insert(ret.messageBytes.end(), unescaped.begin() + 6, unescaped.begin() + 6 + numMsgBytes); 
 		return ret;
 	}
 
@@ -171,6 +181,16 @@ namespace CommandSerializer
 		return val;
 	}
 
+	Buffer serializeXM4OptimizeCommand(OPTIMIZER_STATE state)
+	{
+	    Buffer ret;
+	    ret.push_back(static_cast<unsigned char>(COMMAND_TYPE::XM4_OPTIMIZER_PARAM));
+	    ret.push_back(static_cast<unsigned char>(0x01));
+	    ret.push_back(static_cast<unsigned char>(0x00));
+	    ret.push_back(static_cast<unsigned char>(state));
+	    return ret;
+	}
+
 	Buffer serializeNcAndAsmSetting(NC_ASM_EFFECT ncAsmEffect, NC_ASM_SETTING_TYPE ncAsmSettingType, ASM_SETTING_TYPE asmSettingType, ASM_ID asmId, char asmLevel)
 	{
 		Buffer ret;
@@ -195,5 +215,40 @@ namespace CommandSerializer
 		return ret;
 	}
 
+	Buffer serializeXM4SpeakToChat(S2C_TOGGLE s2cState)
+	{
+		Buffer ret;
+		ret.push_back(static_cast<unsigned char>(COMMAND_TYPE::XM4_S2C_TOGGLE_PARAM));
+		ret.push_back(static_cast<unsigned char>(0x05));
+		ret.push_back(static_cast<unsigned char>(0x01));
+		ret.push_back(static_cast<unsigned char>(s2cState));
+		return ret;
+	}
+
+	Buffer serializeXM4_S2C_Options(unsigned char sensitivity, unsigned char voice, unsigned char offTime)
+	{
+		Buffer ret;
+		ret.push_back(static_cast<unsigned char>(COMMAND_TYPE::XM4_S2C_OPTIONS_PARAM));
+		ret.push_back(static_cast<unsigned char>(0x05));
+		ret.push_back(static_cast<unsigned char>(0x00));
+		ret.push_back(static_cast<unsigned char>(sensitivity));
+		ret.push_back(static_cast<unsigned char>(voice));
+		ret.push_back(static_cast<unsigned char>(offTime));
+		return ret;
+	}
+
+	Buffer serializeMultiPointCommand(MULTI_POINT_COMMANDS cmd, std::string macAddr)
+	{
+		Buffer ret;
+		ret.push_back(static_cast<unsigned char>(COMMAND_TYPE::MULTI_POINT_PARAM));
+		ret.push_back(static_cast<unsigned char>(0x01));
+		ret.push_back(static_cast<unsigned char>(0x00));
+		ret.push_back(static_cast<unsigned char>(cmd));
+		for (unsigned char c: macAddr)
+		{
+			ret.push_back(c);
+		}
+		return ret;
+	}
 }
 
diff --git a/Client/CommandSerializer.h b/Client/CommandSerializer.h
index 49e0aec..3538ff4 100644
--- a/Client/CommandSerializer.h
+++ b/Client/CommandSerializer.h
@@ -9,16 +9,17 @@
 constexpr int MINIMUM_VOICE_FOCUS_STEP = 2;
 constexpr unsigned int ASM_LEVEL_DISABLED = -1;
 
-namespace CommandSerializer
+struct BtMessage
 {
-	struct Message
-	{
-		DATA_TYPE dataType;
-		unsigned char seqNumber;
-		//Not really needed for now
-		//Buffer messageBytes;
-	};
+	DATA_TYPE dataType;
+	unsigned char seqNumber;
+	//Not really needed for now
+	Buffer messageBytes;
+};
+
 
+namespace CommandSerializer
+{
 	//escape special chars
 
 	Buffer _escapeSpecials(const Buffer& src);
@@ -36,10 +37,14 @@ namespace CommandSerializer
 	*/
 	Buffer packageDataForBt(const Buffer& src, DATA_TYPE dataType, unsigned int seqNumber);
 
-	Message unpackBtMessage(const Buffer& src);
+	BtMessage unpackBtMessage(const Buffer& src);
 
 	NC_DUAL_SINGLE_VALUE getDualSingleForAsmLevel(char asmLevel);
+	Buffer serializeXM4OptimizeCommand(OPTIMIZER_STATE state);
 	Buffer serializeNcAndAsmSetting(NC_ASM_EFFECT ncAsmEffect, NC_ASM_SETTING_TYPE ncAsmSettingType, ASM_SETTING_TYPE asmSettingType, ASM_ID asmId, char asmLevel);
 	Buffer serializeVPTSetting(VPT_INQUIRED_TYPE type, unsigned char preset);
+	Buffer serializeXM4SpeakToChat(S2C_TOGGLE s2cState);
+	Buffer serializeXM4_S2C_Options(unsigned char sensitivity, unsigned char voice, unsigned char offTime);
+	Buffer serializeMultiPointCommand(MULTI_POINT_COMMANDS cmd, std::string macAddr);
 }
 
diff --git a/Client/Constants.h b/Client/Constants.h
index fcdab63..2ab3d95 100644
--- a/Client/Constants.h
+++ b/Client/Constants.h
@@ -86,7 +86,21 @@ enum class NC_DUAL_SINGLE_VALUE : signed char
 enum class COMMAND_TYPE : signed char
 {
 	VPT_SET_PARAM = 72,
-	NCASM_SET_PARAM = 104
+	NCASM_SET_PARAM = 104,
+	XM4_OPTIMIZER_PARAM = (signed char) 0x84,
+	XM4_S2C_TOGGLE_PARAM = (signed char) 0xf8,
+	XM4_S2C_OPTIONS_PARAM = (signed char) 0xfc,
+	MULTI_POINT_PARAM = (signed char) 0x3c,
+
+	XM4_OPTIMIZER_RESPONSE = (signed char) 0x85,
+	DEVICES_QUERY_RESPONSE = (signed char) 0x37,
+	DEVICES_STATE_RESPONSE = (signed char) 0x39,
+	CAPABILITY_QUERY_RESPONSE = (signed char) 0x07,
+
+	CAPABILITY_QUERY = (signed char) 0x06,
+	MULTI_POINT_DEVICES_QUERY = (signed char) 0x36,
+	S2C_QUERY = (signed char) 0xf6,
+	S2C_OPTIONS_QUERY = (signed char) 0xfa
 };
 
 enum class VPT_PRESET_ID : signed char
@@ -128,3 +142,41 @@ enum class VPT_INQUIRED_TYPE : signed char
 	SOUND_POSITION = 2,
 	OUT_OF_RANGE = -1
 };
+
+enum class OPTIMIZER_STATE : signed char
+{
+	IDLE = 0,
+	OPTIMIZING = 1
+};
+
+enum class S2C_TOGGLE : signed char
+{
+	ACTIVE = 1,
+	INACTIVE = 0
+};
+
+enum class MULTI_POINT_COMMANDS	: signed char
+{
+	CONNECT = (signed char) 0x01,
+	DISCONNECT = (signed char) 0x00,
+	UNPAIR = (signed char) 0x02
+};
+
+enum DEVICE_CAPABILITIES
+{
+	NC_ASM = 0x01 << 0,
+	VPT = 0x01 << 1,
+	MULTI_POINT = 0x01 << 2,
+	OPTIMIZER = 0x01 << 3,
+	SPEAK_TO_CHAT = 0x01 << 4,
+
+};
+
+enum class FUNCTION_TYPE : signed char
+{
+	DEVICE_MANAGEMENT = 56,
+	VPT = 65,
+	OPTIMIZER = -127,
+	NC_ASM = 98,
+	SMART_TALKING_MODE = -11
+};
\ No newline at end of file
diff --git a/Client/CrossPlatformGUI.cpp b/Client/CrossPlatformGUI.cpp
index 5fa4498..12775d4 100644
--- a/Client/CrossPlatformGUI.cpp
+++ b/Client/CrossPlatformGUI.cpp
@@ -1,4 +1,4 @@
-#include "CrossPlatformGUI.h"
+#include "CrossPlatformGUI.h"
 
 bool CrossPlatformGUI::performGUIPass()
 {
@@ -25,9 +25,22 @@ bool CrossPlatformGUI::performGUIPass()
 
 		if (this->_bt.isConnected())
 		{
-			ImGui::Spacing();
-			this->_drawASMControls();
-			this->_drawSurroundControls();
+			unsigned int cap = this->_headphones.getCapabilities();
+
+			// ImGui::Spacing();
+			ImGui::Separator();
+			if (cap & DEVICE_CAPABILITIES::NC_ASM)
+				this->_drawASMControls();
+			if (cap & DEVICE_CAPABILITIES::VPT)
+				this->_drawSurroundControls();
+			if (cap & DEVICE_CAPABILITIES::SPEAK_TO_CHAT)
+				this->_drawSpeakToChat();
+			if (cap & DEVICE_CAPABILITIES::MULTI_POINT)
+				this->_drawMultiPointConn();
+			if (cap & DEVICE_CAPABILITIES::OPTIMIZER)
+				ImGui::Separator();
+				this->_drawOptimizerButton();
+
 			this->_setHeadphoneSettings();
 		}
 	}
@@ -112,7 +125,16 @@ void CrossPlatformGUI::_drawDeviceDiscovery()
 					if (selectedDevice != -1)
 					{
 						this->_connectedDevice = connectedDevices[selectedDevice];
-						this->_connectFuture.setFromAsync([this]() { this->_bt.connect(this->_connectedDevice.mac); });
+						this->_connectFuture.setFromAsync([this]() { 
+								this->_bt.connect(this->_connectedDevice.mac); 
+
+								// Add post-connection setup here
+								this->_listener = std::make_unique<Listener>(this->_headphones, this->_bt);
+								auto useless_future = std::async(std::launch::async, &Listener::listen, this->_listener.get());
+								if (this->_connectedDevice.name == "WH-1000XM4")
+									this->_headphones.queryState();
+							}
+						);
 					}
 				}
 			}
@@ -211,6 +233,140 @@ void CrossPlatformGUI::_drawSurroundControls()
 	}
 }
 
+void CrossPlatformGUI::_drawOptimizerButton()
+{
+	if (this->_headphones.getOptimizerState() == OPTIMIZER_STATE::IDLE)
+	{
+		if (ImGui::Button("Optimize"))
+		{
+			this->_headphones.setOptimizerState(OPTIMIZER_STATE::OPTIMIZING);
+		}
+	}
+	else {
+		// TODO: Change button to show cancel option while optimizing state
+		if (ImGui::Button("Optimizing... (Press To Cancel)"))
+		{
+			this->_headphones.setOptimizerState(OPTIMIZER_STATE::IDLE);
+		}
+	}
+}
+
+void CrossPlatformGUI::_drawSpeakToChat()
+{
+	enum Sensitivity { Sens_Auto, Sens_High, Sens_Low, Sens_count};
+	const char* Sens_hints[Sens_count] = { "Auto", "High", "Low" };
+
+	enum AutoOff { Time_Short, Time_Std, Time_Long, Time_Inf, Time_count};
+	const char* Time_hints[Time_count] = { "Short", "Standard", "Long", "Off" };
+
+	static bool S2Ctoggle_check = false;
+	static int S2C_Sens = Sens_Auto;
+	static bool S2C_Voice = 0;
+	static int S2C_AutoOff = Time_Short;
+	
+	const char* Sens_name = (S2C_Sens >= 0 && S2C_Sens < Sens_count) ? Sens_hints[S2C_Sens] : "Unknown";
+	const char* Time_name = (S2C_AutoOff >= 0 && S2C_AutoOff < Time_count) ? Time_hints[S2C_AutoOff] : "Unknown";
+
+	if (ImGui::CollapsingHeader("Speak To Chat Controls", ImGuiTreeNodeFlags_DefaultOpen))
+	{
+		ImGui::Checkbox("Speak To Chat", &S2Ctoggle_check);
+
+		if (S2Ctoggle_check)
+		{
+			if (S2Ctoggle_check){
+				// ImGui::SameLine();
+				ImGui::SliderInt("Speak to chat sensitivity", &S2C_Sens, 0, Sens_count - 1, Sens_name);
+				ImGui::SliderInt("Speak to chat Auto close", &S2C_AutoOff, 0, Time_count - 1, Time_name);
+				ImGui::Checkbox("Voice passthrough", &S2C_Voice);
+			}
+			else {
+			}
+		}
+	}
+
+	if (S2Ctoggle_check)
+		this->_headphones.setS2CToggle(S2C_TOGGLE::ACTIVE);
+	else
+		this->_headphones.setS2CToggle(S2C_TOGGLE::INACTIVE);
+
+	this->_headphones.setS2COptions(S2C_Sens, S2C_Voice, S2C_AutoOff);
+
+}
+
+void CrossPlatformGUI::_drawMultiPointConn()
+{
+	auto devices = this->_headphones.getDevices();
+	auto connectedDevices = this->_headphones.getConnectedDevices();
+
+	int dev1 = connectedDevices.first;
+	int dev2 = connectedDevices.second;
+	int old_dev1 = dev1;
+	int old_dev2 = dev2;
+	
+	if(ImGui::CollapsingHeader("Connected Devices", ImGuiTreeNodeFlags_DefaultOpen))
+	{
+		if (devices[dev1].name!="")
+		{
+			ImGui::Text(devices[dev1].name.c_str());
+			ImGui::SameLine();
+			if (ImGui::Button("Disconnect device 1"))
+			{
+				this->_headphones.setMultiPointConnection(1, 0, dev1);
+			}
+		}
+
+		if (devices[dev1].name=="" || devices[dev2].name=="")
+		{
+			// const char* combo_preview_value = devices[dev1].name // &(devices[dev1].name);
+			if (ImGui::BeginCombo("Connect", ""))
+			{
+				for (int i = 0; i < devices.size(); i++)
+				{
+					if (i==dev2 || i==dev1)
+						continue;
+
+					const bool is_selected = false;
+					if (ImGui::Selectable(devices[i].name.c_str(), is_selected))
+					{
+						if (devices[dev1].name=="")
+						{
+							dev1 = i;
+							this->_headphones.setMultiPointConnection(1, dev1, 0);
+						}
+						else {
+							dev2 = i;
+							this->_headphones.setMultiPointConnection(2, dev2, 0);
+						}
+					}
+
+					// Set the initial focus when opening the combo (scrolling + keyboard navigation focus)
+					if (is_selected)
+						ImGui::SetItemDefaultFocus();
+				}
+				ImGui::EndCombo();
+			}
+		
+			ImGui::SameLine();
+			if (devices[dev1].name=="")
+				ImGui::Text("Device 1");
+			else 
+				ImGui::Text("Device 2");
+		}
+
+		if (devices[dev2].name!="")
+		{
+			ImGui::Text(devices[dev2].name.c_str());
+			ImGui::SameLine();
+			if (ImGui::Button("Disconnect device 2"))
+			{
+				this->_headphones.setMultiPointConnection(2,0,dev2);				
+			}
+		}
+		
+	}
+
+}
+
 void CrossPlatformGUI::_setHeadphoneSettings() {
 	//Don't show if the command only takes a few frames to send
 	static int commandLinger = 0;
diff --git a/Client/CrossPlatformGUI.h b/Client/CrossPlatformGUI.h
index e24dbaa..fd91095 100644
--- a/Client/CrossPlatformGUI.h
+++ b/Client/CrossPlatformGUI.h
@@ -10,8 +10,11 @@
 #include "SingleInstanceFuture.h"
 #include "CascadiaCodeFont.h"
 #include "Headphones.h"
+#include "Listener.h"
 
+#include <iostream>
 #include <future>
+#include <string>
 
 constexpr auto GUI_MAX_MESSAGES = 5;
 constexpr auto GUI_HEIGHT = 380;
@@ -36,6 +39,9 @@ class CrossPlatformGUI
 	void _drawASMControls();
 	void _drawSurroundControls();
 	void _setHeadphoneSettings();
+	void _drawOptimizerButton();
+	void _drawSpeakToChat();
+	void _drawMultiPointConn();
 
 	BluetoothDevice _connectedDevice;
 	BluetoothWrapper _bt;
@@ -44,6 +50,8 @@ class CrossPlatformGUI
 	SingleInstanceFuture<void> _connectFuture;
 	TimedMessageQueue _mq;
 	Headphones _headphones;
+	// Listener _listener;
+	std::unique_ptr<Listener> _listener;
 };
 
 
diff --git a/Client/Headphones.cpp b/Client/Headphones.cpp
index 9c92f95..73da78c 100644
--- a/Client/Headphones.cpp
+++ b/Client/Headphones.cpp
@@ -3,8 +3,11 @@
 
 #include <stdexcept>
 
-Headphones::Headphones(BluetoothWrapper& conn) : _conn(conn)
+Headphones::Headphones(BluetoothWrapper& conn) : 
+_conn(conn)
 {
+	std::vector<BluetoothDevice> devices({BluetoothDevice({"",""})});
+	this->_savedDevices = devices;
 }
 
 void Headphones::setAmbientSoundControl(bool val)
@@ -50,6 +53,41 @@ int Headphones::getAsmLevel()
 	return this->_asmLevel.current;
 }
 
+void Headphones::setOptimizerState(OPTIMIZER_STATE val)
+{
+	std::lock_guard guard(this->_propertyMtx);
+	this->_optimizerState.desired = val;
+}
+
+OPTIMIZER_STATE Headphones::getOptimizerState()
+{
+	return this->_optimizerState.current;
+}
+
+void Headphones::setS2CToggle(S2C_TOGGLE val)
+{
+	std::lock_guard guard(this->_propertyMtx);
+	this->_speakToChat.desired = val;
+}
+
+void Headphones::setS2COptions(int sensitivity, bool voice, int offTime)
+{
+	unsigned int sens = (unsigned int) sensitivity;
+	unsigned int time = (unsigned int) offTime;
+	std::lock_guard guard(this->_propertyMtx);
+	this->_s2cOptions.desired = { (sens << 16) | (voice << 8) | (time) };
+}
+
+S2C_TOGGLE Headphones::getS2CToggle()
+{
+	return this->_speakToChat.current;
+}
+
+unsigned int Headphones::getS2COptions()
+{
+	return this->_s2cOptions.current;
+}
+
 void Headphones::setSurroundPosition(SOUND_POSITION_PRESET val)
 {
 	std::lock_guard guard(this->_propertyMtx);
@@ -72,13 +110,116 @@ int Headphones::getVptType()
 	return this->_vptType.current;
 }
 
+bool Headphones::getMultiPointSetting()
+{
+	return this->_multiPointSetting;
+}
+
+const std::vector<BluetoothDevice>& Headphones::getDevices()
+{
+	return this->_savedDevices;
+}
+
+std::pair<int, int> Headphones::getConnectedDevices()
+{
+	return {this->_dev1.current, this->_dev2.current};
+}
+
+void Headphones::setMultiPointConnection(int connectionId, int newDevice, int oldDevice)
+{
+	Property<int>* dev = & this->_dev1;
+
+	if (connectionId==1)
+		dev = & this->_dev1;
+	else
+		dev = & this->_dev2;
+
+	std::lock_guard guard(this->_propertyMtx);
+	dev->desired = newDevice;
+}
+
+inline void Headphones::disconnect(int deviceIdx)
+{
+	std::string deviceMac = this->_savedDevices[deviceIdx].mac;
+	this->_conn.sendCommand(CommandSerializer::serializeMultiPointCommand(
+			MULTI_POINT_COMMANDS::DISCONNECT,
+			deviceMac
+		),
+		DATA_TYPE::DATA_MDR_NO2
+	);
+}
+
+inline void Headphones::connect(int deviceIdx)
+{
+	std::string deviceMac = this->_savedDevices[deviceIdx].mac;
+	this->_conn.sendCommand(CommandSerializer::serializeMultiPointCommand(
+			MULTI_POINT_COMMANDS::CONNECT,
+			deviceMac
+		),
+		DATA_TYPE::DATA_MDR_NO2
+	);
+}
+
 bool Headphones::isChanged()
 {
-	return !(this->_ambientSoundControl.isFulfilled() && this->_asmLevel.isFulfilled() && this->_focusOnVoice.isFulfilled() && this->_surroundPosition.isFulfilled() && this->_vptType.isFulfilled());
+	return !(this->_ambientSoundControl.isFulfilled() && 
+		this->_asmLevel.isFulfilled() &&
+		this->_focusOnVoice.isFulfilled() &&
+		this->_surroundPosition.isFulfilled() &&
+		this->_vptType.isFulfilled() &&
+		this->_optimizerState.isFulfilled() &&
+		this->_speakToChat.isFulfilled() &&
+		this->_s2cOptions.isFulfilled() && 
+		this->_dev1.isFulfilled() &&
+		this->_dev2.isFulfilled()
+		);
 }
 
+// At most one instance of this function is invoked at any time
+// Synchronization not required
 void Headphones::setChanges()
 {
+	if (!(this->_optimizerState.isFulfilled()))
+	{
+		auto state = this->_optimizerState.desired;
+
+		this->_conn.sendCommand(CommandSerializer::serializeXM4OptimizeCommand(
+			state
+		));
+
+		std::lock_guard guard(this->_propertyMtx);
+		this->_optimizerState.fulfill();
+	}
+
+	if (!(this->_speakToChat.isFulfilled()))
+	{
+		auto s2cState = this->_speakToChat.desired;
+
+		this->_conn.sendCommand(CommandSerializer::serializeXM4SpeakToChat(
+			s2cState
+		));
+
+		std::lock_guard guard(this->_propertyMtx);
+		this->_speakToChat.fulfill();
+	}
+
+	if (!(this->_s2cOptions.isFulfilled()))
+	{
+		auto s2cOptions = this->_s2cOptions.desired;
+		unsigned char sensitivity = (unsigned char) (s2cOptions >> 16) & 0xff;
+		unsigned char voice = (unsigned char) (s2cOptions >> 8) & 0xff;
+		unsigned char offTime = (unsigned char) (s2cOptions) & 0xff;
+
+		this->_conn.sendCommand(CommandSerializer::serializeXM4_S2C_Options(
+			sensitivity,
+			voice,
+			offTime
+		));
+
+		std::lock_guard guard(this->_propertyMtx);
+		this->_s2cOptions.fulfill();
+	}
+
 	if (!(this->_ambientSoundControl.isFulfilled() && this->_focusOnVoice.isFulfilled() && this->_asmLevel.isFulfilled()))
 	{
 		auto ncAsmEffect = this->_ambientSoundControl.desired ? NC_ASM_EFFECT::ADJUSTMENT_COMPLETION : NC_ASM_EFFECT::OFF;
@@ -132,4 +273,173 @@ void Headphones::setChanges()
 		this->_vptType.fulfill();
 		this->_surroundPosition.fulfill();
 	}
+	
+	if (!(this->_dev1.isFulfilled()))
+	{
+		if (this->_dev1.current!=0)
+			this->disconnect(this->_dev1.current);
+		if (this->_dev1.desired!=0)
+			this->connect(this->_dev1.desired);
+
+		std::lock_guard guard(this->_propertyMtx);
+		this->_dev1.desired = this->_dev1.current;
+	}
+
+	if (!(this->_dev2.isFulfilled()))
+	{
+		if (this->_dev2.current!=0)
+			this->disconnect(this->_dev2.current);
+		if (this->_dev2.desired!=0)
+			this->connect(this->_dev2.desired);
+
+		std::lock_guard guard(this->_propertyMtx);
+		this->_dev2.desired = this->_dev2.current;
+	}
+}
+
+unsigned int Headphones::getCapabilities()
+{
+	return this->_capabilities;
+}
+
+void Headphones::queryState()
+{
+	this->_conn.sendCommand({0x00,0x00}); // Init
+	this->_conn.sendCommand({0x02,0x00}); // get mac address of device
+	this->queryDeviceCapabilities();
+	this->queryDevices();
+}
+
+void Headphones::setStateFromReply(BtMessage replyMessage)
+{
+	Buffer bytes = replyMessage.messageBytes;
+	COMMAND_TYPE cmd = static_cast<COMMAND_TYPE>(bytes[0]);
+
+	switch (cmd)
+	{
+	case COMMAND_TYPE::DEVICES_QUERY_RESPONSE:
+	case COMMAND_TYPE::DEVICES_STATE_RESPONSE:
+	{
+		if (bytes[1] != 0x01)
+			// Wrong query type, break
+			break;
+
+		std::vector<BluetoothDevice> savedDevices = std::vector<BluetoothDevice>({BluetoothDevice("","")});
+		int dev1 = 0, dev2 = 0;
+		int idx = 3;
+		int numDevices = static_cast<unsigned char>(bytes[2]);
+		for (; numDevices > 0; numDevices--)
+		{
+			std::string mac_addr = "";
+			for (int i = idx; i<idx+MAC_ADDR_STR_SIZE; i++)
+			{
+				mac_addr += bytes[i];
+			}
+
+			idx += MAC_ADDR_STR_SIZE;
+
+			unsigned char number = static_cast<unsigned char>(bytes[idx]);
+
+			idx++;
+			unsigned char name_length = static_cast<unsigned char>(bytes[idx]);
+			idx++;
+			std::string name = "";
+			for (int i = idx; i<idx+name_length; i++)
+			{
+				name += bytes[i];
+			}
+
+			idx += name_length;
+
+			BluetoothDevice dev = {name, mac_addr};
+			savedDevices.push_back(dev);
+			if (number == 0x01)
+			{
+				dev1 = savedDevices.size()-1;
+			}
+			if (number == 0x02)
+			{
+				dev2 = savedDevices.size()-1;
+			}
+		}
+
+		{
+			std::lock_guard guard(this->_propertyMtx);
+			this->_savedDevices = std::move(savedDevices);
+			this->_dev1.setState(dev1);
+			this->_dev2.setState(dev2);
+		}
+		break;
+	}
+	
+	case COMMAND_TYPE::XM4_OPTIMIZER_RESPONSE:
+	{
+		if (bytes[2] != 0x00)
+			this->_optimizerState.setState(OPTIMIZER_STATE::OPTIMIZING);
+		else
+			this->_optimizerState.setState(OPTIMIZER_STATE::IDLE);
+		break;
+	}
+
+	case COMMAND_TYPE::CAPABILITY_QUERY_RESPONSE:
+	{
+		for (char c: bytes){
+			switch (c)
+			{
+			case static_cast<char>(FUNCTION_TYPE::DEVICE_MANAGEMENT):
+				this->_capabilities |= DEVICE_CAPABILITIES::MULTI_POINT;
+				break;
+			
+			case static_cast<char>(FUNCTION_TYPE::VPT):
+				this->_capabilities |= DEVICE_CAPABILITIES::VPT;
+				break;
+			
+			case static_cast<char>(FUNCTION_TYPE::OPTIMIZER):
+				this->_capabilities |= DEVICE_CAPABILITIES::OPTIMIZER;
+				break;
+			
+			case static_cast<char>(FUNCTION_TYPE::NC_ASM):
+				this->_capabilities |= DEVICE_CAPABILITIES::NC_ASM;
+				break;
+			
+			case static_cast<char>(FUNCTION_TYPE::SMART_TALKING_MODE):
+				this->_capabilities |= DEVICE_CAPABILITIES::SPEAK_TO_CHAT;
+				break;
+			}
+		}
+		
+		break;
+	}
+
+	default:
+		break;
+	}
+}
+
+void Headphones::queryDeviceCapabilities()
+{
+	this->_conn.sendCommand({
+		static_cast<char>(COMMAND_TYPE::CAPABILITY_QUERY),
+		0x01
+	});
+}
+
+void Headphones::queryDevices()
+{
+	this->_conn.sendCommand({
+			static_cast<char>(COMMAND_TYPE::MULTI_POINT_DEVICES_QUERY),
+			0x01
+		},
+		DATA_TYPE::DATA_MDR_NO2
+	);
 }
+
+void Headphones::queryS2C()
+{
+
+}
+
+void Headphones::queryS2COptions()
+{
+
+}
\ No newline at end of file
diff --git a/Client/Headphones.h b/Client/Headphones.h
index b3f20f4..345bc6f 100644
--- a/Client/Headphones.h
+++ b/Client/Headphones.h
@@ -1,8 +1,13 @@
+#pragma once 
+
 #include "SingleInstanceFuture.h"
 #include "BluetoothWrapper.h"
 #include "Constants.h"
 
+#include <iostream>
 #include <mutex>
+#include <vector>
+#include <tuple>
 
 template <class T>
 struct Property {
@@ -11,6 +16,7 @@ struct Property {
 
 	void fulfill();
 	bool isFulfilled();
+	void setState(T val);
 };
 
 class Headphones {
@@ -28,20 +34,58 @@ class Headphones {
 	void setAsmLevel(int val);
 	int getAsmLevel();
 
+	void setOptimizerState(OPTIMIZER_STATE val);
+	OPTIMIZER_STATE getOptimizerState();
+
+	void setS2CToggle(S2C_TOGGLE val);
+	void setS2COptions(int sensitivity, bool voice, int offTime);
+	S2C_TOGGLE getS2CToggle();
+	unsigned int getS2COptions();
+
 	void setSurroundPosition(SOUND_POSITION_PRESET val);
 	SOUND_POSITION_PRESET getSurroundPosition();
 
 	void setVptType(int val);
 	int getVptType();
-	
+
+	bool getMultiPointSetting();
+	const std::vector<BluetoothDevice>& getDevices();
+	std::pair<int, int> getConnectedDevices();
+	void setMultiPointConnection(int connectionId, int newDevice, int oldDevice);
+	inline void disconnect(int deviceIdx);
+	inline void connect(int deviceIdx);
+
 	bool isChanged();
 	void setChanges();
+
+	void queryState();
+	void setStateFromReply(BtMessage replyMessage);
+
+	void queryDeviceCapabilities();
+	void queryDevices();
+	void queryS2C();
+	void queryS2COptions();
+	
+	unsigned int getCapabilities();
 private:
+	unsigned int _capabilities = 0;
+
 	Property<bool> _ambientSoundControl = { 0 };
 	Property<bool> _focusOnVoice = { 0 };
 	Property<int> _asmLevel = { 0 };
+
 	Property<SOUND_POSITION_PRESET> _surroundPosition = { SOUND_POSITION_PRESET::OUT_OF_RANGE, SOUND_POSITION_PRESET::OFF };
 	Property<int> _vptType = { 0 };
+
+	Property<OPTIMIZER_STATE> _optimizerState = { OPTIMIZER_STATE::IDLE };
+
+	Property<S2C_TOGGLE> _speakToChat = { S2C_TOGGLE::INACTIVE, S2C_TOGGLE::INACTIVE };
+	Property<unsigned int> _s2cOptions = { 0 };
+	
+	std::vector<BluetoothDevice> _savedDevices;
+	Property<int> _dev1 = { 0, 0 }, _dev2 = { 0, 0 };
+	bool _multiPointSetting = true;
+
 	std::mutex _propertyMtx;
 
 	BluetoothWrapper& _conn;
@@ -58,3 +102,10 @@ inline bool Property<T>::isFulfilled()
 {
 	return this->desired == this->current;
 }
+
+template<class T>
+inline void Property<T>::setState(T val)
+{
+	this->current = val;
+	this->desired = val;
+}
\ No newline at end of file
diff --git a/Client/Listener.cpp b/Client/Listener.cpp
new file mode 100644
index 0000000..5d1eaa3
--- /dev/null
+++ b/Client/Listener.cpp
@@ -0,0 +1,39 @@
+#include "Listener.h"
+
+Listener::Listener(Headphones& headphones, BluetoothWrapper& bt): 
+_headphones(headphones),
+_bt(bt)
+{}
+
+void Listener::listen()
+{
+    std::cout << "Listener registered." << std::endl;
+    for(;;)
+    {
+        Buffer reply = this->_bt.readReplies();
+        this->handle_message(reply);
+    }
+}
+
+inline BtMessage Listener::parse(Buffer msg)
+{
+    return CommandSerializer::unpackBtMessage(msg);
+}
+
+void Listener::handle_message(Buffer msg)
+{
+    BtMessage m = this->parse(msg);
+
+    if (m.dataType == DATA_TYPE::ACK)
+    {
+        this->_bt.setSeqNumber(m.seqNumber);
+        this->_bt.postAck();
+        this->_bt._ack.notify_all();
+    } 
+    else if (m.dataType == DATA_TYPE::DATA_MDR || m.dataType == DATA_TYPE::DATA_MDR_NO2) 
+    {
+        // Set these values as current values of Headphone property
+        this->_headphones.setStateFromReply(m);
+        this->_bt.sendAck(m.seqNumber);
+    }
+}
diff --git a/Client/Listener.h b/Client/Listener.h
new file mode 100644
index 0000000..c7726eb
--- /dev/null
+++ b/Client/Listener.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#include "Constants.h"
+#include "BluetoothWrapper.h"
+#include "CommandSerializer.h"
+#include "Exceptions.h"
+#include "Headphones.h"
+
+#include <mutex>
+#include <future>
+
+class Listener
+{
+public:
+    Listener(Headphones& headphones, BluetoothWrapper& bt);
+    void listen();
+    inline BtMessage parse(Buffer msg);
+    void handle_message(Buffer msg);
+
+private:
+    Headphones& _headphones;
+    BluetoothWrapper& _bt;
+};
\ No newline at end of file