

//_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/
//_/_/
//_/_/  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 "ProcessMemory.h"

namespace cmlabs {

ProcessMemory::ProcessMemory(MasterMemory* master) {
	memset(qSemaphores, 0, MAXPROC4*sizeof(utils::Semaphore*));
	mutex = NULL;
	this->master = master;
	header = NULL;
	memorySize = 0;
	port = 0;
	serial = 0;
}

ProcessMemory::~ProcessMemory() {
	if (mutex)
		mutex->enter(5000, __FUNCTION__);
	if (memorySize)
		utils::CloseSharedMemorySegment((char*) header, memorySize);
	memset(qSemaphores, 0, MAXPROC4*sizeof(utils::Semaphore*));
	header = NULL;
	if (mutex)
		mutex->leave();
	delete(mutex);
	mutex = NULL;
}

bool ProcessMemory::getMemoryUsage(uint64& alloc, uint64& usage) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL

//	printf(" --------- Get ProcessMap usage size: %u header: %u usage: %u serial %u --------\n", memorySize, header->size, header->usage, serial);

	alloc = memorySize;
	usage = header->usage;

	mutex->leave();
	return true;
}

bool ProcessMemory::checkProcessHeartbeats(uint32 timeoutMS, std::list<ProcessInfoStruct>& procIssues) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL

	bool issuesFound = false;
	ProcessInfoStruct* pInfo = &(header->processes[1]);
	for (uint32 n=1; n<MAXPROC; n++, pInfo++) {
		if (pInfo->createdTime && (pInfo->nodeID == this->master->getNodeID())) {
			//if (!pInfo->lastseen) {
			//	procIssues.push_back(*pInfo);
			//	issuesFound = true;
			//}
			if (pInfo->status > PSYPROC_ACTIVE) {
				procIssues.push_back(*pInfo);
				issuesFound = true;
			}
			else if ((pInfo->status > PSYPROC_IDLE) && (GetTimeAgeMS(pInfo->lastseen) > (int64)timeoutMS)) {
				procIssues.push_back(*pInfo);
				issuesFound = true;
			}
		}
	}
	mutex->leave();
	return (!issuesFound);
}

std::vector<ProcessInfoStruct>* ProcessMemory::getAllProcesses() {
	std::vector<ProcessInfoStruct>* processes = new std::vector<ProcessInfoStruct>;
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return processes;
	CHECKPROCESSMEMORYSERIAL

	uint16 thisNodeID = this->master->getNodeID();
	ProcessInfoStruct* pInfo = &(header->processes[1]);
	for (uint32 n=1; n<MAXPROC; n++, pInfo++) {
		if (pInfo->createdTime && (pInfo->nodeID == thisNodeID))
			processes->push_back(*pInfo);
	}
	mutex->leave();
	return processes;
}

bool ProcessMemory::addLocalPerformanceStats(std::list<PerfStats> &perfStats) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL

	uint16 thisNodeID = this->master->getNodeID();

	PerfStats stats;
	ProcessInfoStruct* pInfo = &(header->processes[1]);
	for (uint32 n=1; n<MAXPROC; n++, pInfo++) {
		if (pInfo->createdTime && (pInfo->nodeID == thisNodeID)) {
			memset(&stats, 0, sizeof(PerfStats));
			stats.spaceID = pInfo->id;
			stats.nodeID = pInfo->nodeID;
			stats.osID = pInfo->osID;
			// stats.type = stats.space;
			if (pInfo->type == 1) // inside node
				stats.currentCPUTicks = 0;
			else
				stats.currentCPUTicks = pInfo->stats.currentCPUTicks;
			// stats.currentMemoryBytes = pInfo->stats.procMemUsage;
			stats.currentMemoryBytes = utils::GetProcessMemoryUsage();
			stats.totalInputBytes = pInfo->stats.msgInBytes;
			stats.totalInputCount = pInfo->stats.msgInCount;
			stats.totalOutputBytes = pInfo->stats.msgOutBytes;
			stats.totalOutputCount = pInfo->stats.msgOutCount;
			// stats.runCount = 0;
			getQueueSizes(pInfo->id, stats.totalQueueBytes, stats.totalQueueCount);
			// stats.currentRunStartTime = 0;
			stats.firstRunStartTime = pInfo->createdTime;
			// stats.totalCycleCount = 0;
			// stats.totalRunCount = 0;
			// stats.migrationCount = 0;
			perfStats.push_back(stats);
		}
	}
	mutex->leave();

	return true;
}

bool ProcessMemory::getQueueSizes(uint16 procID, uint64 &bytes, uint32 &count) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return 0;
	CHECKPROCESSMEMORYSERIAL

	bytes = 0;
	count = 0;

	MessageQueueHeader* qHeader;
	for (uint8 t = 1; t<5; t++) {
		qHeader = getQHeader(procID, t);
		if (qHeader) {
			count += qHeader->count;
			if (qHeader->count) {
				if (qHeader->endPos > qHeader->startPos)
					bytes = qHeader->endPos - qHeader->startPos;
				else
					bytes = qHeader->startPos - qHeader->endPos;
			}
		}
	}
	mutex->leave();
	return true;
}

