diff --git a/MQTTSNGateway/README.md b/MQTTSNGateway/README.md
index 67f5e57..bc3314e 100644
--- a/MQTTSNGateway/README.md
+++ b/MQTTSNGateway/README.md
@@ -35,7 +35,8 @@ ClientAuthentication=NO
     
 #RootCAfile=/path/to/your_Root_CA.crt    
 #RootCApath=/path/to/your_certs_directory/   
-#CertsDirectory=/path/to/your_client_certs_directory/
+#CertKey=/path/to/your_cert.pem
+#PrivateKey=/path/to/your_private-key.pem
     
 GatewayID=1
 GatewayName=PahoGateway-01
@@ -59,7 +60,7 @@ ApiMode=2
 Client should know the BroadcastIP and PortNo to send a SEARCHGW message.    
 **GatewayId** is defined by GWSEARCH message.    
 **KeepAlive** is a duration of ADVERTISE message in seconds.    
-when **ClientAuthentication** is YES, see MQTTSNGWClient.cpp line53, clients file specified by ClientsList is required. This file defines connect allowed clients by ClientId, IPaddress and PortNo.    
+when **ClientAuthentication** is YES, see MQTTSNGWClient.cpp line53, clients file specified by ClientsList is required. This file defines connect allowed clients by ClientId and SensorNetwork Address. e.g. IP address and Port No.    
 
 
 
diff --git a/MQTTSNGateway/src/MQTTSNGWBrokerSendTask.cpp b/MQTTSNGateway/src/MQTTSNGWBrokerSendTask.cpp
index 50bab94..df827d2 100644
--- a/MQTTSNGateway/src/MQTTSNGWBrokerSendTask.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWBrokerSendTask.cpp
@@ -114,7 +114,6 @@ void BrokerSendTask::run()
 			{
 				if ( packet->getType() == CONNECT )
 				{
-					client->setWaitWillMsgFlg(false);
 					client->connectSended();
 				}
 				log(client, packet);
diff --git a/MQTTSNGateway/src/MQTTSNGWClient.cpp b/MQTTSNGateway/src/MQTTSNGWClient.cpp
index 74b07ac..136c51b 100644
--- a/MQTTSNGateway/src/MQTTSNGWClient.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWClient.cpp
@@ -703,75 +703,95 @@ uint16_t Topic::getTopicId(void)
 	return _topicId;
 }
 
-int Topic::hasWildCard(unsigned int* pos)
-{
-	unsigned int p = _topicName->find("+", 0);
-	if (p != string::npos)
-	{
-		*pos = p;
-		return 1;
-	}
-	else
-	{
-		string::iterator it = _topicName->end();
-		if (*it == '#')
-		{
-			*pos = _topicName->size() - 1;
-			return 2;
-		}
-	}
-	*pos = 0;
-	return 0;
-}
-
 bool Topic::isMatch(string* topicName)
 {
-	unsigned int pos;
-
-	if (topicName->size() < _topicName->size())
+	string::size_type tlen = _topicName->size();
+	if (topicName->size() < tlen - 2)
 	{
 		return false;
 	}
 
-	if (hasWildCard(&pos) == 1)
+	string::size_type tpos = 0;
+	string::size_type tloc = 0;
+	string::size_type pos = 0;
+	string::size_type loc = 0;
+	string wildcard = "#";
+	string wildcards = "+";
+
+	while(true)
 	{
-		if (_topicName->compare(0, pos - 1, *topicName, 0, pos - 1) == 0)
+		loc = topicName->find('/', pos);
+		tloc = _topicName->find('/', tpos);
+
+		if ( loc != string::npos && tloc != string::npos )
 		{
-			if (_topicName->compare(pos + 1, 1, "/") == 0)
+			string subtopic = topicName->substr(pos, loc - pos);
+			string subtopict = _topicName->substr(tpos, tloc - tpos);
+			if (subtopict == wildcard)
 			{
-				unsigned int loc = topicName->find('/', pos);
-				if (loc != 0)
+				return true;
+			}
+			else if (subtopict == wildcards)
+			{
+				if ( (tpos = tloc + 1 ) > tlen )
 				{
-					if (_topicName->compare(pos + 1, _topicName->size() - pos - 1, *topicName, loc,
-							topicName->size() - pos - 1) == 0)
+					pos = loc + 1;
+					loc = topicName->find('/', pos);
+					if ( loc == string::npos )
 					{
 						return true;
 					}
-				}
-			}
-			else
-			{
-				unsigned int loc = _topicName->find(pos, '/');
-				if (loc != 0)
-				{
-					if (topicName->find('/', loc) != 0)
+					else
 					{
 						return false;
 					}
 				}
+				pos = loc + 1;
+			}
+			else if ( subtopic != subtopict )
+			{
+				return false;
+			}
+			else
+			{
+				if ( (tpos = tloc + 1) > tlen )
+				{
+					return false;
+				}
+
+				pos = loc + 1;
+			}
+		}
+		else if ( loc == string::npos && tloc == string::npos )
+		{
+			string subtopic = topicName->substr(pos);
+			string subtopict = _topicName->substr(tpos);
+			if ( subtopict == wildcard || subtopict == wildcards)
+			{
+				return true;
+			}
+			else if ( subtopic == subtopict )
+			{
+				return true;
+			}
+			else
+			{
+				return false;
+			}
+		}
+		else if ( loc != string::npos && tloc == string::npos )
+		{
+			tloc = _topicName->find('#', --tpos);
+			if ( tloc == string::npos )
+			{
+				return false;
+			}
+			else
+			{
+				return true;
 			}
-			return true;
 		}
 	}
-	else if (hasWildCard(&pos) == 2 && (_topicName->compare(0, pos, *topicName, 0, pos) == 0))
-	{
-		return true;
-	}
-	else if (_topicName->compare(*topicName) == 0)
-	{
-		return true;
-	}
-	return false;
 }
 
 /*=====================================
@@ -845,7 +865,6 @@ Topic* Topics::add(MQTTSN_topicid* topicid)
 {
 	Topic* topic;
 	uint16_t id = 0;
-	string* topicName = 0;
 
 	if (topicid->type != MQTTSN_TOPIC_TYPE_NORMAL)
 	{
@@ -859,8 +878,8 @@ Topic* Topics::add(MQTTSN_topicid* topicid)
 	}
 	else
 	{
-		topicName = new string(topicid->data.long_.name, topicid->data.long_.len);
-		topic = add(topicName);
+		string topicName = string(topicid->data.long_.name, topicid->data.long_.len);
+		topic = add(&topicName);
 	}
 	return topic;
 }
diff --git a/MQTTSNGateway/src/MQTTSNGWClient.h b/MQTTSNGateway/src/MQTTSNGWClient.h
index d210fc3..396ff43 100644
--- a/MQTTSNGateway/src/MQTTSNGWClient.h
+++ b/MQTTSNGateway/src/MQTTSNGWClient.h
@@ -109,10 +109,8 @@ public:
 	Topic();
 	Topic(string* topic);
 	~Topic();
-
 	string* getTopicName(void);
 	uint16_t getTopicId(void);
-	int hasWildCard(unsigned int* pos);
 	bool isMatch(string* topicName);
 
 private:
@@ -167,12 +165,10 @@ public:
 	TopicIdMap();
 	~TopicIdMap();
 	uint16_t getTopicId(uint16_t msgId, MQTTSN_topicTypes* type);
-	Topic* getTopic(MQTTSN_topicTypes type);
 	int add(uint16_t msgId, uint16_t topicId, MQTTSN_topicTypes type);
 	void erase(uint16_t msgId);
 	void clear(void);
 private:
-	int find(uint16_t msgId);
 	uint16_t* _msgIds;
 	TopicIdMapelement* _first;
 	TopicIdMapelement* _end;
diff --git a/MQTTSNGateway/src/MQTTSNGWClientRecvTask.cpp b/MQTTSNGateway/src/MQTTSNGWClientRecvTask.cpp
index a98102e..d8c482a 100644
--- a/MQTTSNGateway/src/MQTTSNGWClientRecvTask.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWClientRecvTask.cpp
@@ -113,7 +113,8 @@ void ClientRecvTask::run()
 				log(client, packet, &data.clientID);
 				if (!client)
 				{
-					WRITELOG("%s Client was rejected. CONNECT message has been discarded.%s\n", ERRMSG_HEADER, ERRMSG_FOOTER);
+					char buf[128];
+					WRITELOG("%s Client(%s) was rejected. CONNECT message has been discarded.%s\n", ERRMSG_HEADER, _sensorNetwork->getSenderAddress()->sprint(buf), ERRMSG_FOOTER);
 					delete packet;
 					continue;
 				}
@@ -151,7 +152,7 @@ void ClientRecvTask::log(Client* client, MQTTSNPacket* packet, MQTTSNString* id)
 
 	if ( id )
 	{
-		memset((void*)cstr, 0, id->lenstring.len);
+		memset((void*)cstr, 0, id->lenstring.len + 1);
 		strncpy(cstr, id->lenstring.data, id->lenstring.len) ;
 		clientId = cstr;
 	}
diff --git a/MQTTSNGateway/src/MQTTSNGWClientSendTask.cpp b/MQTTSNGateway/src/MQTTSNGWClientSendTask.cpp
index 1ba6398..62010e1 100644
--- a/MQTTSNGateway/src/MQTTSNGWClientSendTask.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWClientSendTask.cpp
@@ -40,6 +40,7 @@ void ClientSendTask::run()
 {
 	Client* client = 0;
 	MQTTSNPacket* packet = 0;
+	int rc = 0;
 
 	while (true)
 	{
@@ -55,20 +56,27 @@ void ClientSendTask::run()
 		{
 			client = ev->getClient();
 			packet = ev->getMQTTSNPacket();
-			packet->unicast(_sensorNetwork, client->getSensorNetAddress());
+			log(client, packet);
+			rc = packet->unicast(_sensorNetwork, client->getSensorNetAddress());
 		}
 		else if (ev->getEventType() == EtBroadcast)
 		{
 			packet = ev->getMQTTSNPacket();
-			packet->broadcast(_sensorNetwork);
+			log(client, packet);
+			rc = packet->broadcast(_sensorNetwork);
 		}
 		else if (ev->getEventType() == EtSensornetSend)
 		{
 			packet = ev->getMQTTSNPacket();
-			packet->unicast(_sensorNetwork, ev->getSensorNetAddress());
+			log(client, packet);
+			rc = packet->unicast(_sensorNetwork, ev->getSensorNetAddress());
 		}
 
-		log(client, packet);
+		if ( rc < 0 )
+		{
+			WRITELOG("%s ClientSendTask can't send a packet to the client.\n",
+				ERRMSG_HEADER, (client ? (const char*)client->getClientId() : UNKNOWNCL ), ERRMSG_FOOTER);
+		}
 		delete ev;
 	}
 }
diff --git a/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp b/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp
index 5783646..5ef17b1 100644
--- a/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWConnectionHandler.cpp
@@ -169,17 +169,17 @@ void MQTTSNConnectionHandler::handleWillmsg(Client* client, MQTTSNPacket* packet
 	if ( !client->isWaitWillMsg() )
 	{
 		DEBUGLOG("     MQTTSNConnectionHandler::handleWillmsg  WaitWillMsgFlg is off.\n");
-		if ( !client->isSecureNetwork() )
-		{
-			/* create CONNACK message */
-			MQTTSNPacket* connack =  new MQTTSNPacket();
-			connack->setCONNACK(MQTTSN_RC_REJECTED_CONGESTED);
+		//if ( !client->isSecureNetwork() )
+		//{
+		//	/* create CONNACK message */
+		//	MQTTSNPacket* connack =  new MQTTSNPacket();
+		//	connack->setCONNACK(MQTTSN_RC_REJECTED_CONGESTED);
 
