

//_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/
//_/_/
//_/_/  This file is part of CMSDK version 2.0.0.170915_Beta4 (15 September 2017)
//_/_/  
//_/_/  developed by
//_/_/  
//_/_/  Communicative Machines Ltd. 
//_/_/    http://cmlabs.com/
//_/_/  Copyright (c) 2017
//_/_/  
//_/_/  in collaboration with 
//_/_/  
//_/_/  Icelandic Institute for Intelligent Machines ses
//_/_/    Menntavegur 1
//_/_/    101 Reykjavik
//_/_/    ICELAND
//_/_/    http://www.iiim.is/
//_/_/
//_/_/  and is released under the following
//_/_/  
//_/_/  EXTENDED BSD License
//_/_/
//_/_/  Redistribution and use in source and binary forms, with or without
//_/_/  modification, is permitted provided that the following conditions 
//_/_/  are met:
//_/_/
//_/_/  - Redistributions of source code must retain the above copyright 
//_/_/    and collaboration notice, this list of conditions and the following 
//_/_/    disclaimer.
//_/_/
//_/_/  - Redistributions in binary form must reproduce the above copyright 
//_/_/    notice, this list of conditions and the following disclaimer in the 
//_/_/    documentation and/or other materials provided with the distribution.
//_/_/
//_/_/  - Neither the name of its copyright holders nor the names of its 
//_/_/    contributors may be used to endorse or promote products derived from 
//_/_/    this software without specific prior written permission.
//_/_/
//_/_/  - CADIA Clause: The license granted in and to the software under this 
//_/_/    agreement is a limited-use license. The software may not be used in 
//_/_/    furtherance of: (i) intentionally causing bodily injury or severe 
//_/_/    emotional harm to any person; (ii) invading the personal privacy or 
//_/_/    violating the human rights of any person; or (iii) committing or 
//_/_/    preparing for any act of war. 
//_/_/
//_/_/  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
//_/_/  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
//_/_/  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 
//_/_/  PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER 
//_/_/  OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
//_/_/  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
//_/_/  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 
//_/_/  PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
//_/_/  LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
//_/_/  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
//_/_/  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//_/_/
//_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/
#include "MemoryQueues.h"
#include "PsyTime.h"