bool ProcessMemory::create(uint32 initialProcCount) {
	serial = master->incrementProcessShmemSerial();
	port = master->port;

//	LogPrint(0,LOG_MEMORY,0," --------- Creating ProcessMemory section serial %u --------", serial);
	if (!mutex)
		mutex = new utils::Mutex(utils::StringFormat("PsycloneProcessMemoryMutex_%u", port).c_str(), true);
	if (!mutex->enter(5000, __FUNCTION__))
		return false;
	
	memorySize = sizeof(ProcessMemoryStruct) + (initialProcCount * INITIALQSIZE * 4);

	//header = (ProcessMemoryStruct*) utils::OpenSharedMemorySegment(utils::StringFormat("PsycloneProcessMemory_%u_%u", port, serial).c_str(), memorySize);
	//if (header) {
	//	utils::CloseSharedMemorySegment((char*)header, memorySize);
	//	LogPrint(0,LOG_MEMORY,2,"ProcessMemory removing stale shared memory (%u/%u)...", port, serial);
	//}

	header = (ProcessMemoryStruct*) utils::CreateSharedMemorySegment(utils::StringFormat("PsycloneProcessMemory_%u_%u", port, serial).c_str(), memorySize, true);
	if (!header) {
		LogPrint(0,LOG_MEMORY,0,"Cannot create memory");
		mutex->leave();
		return false;
	}
	memset(header, 0, (size_t)memorySize);
	header->size = memorySize;
	header->cid = PROCESSMEMORYID;
	header->createdTime = GetTimeNow();
	header->usage = sizeof(ProcessMemoryStruct);
	this->port = port;

//	printf(" --------- Creating new ProcessMap header: %u usage: %u serial %u --------\n", header->size, header->usage, serial);

	// statically initialise base process id 0
	ProcessInfoStruct* pInfo = &(header->processes[0]);
	pInfo->createdTime = pInfo->lastseen = GetTimeNow();
	pInfo->status = PSYPROC_ACTIVE;
	pInfo->nodeID = master->getNodeID();
	utils::strcpyavail(pInfo->name, "Node", MAXKEYNAMELEN, true);

	uint16 cmdQID;
	uint16 msgQID;
	uint16 sigQID;
	uint16 reqQID;

	if (
		!setupNextAvailableQ(cmdQID) ||
		!setupNextAvailableQ(msgQID) ||
		!setupNextAvailableQ(sigQID) ||
		!setupNextAvailableQ(reqQID) ) {
		mutex->leave();
		LogPrint(0,LOG_MEMORY,0,"Cannot setup queues");
		return false;
	}

	// reset as header might have changed...
	pInfo = &(header->processes[0]);
	pInfo->cmdQID = cmdQID;
	pInfo->msgQID = msgQID;
	pInfo->sigQID = sigQID;
	pInfo->reqQID = reqQID;

	master->setProcessShmemSize(memorySize);

//	printf(" --------- Created new ProcessMap header: %u usage: %u serial %u --------\n", header->size, header->usage, serial);
	mutex->leave();
	return true;
}

bool ProcessMemory::open() {
	serial = master->getProcessShmemSerial();
	uint64 size = master->getProcessShmemSize();
	this->port = master->port;
//	printf(" --------- Opening new ProcessMemory section serial %u --------\n", serial);

	bool createdMutex = false;
	if (!mutex) {
		//LogPrint(0,LOG_MEMORY,0," --------- Opening new ProcessMemory section serial %u --------", serial);
		mutex = new utils::Mutex(utils::StringFormat("PsycloneProcessMemoryMutex_%u", port).c_str());
		createdMutex = true;
		if (!mutex->enter(5000, __FUNCTION__))
			return false;
	}
	//else
	//	LogPrint(0,LOG_MEMORY,0," --------- Reopening ProcessMemory section serial %u --------", serial);

	ProcessMemoryStruct* newHeader = (ProcessMemoryStruct*) utils::OpenSharedMemorySegment(utils::StringFormat("PsycloneProcessMemory_%u_%u", port, serial).c_str(), size);
	if (!newHeader) {
		LogPrint(0,LOG_MEMORY,0,"Cannot open shared memory: %s size: %u", utils::StringFormat("PsycloneProcessMemory_%u_%u", port, serial).c_str(), size);
		if (createdMutex)
			mutex->leave();
		return false;
	}
	if (newHeader->cid != PROCESSMEMORYID) {
		LogPrint(0,LOG_MEMORY,0,"Cannot verify shared memory: %s size: %u", utils::StringFormat("PsycloneProcessMemory_%u_%u", port, serial).c_str(), size);
		utils::CloseSharedMemorySegment((char*)newHeader, size);
		if (createdMutex)
			mutex->leave();
		return false;
	}

	memorySize = size;
	if (header) 
		utils::CloseSharedMemorySegment((char*)header, header->size);
	header = newHeader;
	if (createdMutex)
		mutex->leave();
	// else leave mutex locked as we are calling from within the object
	return true;
}








// static 
bool ProcessMemory::createNewProcess(const char* name, uint16 &id) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL

	if (!header->size) {
		printf(" --------- Process header error: %llu usage: %llu serial %u --------\n", header->size, header->usage, serial);
		return false;
	}

	// Find first empty offset, 0 is the base and always reserved
	id = 0;
	ProcessInfoStruct* pInfo = &(header->processes[1]);
	for (uint32 n=1; n<MAXPROC; n++, pInfo++) {
		if (!pInfo->createdTime) {
			id = n;
			break;
		}
	}
	if (!id) {
		mutex->leave();
		return false;
	}
	pInfo->id = id;
	pInfo->createdTime = pInfo->lastseen = GetTimeNow();
	pInfo->status = PSYPROC_CREATED;
	pInfo->nodeID = master->getNodeID();
	utils::strcpyavail(pInfo->name, name, MAXKEYNAMELEN, true);

	uint16 cmdQID;
	uint16 msgQID;
	uint16 sigQID;
	uint16 reqQID;

	if (
		!setupNextAvailableQ(cmdQID) ||
		!setupNextAvailableQ(msgQID) ||
		!setupNextAvailableQ(sigQID) ||
		!setupNextAvailableQ(reqQID) ) {
		mutex->leave();
		return false;
	}

	// reset as header might have changed...
	pInfo = &(header->processes[id]);
	pInfo->cmdQID = cmdQID;
	pInfo->msgQID = msgQID;
	pInfo->sigQID = sigQID;
	pInfo->reqQID = reqQID;

	mutex->leave();
	return true;
}

// static
bool ProcessMemory::deleteProcess(uint16 id) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL
	// delete queues
	deleteQ(header->processes[id].cmdQID);
	deleteQ(header->processes[id].msgQID);
	deleteQ(header->processes[id].sigQID);
	deleteQ(header->processes[id].reqQID);
	memset(&(header->processes[id]), 0, sizeof(ProcessInfoStruct));
	return true;
}

// static
bool ProcessMemory::getProcessName(uint16 id, char* name, uint32 maxSize) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL
	if (!header->processes[id].createdTime) {
		mutex->leave();
		return false;
	}
	utils::strcpyavail(name, header->processes[id].name, maxSize, true);
	mutex->leave();
	return true;
}