-			/* return to the client */
-			Event* evt = new Event();
-			evt->setClientSendEvent(client, connack);
-			_gateway->getClientSendQue()->post(evt);
-		}
+		//	/* return to the client */
+		//	Event* evt = new Event();
+		//	evt->setClientSendEvent(client, connack);
+		//	_gateway->getClientSendQue()->post(evt);
+		//}
 		return;
 	}
 
@@ -200,6 +200,7 @@ void MQTTSNConnectionHandler::handleWillmsg(Client* client, MQTTSNPacket* packet
 		/* Send CONNECT to the broker */
 		Event* evt = new Event();
 		evt->setBrokerSendEvent(client, mqttPacket);
+		client->setWaitWillMsgFlg(false);	
 		_gateway->getBrokerSendQue()->post(evt);
 	}
 }
diff --git a/MQTTSNGateway/src/MQTTSNGWPacket.cpp b/MQTTSNGateway/src/MQTTSNGWPacket.cpp
index 23ec9f9..74c00fb 100644
--- a/MQTTSNGateway/src/MQTTSNGWPacket.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWPacket.cpp
@@ -334,7 +334,10 @@ int MQTTSNPacket::getPINGREQ(void)
 
 int MQTTSNPacket::getDISCONNECT(uint16_t* duration)
 {
-	return MQTTSNDeserialize_disconnect((int*) duration, _buf, _bufLen);
+	int dur = 0;
+	int rc = MQTTSNDeserialize_disconnect(&dur, _buf, _bufLen);
+	*duration = (uint16_t)dur;
+	return rc;
 }
 
 int MQTTSNPacket::getWILLTOPICUPD(uint8_t* willQoS, uint8_t* willRetain, MQTTSNString* willTopic)
diff --git a/MQTTSNGateway/src/MQTTSNGWProcess.cpp b/MQTTSNGateway/src/MQTTSNGWProcess.cpp
index 18f5df3..549b439 100644
--- a/MQTTSNGateway/src/MQTTSNGWProcess.cpp
+++ b/MQTTSNGateway/src/MQTTSNGWProcess.cpp
@@ -255,7 +255,10 @@ MultiTaskProcess::MultiTaskProcess()
 
 MultiTaskProcess::~MultiTaskProcess()
 {
-
+	for (int i = 0; i < _threadCount; i++)
+	{
+		_threadList[i]->stop();
+	}
 }
 
 void MultiTaskProcess::initialize(int argc, char** argv)
@@ -309,6 +312,7 @@ void MultiTaskProcess::threadStoped(void)
 	_mutex.lock();
 	_stopCount++;
 	_mutex.unlock();
+
 }
 
 void MultiTaskProcess::attach(Thread* thread)