namespace cmlabs {

// static
bool MemoryQueues::CreateMessageQueue(uint32& qid, const char* name) {
	uint32 newQID;

	uint32 newSize = DEFAULTQUEUESIZE;
	uint32 pageID;
	char* data = MemoryManager::CreateAndLockNewSystemPage(newSize, pageID);
	if (data == NULL)
		return false;
	if (!MemoryMaps::CreateNewQueue(pageID, name, newQID)) {
		MemoryManager::UnlockSystemBlock(pageID);
		MemoryManager::DestroySystemPage(pageID);
		return false;
	}

	MessageQueueHeader* header = (MessageQueueHeader*)data;
	header->size = newSize;
	header->id = newQID;
	header->pageID = pageID;
	header->count = 0;
	header->startPos = 0;
	header->endPos = 0;
	header->padding = 0;
	header->id = 0;
	memset(data + sizeof(MessageQueueHeader), 0, newSize - sizeof(MessageQueueHeader));

	qid = newQID;
	MemoryManager::UnlockSystemBlock(pageID);
	return true;
}

// static
bool MemoryQueues::GetMessageQueueByName(uint32& qid, const char* name) {
	return MemoryMaps::GetQueueID(name, qid);
}

// static
bool MemoryQueues::AddMessageToQueue(uint32 qid, DataMessage* msg) {

	uint32 pageID = MemoryMaps::GetQueuePageID(qid);
	uint32 dataSize = 0;
	char* data = MemoryManager::GetAndLockSystemBlock(pageID, dataSize);
	if (data == NULL)
		return false;

	uint32 msgSize = msg->getSize();
	uint32 spaceToEnd;
	MessageQueueHeader* header = (MessageQueueHeader*)data;
	char* qData = data + sizeof(MessageQueueHeader);
	uint32 qDataSize = dataSize - sizeof(MessageQueueHeader);

	if (header->endPos >= header->startPos) {
		spaceToEnd = qDataSize - header->endPos;
		if (spaceToEnd >= msgSize) {
			// add msg here
			memcpy(qData+header->endPos, msg->data, msgSize);
			header->endPos += msgSize;
			header->count++;
			header->padding = 0;
		}
		else if (header->startPos >= msgSize) {
			// fill end rest of space with 0
			memset(qData+header->endPos, 0, spaceToEnd);
			header->padding = spaceToEnd;
			// add msg at buffer start
			memcpy(qData, msg->data, msgSize);
			header->endPos = msgSize;
			header->count++;
		}
		else {
			// we need to resize queue
			MemoryManager::UnlockSystemBlock(pageID);
			MemoryManager::ResizeSystemPage(pageID, dataSize*4);
			data = MemoryManager::GetAndLockSystemBlock(pageID, dataSize);
			header = (MessageQueueHeader*)data;
			qData = data + sizeof(MessageQueueHeader);
			qDataSize = dataSize - sizeof(MessageQueueHeader);
			// rearrange data if needed
			if (header->startPos > 0) {
				memcpy(qData, qData+header->startPos, header->endPos - header->startPos);
				header->endPos = header->endPos - header->startPos;
				header->startPos = 0;
			}
			MemoryManager::UnlockSystemBlock(pageID);
			// call this function again...
			return AddMessageToQueue(qid, msg);
		}
	}
	else {
		spaceToEnd = header->startPos - header->endPos;
		if (spaceToEnd >= msgSize) {
			// add msg here
			memcpy(qData+header->endPos, msg->data, msgSize);
			header->endPos += msgSize;
			header->count++;
		}
		else {
			// we need to resize queue
			MemoryManager::UnlockSystemBlock(pageID);
			MemoryManager::ResizeSystemPage(pageID, dataSize*4);
			uint32 newDataSize = 0;
			uint32 oldQDataSize = qDataSize;
			data = MemoryManager::GetAndLockSystemBlock(pageID, newDataSize);
			header = (MessageQueueHeader*)data;
			qData = data + sizeof(MessageQueueHeader);
			qDataSize = newDataSize - sizeof(MessageQueueHeader);
			// rearrange data 
			// move end data from start to after start data
			memcpy(qData+oldQDataSize-header->padding, qData, header->endPos);
			header->endPos = oldQDataSize + header->endPos - header->padding;

			//// first move end data away
			//memcpy(qData+oldQDataSize, qData, header->endPos);
			//// then move first data to beginning
			//memcpy(qData, qData+header->startPos, oldQDataSize - header->startPos);
			//// finally move end data back down just after start data
			//memcpy(qData + oldQDataSize - header->startPos, qData+oldQDataSize, header->endPos);
			//header->endPos = header->endPos + oldQDataSize - header->startPos;
			//header->startPos = 0;
			MemoryManager::UnlockSystemBlock(pageID);
			// call this function again...
			return AddMessageToQueue(qid, msg);
		}
	}

	// Signal a named Semaphore
	utils::SignalSemaphore(qid);
	//utils::Semaphore* sem = GetQueueSemaphore(qid);
	//if (sem) {
	//	sem->signal();
	//	delete(sem);
	//}

	MemoryManager::UnlockSystemBlock(pageID);
	return true;
}

// static
DataMessage* MemoryQueues::WaitForMessageQueue(uint32 qid, uint32 ms) {
	uint32 pageID = MemoryMaps::GetQueuePageID(qid);
	uint32 dataSize = 0;
	uint64 now, end;
	MessageQueueHeader* header = (MessageQueueHeader*) MemoryManager::GetAndLockSystemBlock(pageID, dataSize);
	if (!header)
		return NULL;

	if (header->count == 0) {
		// we no data and need to wait for the semaphore...
	//	utils::Semaphore* sem = GetQueueSemaphore(qid);
		MemoryManager::UnlockSystemBlock(pageID);
//		if (!sem)
	//		return NULL;
		end = GetTimeNow() + ms*1000;
		
		// Wait for semaphore to be signalled
		while (true) {
			now = GetTimeNow();
		//	if ( (now >= end) || !sem->wait((uint32)((end-now)/1000)) ) {
			if ( (now >= end) || !utils::WaitForSemaphore(qid, (uint32)((end-now)/1000)) ) {
			//	delete(sem);
				return NULL;
			}

			if (!(header = (MessageQueueHeader*) MemoryManager::GetAndLockSystemBlock(pageID, dataSize)))
				return NULL;
			if (header->count > 0)
				break;
			MemoryManager::UnlockSystemBlock(pageID);
		}
		//delete(sem);
	}
	char* qData = ((char*)header) + sizeof(MessageQueueHeader);
	uint32 qDataSize = dataSize - sizeof(MessageQueueHeader);

	// We have data, get it
	uint32 msgDataSize;
	DataMessage* msg = NULL;
	char* msgData = qData + header->startPos;
	if (((DataMessageHeader*)msgData)->cid == DATAMESSAGEID) {
		msgDataSize = ((DataMessageHeader*)msgData)->size;
		// we have data here, get it
		header->startPos += msgDataSize;
		header->count--;
	}
	else {
		// we have wrapped around, msg is now at the start of the data
		msgData = qData;
		if (((DataMessageHeader*)msgData)->cid == DATAMESSAGEID) {
			msgDataSize = ((DataMessageHeader*)msgData)->size;
			// we have data here, get it
			header->startPos = msgDataSize;
			header->count--;
			header->padding = 0;
		}
		else {
			MemoryManager::UnlockSystemBlock(pageID);
			return NULL;
		}
	}

	char* newData = (char*)malloc(msgDataSize);
	memcpy(newData, msgData, msgDataSize);
	msg = new DataMessage(newData);

	MemoryManager::UnlockSystemBlock(pageID);
	return msg;
}

// static
uint32 MemoryQueues::GetMessageQueueSize(uint32 qid) {
	uint32 pageID = MemoryMaps::GetQueuePageID(qid);
	uint32 dataSize = 0;
	char* data = MemoryManager::GetAndLockSystemBlock(pageID, dataSize);
	if (data == NULL)
		return false;

	MessageQueueHeader* header = (MessageQueueHeader*)data;
	uint32 qSize = header->count;

	MemoryManager::UnlockSystemBlock(pageID);
	return qSize;
}

// static
bool MemoryQueues::DestroyMessageQueue(uint32 qid) {
	uint32 pageID = MemoryMaps::GetQueuePageID(qid);
	// Lock it so no-one else can use it
	uint32 dataSize = 0;
	char* data = MemoryManager::GetAndLockSystemBlock(pageID, dataSize);

	MemoryMaps::DeleteQueue(qid);
	MemoryManager::UnlockSystemBlock(pageID);

	MemoryManager::DestroySystemPage(pageID);
	return true;
}


////////////////////////////////////////////////////////////////////////////////

// static 
bool MemoryQueues::AddRequest(DataMessage* msg, uint32& reqID) {
	// Create request entry
	RequestMapEntry* entry = MemoryMaps::CreateAndLockNewRequest(msg->getFrom(), msg->getTo(), reqID);
	if (!entry)
		return false;
	// Add reqID to message
	msg->setReference(reqID);
	// Put message into appropriate request queue
	// Find process for component
	uint32 procID = ComponentData::GetComponentProcessID(msg->getTo());
	if (procID == 0) {
		// if not local, add request to Node Request Queue
		procID = ComponentData::GetComponentProcessID(NODE_PROCESS_ID);
	}
	// Find Queue ID for process
	uint32 qID;
	if (!MemoryMaps::GetProcessQueueID(procID, REQQ_ID, qID)) {
		MemoryMaps::UnlockRequestMap();
		return false;
	}
	// Add request message to queue
	if (!AddMessageToQueue(qID, msg)) {
		MemoryMaps::UnlockRequestMap();
		return false;
	}
	entry->status = REQ_CREATED;
	MemoryMaps::UnlockRequestMap();
	return true;
}

// static 
bool MemoryQueues::AddReply(uint32 reqID, bool success, DataMessage* msg) {
	uint8 status;
	RequestMapEntry* entry = MemoryMaps::GetAndLockRequestEntry(reqID);
	if (!entry)
		return false;
	if (msg) {
		uint64 memID;
		uint64 msgEOL = msg->getEOL();
		if (!MemoryManager::InsertMemoryBlock((char*)msg->data, msg->getSize(), msgEOL, memID)) {
			MemoryMaps::UnlockRequestMap();
			return false;
		}
		entry->dataMessageID = memID;
		entry->dataMessageEOL = msgEOL;
		if (success)
			status = REQ_SUCCESS_DATA;
		else
			status = REQ_FAILED_DATA;
	}
	else {
		entry->dataMessageID = 0;
		entry->dataMessageEOL = 0;
		if (success)
			status = REQ_SUCCESS_NODATA;
		else
			status = REQ_FAILED_NODATA;
	}
	entry->status = status;

	utils::Semaphore* sem = GetRequestSemaphore(reqID);
	if (sem) {
		sem->signal();
		delete(sem);
	}
	MemoryMaps::UnlockRequestMap();
	return true;
}

// static 
bool MemoryQueues::WaitForReply(uint32 reqID, uint32 ms, uint8& status, DataMessage** outMsg) {

	RequestMapEntry* entry = MemoryMaps::GetAndLockRequestEntry(reqID);
	if (!entry)
		return false;

	uint64 now = GetTimeNow();
	if (entry->status < REQ_REPLY_READY) {
		// No answer yet, wait for it
		utils::Semaphore* sem = GetRequestSemaphore(reqID);
		if (!sem)
			return NULL;
		uint64 end = now + ms*1000;
		MemoryMaps::UnlockRequestMap();
		
		// Wait for semaphore to be signalled
		do {
			if (!sem->wait((uint32)((end-now)/1000))) {
				delete(sem);
				return false;
			}

			entry = MemoryMaps::GetAndLockRequestEntry(reqID);
			if (!entry)
				return false;

			if (entry->status > REQ_REPLY_READY)
				break;

		} while ((now = GetTimeNow()) < end);

		delete(sem);
	}
	
	if (entry->status < REQ_REPLY_READY) {
		MemoryMaps::UnlockRequestMap();
		return false;
	}

	// We have an answer, get it
	status = entry->status;
	*outMsg = NULL;
	now = GetTimeNow();
	if ( (status == REQ_SUCCESS_DATA) || (status == REQ_FAILED_DATA) ) {
		if (entry->dataMessageEOL > now) {
			uint32 size = 0;
			uint64 eol = 0;
			char* data = MemoryManager::GetCopyMemoryBlock(entry->dataMessageID, size, eol);
			if (data != NULL)
				*outMsg = new DataMessage(data);
		}
		if (*outMsg == NULL) {
			// set status to _DATA_EOL
			status += 2;
		}
	}

	MemoryMaps::UnlockRequestMap();
	MemoryMaps::DeleteRequest(reqID);
	return true;
}

// static 
uint32 MemoryQueues::GetRequestCount() {
	uint32 count;
	if (MemoryMaps::GetRequestCount(count))
		return count;
	else
		return 0;
}



// Get an existing Queue Mutex or creates it
//utils::Semaphore* MemoryQueues::GetQueueSemaphore(uint32 qID) {
//	char* name = new char[MAXKEYNAMELEN];
//	sprintf(name, "QueueSemaphore_%u", qID);
//	utils::Semaphore* sem = new utils::Semaphore(name);
//	delete [] name;
//	return sem;
//}

// Get an existing Request Mutex or creates it
utils::Semaphore* MemoryQueues::GetRequestSemaphore(uint32 reqID) {
	char* name = new char[MAXKEYNAMELEN];
	sprintf(name, "RequestSemaphore_%u", reqID);
	utils::Semaphore* sem = new utils::Semaphore(name);
	delete [] name;
	return sem;
}



////////////////////////////////////////////////////////////////////////////////

bool MemoryQueues::UnitTest() {
	printf("Testing Memory Queues...\n\n");

	// First create and initialise the MemoryManager
	MemoryManager* manager = new MemoryManager();
	uint32 maxPageCount = 100;
	if (!manager->create(0, maxPageCount)) {
		fprintf(stderr, "MemoryManager init() failed...\n");
		delete(manager);
		return false;
	}

	uint32 qid1;
	if (!CreateMessageQueue(qid1)) {
		fprintf(stderr, "Could not create MessageQueue...\n");
		return false;
	}
	uint32 count = 10000;
	uint32 writeCount = 0;
	uint32 readCount = 0;
	uint32 n;
	DataMessage* msg;
	char str[128];
	uint64 time = GetTimeNow();
	for (n=0; n<count/4; n++) {
		msg = new DataMessage(CTRL_TEST, writeCount);
		sprintf(str, "Test%2d", writeCount);
		msg->setString("TestEntry", str);
		msg->setTime("TestTime", time);
		msg->setString("TestEntry2", str);
		msg->setTime("TestTime2", time);
		if (!AddMessageToQueue(qid1, msg)) {
			fprintf(stderr, "Could not add Message %d to Queue...\n", n);
			return false;
		}
		delete(msg);
		writeCount++;
	}
	
	if ( (n = GetMessageQueueSize(qid1)) != count/4) {
		fprintf(stderr, "Queue contains %u messages instead of %u...\n", n, count);
		return false;
	}
	
	for (n=0; n<count/8; n++) {
		if ( (msg = WaitForMessageQueue(qid1, 1000)) == NULL) {
			fprintf(stderr, "Could not get Message %u from Queue...\n", n);
			return false;
		}
		sprintf(str, "Test%2d", readCount);
		if (strcmp(str, msg->getString("TestEntry")) != 0) {
			fprintf(stderr, "Message %u from Queue corrupted string '%s'...\n", readCount, msg->getString("TestEntry"));
			return false;
		}
		if (msg->getTime("TestTime") != time) {
			fprintf(stderr, "Message %u from Queue corrupted time '%llu'...\n", readCount, msg->getTime("TestTime"));
			return false;
		}
		if (strcmp(str, msg->getString("TestEntry2")) != 0) {
			fprintf(stderr, "Message %u from Queue corrupted string '%s'...\n", readCount, msg->getString("TestEntry2"));
			return false;
		}
		if (msg->getTime("TestTime2") != time) {
			fprintf(stderr, "Message %u from Queue corrupted time '%llu'...\n", readCount, msg->getTime("TestTime2"));
			return false;
		}
		readCount++;
		delete(msg);
	}

	if ( (n = GetMessageQueueSize(qid1)) != count/8) {
		fprintf(stderr, "Queue contains %u messages instead of 0...\n", n);
		return false;
	}

	for (n=0; n<3*count/4; n++) {
		msg = new DataMessage(CTRL_TEST, n);
		sprintf(str, "Test%2d", writeCount);
		msg->setString("TestEntry", str);
		msg->setTime("TestTime", time);
		msg->setString("TestEntry2", str);
		msg->setTime("TestTime2", time);
		if (!AddMessageToQueue(qid1, msg)) {
			fprintf(stderr, "Could not add Message %d to Queue...\n", n);
			return false;
		}
		delete(msg);
		writeCount++;
	}

	if ( (n = GetMessageQueueSize(qid1)) != 7*count/8) {
		fprintf(stderr, "Queue contains %u messages instead of %u...\n", n, count);
		return false;
	}
	
	for (n=0; n<7*count/8; n++) {
		if ( (msg = WaitForMessageQueue(qid1, 1000)) == NULL) {
			fprintf(stderr, "Could not get Message %u from Queue...\n", n);
			return false;
		}
		sprintf(str, "Test%2d", readCount);
		if (strcmp(str, msg->getString("TestEntry")) != 0) {
			fprintf(stderr, "Message %u from Queue corrupted string '%s'...\n", readCount, msg->getString("TestEntry"));
			fprintf(stderr, "Queue contains %u messages...\n", GetMessageQueueSize(qid1));
			return false;
		}
		//fprintf(stderr, "Message %u from Queue '%s'...\n", readCount, msg->getString("TestEntry"));
		if (msg->getTime("TestTime") != time) {
			fprintf(stderr, "Message %u from Queue corrupted time '%llu'...\n", readCount, msg->getTime("TestTime"));
			return false;
		}
		if (strcmp(str, msg->getString("TestEntry2")) != 0) {
			fprintf(stderr, "Message %u from Queue corrupted string '%s'...\n", readCount, msg->getString("TestEntry2"));
			return false;
		}
		if (msg->getTime("TestTime2") != time) {
			fprintf(stderr, "Message %u from Queue corrupted time '%llu'...\n", readCount, msg->getTime("TestTime2"));
			return false;
		}
		readCount++;
		delete(msg);
	}

	if ( (n = GetMessageQueueSize(qid1)) != 0) {
		fprintf(stderr, "Queue contains %u messages instead of 0...\n", n);
		return false;
	}

	if (!DestroyMessageQueue(qid1)) {
		fprintf(stderr, "Could not destroy MessageQueue...\n");
		return false;
	}


	printf("Testing Request Queue...\n\n");

	// First create two components
	uint16 procID1, procID2;
	uint32 compID1 = 1, compID2 = 2;
	uint32 qID;
	//if (!MemoryMaps::CreateNewNode(1, "", 1)) {
	//	printf("Error: Testing Request Queue - CreateNewNode\n");
	//	return false;
	//}
	if (!MemoryMaps::CreateNewProcess("Proc1", procID1)) {
		printf("Error: Testing Request Queue - CreateNewProcess\n");
		return false;
	}
	if (!MemoryMaps::CreateNewProcess("Proc2", procID2)) {
		printf("Error: Testing Request Queue - CreateNewProcess 2\n");
		return false;
	}
	if (!MemoryQueues::CreateMessageQueue(qID)) {
		printf("Error: Testing Request Queue - CreateMessageQueue\n");
		return false;
	}
	if (!MemoryMaps::SetProcessQueueID(procID1, MSGQ_ID, qID)) {
		printf("Error: Testing Request Queue - SetProcessQueueID\n");
		return false;
	}
	if (!MemoryQueues::CreateMessageQueue(qID)) {
		printf("Error: Testing Request Queue - CreateMessageQueue\n");
		return false;
	}
	if (!MemoryMaps::SetProcessQueueID(procID2, REQQ_ID, qID)) {
		printf("Error: Testing Request Queue - SetProcessQueueID\n");
		return false;
	}

	ComponentData* compData1 = ComponentData::CreateComponent(compID1, "Comp1", 10*1024, 1, procID1);
	if (compData1 == NULL) {
		printf("Error: Testing Request Queue - CreateComponent1\n");
		return false;
	}
	ComponentData* compData2 = ComponentData::CreateComponent(compID2, "Comp2", 10*1024, 1, procID2);
	if (compData2 == NULL) {
		printf("Error: Testing Request Queue - CreateComponent2\n");
		return false;
	}

	DataMessage* reqMsg = new DataMessage(CTRL_TEST, compID1, compID2);
	uint32 reqID = 0;
	if (!MemoryQueues::AddRequest(reqMsg, reqID)) {
		printf("Error: Testing Request Queue - AddRequest\n");
		return false;
	}
//	MemoryQueues::AddRequest(reqMsg, reqID);
//	MemoryQueues::AddRequest(reqMsg, reqID);
	delete(reqMsg);

	if (!MemoryMaps::GetProcessQueueID(procID2, REQQ_ID, qID)) {
		printf("Error: Testing Request Queue - GetProcessQueueID\n");
		return false;
	}
	reqMsg = MemoryQueues::WaitForMessageQueue(qID, 1000);
	if (reqMsg == NULL) {
		printf("Error: Testing Request Queue - WaitForMessageQueue\n");
		return false;
	}

	uint32 reqID2 = (uint32)reqMsg->getReference();
	if (reqID != reqID2) {
		printf("Error: Testing Request Queue - getReference mismatch\n");
		return false;
	}

	DataMessage* replyMsg = new DataMessage(CTRL_TEST, compID2, 0, 1000000000);
	if (!MemoryQueues::AddReply(reqID, true, replyMsg)) {
		printf("Error: Testing Request Queue - AddReply\n");
		return false;
	}
	delete(reqMsg);
	delete(replyMsg);
	replyMsg = NULL;

	uint8 status;
	if (!MemoryQueues::WaitForReply(reqID2, 1000, status, &replyMsg)) {
		printf("Error: Testing Request Queue - WaitForReply\n");
		return false;
	}

	if (status != REQ_SUCCESS_DATA) {
		printf("Error: Testing Request Queue - Status mismatch\n");
		return false;
	}

	if (replyMsg == NULL) {
		printf("Error: Testing Request Queue - ReplyMsg NULL\n");
		return false;
	}

	delete(replyMsg);
	delete(compData1);
	delete(compData2);

	fprintf(stderr, "*** Memory Queues test successful ***\n\n\n");
	delete(manager);
	return true;
}


bool MemoryQueues::UnitTestQueues() {
	printf("Testing Multithreaded Memory Queues...\n\n");

	// First create and initialise the MemoryManager
	MemoryManager* manager = new MemoryManager();
	if (!manager->create(0, 30)) {
		fprintf(stderr, "MemoryManager init() failed...\n");
		delete(manager);
		return false;
	}

	uint64 t1, t2, t3;
	DataMessage* msg;
	int64 c = 0;
	uint32 q1, q2;

	if (!MemoryQueues::CreateMessageQueue(q1, "Q1")) {
		LogPrint(0, 0, 0, "[1] Could not create Test Queue 1...");
		delete(manager);
		return false;
	}
	if (!MemoryQueues::CreateMessageQueue(q2, "Q2")) {
		LogPrint(0, 0, 0, "[1] Could not create Test Queue 2...");
		delete(manager);
		return false;
	}

	uint32 queueTestThreadID;
	if (!ThreadManager::CreateThread(QueueTest, NULL, queueTestThreadID)) {
		LogPrint(0, 0, 0, "[1] Could not create Queue Test thread...");
		delete(manager);
		return false;
	}

	msg = new DataMessage();
	msg->setInt("Counter", 0);

	if (!MemoryQueues::AddMessageToQueue(q1, msg)) {
		LogPrint(0, 0, 0, "[1] Could not add initial message to Q1...");
		delete(manager);
		return false;
	}

	uint64 start = 0;

	while (true) {
		t1 = GetTimeNow();
		if (msg = MemoryQueues::WaitForMessageQueue(q2, 50)) {
			t2 = GetTimeNow();
			msg->getInt("Counter", c);
			if (c && (c % 99999 == 0)) {
				if (start)
					LogPrint(0,0,0,"[1] Average path time: %.3fus", (double)GetTimeAge(start)/99999.0);
				start = GetTimeNow();
			}
			msg->setInt("Counter", c+1);
			msg->setSendTime(GetTimeNow());
			if (!MemoryQueues::AddMessageToQueue(q1, msg)) {
				LogPrint(0, 0, 0, "[1] Could not add message %lld to Q1...", c+1);
				delete(msg);
				thread_ret_val(0);
			}
			t3 = GetTimeNow();
			if (t3-t1 > 10000)
				LogPrint(0,0,0,"[1 - %lld] Wait: %lld   Add: %lld   Total: %lld   Msg: %lld\n", c+1, t2-t1, t3-t2, t3-t1, t3-msg->getSendTime());
			delete(msg);
		}
		else {
			t3 = GetTimeNow();
			LogPrint(0,0,0,"[1 - %lld] Timeout: %lld\n", c+1, t3-t1);
		}
	}

	return true;
}

THREAD_RET THREAD_FUNCTION_CALL QueueTest(THREAD_ARG arg) {
	if (!MemoryManager::Singleton)
		thread_ret_val(0);
	
	uint64 t1, t2, t3;
	DataMessage* msg;
	int64 c = 0;
	uint32 q1, q2;

	if (!MemoryQueues::GetMessageQueueByName(q1, "Q1")) {
		LogPrint(0, 0, 0, "[2] Could not Get Test Queue 1...");
		thread_ret_val(0);
	}

	if (!MemoryQueues::GetMessageQueueByName(q2, "Q2")) {
		LogPrint(0, 0, 0, "[2] Could not Get Test Queue 2...");
		thread_ret_val(0);
	}

	while (true) {
		t1 = GetTimeNow();
		if (msg = MemoryQueues::WaitForMessageQueue(q1, 50)) {
			t2 = GetTimeNow();
			msg->getInt("Counter", c);
			msg->setInt("Counter", c+1);
			msg->setSendTime(GetTimeNow());
			if (!MemoryQueues::AddMessageToQueue(q2, msg)) {
				LogPrint(0, 0, 0, "[2] Could not add message %lld to Q2...", c+1);
				delete(msg);
				thread_ret_val(0);
			}
			t3 = GetTimeNow();
			if (t3-t1 > 10000)
				LogPrint(0,0,0,"[2 - %lld] Wait: %lld   Add: %lld   Total: %lld   Msg: %lld\n", c+1, t2-t1, t3-t2, t3-t1, t3-msg->getSendTime());
			delete(msg);
		}
		else {
			t3 = GetTimeNow();
			LogPrint(0,0,0,"[2 - %lld] Timeout: %lld\n", c+1, t3-t1);
		}
	}

	thread_ret_val(0);
}

} // namespace cmlabs