// static
bool ProcessMemory::getProcessID(const char* name, uint16 &id) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL
	ProcessInfoStruct* pInfo = &(header->processes[1]);
	for (uint32 n=1; n<MAXPROC; n++, pInfo++) {
		if ((stricmp(name, pInfo->name) == 0) && pInfo->createdTime) {
			id = n;
			mutex->leave();
			return true;
		}
	}
	mutex->leave();
	return false;
}

// static
uint64 ProcessMemory::getProcessCreateTime(uint16 id) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return 0;
	CHECKPROCESSMEMORYSERIAL
	uint64 t = header->processes[id].createdTime;
	mutex->leave();
	return t;
}

// static
bool ProcessMemory::getProcessCommandLine(uint16 id, char* cmdline, uint32 maxSize) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL
	if (!header->processes[id].createdTime) {
		mutex->leave();
		return false;
	}
	utils::strcpyavail(cmdline, header->processes[id].commandline, maxSize, true);
	mutex->leave();
	return true;
}

// static
uint8 ProcessMemory::getProcessStatus(uint16 id, uint64& lastseen) {
	uint64 createTime;
	return getProcessStatus(id, lastseen, createTime);
}

uint8 ProcessMemory::getProcessStatus(uint16 id, uint64& lastseen, uint64& createTime) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return 0;
	CHECKPROCESSMEMORYSERIAL
	if (!header->processes[id].createdTime) {
		mutex->leave();
		lastseen = 0;
		return 0;
	}
	uint8 status = header->processes[id].status;
	lastseen = header->processes[id].lastseen;
	createTime = header->processes[id].createdTime;
	mutex->leave();
	return status;
}

// static
uint16 ProcessMemory::getProcessIDFromOSID(uint32 osid) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return 0;
	CHECKPROCESSMEMORYSERIAL
	ProcessInfoStruct* pInfo = &(header->processes[1]);
	for (uint32 n=1; n<MAXPROC; n++, pInfo++) {
		if (pInfo->osID == osid) {
			uint32 id = n;
			mutex->leave();
			return id;
		}
	}
	mutex->leave();
	return 0;
}

// static
uint32 ProcessMemory::getProcessOSID(uint16 id) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return 0;
	CHECKPROCESSMEMORYSERIAL
	if (!header->processes[id].createdTime) {
		mutex->leave();
		return 0;
	}
	uint32 osID = header->processes[id].osID;
	mutex->leave();
	return osID;
}

// static
bool ProcessMemory::setProcessOSID(uint16 id, uint32 osid) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL
	if (!header->processes[id].createdTime) {
		mutex->leave();
		return false;
	}
	header->processes[id].osID = osid;
	mutex->leave();
	return true;
}

// static
bool ProcessMemory::setProcessType(uint16 id, uint8 type) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL
	if (!header->processes[id].createdTime) {
		mutex->leave();
		return false;
	}
	header->processes[id].type = type;
	mutex->leave();
	return true;
}

// static
bool ProcessMemory::setProcessCommandLine(uint16 id, const char* cmdline) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL
	if (!header->processes[id].createdTime) {
		mutex->leave();
		return false;
	}
	utils::strcpyavail(header->processes[id].commandline, cmdline, MAXCOMMANDLINELEN, false);
	mutex->leave();
	return true;
}

// static
bool ProcessMemory::setProcessStatus(uint16 id, uint8 status, uint64 currentCPUTicks) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL
	ProcessInfoStruct* pInfo = &header->processes[id]; 
	if (!pInfo->createdTime) {
		mutex->leave();
		return false;
	}
	pInfo->status = status;
	pInfo->lastseen = GetTimeNow();
	pInfo->stats.procMemUsage = utils::GetProcessMemoryUsage();
	if (currentCPUTicks)
		pInfo->stats.currentCPUTicks = currentCPUTicks;
	mutex->leave();
	return true;
}

AveragePerfStats ProcessMemory::getProcessPerfStats(uint16 procID) {
	AveragePerfStats perfStats;
	memset(&perfStats, 0, sizeof(AveragePerfStats));

	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return perfStats;
	if (serial != master->getProcessShmemSerial()) {if (!open()) {mutex->leave();return perfStats;}}

	ProcessInfoStruct* pInfo = &header->processes[procID]; 
	if (!pInfo->createdTime) {
		mutex->leave();
		return perfStats;
	}
	memcpy(&perfStats, &pInfo->perfStats, sizeof(AveragePerfStats));
	mutex->leave();
	return perfStats;
}

bool ProcessMemory::setProcessPerfStats(uint16 procID, AveragePerfStats &perfStruct) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL
	ProcessInfoStruct* pInfo = &header->processes[procID]; 
	if (!pInfo->createdTime) {
		mutex->leave();
		return false;
	}
	memcpy(&pInfo->perfStats, &perfStruct, sizeof(AveragePerfStats));
	mutex->leave();
	return true;
}


bool ProcessMemory::addToProcessStats(uint16 id, DataMessage* inputMsg, DataMessage* outputMsg) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL
	ProcessInfoStruct* pInfo = &header->processes[id]; 
	if (!pInfo->createdTime) {
		mutex->leave();
		return false;
	}

	pInfo->stats.time = GetTimeNow();
	DataMessage* draft;
	if (inputMsg) {
		pInfo->stats.msgInCount++;
		pInfo->stats.msgInBytes += inputMsg->getSize();
		//memmove(((char*)pInfo->stats.recentInMsg)+sizeof(DataMessageHeader), (char*)pInfo->stats.recentInMsg, 9*sizeof(DataMessageHeader));
		//memcpy((char*)pInfo->stats.recentInMsg, inputMsg->data, sizeof(DataMessageHeader));
		memmove((pInfo->stats.recentInMsg) + DRAFTMSGSIZE, pInfo->stats.recentInMsg, 9 * DRAFTMSGSIZE);
		if (inputMsg->data->size < DRAFTMSGSIZE)
			memcpy(pInfo->stats.recentInMsg, inputMsg->data, inputMsg->data->size);
		else {
			draft = new DataMessage(*inputMsg, DRAFTMSGSIZE);
			memcpy(pInfo->stats.recentInMsg, draft->data, draft->data->size);
			delete(draft);
		}
	}
	if (outputMsg) {
		pInfo->stats.msgOutCount++;
		pInfo->stats.msgOutBytes += outputMsg->getSize();
		//memmove(((char*)pInfo->stats.recentOutMsg)+sizeof(DataMessageHeader), (char*)pInfo->stats.recentOutMsg, 9*sizeof(DataMessageHeader));
		//memcpy((char*)pInfo->stats.recentOutMsg, outputMsg->data, sizeof(DataMessageHeader));
		memmove((pInfo->stats.recentOutMsg) + DRAFTMSGSIZE, pInfo->stats.recentOutMsg, 9 * DRAFTMSGSIZE);
		if (outputMsg->data->size < DRAFTMSGSIZE)
			memcpy(pInfo->stats.recentOutMsg, outputMsg->data, outputMsg->data->size);
		else {
			draft = new DataMessage(*outputMsg, DRAFTMSGSIZE);
			memcpy(pInfo->stats.recentOutMsg, draft->data, draft->data->size);
			delete(draft);
		}
	}

	mutex->leave();
	return true;
}