diff --git a/MQTTSNGateway/src/MQTTSNGWProcess.h b/MQTTSNGateway/src/MQTTSNGWProcess.h
index 7fdd52f..3ea6a45 100644
--- a/MQTTSNGateway/src/MQTTSNGWProcess.h
+++ b/MQTTSNGateway/src/MQTTSNGWProcess.h
@@ -207,7 +207,7 @@ public:
 
 	int post(T* t)
 	{
-		if ( t && ( _maxSize == 0 || size() < _maxSize ))
+		if ( t && ( _maxSize == 0 || _cnt < _maxSize ))
 		{
 			QueElement<T>* elm = new QueElement<T>(t);
 			if ( _head )
diff --git a/MQTTSNGateway/src/MQTTSNGateway.cpp b/MQTTSNGateway/src/MQTTSNGateway.cpp
index fbed95c..1065612 100644
--- a/MQTTSNGateway/src/MQTTSNGateway.cpp
+++ b/MQTTSNGateway/src/MQTTSNGateway.cpp
@@ -235,7 +235,7 @@ void Gateway::run(void)
 
 	MultiTaskProcess::run();
 
-	/* stop threads */
+	/* stop Tasks */
 	Event* ev = new Event();
 	ev->setStop();
 	_packetEventQue.post(ev);
@@ -246,7 +246,7 @@ void Gateway::run(void)
 	ev->setStop();
 	_clientSendQue.post(ev);
 
-	/* wait until all threads stop */
+	/* wait until all Task stop */
 	MultiTaskProcess::waitStop();
 
 	WRITELOG("\n%s MQTT-SN Gateway  stoped\n\n", currentDateTime());
@@ -314,25 +314,29 @@ void  EventQue::setMaxSize(uint16_t maxSize)
 
 Event* EventQue::wait(void)
 {
-	Event* ev;
-	while ( true )
+	Event* ev = 0;
+
+	while(ev == 0)
 	{
-		_sem.wait();
+		if ( _que.size() == 0 )
+		{
+			_sem.wait();
+		}
 		_mutex.lock();
 		ev = _que.front();
 		_que.pop();
 		_mutex.unlock();
-		if ( ev )
-		{
-			return ev;
-		}
 	}
+	return ev;
 }
 
 Event* EventQue::timedwait(uint16_t millsec)
 {
 	Event* ev;
-	_sem.timedwait(millsec);
+	if ( _que.size() == 0 )
+	{
+		_sem.timedwait(millsec);
+	}
 	_mutex.lock();
 
 	if (_que.size() == 0)
@@ -344,11 +348,6 @@ Event* EventQue::timedwait(uint16_t millsec)
 	{
 		ev = _que.front();
 		_que.pop();
-		if ( !ev )
-		{
-			ev = new Event();
-			ev->setTimeout();
-		}
 	}
 	_mutex.unlock();
 	return ev;
@@ -356,16 +355,19 @@ Event* EventQue::timedwait(uint16_t millsec)
 
 void EventQue::post(Event* ev)
 {
-	_mutex.lock();
-	if ( _que.post(ev) )
+	if ( ev )
 	{
-		_sem.post();
+		_mutex.lock();
+		if ( _que.post(ev) )
+		{
+			_sem.post();
+		}
+		else
+		{
+			delete ev;
+		}
+		_mutex.unlock();
 	}
-	else
-	{
-		delete ev;
-	}
-	_mutex.unlock();
 }
 
 int EventQue::size()
diff --git a/MQTTSNGateway/src/MQTTSNGateway.h b/MQTTSNGateway/src/MQTTSNGateway.h
index 04b6dcf..86ceb76 100644
--- a/MQTTSNGateway/src/MQTTSNGateway.h
+++ b/MQTTSNGateway/src/MQTTSNGateway.h
@@ -25,7 +25,7 @@ namespace MQTTSNGW
 /*=================================
  *    Starting prompt
  ==================================*/
-#define GATEWAY_VERSION " * Version: 0.9.1"
+#define GATEWAY_VERSION " * Version: 0.9.5"
 
 #define PAHO_COPYRIGHT0 " * MQTT-SN Transparent Gateway"
 #define PAHO_COPYRIGHT1 " * Part of Project Paho in Eclipse"
diff --git a/MQTTSNGateway/src/linux/Network.cpp b/MQTTSNGateway/src/linux/Network.cpp
index 9af3697..d12cc8d 100644
--- a/MQTTSNGateway/src/linux/Network.cpp
+++ b/MQTTSNGateway/src/linux/Network.cpp
@@ -283,7 +283,7 @@ bool Network::connect(const char* host, const char* port, const char* caPath, co
 		if (!_secureFlg)
 		{
 			WRITELOG("TLS is not required.\n");
-			throw;
+			throw false;
 		}
 
 		if (_ctx == 0)
@@ -295,14 +295,14 @@ bool Network::connect(const char* host, const char* port, const char* caPath, co
 			{
 				ERR_error_string_n(ERR_get_error(), errmsg, sizeof(errmsg));
 				WRITELOG("SSL_CTX_new() %s\n", errmsg);
-				throw;
+				throw false;
 			}
 
 			if (!SSL_CTX_load_verify_locations(_ctx, caFile, caPath))
 			{
 				ERR_error_string_n(ERR_get_error(), errmsg, sizeof(errmsg));
 				WRITELOG("SSL_CTX_load_verify_locations() %s\n", errmsg);
-				throw;
+				throw false;
 			}
 
 			if ( certkey )
@@ -311,7 +311,7 @@ bool Network::connect(const char* host, const char* port, const char* caPath, co
 				{
 					ERR_error_string_n(ERR_get_error(), errmsg, sizeof(errmsg));
 					WRITELOG("SSL_CTX_use_certificate_file() %s %s\n", certkey, errmsg);
-					throw;
+					throw false;
 				}
 			}
 			if ( prvkey )
@@ -320,7 +320,7 @@ bool Network::connect(const char* host, const char* port, const char* caPath, co
 				{
 					ERR_error_string_n(ERR_get_error(), errmsg, sizeof(errmsg));
 					WRITELOG("SSL_use_PrivateKey_file() %s %s\n", prvkey, errmsg);
-					throw;
+					throw false;
 				}
 			}
 		}
@@ -329,7 +329,7 @@ bool Network::connect(const char* host, const char* port, const char* caPath, co
 		{
 			if ( !TCPStack::connect(host, port) )
 			{
-				throw;
+				throw false;
 			}
 		}
 
@@ -338,7 +338,7 @@ bool Network::connect(const char* host, const char* port, const char* caPath, co
 		{
 			ERR_error_string_n(ERR_get_error(), errmsg, sizeof(errmsg));
 			WRITELOG("SSL_new()  %s\n", errmsg);
-			throw;
+			throw false;
 		}
 
 		if (!SSL_set_fd(_ssl, TCPStack::getSock()))
@@ -347,7 +347,7 @@ bool Network::connect(const char* host, const char* port, const char* caPath, co
 			WRITELOG("SSL_set_fd()  %s\n", errmsg);
 			SSL_free(_ssl);
 			_ssl = 0;
-			throw;
+			throw false;
 		}
 
 		if (_session)
@@ -361,7 +361,7 @@ bool Network::connect(const char* host, const char* port, const char* caPath, co
 			WRITELOG("SSL_connect() %s\n", errmsg);
 			SSL_free(_ssl);
 			_ssl = 0;
-			throw;
+			throw false;
 		}
 
 		int result;
@@ -370,7 +370,7 @@ bool Network::connect(const char* host, const char* port, const char* caPath, co
 			WRITELOG("SSL_get_verify_result() error: %s.\n", X509_verify_cert_error_string(result));
 			SSL_free(_ssl);
 			_ssl = 0;
-			throw;
+			throw false;
 		}
 
 		X509* peer = SSL_get_peer_certificate(_ssl);
@@ -386,7 +386,7 @@ bool Network::connect(const char* host, const char* port, const char* caPath, co
 			WRITELOG("SSL_get_peer_certificate() error: Broker %s dosen't match the host name %s\n", peer_CN, host);
 			SSL_free(_ssl);
 			_ssl = 0;
-			throw;
+			throw false;
 		}
 
 		if (_session == 0)
@@ -397,9 +397,9 @@ bool Network::connect(const char* host, const char* port, const char* caPath, co
 		_sslValid = true;
 		rc = true;
 	}
-	catch (...)
+	catch (bool x)
 	{
-		rc = false;
+		rc = x;
 	}
 	_mutex.unlock();
 	return rc;
diff --git a/MQTTSNGateway/src/linux/Threading.cpp b/MQTTSNGateway/src/linux/Threading.cpp
index a245118..c214a44 100644
--- a/MQTTSNGateway/src/linux/Threading.cpp
+++ b/MQTTSNGateway/src/linux/Threading.cpp
@@ -514,3 +514,11 @@ void Thread::stopProcess(void)
 	theMultiTaskProcess->threadStoped();
 }
 
+void Thread::stop(void)
+{
+	if ( _threadID )
+	{
+		pthread_join(_threadID, NULL);
+		_threadID = 0;
+	}
+}
diff --git a/MQTTSNGateway/src/linux/Threading.h b/MQTTSNGateway/src/linux/Threading.h
index 91e7e05..df58f1c 100644
--- a/MQTTSNGateway/src/linux/Threading.h
+++ b/MQTTSNGateway/src/linux/Threading.h
@@ -128,6 +128,7 @@ public:
 	virtual void initialize(int argc, char** argv);
 	void stopProcess(void);
 	void waitStop(void);
+	void stop(void);
 private:
 	static void* _run(void*);
 	pthread_t _threadID;
diff --git a/MQTTSNGateway/src/linux/udp/SensorNetwork.cpp b/MQTTSNGateway/src/linux/udp/SensorNetwork.cpp
index 263d3ce..c64a0a2 100644
--- a/MQTTSNGateway/src/linux/udp/SensorNetwork.cpp
+++ b/MQTTSNGateway/src/linux/udp/SensorNetwork.cpp
@@ -101,6 +101,15 @@ SensorNetAddress& SensorNetAddress::operator =(SensorNetAddress& addr)
 	return *this;
 }
 
+
+char* SensorNetAddress::sprint(char* buf)
+{
+	struct in_addr  inaddr = { _IpAddr };
+	char* ip = inet_ntoa(inaddr);
+	sprintf( buf, "%s:", ip);
+	sprintf( buf + strlen(buf), "%d", ntohs(_portNo));
+	return buf;
+}
 /*===========================================
  Class  SensorNetwork
  ============================================*/
@@ -292,7 +301,6 @@ int UDPPort::unicast(const uint8_t* buf, uint32_t length, SensorNetAddress* addr
 	dest.sin_family = AF_INET;
 	dest.sin_port = addr->getPortNo();
 	dest.sin_addr.s_addr = addr->getIpAddress();
-	;
 
 	int status = ::sendto(_sockfdUnicast, buf, length, 0, (const sockaddr*) &dest, sizeof(dest));
 	if (status < 0)
diff --git a/MQTTSNGateway/src/linux/udp/SensorNetwork.h b/MQTTSNGateway/src/linux/udp/SensorNetwork.h
index 021c9bc..07967f8 100644
--- a/MQTTSNGateway/src/linux/udp/SensorNetwork.h
+++ b/MQTTSNGateway/src/linux/udp/SensorNetwork.h
@@ -45,7 +45,7 @@ public:
 	uint32_t getIpAddress(void);
 	bool isMatch(SensorNetAddress* addr);
 	SensorNetAddress& operator =(SensorNetAddress& addr);
-
+	char* sprint(char* buf);
 private:
 	uint16_t _portNo;
 	uint32_t _IpAddr;
diff --git a/MQTTSNGateway/src/linux/xbee/SensorNetwork.cpp b/MQTTSNGateway/src/linux/xbee/SensorNetwork.cpp
index 75e90a0..1152890 100644
--- a/MQTTSNGateway/src/linux/xbee/SensorNetwork.cpp
+++ b/MQTTSNGateway/src/linux/xbee/SensorNetwork.cpp
@@ -78,6 +78,17 @@ SensorNetAddress& SensorNetAddress::operator =(SensorNetAddress& addr)
 	return *this;
 }
 
+char* SensorNetAddress::sprint(char* buf)
+{
+	char* pbuf = buf;
+	for ( int i = 0; i < 8; i++ )
+	{
+		sprintf(pbuf, "%02X", _address64[i]);
+		pbuf += 2;
+	}
+	return buf;
+}
+
 /*===========================================
  Class  SensorNetwork
  ============================================*/
@@ -196,13 +207,11 @@ int XBee::open(char* device, int baudrate)
 int XBee::broadcast(const uint8_t* payload, uint16_t payloadLen){
 	SensorNetAddress addr;
 	addr.setBroadcastAddress();
-	send(payload, (uint8_t) payloadLen, &addr);
-	return 1;
+	return send(payload, (uint8_t) payloadLen, &addr);
 }
 
 int XBee:: unicast(const uint8_t* payload, uint16_t payloadLen, SensorNetAddress* addr){
-	send(payload, (uint8_t) payloadLen, addr);
-	return 1;
+	return send(payload, (uint8_t) payloadLen, addr);
 }
 
 int XBee::recv(uint8_t* buf, uint16_t bufLen, SensorNetAddress* clientAddr)
@@ -344,14 +353,14 @@ int XBee::send(const uint8_t* payload, uint8_t pLen, SensorNetAddress* addr){
     D_NWSTACK("\r\n");
 
     /* wait Txim Status 0x8B */
-    _sem.timedwait(5000);   // 5sec
+    _sem.timedwait(XMIT_STATUS_TIME_OVER);
 
     if ( _respCd || _frameId != _respId )
     {
     	 D_NWSTACK(" frameId = %02x  Not Acknowleged\r\n", _frameId);
     	return -1;
     }
-    return 0;
+    return (int)pLen;
 }
 
 void XBee::send(uint8_t c)
diff --git a/MQTTSNGateway/src/linux/xbee/SensorNetwork.h b/MQTTSNGateway/src/linux/xbee/SensorNetwork.h
index 4de0932..202f08b 100644
--- a/MQTTSNGateway/src/linux/xbee/SensorNetwork.h
+++ b/MQTTSNGateway/src/linux/xbee/SensorNetwork.h
@@ -38,7 +38,8 @@ namespace MQTTSNGW
 #define API_MODEMSTATUS          0x8A
 #define API_XMITSTATUS           0x8B
 
-#define START_BYTE 0x7e
+#define XMIT_STATUS_TIME_OVER    5000
+
 #define ESCAPE     0x7d
 #define XON        0x11
 #define XOFF       0x13
@@ -74,7 +75,7 @@ public:
 	void setBroadcastAddress(void);
 	bool isMatch(SensorNetAddress* addr);
 	SensorNetAddress& operator =(SensorNetAddress& addr);
-
+	char* sprint(char*);
 private:
 	uint8_t _address16[2];
 	uint8_t _address64[8];
diff --git a/MQTTSNGateway/src/tests/TestProcessFramework.cpp b/MQTTSNGateway/src/tests/TestProcess.cpp
similarity index 60%
rename from MQTTSNGateway/src/tests/TestProcessFramework.cpp
rename to MQTTSNGateway/src/tests/TestProcess.cpp
index 627b2e0..f460335 100644
--- a/MQTTSNGateway/src/tests/TestProcessFramework.cpp
+++ b/MQTTSNGateway/src/tests/TestProcess.cpp
@@ -16,8 +16,13 @@
 
 #include <string.h>
 #include <cassert>
-#include "TestProcessFramework.h"
+#include "TestProcess.h"
+#include "TestTopics.h"
+#include "TestQue.h"
+#include "TestTopicIdMap.h"
 #include "MQTTSNGWProcess.h"
+#include "MQTTSNGWClient.h"
+#include "MQTTSNGWPacket.h"
 #include "Timer.h"
 
 using namespace std;
@@ -29,18 +34,18 @@ using namespace MQTTSNGW;
 
 const char* currentDateTime(void);
 
-TestProcessFramework::TestProcessFramework()
+TestProcess::TestProcess()
 {
 	theMultiTaskProcess = this;
 	theProcess = this;
 }
 
-TestProcessFramework::~TestProcessFramework()
+TestProcess::~TestProcess()
 {
 
 }
 
-void TestProcessFramework::initialize(int argc, char** argv)
+void TestProcess::initialize(int argc, char** argv)
 {
 	MultiTaskProcess::initialize(argc, argv);
 	assert(0 == strcmp(CONFDIR, getConfigDirName()->c_str()));
@@ -48,63 +53,28 @@ void TestProcessFramework::initialize(int argc, char** argv)
 	resetRingBuffer();
 }
 
-void TestProcessFramework::run(void)
+void TestProcess::run(void)
 {
 	char value[256];
-	int* v = 0;
 	int i = 0;
 	Timer tm;
-	TestQue que;
+
+	/* Test command line parameter */
 
 	assert(1 == getArgc() || 3 == getArgc() );
 	assert(0 == strcmp(ARGV, *getArgv()));
 	getParam("BrokerName", value);
 	assert(0 == strcmp("iot.eclipse.org", value));
 
+	/* Test RingBuffer */
 	for ( i = 0; i < 1000; i++)
 	{
 		putLog("Test RingBuffer %d ", 1234567890);
 	}
-	putLog("\n\nRingBuffer Test complieted. Enter CTRL+C\n");
-
-	for ( i = 0; i < 10; i++ )
-	{
-		v = new int(i);
-		que.post(v);
-	}
-	assert( 10 == que.size());
-
-	for ( i = 0; i < 10; i++ )
-	{
-		assert(i == *que.front());
-		int* p = que.front();
-		if ( p )
-		{
-			assert(i == *p);
-			que.pop();
-			delete p;
-		}
-	}
-	assert(0 == que.front());
-	assert(0 == que.size());
-
-	que.setMaxSize(5);
-	for ( i = 0; i < 10; i++ )
-	{
-		v = new int(i);
-		que.post(v);
-		assert( 5 >= que.size());
-	}
-	for ( i = 0; i < 10; i++ )
-	{
-		int* p = que.front();
-		if ( p )
-		{
-			que.pop();
-			delete p;
-		}
-	}
+	putLog("\n\nRingBuffer Test complieted.\n\n");
 
+	/* Test Timer */
+	printf("Timer Test start\n");
 	printf("%s Timer start\n",  currentDateTime());
 	tm.start(1000);
 	while (!tm.isTimeup());
@@ -113,40 +83,39 @@ void TestProcessFramework::run(void)
 	tm.start();
 	while (!tm.isTimeup(1000));
 	printf("%s Timer 1sec\n", currentDateTime());
+	printf("Timer Test completed\n\n");
+
+	/* Test Que */
+	TestQue* tque = new TestQue();
+	tque->test();
+	delete tque;
+
+	/* Test TopicTable */
+	TestTopics* testTopic = new TestTopics();
+	testTopic->test();
+	delete testTopic;
+
+	/* Test TopicIdMap */
+	TestTopicIdMap* testMap = new TestTopicIdMap();
+	testMap->test();
+	delete testMap;
+
+
+
+	/* Test EventQue */
+	printf("EventQue test start.\n");
+	Client* client = new Client();
+	_evQue.setMaxSize(EVENT_CNT);
+	for ( int i = 0; i < EVENT_CNT + 4; i++ )
+	{
+		Event* ev = new Event();
+		MQTTSNPacket* packet = new MQTTSNPacket();
+		packet->setDISCONNECT(i);
+		ev->setClientSendEvent(client, packet);
+		_evQue.post(ev);
+	}
 
 	MultiTaskProcess::run();
 
-	printf("ProcessFramework test complited.\n");
-}
-
-TestQue::TestQue()
-{
-
-}
-
-TestQue::~TestQue()
-{
-
-}
-
-int* TestQue::front(void)
-{
-	return _que.front();
-}
-void TestQue::pop(void)
-{
-	_que.pop();
-}
-int TestQue::size(void)
-{
-	return _que.size();
-}
-void TestQue::setMaxSize(int maxsize)
-{
-	_que.setMaxSize(maxsize);
-}
-
-void TestQue::post(int* val)
-{
-	_que.post(val);
+	printf("\n\nAll Tests completed.\n");
 }
diff --git a/MQTTSNGateway/src/tests/TestProcess.h b/MQTTSNGateway/src/tests/TestProcess.h
new file mode 100644
index 0000000..6890b8d
--- /dev/null
+++ b/MQTTSNGateway/src/tests/TestProcess.h
@@ -0,0 +1,38 @@
+/**************************************************************************************
+ * Copyright (c) 2016, Tomoaki Yamaguchi
+ *
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * and Eclipse Distribution License v1.0 which accompany this distribution.
+ *
+ * The Eclipse Public License is available at
+ *    http://www.eclipse.org/legal/epl-v10.html
+ * and the Eclipse Distribution License is available at
+ *   http://www.eclipse.org/org/documents/edl-v10.php.
+ *
+ * Contributors:
+ *    Tomoaki Yamaguchi - initial API and implementation 
+ **************************************************************************************/
+#ifndef TESTPROCESS_H_
+#define TESTPROCESS_H_
+
+#include "MQTTSNGWProcess.h"
+#include "MQTTSNGateway.h"
+#define EVENT_CNT 10
+namespace MQTTSNGW
+{
+class TestProcess: public MultiTaskProcess{
+public:
+	TestProcess();
+	~TestProcess();
+	virtual void initialize(int argc, char** argv);
+	void run(void);
+	EventQue* getEventQue(void) { return &_evQue; }
+
+private:
+	EventQue _evQue;
+};
+
+}
+
+#endif /* TESTPROCESS_H_ */
diff --git a/MQTTSNGateway/src/tests/TestQue.cpp b/MQTTSNGateway/src/tests/TestQue.cpp
new file mode 100644
index 0000000..984c74f
--- /dev/null
+++ b/MQTTSNGateway/src/tests/TestQue.cpp
@@ -0,0 +1,101 @@
+/**************************************************************************************
+ * Copyright (c) 2016, Tomoaki Yamaguchi
+ *
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * and Eclipse Distribution License v1.0 which accompany this distribution.
+ *
+ * The Eclipse Public License is available at
+ *    http://www.eclipse.org/legal/epl-v10.html
+ * and the Eclipse Distribution License is available at
+ *   http://www.eclipse.org/org/documents/edl-v10.php.
+ *
+ * Contributors:
+ *    Tomoaki Yamaguchi - initial API and implementation 
+ **************************************************************************************/
+#include <string.h>
+#include <tests/TestProcess.h>
+#include <cassert>
+#include "TestQue.h"
+using namespace std;
+using namespace MQTTSNGW;
+
+TestQue::TestQue()
+{
+
+}
+
+TestQue::~TestQue()
+{
+
+}
+
+void TestQue::test(void)
+{
+	int* v = 0;
+	int i = 0;
+
+	printf("Que Test start.\n");
+	for ( i = 0; i < 10; i++ )
+	{
+		v = new int(i);
+		this->post(v);
+	}
+	assert( 10 == this->size());
+
+	for ( i = 0; i < 10; i++ )
+	{
+		assert(i == *this->front());
+		int* p = this->front();
+		if ( p )
+		{
+			assert(i == *p);
+			this->pop();
+			delete p;
+		}
+	}
+	assert(0 == this->front());
+	assert(0 == this->size());
+
+	this->setMaxSize(5);
+	for ( i = 0; i < 10; i++ )
+	{
+		v = new int(i);
+		this->post(v);
+		assert( 5 >= this->size());
+	}
+	for ( i = 0; i < 10; i++ )
+	{
+		int* p = this->front();
+		if ( p )
+		{
+			this->pop();
+			delete p;
+		}
+	}
+	printf("Que test completed.\n\n");
+}
+
+int* TestQue::front(void)
+{
+	return _que.front();
+}
+void TestQue::pop(void)
+{
+	_que.pop();
+}
+int TestQue::size(void)
+{
+	return _que.size();
+}
+void TestQue::setMaxSize(int maxsize)
+{
+	_que.setMaxSize(maxsize);
+}
+
+void TestQue::post(int* val)
+{
+	_que.post(val);
+}
+
+
diff --git a/MQTTSNGateway/src/tests/TestProcessFramework.h b/MQTTSNGateway/src/tests/TestQue.h
similarity index 76%
rename from MQTTSNGateway/src/tests/TestProcessFramework.h
rename to MQTTSNGateway/src/tests/TestQue.h
index 88617f1..3511895 100644
--- a/MQTTSNGateway/src/tests/TestProcessFramework.h
+++ b/MQTTSNGateway/src/tests/TestQue.h
@@ -13,24 +13,13 @@
  * Contributors:
  *    Tomoaki Yamaguchi - initial API and implementation 
  **************************************************************************************/
-#ifndef TESTPROCESSFRAMEWORK_H_
-#define TESTPROCESSFRAMEWORK_H_
+#ifndef MQTTSNGATEWAY_SRC_TESTS_TESTQUE_H_
+#define MQTTSNGATEWAY_SRC_TESTS_TESTQUE_H_
 
 #include "MQTTSNGWProcess.h"
 
-
 namespace MQTTSNGW
 {
-class TestProcessFramework: public MultiTaskProcess{
-public:
-	TestProcessFramework();
-	~TestProcessFramework();
-	virtual void initialize(int argc, char** argv);
-	void run(void);
-
-private:
-
-};
 
 class TestQue
 {
@@ -42,9 +31,11 @@ public:
 	void pop(void);
 	int size(void);
 	void setMaxSize(int maxsize);
+	void test(void);
 private:
 	Que<int> _que;
 };
 }
 
-#endif /* TESTPROCESSFRAMEWORK_H_ */
+
+#endif /* MQTTSNGATEWAY_SRC_TESTS_TESTQUE_H_ */
diff --git a/MQTTSNGateway/src/tests/TestTask.cpp b/MQTTSNGateway/src/tests/TestTask.cpp
index ae5cff8..dcbfa4f 100644
--- a/MQTTSNGateway/src/tests/TestTask.cpp
+++ b/MQTTSNGateway/src/tests/TestTask.cpp
@@ -14,15 +14,17 @@
  *    Tomoaki Yamaguchi - initial API and implementation 
  **************************************************************************************/
 #include <unistd.h>
+#include <cassert>
 #include "TestTask.h"
 #include "Threading.h"
-
+#include "TestProcess.h"
 using namespace std;
 using namespace MQTTSNGW;
 
-TestTask::TestTask(TestProcessFramework* proc)
+TestTask::TestTask(TestProcess* proc)
 {
 	proc->attach((Thread*)this);
+	_proc = proc;
 }
 
 TestTask::~TestTask()
@@ -37,6 +39,27 @@ void TestTask::initialize(int argc, char** argv)
 
 void TestTask::run(void)
 {
+	int evcnt = 0;
+	EventQue* evQue = _proc->getEventQue();
+	uint16_t duration = 0;
+
+
+	while (true)
+	{
+		Event* ev = evQue->timedwait(5000);
+		evcnt++;
+		if ( ev->getEventType() == EtTimeout )
+		{
+			assert(EVENT_CNT + 1 == evcnt);
+			delete ev;
+			printf("EventQue test complete.\n\n");
+			break;
+		}
+		MQTTSNPacket* packet = ev->getMQTTSNPacket();
+		packet->getDISCONNECT(&duration);
+		delete ev;
+	}
+
 	while(true)
 	{
 		if ( CHK_SIGINT)
diff --git a/MQTTSNGateway/src/tests/TestTask.h b/MQTTSNGateway/src/tests/TestTask.h
index c79db00..42ba855 100644
--- a/MQTTSNGateway/src/tests/TestTask.h
+++ b/MQTTSNGateway/src/tests/TestTask.h
@@ -16,8 +16,8 @@
 #ifndef TESTTASK_H_
 #define TESTTASK_H_
 
+#include "TestProcess.h"
 #include "Threading.h"
-#include "TestProcessFramework.h"
 
 namespace MQTTSNGW
 {
@@ -27,13 +27,13 @@ class TestTask: public Thread
 MAGIC_WORD_FOR_THREAD;
 	;
 public:
-	TestTask(TestProcessFramework* proc);
+	TestTask(TestProcess* proc);
 	~TestTask();
 	void initialize(int argc, char** argv);
 	void run(void);
 
 private:
-
+	TestProcess* _proc;
 };
 
 }
diff --git a/MQTTSNGateway/src/tests/TestTopicIdMap.cpp b/MQTTSNGateway/src/tests/TestTopicIdMap.cpp
new file mode 100644
index 0000000..3f6510a
--- /dev/null
+++ b/MQTTSNGateway/src/tests/TestTopicIdMap.cpp
@@ -0,0 +1,125 @@
+/**************************************************************************************
+ * Copyright (c) 2016, Tomoaki Yamaguchi
+ *
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * and Eclipse Distribution License v1.0 which accompany this distribution.
+ *
+ * The Eclipse Public License is available at
+ *    http://www.eclipse.org/legal/epl-v10.html
+ * and the Eclipse Distribution License is available at
+ *   http://www.eclipse.org/org/documents/edl-v10.php.
+ *
+ * Contributors:
+ *    Tomoaki Yamaguchi - initial API and implementation 
+ **************************************************************************************/
+#include <stdlib.h>
+#include <string.h>
+#include <cassert>
+#include "TestTopicIdMap.h"
+
+using namespace std;
+using namespace MQTTSNGW;
+
+TestTopicIdMap::TestTopicIdMap()
+{
+	_map = new TopicIdMap();
+}
+
+TestTopicIdMap::~TestTopicIdMap()
+{
+	delete _map;
+}
+
+#define MAXID 30
+
+void TestTopicIdMap::test(void)
+{
+	uint16_t id[MAXID];
+	printf("Test TopicIdMat start.\n");
+
+	for ( int i = 0; i < MAXID; i++ )
+	{
+		id[i] = i + 1;
+	}
+
+	for ( int i = 0; i < MAXID; i++ )
+	{
+		_map->add(id[i], id[i], MQTTSN_TOPIC_TYPE_NORMAL);
+	}
+
+	for ( int i = 0; i < MAXID; i++ )
+	{
+		MQTTSN_topicTypes type = MQTTSN_TOPIC_TYPE_SHORT;
+		uint16_t topicId = _map->getTopicId((uint16_t)i, &type);
+		//printf("TopicId=%d  msgId=%d type=%d\n", topicId, i, type);
+		assert((i <= MAX_INFLIGHTMESSAGES * 2 + 1 && topicId == i) || (i > MAX_INFLIGHTMESSAGES * 2 + 1 && topicId == 0));
+	}
+
+	//printf("\n");
+
+	for ( int i = 0; i < 5; i++ )
+	{
+		_map->erase(i);
+	}
+	for ( int i = 0; i < MAXID; i++ )
+	{
+		MQTTSN_topicTypes type = MQTTSN_TOPIC_TYPE_SHORT;
+		uint16_t topicId = _map->getTopicId((uint16_t)i, &type);
+		//printf("TopicId=%d  msgId=%d type=%d\n", topicId, i, type);
+		assert((i < 5 && topicId == 0) || (i >= 5 && topicId != 0) || (i > MAX_INFLIGHTMESSAGES * 2 + 1 && topicId == 0) );
+	}
+
+
+	_map->clear();
+	//printf("\n");
+
+	for ( int i = 0; i < MAXID; i++ )
+	{
+		MQTTSN_topicTypes type = MQTTSN_TOPIC_TYPE_SHORT;
+		uint16_t topicId = _map->getTopicId((uint16_t)i, &type);
+		//printf("TopicId=%d  msgId=%d type=%d\n", topicId, i, type);
+		assert( topicId == 0 );
+	}
+
+	for ( int i = 0; i < MAXID; i++ )
+	{
+		_map->add(id[i], id[i], MQTTSN_TOPIC_TYPE_SHORT);
+	}
+
+	for ( int i = 0; i < MAXID; i++ )
+	{
+		MQTTSN_topicTypes type = MQTTSN_TOPIC_TYPE_NORMAL;
+		uint16_t topicId = _map->getTopicId((uint16_t)i, &type);
+		//printf("TopicId=%d  msgId=%d type=%d\n", topicId, i, type);
+		assert((i <= MAX_INFLIGHTMESSAGES * 2 + 1 && topicId == i) || (i > MAX_INFLIGHTMESSAGES * 2 + 1 && topicId == 0));
+	}
+
+	//printf("\n");
+
+	for ( int i = 0; i < 5; i++ )
+	{
+		_map->erase(i);
+	}
+	for ( int i = 0; i < MAXID; i++ )
+	{
+		MQTTSN_topicTypes type = MQTTSN_TOPIC_TYPE_NORMAL;
+		uint16_t topicId = _map->getTopicId((uint16_t)i, &type);
+		//printf("TopicId=%d  msgId=%d type=%d\n", topicId, i, type);
+		assert((i < 5 && topicId == 0) || (i >= 5 && topicId != 0) || (i > MAX_INFLIGHTMESSAGES * 2 + 1 && topicId == 0) );
+	}
+
+
+	_map->clear();
+
+	//printf("\n");
+	for ( int i = 0; i < MAXID; i++ )
+	{
+		MQTTSN_topicTypes type = MQTTSN_TOPIC_TYPE_NORMAL;
+		uint16_t topicId = _map->getTopicId((uint16_t)i, &type);
+		//printf("TopicId=%d  msgId=%d type=%d\n", topicId, i, type);
+		assert( topicId == 0 );
+	}
+	printf("Test TopicIdMat completed.\n\n");
+}
+
diff --git a/MQTTSNGateway/src/tests/TestTopicIdMap.h b/MQTTSNGateway/src/tests/TestTopicIdMap.h
new file mode 100644
index 0000000..e70909a
--- /dev/null
+++ b/MQTTSNGateway/src/tests/TestTopicIdMap.h
@@ -0,0 +1,32 @@
+/**************************************************************************************
+ * Copyright (c) 2016, Tomoaki Yamaguchi
+ *
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * and Eclipse Distribution License v1.0 which accompany this distribution.
+ *
+ * The Eclipse Public License is available at
+ *    http://www.eclipse.org/legal/epl-v10.html
+ * and the Eclipse Distribution License is available at
+ *   http://www.eclipse.org/org/documents/edl-v10.php.
+ *
+ * Contributors:
+ *    Tomoaki Yamaguchi - initial API and implementation 
+ **************************************************************************************/
+#ifndef MQTTSNGATEWAY_SRC_TESTS_TESTTOPICIDMAP_H_
+#define MQTTSNGATEWAY_SRC_TESTS_TESTTOPICIDMAP_H_
+
+#include "MQTTSNGWClient.h"
+
+class TestTopicIdMap
+{
+public:
+	TestTopicIdMap();
+	~TestTopicIdMap();
+	void test(void);
+
+private:
+	TopicIdMap* _map;
+};
+
+#endif /* MQTTSNGATEWAY_SRC_TESTS_TESTTOPICIDMAP_H_ */
diff --git a/MQTTSNGateway/src/tests/TestTopics.cpp b/MQTTSNGateway/src/tests/TestTopics.cpp
new file mode 100644
index 0000000..624f4df
--- /dev/null
+++ b/MQTTSNGateway/src/tests/TestTopics.cpp
@@ -0,0 +1,183 @@
+/**************************************************************************************
+ * Copyright (c) 2016, Tomoaki Yamaguchi
+ *
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * and Eclipse Distribution License v1.0 which accompany this distribution.
+ *
+ * The Eclipse Public License is available at
+ *    http://www.eclipse.org/legal/epl-v10.html
+ * and the Eclipse Distribution License is available at
+ *   http://www.eclipse.org/org/documents/edl-v10.php.
+ *
+ * Contributors:
+ *    Tomoaki Yamaguchi - initial API and implementation 
+ **************************************************************************************/
+
+#include <stdlib.h>
+#include <string.h>
+#include <cassert>
+#include "TestTopics.h"
+
+using namespace std;
+using namespace MQTTSNGW;
+
+TestTopics::TestTopics()
+{
+	_topics = new Topics();
+}
+
+TestTopics::~TestTopics()
+{
+	delete _topics;
+}
+
+void TestTopics::test(void)
+{
+	printf("Topics Test start.\n");
+
+	MQTTSN_topicid topic[12];
+	char tp[12][10];
+
+	/* create Topic */
+	strcpy(tp[0], "Topic/+");
+	tp[0][7] = 0;
+	topic[0].type = MQTTSN_TOPIC_TYPE_NORMAL;
+	topic[0].data.long_.len = strlen(tp[0]);
+	topic[0].data.long_.name = tp[0];
+
+	for ( int i = 1; i < 10 ; i++ )
+	{
+		strcpy(tp[i], "Topic/+/");
+		tp[i][8] = 0x30 + i;
+		tp[i][9] = 0;
+		topic[i].type = MQTTSN_TOPIC_TYPE_NORMAL;
+		topic[i].data.long_.len = strlen(tp[i]);
+		topic[i].data.long_.name = tp[i];
+	}
+	strcpy(tp[10], "TOPIC/#");
+	tp[10][7] = 0;
+	topic[10].type = MQTTSN_TOPIC_TYPE_NORMAL;
+	topic[10].data.long_.len = strlen(tp[10]);
+	topic[10].data.long_.name = tp[10];
+
+	strcpy(tp[11], "+/0/#");
+	tp[11][7] = 0;
+	topic[11].type = MQTTSN_TOPIC_TYPE_NORMAL;
+	topic[11].data.long_.len = strlen(tp[11]);
+	topic[11].data.long_.name = tp[11];
+
+	/* Add Topic to Topics */
+	for ( int i = 0; i < 12; i++ )
+	{
+		MQTTSN_topicid pos = topic[i];
+		Topic* t = _topics->add(&pos);
+		//printf("Topic=%s ID=%d\n", t->getTopicName()->c_str(), t->getTopicId());
+		assert(t !=0);
+	}
+
+	for ( int i = 0; i < 5; i++ )
+	{
+		string str = "Test/";
+		str += 0x30 + i;
+		Topic* t = _topics->add(&str);
+		//printf("Topic=%s ID=%d\n", t->getTopicName()->c_str(), t->getTopicId());
+		assert(t !=0);
+	}
+
+	/* Get Topic by MQTTSN_topicid */
+	for ( int i = 0; i < 12; i++ )
+	{
+		Topic* t = _topics->getTopic(&topic[i]);
+		//printf("Topic=%s ID=%d ID=%d\n", t->getTopicName()->c_str(), t->getTopicId(),_topics->getTopicId(&topic[i]));
+		assert(t->getTopicId() == i + 1);
+	}
+
+	/* Get TopicId by MQTTSN_topicid */
+	for ( int i = 0; i < 12; i++ )
+	{
+		uint16_t id = _topics->getTopicId(&topic[i]);
+		//printf("ID=%d \n", id);
+		assert(id == i + 1);
+	}
+
+	/* Test Wilecard */
+	for ( int i = 0; i < 10 ; i++ )
+	{
+		MQTTSN_topicid tp1;
+		char tp0[10];
+		strcpy(tp0, "Topic/");
+		tp0[6] = 0x30 + i;
+		tp0[7] = '/';
+		tp0[8] = 0x30 + i;
+		tp0[9] = 0;
+		tp1.type = MQTTSN_TOPIC_TYPE_NORMAL;
+		tp1.data.long_.len = strlen(tp0);
+		tp1.data.long_.name = tp0;
+
+		Topic* t = _topics->match(&tp1);
+/*
+		if (t)
+		{
+			printf("Topic=%s  match to %s\n", tp0, t->getTopicName()->c_str());
+		}
+		else
+		{
+			printf("Topic=%s unmatch\n", tp0);
+		}
+*/
+		assert(t != 0);
+	}
+	for ( int i = 0; i < 10 ; i++ )
+	{
+		MQTTSN_topicid tp1;
+		char tp0[10];
+		strcpy(tp0, "Topic/");
+		tp0[6] = 0x30 + i;
+		tp0[7] = 0;
+		tp1.type = MQTTSN_TOPIC_TYPE_NORMAL;
+		tp1.data.long_.len = strlen(tp0);
+		tp1.data.long_.name = tp0;
+
+		Topic* t = _topics->match(&tp1);
+/*
+		if (t)
+		{
+			printf("Topic=%s  match to %s\n", tp0, t->getTopicName()->c_str());
+		}
+		else
+		{
+			printf("Topic=%s unmatch\n", tp0);
+		}
+*/
+		assert(t != 0);
+	}
+
+	for ( int i = 0; i < 10 ; i++ )
+	{
+		MQTTSN_topicid tpid1;
+		char tp0[10];
+		strcpy(tp0, "TOPIC/");
+		tp0[6] = 0x30 + i;
+		tp0[7] = '/';
+		tp0[8] = 0x30 + i;
+		tp0[9] = 0;
+		tpid1.type = MQTTSN_TOPIC_TYPE_NORMAL;
+		tpid1.data.long_.len = strlen(tp0);
+		tpid1.data.long_.name = tp0;
+
+		Topic* t = _topics->match(&tpid1);
+/*
+		if (t)
+		{
+			printf("Topic=%s  match to %s\n", tp0, t->getTopicName()->c_str());
+		}
+		else
+		{
+			printf("Topic=%s unmatch\n", tp0);
+		}
+*/
+		assert( t != 0);
+	}
+	printf("Topics Test complete.\n\n");
+}
diff --git a/MQTTSNGateway/src/tests/TestTopics.h b/MQTTSNGateway/src/tests/TestTopics.h
new file mode 100644
index 0000000..9665851
--- /dev/null
+++ b/MQTTSNGateway/src/tests/TestTopics.h
@@ -0,0 +1,32 @@
+/**************************************************************************************
+ * Copyright (c) 2016, Tomoaki Yamaguchi
+ *
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * and Eclipse Distribution License v1.0 which accompany this distribution.
+ *
+ * The Eclipse Public License is available at
+ *    http://www.eclipse.org/legal/epl-v10.html
+ * and the Eclipse Distribution License is available at
+ *   http://www.eclipse.org/org/documents/edl-v10.php.
+ *
+ * Contributors:
+ *    Tomoaki Yamaguchi - initial API and implementation 
+ **************************************************************************************/
+#ifndef MQTTSNGATEWAY_SRC_TESTS_TESTTOPICS_H_
+#define MQTTSNGATEWAY_SRC_TESTS_TESTTOPICS_H_
+
+#include "MQTTSNGWClient.h"
+
+class TestTopics
+{
+public:
+	TestTopics();
+	~TestTopics();
+	void test(void);
+
+private:
+	Topics* _topics;
+};
+
+#endif /* MQTTSNGATEWAY_SRC_TESTS_TESTTOPICS_H_ */
diff --git a/MQTTSNGateway/src/tests/mainTestProcessFramework.cpp b/MQTTSNGateway/src/tests/mainTestProcess.cpp
similarity index 81%
rename from MQTTSNGateway/src/tests/mainTestProcessFramework.cpp
rename to MQTTSNGateway/src/tests/mainTestProcess.cpp
index 9c23e04..ab99096 100644
--- a/MQTTSNGateway/src/tests/mainTestProcessFramework.cpp
+++ b/MQTTSNGateway/src/tests/mainTestProcess.cpp
@@ -13,19 +13,19 @@
  * Contributors:
  *    Tomoaki Yamaguchi - initial API and implementation 
  **************************************************************************************/
-#include "TestProcessFramework.h"
+#include "TestProcess.h"
 #include "TestTask.h"
 
 using namespace MQTTSNGW;
 
-TestProcessFramework* proc = new TestProcessFramework();
-TestTask* task = new TestTask(proc);
+TestProcess* test = new TestProcess();
+TestTask* task = new TestTask(test);
 
 int main(int argc, char** argv)
 {
-	proc->initialize(argc, argv);
-	proc->run();
-	delete proc;
+	test->initialize(argc, argv);
+	test->run();
+	delete test;
 	return 0;
 }
 
diff --git a/Makefile b/Makefile
index 8fee46a..26df06a 100644
--- a/Makefile
+++ b/Makefile
@@ -5,7 +5,7 @@ LPROGNAME := MQTT-SNLogmonitor
 LAPPL := mainLogmonitor
 
 TESTPROGNAME := testPFW
-TESTAPPL := mainTestProcessFramework
+TESTAPPL := mainTestProcess
 
 CONFIG := MQTTSNGateway/gateway.conf
 CLIENTS := MQTTSNGateway/clients.conf
@@ -39,7 +39,10 @@ $(SRCDIR)/$(OS)/$(SENSORNET)/SensorNetwork.cpp \
 $(SRCDIR)/$(OS)/Timer.cpp  \
 $(SRCDIR)/$(OS)/Network.cpp \
 $(SRCDIR)/$(OS)/Threading.cpp \
-$(SRCDIR)/$(TEST)/TestProcessFramework.cpp \
+$(SRCDIR)/$(TEST)/TestProcess.cpp \
+$(SRCDIR)/$(TEST)/TestQue.cpp \
+$(SRCDIR)/$(TEST)/TestTopics.cpp \
+$(SRCDIR)/$(TEST)/TestTopicIdMap.cpp \
 $(SRCDIR)/$(TEST)/TestTask.cpp