bool ProcessMemory::addToCmdQ(uint16 procID, DataMessage* msg) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL

	if (!addToQ(procID, CMDQ_TYPE, msg)) {
		mutex->leave();
		return false;
	}
	mutex->leave();
	return true;
}

bool ProcessMemory::addToMsgQ(uint16 procID, DataMessage* msg) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL

	if (!addToQ(procID, MSGQ_TYPE, msg)) {
		mutex->leave();
		return false;
	}
	mutex->leave();
	return true;
}

bool ProcessMemory::addToSigQ(uint16 procID, DataMessage* msg) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL

	if (!addToQ(procID, SIGQ_TYPE, msg)) {
		mutex->leave();
		return false;
	}
	mutex->leave();
	return true;
}

bool ProcessMemory::addToAllSignalQs(DataMessage* msg) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL

	ProcessInfoStruct* pInfo = &(header->processes[0]);
	for (uint32 n=1; n<MAXPROC; n++, pInfo++) {
		if (pInfo->createdTime)
			addToQ(pInfo->id, SIGQ_TYPE, msg);
	}

	mutex->leave();
	return true;
}

bool ProcessMemory::addToAllSignalQsExcept(DataMessage* msg, uint16 except) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL

	ProcessInfoStruct* pInfo = &(header->processes[0]);
	for (uint32 n=1; n<MAXPROC; n++, pInfo++) {
		if (pInfo->createdTime && (pInfo->id != except))
			addToQ(pInfo->id, SIGQ_TYPE, msg);
	}

	mutex->leave();
	return true;
}

bool ProcessMemory::addToReqQ(uint16 procID, DataMessage* msg) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;
	CHECKPROCESSMEMORYSERIAL

	if (!addToQ(procID, REQQ_TYPE, msg)) {
		mutex->leave();
		return false;
	}
	mutex->leave();
	return true;
}


DataMessage* ProcessMemory::waitForCmdQ(uint16 procID, uint32 timeout) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return NULL;
	CHECKPROCESSMEMORYSERIAL
	DataMessage* msg = waitForQ(procID, CMDQ_TYPE, timeout);
	mutex->leave();
	return msg;
}

DataMessage* ProcessMemory::waitForMsgQ(uint16 procID, uint32 timeout) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return NULL;
	CHECKPROCESSMEMORYSERIAL
	DataMessage* msg = waitForQ(procID, MSGQ_TYPE, timeout);
	mutex->leave();
	return msg;
}

DataMessage* ProcessMemory::waitForSigQ(uint16 procID, uint32 timeout) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return NULL;
	CHECKPROCESSMEMORYSERIAL
	DataMessage* msg = waitForQ(procID, SIGQ_TYPE, timeout);
	mutex->leave();
	return msg;
}

DataMessage* ProcessMemory::waitForReqQ(uint16 procID, uint32 timeout) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return NULL;
	CHECKPROCESSMEMORYSERIAL
	DataMessage* msg = waitForQ(procID, REQQ_TYPE, timeout);
	mutex->leave();
	return msg;
}

uint32 ProcessMemory::getCmdQCount(uint16 procID) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return 0;
	CHECKPROCESSMEMORYSERIAL

	MessageQueueHeader* qHeader = getQHeader(procID, CMDQ_TYPE);
	if (!qHeader) {
		mutex->leave();
		return 0;
	}
	uint32 count = qHeader->count;
	mutex->leave();
	return count;
}

uint32 ProcessMemory::getMsgQCount(uint16 procID) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return 0;
	CHECKPROCESSMEMORYSERIAL

	MessageQueueHeader* qHeader = getQHeader(procID, MSGQ_TYPE);
	if (!qHeader) {
		mutex->leave();
		return 0;
	}
	uint32 count = qHeader->count;
	mutex->leave();
	return count;
}

uint32 ProcessMemory::getSigQCount(uint16 procID) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return 0;
	CHECKPROCESSMEMORYSERIAL

	MessageQueueHeader* qHeader = getQHeader(procID, SIGQ_TYPE);
	if (!qHeader) {
		mutex->leave();
		return 0;
	}
	uint32 count = qHeader->count;
	mutex->leave();
	return count;
}

uint32 ProcessMemory::getReqQCount(uint16 procID) {
	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return 0;
	CHECKPROCESSMEMORYSERIAL

	MessageQueueHeader* qHeader = getQHeader(procID, REQQ_TYPE);
	if (!qHeader) {
		mutex->leave();
		return 0;
	}
	uint32 count = qHeader->count;
	mutex->leave();
	return count;
}

MessageQueueHeader* ProcessMemory::getQHeader(uint16 procID, uint8 qType) {
	if (!header->processes[procID].createdTime)
		return NULL;
	uint16 qID;
	switch(qType) {
		case CMDQ_TYPE:
			qID = header->processes[procID].cmdQID;
			break;
		case MSGQ_TYPE:
			qID = header->processes[procID].msgQID;
			break;
		case SIGQ_TYPE:
			qID = header->processes[procID].sigQID;
			break;
		case REQQ_TYPE:
			qID = header->processes[procID].reqQID;
			break;
		default:
			return NULL;
	}
	if (!header->qIndex[qID])
		return NULL;

	MessageQueueHeader* qHeader = (MessageQueueHeader*) (((char*)header) + header->qIndex[qID]);
	if (qHeader->id != qID)
		return NULL;
	return qHeader;
}



bool ProcessMemory::setupNextAvailableQ(uint16& qID) {
	qID = 0;
	uint16 nextID = 0;
	bool found = false;
	for (uint32 n=0; n<MAXPROC4; n++) {
		if (!found && !header->qIndex[n]) {
			qID = n;
			found = true;
		}
		else if (found && header->qIndex[n]) {
			nextID = n;
			break;
		}
	}
	if (!found)
		return false;

	// Insert a new standard size queue in place
	uint64 size = INITIALQSIZE;
	// Do we have enough space free at the end?
	if (header->size - header->usage < size) {
		LogPrint(0,LOG_MEMORY,0,"SetupQ need more memory size: %u use: %u need: %u", header->size, header->usage, size);
		// Resize whole memory block
		if (!resize(memorySize * 2)) {
			LogPrint(0,LOG_MEMORY,0,"Cannot resize ProcessMemory");
			return false;
		}
	}

	char* data;
	// If any active queues follow the new queue
	if (nextID) {
		// Push every subsequent queue by size
		header->qIndex[qID] = header->qIndex[nextID];
		data = ((char*)header) + header->qIndex[nextID];
		uint64 sizeMove = header->usage - header->qIndex[nextID];
		memmove(data+size, data, (size_t)sizeMove);
		// Adjust indexes
		for (uint32 n=nextID; n<MAXPROC4; n++) {
			if (header->qIndex[n])
				header->qIndex[n] += size;
		}
	}
	else {
		header->qIndex[qID] = header->usage;
		data = ((char*)header) + header->usage;
	}

	// We now have space, a start point and the indexes filled in
	memset(data, 0, (size_t)size); // could just be the header
	((MessageQueueHeader*)data)->size = size;
	((MessageQueueHeader*)data)->id = qID;
	snprintf(((MessageQueueHeader*)data)->name, MAXKEYNAMELEN, "PsycloneProcessMemoryQueue_%u_%u", port, qID);
//	snprintf(((MessageQueueHeader*)data)->name, MAXKEYNAMELEN, "%u_%u_PsycloneProcessMemoryQueue", port, qID);

	header->usage += size;
	return true;
}

bool ProcessMemory::deleteQ(uint16 id) {
	if (!header->qIndex[id])
		return false;
	MessageQueueHeader* qHeader = (MessageQueueHeader*)(((char*)header) + header->qIndex[id]);
	uint64 size = qHeader->size;

	// Check if any data comes after this queue
	uint16 nextID = 0;
	if (id < MAXPROC4-2) {
		for (uint32 n=id+1; n<MAXPROC4; n++) {
			if (header->qIndex[n]) {
				if (!nextID) {
					nextID = n;
					// move memory
					memmove(qHeader, ((char*)qHeader)+size, (size_t)(header->usage - header->qIndex[nextID]));
				}
				header->qIndex[n] -= size;
			}
		}
	}
	
	header->qIndex[id] = 0;
	header->usage -= size;
	return true;
}


bool ProcessMemory::addToQ(uint16 procID, uint8 qType, DataMessage* msg) {
	MessageQueueHeader* qHeader = getQHeader(procID, qType);
	if (!qHeader)
		return false;

	uint32 msgSize = msg->getSize();
	uint64 spaceToEnd, inUse = 0, inUse2 = 0;
	char* qData = ((char*)qHeader) + sizeof(MessageQueueHeader);
	uint64 qDataSize = qHeader->size - sizeof(MessageQueueHeader);

	//if (qHeader->endPos >= qHeader->startPos)
	//	inUse = qHeader->endPos - qHeader->startPos;
	//else
	//	inUse = qDataSize - qHeader->startPos - qHeader->padding + qHeader->endPos;

	if (qHeader->endPos == qHeader->startPos) {
		spaceToEnd = qDataSize;
		if (spaceToEnd >= msgSize) {
			// add msg at buffer start
			memcpy(qData, msg->data, msgSize);
			qHeader->startPos = 0;
			qHeader->endPos = msgSize;
			qHeader->count++;
			qHeader->padding = 0;
		}
		else {
			// we need to resize queue
			if (!resizeQ(qHeader->id, qHeader->size*2))
				return false;
			// call this function again...
			return addToQ(procID, qType, msg);
		}
	}
	else if (qHeader->endPos > qHeader->startPos) {
		spaceToEnd = qDataSize - qHeader->endPos;
		if (spaceToEnd >= msgSize) {
			// add msg here
			memcpy(qData+qHeader->endPos, msg->data, msgSize);
			qHeader->endPos += msgSize;
			qHeader->count++;
			qHeader->padding = 0;
		}
		else if (qHeader->startPos > msgSize) {
			// fill end rest of space with 0
			memset(qData+qHeader->endPos, 0, (size_t)spaceToEnd);
			qHeader->padding = spaceToEnd;
			// add msg at buffer start
			memcpy(qData, msg->data, msgSize);
			qHeader->endPos = msgSize;
			qHeader->count++;
		}
		else {
			// we need to resize queue
			if (!resizeQ(qHeader->id, qHeader->size*2))
				return false;
			// call this function again...
			return addToQ(procID, qType, msg);
		}
	}
	else {
		spaceToEnd = qHeader->startPos - qHeader->endPos;
		if (spaceToEnd > msgSize) {
			// add msg here
			memcpy(qData+qHeader->endPos, msg->data, msgSize);
			qHeader->endPos += msgSize;
			qHeader->count++;
		}
		else {
			// we need to resize queue
			if (!resizeQ(qHeader->id, qHeader->size*2))
				return false;
			// call this function again...
			return addToQ(procID, qType, msg);
		}
	}

	//if (qHeader->endPos >= qHeader->startPos)
	//	inUse2 = qHeader->endPos - qHeader->startPos;
	//else
	//	inUse2 = qDataSize - qHeader->startPos - qHeader->padding + qHeader->endPos;
	//if (inUse2 < (qHeader->count * msgSize))
	//	int ee = 0;

	utils::Semaphore** sem = &qSemaphores[qHeader->id];
	if (!*sem)
		if (!(*sem = utils::GetSemaphore(qHeader->name)))
			return false;
	(*sem)->signal();

//	utils::SignalSemaphore(qHeader->name);
//	printf("Signal %s at %s\n", qHeader->name, PrintTimeSortableMicrosecString(GetTimeNow(), true).c_str());
	return true;
}

DataMessage* ProcessMemory::waitForQ(uint16 procID, uint8 qType, uint32 timeout) {
	//char* name = NULL;
	MessageQueueHeader* qHeader = getQHeader(procID, qType);
	if (!qHeader)
		return NULL;

	uint64 start = GetTimeNow();
	int32 remain;
	while (!qHeader->count) {
		utils::Semaphore** sem = &qSemaphores[qHeader->id];
		if (!*sem) {
			if (!(*sem = utils::GetSemaphore(qHeader->name))) {
				return NULL;
			}
		}

		//if (!name) {
		//	name = new char[strlen(qHeader->name)+1];
		//	utils::strcpyavail(name, qHeader->name, (uint32)strlen(qHeader->name)+1, true);
		//}
		if ((remain = timeout - GetTimeAgeMS(start)) <= 0) {
		//	printf("StartWait1 %s at %s\n", qHeader->name, PrintTimeSortableMicrosecString(start, true).c_str());
		//	printf("Timeout1 %s at %s\n", qHeader->name, PrintTimeSortableMicrosecString(GetTimeNow(), true).c_str());
		//	delete [] name;
			return NULL;
		}
		mutex->leave();
		bool result = (*sem)->wait(remain);
		if (!mutex || !mutex->enter(5000, __FUNCTION__))
			return NULL;
		CHECKPROCESSMEMORYSERIAL

		if (!result) {
		//	printf("StartWait2 %s at %s\n", qHeader->name, PrintTimeSortableMicrosecString(start, true).c_str());
		//	printf("Timeout2 %s at %s\n", qHeader->name, PrintTimeSortableMicrosecString(GetTimeNow(), true).c_str());
		//	delete [] name;
			// printf("Waited %.3fms (%ums)...\n", GetTimeAge(start)/1000.0, remain);
			return NULL;
		}
		
		// header might be changed
		qHeader = getQHeader(procID, qType);
		if (!qHeader) {
		//	mutex->leave();
		//	delete [] name;
			return NULL;
		}
	}
	//delete [] name;

	DataMessage* msg = NULL;
	if (qHeader->count) {
		msg = new DataMessage(((char*)qHeader) + sizeof(MessageQueueHeader) + qHeader->startPos, true);
		qHeader->startPos += msg->getSize();
		if (qHeader->startPos >= qHeader->size - sizeof(MessageQueueHeader) - qHeader->padding) {
			qHeader->startPos = 0;
			qHeader->padding = 0;
		}
		qHeader->count--;
//if (qHeader->count && (GetObjID(((char*)qHeader) + sizeof(MessageQueueHeader) + qHeader->startPos) != DATAMESSAGEID)) {
//	printQ(qHeader);
//	return false;
//}
	}

	return msg;
}

bool ProcessMemory::printQ(MessageQueueHeader* qHeader) {
	printf("[%u] Data Size: %llu, count: %u startpos: %llu, endpos: %llu, padding: %llu\n",
		qHeader->id, (uint64)(qHeader->size - sizeof(MessageQueueHeader)), qHeader->count, qHeader->startPos, qHeader->endPos, qHeader->padding);
	char* data = ((char*)qHeader) + sizeof(MessageQueueHeader) + qHeader->startPos;
	for (uint32 n=0; n<qHeader->count; n++) {
		if (GetObjID(data) == DATAMESSAGEID)
			printf(" [%u: %llu - %llu] Msg size: %u Time: %s Type: %u.%u.%u\n", n,
				(uint64)(data - ((char*)qHeader) - sizeof(MessageQueueHeader)),
				(uint64)(data - ((char*)qHeader) - sizeof(MessageQueueHeader) + ((DataMessageHeader*)data)->size),
				((DataMessageHeader*)data)->size, PrintTimeString(((DataMessageHeader*)data)->time).c_str(),
				(((DataMessageHeader*)data)->type).levels[0],
				(((DataMessageHeader*)data)->type).levels[1],
				(((DataMessageHeader*)data)->type).levels[2]);
		else {
			printf(" [%u: %llu] Msg corrupted\n",
				n, (uint64)(data - ((char*)qHeader) - sizeof(MessageQueueHeader)));
			return false;
		}
		data += ((DataMessageHeader*)data)->size;
		if (data == ((char*)qHeader) + qHeader->size - qHeader->padding)
			data = ((char*)qHeader) + sizeof(MessageQueueHeader);
	}
	return true;
}

bool ProcessMemory::resize(uint64 newMemorySize) {

	if (newMemorySize > 100000000) {
		//LogPrint(0,LOG_MEMORY,0,"Memory Error: Process Memory requested size %s denied...", utils::BytifySize((double)newMemorySize).c_str());
		printf("Memory Error: Process Memory requested size %s denied...\n", utils::BytifySize((double)newMemorySize).c_str());
		fflush(stdout);
		return false;
	}

	serial = master->incrementProcessShmemSerial();
	//LogPrint(0,LOG_MEMORY,0," --------- Resizing ProcessMemory section %u -> %u serial %u --------", memorySize, newMemorySize, serial);

	if (!mutex || !mutex->enter(5000, __FUNCTION__))
		return false;

	//ProcessMemoryStruct* newHeader = (ProcessMemoryStruct*) utils::OpenSharedMemorySegment(utils::StringFormat("PsycloneProcessMemory_%u_%u", port, serial).c_str(), newMemorySize);
	//if (newHeader) {
	//	utils::CloseSharedMemorySegment((char*)newHeader, newMemorySize);
	//	//LogPrint(0,LOG_MEMORY,2,"ProcessMemory removing stale shared memory (%u/%u)...", port, serial);
	//}

	ProcessMemoryStruct* newHeader = (ProcessMemoryStruct*) utils::CreateSharedMemorySegment(utils::StringFormat("PsycloneProcessMemory_%u_%u", port, serial).c_str(), newMemorySize, true);
	if (!newHeader) {
		mutex->leave();
		return false;
	}
	memcpy(newHeader, header, (size_t)header->size);
	newHeader->size = newMemorySize;

	utils::CloseSharedMemorySegment((char*)header, memorySize);
	header = newHeader;
	memorySize = newMemorySize;
	master->setProcessShmemSize(memorySize);
	mutex->leave();

	LogPrint(0,LOG_MEMORY,2,"Process Memory resized to %s...", utils::BytifySize((double)newMemorySize).c_str());
	if (newMemorySize > 20000000) {
		LogPrint(0,LOG_MEMORY,2,"Memory Warning: Process Memory now %s...", utils::BytifySize((double)newMemorySize).c_str());
	}

	return true;
}

bool ProcessMemory::resizeQ(uint16 id, uint64 newSize) {
	if (!header->qIndex[id])
		return false;
	MessageQueueHeader* qHeader = (MessageQueueHeader*) (((char*)header) + header->qIndex[id]);
	if (qHeader->id != id)
		return false;

	uint64 difSize = newSize - qHeader->size;
	if (header->size - header->usage < difSize) {
		if (!resize(memorySize * 2))
			return false;
		return resizeQ(id, newSize);
	}

	uint16 nextID = 0;
	uint32 n;
	for (n=id+1; n<MAXPROC4; n++) {
		if (header->qIndex[n]) {
			nextID = n;
			break;
		}
	}

	// shift qID > id down by difsize
	if (nextID) {
		// Push every subsequent queue by size
		char* data = ((char*)header) + header->qIndex[nextID];
		uint64 sizeMove = header->usage - header->qIndex[nextID];
		memmove(data+difSize, data, (size_t)sizeMove);
		// Adjust indexes
		for (n=nextID; n<MAXPROC4; n++) {
			if (header->qIndex[n])
				header->qIndex[n] += difSize;
		}
	}

	// Now rearrange messages in queue
	int64 size = qHeader->endPos - qHeader->startPos;
	uint64 firstSize = 0;
	uint64 secondSize = 0;
	if (size == 0)
		qHeader->endPos = qHeader->startPos = 0;
	else if ((size > 0) && qHeader->startPos) {
		memmove(
			((char*)qHeader) + sizeof(MessageQueueHeader),
			((char*)qHeader) + sizeof(MessageQueueHeader) + qHeader->startPos,
			(size_t)size);
		qHeader->endPos = (uint64)size;
		qHeader->startPos = 0;
	}
	else if (size < 0) {
		firstSize = qHeader->size - sizeof(MessageQueueHeader) - qHeader->startPos - qHeader->padding;
		secondSize = qHeader->endPos;

		// is there enough space to move things around without a safety copy involving a malloc?
		if (difSize > secondSize) {
			// move first part of memory to the newly available space at the end
			memmove(
				((char*)qHeader) + qHeader->size,
				((char*)qHeader) + sizeof(MessageQueueHeader) + qHeader->startPos,
				(size_t)firstSize);
			// move last part of memory to the right place
			memmove(
				((char*)qHeader) + sizeof(MessageQueueHeader) + firstSize,
				((char*)qHeader) + sizeof(MessageQueueHeader),
				(size_t)secondSize);
			// copy first part back
			memmove(
				((char*)qHeader) + sizeof(MessageQueueHeader),
				((char*)qHeader) + qHeader->size,
				(size_t)firstSize);
		}
		else {
			// make safety copy of first part
			char* copy = (char*)malloc((size_t)firstSize);
			memcpy(copy, ((char*)qHeader) + sizeof(MessageQueueHeader) + qHeader->startPos, (size_t)firstSize);
			// move last part of memory to the right place
			memmove(
				((char*)qHeader) + sizeof(MessageQueueHeader) + firstSize,
				((char*)qHeader) + sizeof(MessageQueueHeader),
				(size_t)secondSize);
			// copy first part back from the safety copy
			memcpy(((char*)qHeader) + sizeof(MessageQueueHeader), copy, (size_t)firstSize);
			free(copy);
		}
		qHeader->endPos = firstSize + secondSize;
		qHeader->startPos = 0;
	}

	qHeader->size = newSize;
	qHeader->padding = 0;
	header->usage += difSize;

//if (qHeader->count) {
//	printQ(qHeader);
//	return false;
//}

	return true;
}


bool ProcessMemory::UnitTest() {
	printf("Testing Process Queues...\n\n");

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

	uint32 count = 10000;
	uint32 writeCount = 0;
	uint32 readCount = 0;
	uint32 n;
	DataMessage* msg;
	char str[128];
	const char* cstr;

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

	uint64 time = GetTimeNow();


	if ( (msg = manager->processMemory->waitForSigQ(0, 1000)) != NULL) {
		fprintf(stderr, "Got unexpected Message from empty Queue...\n");
		return false;
	}

	uint64 t = 0;
	if ( (t = GetTimeAge(time)) < 950000) {
		fprintf(stderr, "Message Queue wait 1000ms didn't actually wait more than %.3fms...\n", (t/1000.0));
		return false;
	}


	for (n=0; n<100000; n++) {
		msg = new DataMessage(CTRL_TEST, 0);
		if (!manager->processMemory->addToSigQ(0, msg)) {
			fprintf(stderr, "Could not add Message %u to Queue...\n", n);
			return false;
		}
		delete(msg);
		if ( (msg = manager->processMemory->waitForSigQ(0, 1000)) == NULL) {
			fprintf(stderr, "Could not get Message [%u] from Queue...\n", n);
			return false;
		}
		t += GetTimeAge(msg->getCreatedTime());
		delete(msg);
		if (n && n % 10000 == 0) {
			printf("Single threaded message delay[%u]: %.3fus...\n", n, ((double)t)/(2*10000.0));
			t = 0;
		}
	}

	// warm up the queue
	msg = new DataMessage(CTRL_TEST, 0);
	if (!manager->processMemory->addToCmdQ(0, msg)) {
		fprintf(stderr, "Could not add startup Message to Queue...\n");
		return false;
	}
	delete(msg);
	utils::Sleep(10);
	if ( (msg = manager->processMemory->waitForSigQ(0, 1000)) == NULL) {
		fprintf(stderr, "Could not get startup Message from Queue...\n");
		return false;
	}
	delete(msg);

	t = 0;
	for (n=0; n<100000; n++) {
		msg = new DataMessage(CTRL_TEST, 0);
		if (!manager->processMemory->addToCmdQ(0, msg)) {
			fprintf(stderr, "Could not add Message %u to Queue...\n", n);
			return false;
		}
		delete(msg);
		if ( (msg = manager->processMemory->waitForSigQ(0, 1000)) == NULL) {
			fprintf(stderr, "Could not get Message [%u] from Queue...\n", n);
			return false;
		}
		t += GetTimeAge(msg->getCreatedTime());
		delete(msg);
		if (n && n % 10000 == 0) {
			printf("Multi threaded message delay[%u]: %.3fus...\n", n, ((double)t)/(2*10000.0));
			t = 0;
		}
	}
	
	utils::Sleep(1000);






	time = GetTimeNow();
	for (n=0; n<count/4; n++) {
		msg = new DataMessage(CTRL_TEST, writeCount);
		snprintf(str, 128, "Test%2u", writeCount);
		msg->setString("TestEntry", str);
		msg->setTime("TestTime", time);
		msg->setString("TestEntry2", str);
		msg->setTime("TestTime2", time);
		if (!manager->processMemory->addToMsgQ(0, msg)) {
			fprintf(stderr, "Could not add Message %u to Queue...\n", n);
			return false;
		}
	//	if (n == 1000)
	//		utils::PrintBinary(msg->data, msg->data->size, false, "Msg1000");
		delete(msg);
		writeCount++;
	}
	
	if ( (n = manager->processMemory->getMsgQCount(0)) != 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 = manager->processMemory->waitForMsgQ(0, 1000)) == NULL) {
			fprintf(stderr, "Could not get Message %u from Queue...\n", n);
			return false;
		}
		snprintf(str, 128, "Test%2u", 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;
		}
		cstr = msg->getString("TestEntry2");
		if (!cstr || strcmp(str, cstr) != 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 = manager->processMemory->getMsgQCount(0)) != 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, writeCount);
		snprintf(str, 128, "Test%2u", writeCount);
		msg->setString("TestEntry", str);
		msg->setTime("TestTime", time);
		msg->setString("TestEntry2", str);
		msg->setTime("TestTime2", time);
		if (!manager->processMemory->addToMsgQ(0, msg)) {
			fprintf(stderr, "Could not add Message %u to Queue...\n", n);
			return false;
		}
		delete(msg);
		writeCount++;
	}

	if ( (n = manager->processMemory->getMsgQCount(0)) != 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 = manager->processMemory->waitForMsgQ(0, 1000)) == NULL) {
			fprintf(stderr, "Could not get Message %u from Queue...\n", n);
			return false;
		}
		snprintf(str, 128, "Test%2u", 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", manager->processMemory->getMsgQCount(0));
			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;
		}
		cstr = msg->getString("TestEntry2");
		if (!cstr || strcmp(str, cstr) != 0) {
			fprintf(stderr, "Message %u from Queue corrupted string '%s'...\n", readCount, msg->getString("TestEntry2"));
			utils::PrintBinary(msg->data, msg->data->size, false, "Msg3541");
			DataMessageHeader* m = (DataMessageHeader*) (((char*)msg->data) + msg->data->size - 50);
			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 = manager->processMemory->getMsgQCount(0)) != 0) {
		fprintf(stderr, "Queue contains %u messages instead of 0...\n", 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 ProcessMemory::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) {

	MemoryManager* manager = (MemoryManager*) arg;

	uint64 t = 0;
	uint32 c = 0;
	DataMessage* msg;

	while (true) {
		
		if (msg = manager->processMemory->waitForCmdQ(0, 1000)) {
			c++;
			if (!manager->processMemory->addToSigQ(0, msg)) {
				fprintf(stderr, "Test Slave could not add Message %u to Queue...\n", c);
				delete(msg);
				thread_ret_val(0);
			}
			delete(msg);
		}
	}

	//uint64 t1, t2, t3;
	//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);
}

THREAD_RET THREAD_FUNCTION_CALL ProcessMemoryPerfTest(THREAD_ARG arg) {

	MemoryManager* manager = (MemoryManager*) arg;

	uint64 t = 0;
	uint32 c = 0;
	DataMessage* msg;

	while (true) {
		if (msg = manager->processMemory->waitForCmdQ(0, 1000)) {
			c++;
			if (!manager->processMemory->addToMsgQ(0, msg)) {
				fprintf(stderr, "Test Slave could not add Message %u to Queue...\n", c);
				thread_ret_val(-1);
			}
			delete(msg);
		}
	}
	thread_ret_val(0);
}

bool ProcessMemory::PerfTest() {
	printf("Performance Testing Process Queues...\n\n");

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

	uint32 count = 10000;
	uint32 writeCount = 0;
	uint32 readCount = 0;
	uint32 n;
	DataMessage* msg;

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

	uint64 time = GetTimeNow();

	uint64 t = 0;
	for (n=0; n<100000; n++) {
		msg = new DataMessage(CTRL_TEST, 0);
		if (!manager->processMemory->addToCmdQ(0, msg)) {
			fprintf(stderr, "Could not add Message %u to Queue...\n", n);
			return false;
		}
		delete(msg);
		if ( (msg = manager->processMemory->waitForMsgQ(0, 10000)) == NULL) {
			fprintf(stderr, "Could not get Message [%u] from Queue...\n", n);
			return false;
		}
		t += GetTimeAge(msg->getCreatedTime());
		delete(msg);
		if (n && n % 10000 == 0) {
			printf("Dual threaded message delay[%u]: %.3fus...\n", n, ((double)t)/(2*10000.0));
			t = 0;
		}
	}
	return true;
}
} // namespace cmlabs













