

//_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/_/
//_/_/
//_/_/  This file is part of PSYCLONE version 2.1.1.260726 (26 July 2026)
//_/_/  
//_/_/  developed by
//_/_/  
//_/_/  Communicative Machines Ltd. 
//_/_/    http://cmlabs.com/
//_/_/  Copyright (c) 2026
//_/_/  
//_/_/  in collaboration with 
//_/_/  
//_/_/  Icelandic Institute for Intelligent Machines ses
//_/_/    Menntavegur 1
//_/_/    101 Reykjavik
//_/_/    ICELAND
//_/_/    http://www.iiim.is/
//_/_/
//_/_/  PSYCLONE 2.1 is released under the GNU Lesser General Public License 
//_/_/  (LGPL) version 3.0, and is subject to the following
//_/_/  
//_/_/  - 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. 
//_/_/    
//_/_/  A commercial license for PSYCLONE 2.1 is also available to 
//_/_/  any interested party from the copyright holder.
//_/_/  
//_/_/  You can redistribute this copy of PSYCLONE 2.1 and/or modify it under 
//_/_/  the terms of the GNU General Public License as published by the Free 
//_/_/  Software Foundation, either version 3 of the License, or (at your 
//_/_/  option) any later version.
//_/_/  
//_/_/  You should have received a copy of the GNU General Public License
//_/_/  and the GNU Lesser General Public License along with PSYCLONE 2.1.  
//_/_/  If not, see <http://www.gnu.org/licenses/>.
//_/_/  
//_/_/  PSYCLONE 2.1 is distributed in the hope that it will be useful,
//_/_/  but WITHOUT ANY WARRANTY; without even the implied warranty of
//_/_/  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//_/_/  GNU General Public License for more details.
//_/_/
//_/_/  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 "Networking.h"
#include "InterSystemManager.h"

namespace	cmlabs{

const uint8 Networking::NetworkProtocols[] = { PROTOCOL_MESSAGE, PROTOCOL_TELNET, PROTOCOL_HTTP_SERVER, 0 };

struct PsyType Networking::CTRL_NODE_WELCOME			=	{ { 1,10100,0,0,0,0,0,0,0,0,0,0,0,0,0,0 } }; 
struct PsyType Networking::CTRL_NODE_JOIN				=	{ { 1,10100,1,0,0,0,0,0,0,0,0,0,0,0,0,0 } }; 

struct PsyType Networking::CTRL_NODE_TIMESYNC			=	{ { 1,10101,0,0,0,0,0,0,0,0,0,0,0,0,0,0 } }; 
struct PsyType Networking::CTRL_NODE_TIMESYNC_REPLY		=	{ { 1,10101,1,0,0,0,0,0,0,0,0,0,0,0,0,0 } }; 

struct PsyType Networking::CTRL_NODE_NODEMAP			=	{ { 1,10102,0,0,0,0,0,0,0,0,0,0,0,0,0,0 } }; // NodeMap
struct PsyType Networking::CTRL_NODE_NODEMAP_REQUEST	=	{ { 1,10102,1,0,0,0,0,0,0,0,0,0,0,0,0,0 } }; 
struct PsyType Networking::CTRL_NODE_STATUSMAP			=	{ { 1,10102,2,0,0,0,0,0,0,0,0,0,0,0,0,0 } }; // Heartbeat

struct PsyType Networking::CTRL_NODE_SYNC_REQ			=	{ { 1,10104,0,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_SYNC_REPLY			=	{ { 1,10104,1,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_CONFIG				=	{ { 1,10105,0,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_CONFIG_SUCCESS		=	{ { 1,10105,1,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_CONFIG_FAILED		=	{ { 1,10105,2,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_SYNC_ID			=	{ { 1,10106,0,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_SYNC_ID_SUCCESS	=	{ { 1,10106,1,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_SYNC_ID_FAILED		=	{ { 1,10106,2,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_CONFIRM_ID			=	{ { 1,10106,3,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_CONFIRM_ID_SUCCESS	=	{ { 1,10106,4,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_CONFIRM_ID_FAILED	=	{ { 1,10106,5,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_CANCEL_ID			=	{ { 1,10106,6,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_CANCEL_ID_SUCCESS	=	{ { 1,10106,7,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_CANCEL_ID_FAILED	=	{ { 1,10106,8,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_CONTEXT_ACTIVE		=	{ { 1,10107,0,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_SIGNAL				=	{ { 1,10108,0,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_QUERY				=	{ { 1,10109,0,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_QUERY_SUCCESS		=	{ { 1,10109,1,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_QUERY_FAILED		=	{ { 1,10109,2,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_SYNC_COMPONENT				=	{ { 1,10110,0,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_SYNC_COMPONENT_SUCCESS		=	{ { 1,10110,1,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_SYNC_COMPONENT_FAILED		=	{ { 1,10110,2,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_SYNC_SUBSCRIPTION			=	{ { 1,10110,3,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_SYNC_SUBSCRIPTION_SUCCESS	=	{ { 1,10110,4,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_SYNC_SUBSCRIPTION_FAILED	=	{ { 1,10110,5,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_PSYPROBE_CREATECUSTOMPAGE			=	{ { 1,10111,0,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_PSYPROBE_CREATECUSTOMPAGE_SUCCESS	=	{ { 1,10111,1,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_PSYPROBE_CREATECUSTOMPAGE_FAILED	=	{ { 1,10111,2,0,0,0,0,0,0,0,0,0,0,0,0,0 } };
struct PsyType Networking::CTRL_NODE_PERFSYNC			=	{ { 1,10112,0,0,0,0,0,0,0,0,0,0,0,0,0,0 } };



uint32 NodeEntry::addIPAddresses(uint32* newAddresses, uint32 count) {
	uint32 n, m, added = 0;
	for (n=0; n<count; n++) {
		if (ipAddressCount >= MAXENTRYIPADDRESSCOUNT)
			break;
		if (!newAddresses[n])
			continue;
		for (m=0; m<ipAddressCount; m++)
			if (newAddresses[n] == ipAddresses[m])
				break;
		// if not found in existing list
		if (m >= ipAddressCount) {
			ipAddresses[ipAddressCount] = newAddresses[n];
			ipAddressCount++;
			added++;
		}
	}
	return added;
}


Interface::Interface() {
	id = port = timeout = 0;
	protocol = encryption = ipproto = 0;
	serviceID = 0;
	allowSelfSigned = -1;
	auth = NULL;
}
Interface::~Interface() {
	id = port = timeout = 0;
	protocol = encryption = ipproto = 0;
	serviceID = 0;
	delete(auth);
	auth = NULL;
}

// <interface name="MyWebInterface" node="Node0" port="1234"
//  protocol="HTTP" service="MyWebService" timeout="3000" default="yes" />
bool Interface::fromXML(XMLNode &specNode) {
   if (stricmp(specNode.getName(), "interface") != 0)
	  return false;

	const char* str;
	if (str = specNode.getAttribute("name"))
		name = str;
	else
		return false;

	if (str = specNode.getAttribute("service"))
		serviceName = str;
	else
		return false;

	if (str = specNode.getAttribute("node"))
		nodeName = str;

	if (str = specNode.getAttribute("port"))
		port = (uint16)utils::Ascii2Uint32(str);
	else
		return false;

	if (str = specNode.getAttribute("timeout"))
		timeout = utils::Ascii2Uint32(str);

	if (str = specNode.getAttribute("default"))
		isDefault = (stricmp(str, "yes") == 0);

	// Per-interface SSL client verification override (wins over the global
	// <psyspec allowselfsigned> setting either way)
	if (str = specNode.getAttribute("allowselfsigned"))
		allowSelfSigned = ((stricmp(str, "yes") == 0) || (stricmp(str, "true") == 0) || (strcmp(str, "1") == 0)) ? 1 : 0;

	// Per-interface custom CA location for SSL client verification
	// (overrides the global <psyspec cafile/capath> setting)
	if (str = specNode.getAttribute("cafile"))
		caFile = str;
	if (str = specNode.getAttribute("capath"))
		caPath = str;

	if (str = specNode.getAttribute("protocol")) {
		if (stricmp(str, "Message") == 0)
			protocol = INTERFACE_MESSAGE;
		else if (stricmp(str, "HTTP") == 0)
			protocol = INTERFACE_HTTP;
		else if (stricmp(str, "Telnet") == 0)
			protocol = INTERFACE_TELNET;
		else if (stricmp(str, "RAW") == 0)
			protocol = INTERFACE_RAW;
		else
			return false;
	}
	
	ipproto = INTERFACE_PROTO_TCP;
	if (str = specNode.getAttribute("ip"))
		if (stricmp(str, "UDP") == 0)
			ipproto = INTERFACE_PROTO_UDP;

	XMLNode node;
	uint32 n, count = specNode.nChildNode();
	for(n=0; n<count; n++){
		node=specNode.getChildNode(n);
		if (stricmp("authentication", node.getName()) == 0) {
			auth = new Authentication();
			if (!auth->fromXML(node)) {
				delete(auth);
				auth = NULL;
			}
		}
	}
	return true;
}

Authentication::Authentication() {
}

Authentication::~Authentication() {
}

bool Authentication::fromXML(XMLNode &n) {
	// ##############
	return true;
}


ReplyCheck::~ReplyCheck() {
	std::map<uint32, ReplyCheckEntry*>::iterator it, itEnd;
	for (it=replyEntries.begin(), itEnd=replyEntries.end(); it != itEnd; it++)
		delete(it->second);
}

uint32 ReplyCheck::newEntry() {
	ReplyCheckEntry* entry = new ReplyCheckEntry;
	entry->id = ++lastID;
	replyEntries[entry->id] = entry;
	return entry->id;
}

bool ReplyCheck::addReplier(uint32 id, uint16 replier) {
	ReplyCheckEntry* entry = replyEntries[id];
	if (!entry)
		return false;
	if (!entry->mutex.enter())
		return false;
	entry->entries[replier] = SYNC_NONE;
	entry->mutex.leave();
	return true;
}

bool ReplyCheck::addReply(uint32 id, uint16 replier, bool answer, DataMessage* msg) {
	ReplyCheckEntry* entry = replyEntries[id];
	if (!entry)
		return false;
	if (!entry->mutex.enter())
		return false;
	entry->entries[replier] = answer ? SYNC_SUCCESS : SYNC_FAILED;
	if (msg)
		entry->messages[replier] = msg;
	entry->semaphore.signal();
	entry->mutex.leave();
	return true;
}

uint8 ReplyCheck::getReplyStatus(uint32 id, uint32& total, uint32& success, uint32& failed) {
	ReplyCheckEntry* entry = replyEntries[id];
	if (!entry)
		return SYNC_ERROR;
	if (!entry->mutex.enter())
		return SYNC_ERROR;
	uint8 result = entry->getReplyStatus(total, success, failed);
	entry->mutex.leave();
	return result;
}

uint8 ReplyCheck::waitForReplies(uint32 id, uint32 timeout, std::map<uint16, DataMessage*>* map, bool deleteOnTimeout) {
	
	ReplyCheckEntry* entry = replyEntries[id];
	if (!entry)
		return SYNC_ERROR;
	if (!entry->mutex.enter())
		return SYNC_ERROR;
	switch (entry->getStatus()) {
		case SYNC_SUCCESS:
			if (map) {
				*map = entry->messages;
				entry->messages.clear();
			}
			replyEntries.erase(id);
			entry->mutex.leave();
			delete(entry);
			return SYNC_SUCCESS;
		case SYNC_FAILED:
			if (map) {
				*map = entry->messages;
				entry->messages.clear();
			}
			replyEntries.erase(id);
			entry->mutex.leave();
			delete(entry);
			return SYNC_FAILED;
		default:
			break;
	}

	uint64 start = GetTimeNow();

	int32 left;
	while ( (left = (int32)timeout - GetTimeAgeMS(start)) > 0) {
		entry->mutex.leave();
		entry->semaphore.wait(left);

		if (!entry->mutex.enter())
			return SYNC_ERROR;

		switch (entry->getStatus()) {
			case SYNC_SUCCESS:
				if (map) {
					*map = entry->messages;
					entry->messages.clear();
				}
				replyEntries.erase(id);
				entry->mutex.leave();
				delete(entry);
				return SYNC_SUCCESS;
			case SYNC_FAILED:
				if (map) {
					*map = entry->messages;
					entry->messages.clear();
				}
				replyEntries.erase(id);
				entry->mutex.leave();
				delete(entry);
				return SYNC_FAILED;
			default:
				break;
		}
	}
	if (map) {
		*map = entry->messages;
		entry->messages.clear();
	}
	if (deleteOnTimeout) {
		replyEntries.erase(id);
		entry->mutex.leave();
		delete(entry);
	}
	else
		entry->mutex.leave();
	return SYNC_TIMEOUT;
}

ReplyCheckEntry::~ReplyCheckEntry() {
	// Delete messages
	std::map<uint16, DataMessage*>::iterator it, itEnd;
	for (it=messages.begin(), itEnd=messages.end(); it != itEnd; it++)
		if (it->second)
			delete(it->second);
}

uint8 ReplyCheckEntry::getStatus() {
	std::map<uint16, uint8>::iterator it, itEnd;
	bool didSucceed = true;
	for (it=entries.begin(), itEnd=entries.end(); it != itEnd; it++) {
		// Are we still awaiting answers?
		if (it->first && (it->second < SYNC_FAILED))
			return SYNC_NONE;
		// If answer, was it success or failed
		if (it->second < SYNC_SUCCESS)
			didSucceed = false;
	}
	return didSucceed ? SYNC_SUCCESS : SYNC_FAILED;
}

uint8 ReplyCheckEntry::getReplyStatus(uint32& total, uint32& success, uint32& failed) {
	std::map<uint16, uint8>::iterator it, itEnd;
	uint8 result = SYNC_SUCCESS;
	total = success = failed = 0;
	for (it=entries.begin(), itEnd=entries.end(); it != itEnd; it++) {
		// Are we still awaiting answers?
		if (it->first) {
			total++;
			if (it->second == SYNC_FAILED) {
				failed++;
				result = SYNC_FAILED;
			}
			else if (it->second == SYNC_SUCCESS)
				success++;
			else
				result = SYNC_NONE;
		}
	}
	return result;
}



Networking::Networking(Node* node) {
	localNodeName[0] = 0;
	pingInterval = 120000;
	conCheckInterval = 60000;
	reqNodeTimeout = 12000000;
	heartbeatInterval = 5000;
//	timesyncInterval = 300000; // 5 mins
	timesyncInterval = 60000; // 1 min
//	timesyncInterval = 300;
	timestepInterval = 50;
	timeSyncCount = 5;
	localNodeID = 0;
	altLocalPort = 0;
	timesyncNodeID = 1;
	lastInterfaceID = 0;
	serviceManager = NULL;
	this->node = node;
	manager = new NetworkManager();

	localNodeEntry = new NodeEntry;
	memset(localNodeEntry, 0, sizeof(NodeEntry));
	localNodeEntry->instanceID = localNodeEntry->lastContact = GetTimeNow();
	localNodeEntry->status = NODESTATUS_INIT;

}

Networking::~Networking() {
	stop();
	serviceManager = NULL;
	this->node = NULL;
	delete(manager);
	manager = NULL;
	nodeEntriesMutex.enter();
	delete(localNodeEntry);
	localNodeEntry = NULL;
	nodeEntriesMutex.leave();
}

bool Networking::init(uint16 port, ServiceManager* serviceManager) {
	CHECKNETWORKINIT

	localNodeEntry->port = port;
	this->serviceManager = serviceManager;

	uint32* temp = utils::GetLocalIPAddresses(localNodeEntry->ipAddressCount);
	memcpy(localNodeEntry->ipAddresses, temp, localNodeEntry->ipAddressCount*sizeof(uint32));
	delete [] temp;
	utils::GetLocalHostname(localNodeEntry->hostname, 255);

	if (!addInterface("Main", localNodeEntry->port, INTERFACE_PROTO_TCP, NOENC, INTERFACE_MESSAGE, 5000, false, NULL, NULL))
		return false;
	if (!addInterface("Main", localNodeEntry->port, INTERFACE_PROTO_UDP, NOENC, INTERFACE_MESSAGE, 5000, true, NULL, NULL))
		return false;

	setLocalNodeStatus(NODESTATUS_IDLE);

	if (!ThreadManager::CreateThread(NodeNetworkingRun, this, threadID, 0)) {
		LogPrint(localNodeID, LOG_SYSTEM, 0, "Could not start Node Networking thread...");
		return false;
	}

	return true;
}

bool Networking::shutdown() {
	stop();

	std::map<uint16, InterfacePack>::iterator pit, pitEnd;
	for (pit = portInterfacePacks.begin(), pitEnd = portInterfacePacks.end(); pit != pitEnd; pit++) {
		delete(pit->second.interfaces[0]);
		delete(pit->second.interfaces[1]);
		delete(pit->second.interfaces[2]);
		delete(pit->second.interfaces[3]);
	}
	portInterfacePacks.clear();


	std::map<uint16, MovingAverage*>::iterator asit, asitEnd;
	for (asit = nodeOutStats.begin(), asitEnd = nodeOutStats.end(); asit != asitEnd; asit++)
		if (asit->second)
			delete(asit->second);
	nodeOutStats.clear();
	for (asit = nodeInStats.begin(), asitEnd = nodeInStats.end(); asit != asitEnd; asit++)
		if (asit->second)
			delete(asit->second);
	nodeInStats.clear();

	// Empty queues...
	DataMessage* msg;
	while (inputMessageQueue.size()) {
		msg = inputMessageQueue.front();
		inputMessageQueue.pop();
		delete(msg);
	}
	while (outputMessageQueue.size()) {
		msg = outputMessageQueue.front();
		outputMessageQueue.pop();
		delete(msg);
	}


	nodeEntriesMutex.enter();
	std::map<uint16, NodeEntry*>::iterator entryIt, entryItEnd;
	for (entryIt = nodeEntries.begin(), entryItEnd = nodeEntries.end(); entryIt != entryItEnd; entryIt++)
		if (entryIt->second && (localNodeEntry != entryIt->second))
			delete(entryIt->second);
	nodeEntries.clear();
	nodeEntriesMutex.leave();

	nodeMapsMutex.enter();
	std::map<uint16, NodeMap*>::iterator mapIt, mapItEnd;
	for (mapIt = nodeMaps.begin(), mapItEnd = nodeMaps.end(); mapIt != mapItEnd; mapIt++)
		if (mapIt->second)
			delete(mapIt->second);
	nodeMaps.clear();
	nodeMapsMutex.leave();

	return true;
}

bool Networking::addInterface(XMLNode &n) {
	Interface* iface = new Interface();
	if (!iface->fromXML(n)) {
		delete(iface);
		return false;
	}
	if (!addInterface(iface)) {
		delete(iface);
		return false;
	}
	return true;
}

bool Networking::addInterface(const char* name, uint16 port, uint8 ipproto, uint8 encryption, uint8 protocol, uint32 protocolTimeout, bool isDefaultProtocol, const char* nodeName, const char* serviceName) {
	Interface* iface = new Interface();
	iface->id = lastInterfaceID++;
	iface->port = port;
	iface->protocol = protocol;
	iface->encryption = encryption;
	iface->timeout = protocolTimeout;
	iface->isDefault = isDefaultProtocol;
	iface->ipproto = ipproto;
	iface->name = name;
	if (serviceName)
		iface->serviceName = serviceName;
	if (nodeName)
		iface->nodeName = nodeName;
	if (!addInterface(iface)) {
		delete(iface);
		return false;
	}
	return true;

}

bool Networking::addInterface(Interface* iface) {
	if (!iface)
		return false;

	if (!iface->serviceName.size())
		iface->serviceID = 0;
	else if (!serviceManager->getServiceID(iface->serviceName.c_str(), iface->serviceID))
		return false;

	NetworkChannel* channel = NULL;
	NetworkChannel* existingChannel = NULL;
	// Check if this overwrites an existing interface
	std::map<NetworkChannel*, uint16>::iterator it, itEnd;
	for (it = channelPorts.begin(), itEnd = channelPorts.end(); it != itEnd; it++) {
		if (it->second == iface->port) {
			existingChannel = it->first;
			break;
		}
	}
	
	uint64 conid = 0;
	if (iface->ipproto == INTERFACE_PROTO_TCP)
		channel = manager->createListener(iface->port, iface->encryption, NetworkProtocols[iface->protocol], true, iface->timeout, iface->isDefault, 0, this);
	else if (iface->ipproto == INTERFACE_PROTO_UDP)
		channel = manager->createUDPConnection(iface->port, NetworkProtocols[iface->protocol], true, true, 0, this, conid);

	if (!channel) {
		return false;
	}

	// Per-interface SSL client verification policy (overrides global default)
	if (iface->allowSelfSigned >= 0)
		channel->setSSLAllowSelfSigned(iface->allowSelfSigned != 0);
	if (!iface->caFile.empty() || !iface->caPath.empty())
		channel->setSSLCALocation(iface->caFile.c_str(), iface->caPath.c_str());

	if (existingChannel) {
		if (portInterfacePacks[iface->port].interfaces[iface->protocol] &&
			(portInterfacePacks[iface->port].interfaces[iface->protocol] != iface))
				delete(portInterfacePacks[iface->port].interfaces[iface->protocol]);
	}
	else
		channelPorts[channel] = iface->port;
	portInterfacePacks[iface->port].interfaces[iface->protocol] = iface;
	return true;
}

std::string Networking::getNodeName(uint16 id) {
	nodeEntriesMutex.enter();
	std::map<uint16, NodeEntry*>::iterator it = nodeEntries.find(id);
	if (it == nodeEntries.end())
		return "";
	std::string name = it->second->name;
	nodeEntriesMutex.leave();
	return name;
}

bool Networking::getNodeID(const char* name, uint16& id) {
	std::map<uint16, NodeEntry*>::iterator it, itEnd;
	nodeEntriesMutex.enter();
	for (it = nodeEntries.begin(), itEnd = nodeEntries.end(); it != itEnd; it++) {
		//if (it->second && it->second->name && stricmp(it->second->name, name) == 0) {
		if (it->second && it->second->name[0] != '\0' && stricmp(it->second->name, name) == 0) {
			id = it->first;
			nodeEntriesMutex.leave();
			return true;
		}
	}
	nodeEntriesMutex.leave();
	return false;
}

uint16 Networking::getLocalNodeStatus() {
	uint16 status;
	nodeEntriesMutex.enter();
	status = localNodeEntry->status;
	nodeEntriesMutex.leave();
	return status;
}

uint16 Networking::getNodeIssues(std::list<NodeEntry> &nodeIssues) {
	uint16 minStatus = NODESTATUS_READY;
	std::map<uint16, NodeEntry*>::iterator it, itEnd;
	nodeEntriesMutex.enter();
	for (it = nodeEntries.begin(), itEnd = nodeEntries.end(); it != itEnd; it++) {
		if (it->second) {
			if (it->second->status < NODESTATUS_READY && it->second->constatus == CONNECTION_READY)
				nodeIssues.push_back(*it->second);
			if (it->second->status < minStatus)
				minStatus = it->second->status;
		}
	}
	nodeEntriesMutex.leave();
	return minStatus;
}

bool Networking::setLocalNodeStatus(uint16 newStatus, bool onlyIfLarger) {
	nodeEntriesMutex.enter();
	if (!onlyIfLarger || (newStatus > localNodeEntry->status))
		localNodeEntry->status = newStatus;
	nodeEntriesMutex.leave();
	return true;
}

bool Networking::calcViaNodeID(uint16 id, uint16& viaID) {
	if (!id) return false;
	uint32 n;
	nodeEntriesMutex.enter();
	NodeEntry* entry = nodeEntries[id];
	if (entry && (entry->constatus >= CONNECTION_READY)) {
		viaID = id;
		nodeEntriesMutex.leave();
		return true;
	}
	nodeEntriesMutex.leave();

	nodeMapsMutex.enter();
	std::map<uint16, NodeMap*>::iterator it, itEnd;
	for (it = nodeMaps.begin(), itEnd = nodeMaps.end(); it != itEnd; it++) {
		if (it->second) {
			entry = (NodeEntry*)(((char*)it->second)+sizeof(NodeMap));
			for (n=0; n<it->second->count; n++) {
				if ((entry->conid == id) && (entry->status >= CONNECTION_READY)) {
					viaID = it->first;
					nodeMapsMutex.leave();
					return true;
				}
				entry++;
			}
		}
	}
	nodeMapsMutex.leave();
	return false;
}

bool Networking::removeInterface(uint16 port, uint8 protocol) {
	// Expensive search, but it shouldn't happen that often anyway...
	NetworkChannel* channel = NULL;
	std::map<NetworkChannel*, uint16>::iterator it, itEnd;
	for (it = channelPorts.begin(), itEnd = channelPorts.end(); it != itEnd; it++) {
		if (it->second == port)
			channel = it->first;
	}
	if (!channel)
		return false;
	channelPorts.erase(channel);
	delete(portInterfacePacks[port].interfaces[protocol]);
	portInterfacePacks[port].interfaces[protocol] = NULL;
	return manager->stopListener(port, NetworkProtocols[protocol]);
}



uint16 Networking::connectToNodeNetwork(const char* otherNodeAddress, uint16 otherNodePort) {
	// Set status to NETWORK
	localNodeEntry->type = NODE_EXTERNAL;
	setLocalNodeStatus(NODESTATUS_INIT);

	uint32 address;
	if (!utils::LookupIPAddress(otherNodeAddress, address)) {
		LogPrint(localNodeID, LOG_NETWORK, 0, "Could not find address '%s'...", otherNodeAddress);
		return 0;
	}

	uint64 location = 0;
	uint64 conid;
	NetworkChannel* channel = manager->createTCPConnection(otherNodeAddress, otherNodePort, NOENC, PROTOCOL_MESSAGE, true, true, 0, this, conid, location);
	if (!channel) {
		LogPrint(localNodeID, LOG_NETWORK, 0, "Could not connect to Node '%s' on %u.%u.%u.%u:%u...", otherNodeAddress, GETIPADDRESSQUAD(location), otherNodePort);
		return 0;
	}

	NodeEntry* entry = new NodeEntry;
	memset(entry, 0, sizeof(NodeEntry));
	entry->lastContact = GetTimeNow();
	entry->conid = conid;
	entry->status = CONNECTION_INITIALISING;
	entry->location = location;
	entry->timeout = reqNodeTimeout;

	// put it into position 0 for now until we know where to move it to
	entry->nodeID = 0;
	nodeEntries[0] = entry;

	DataMessage* msg = new DataMessage(CTRL_NODE_JOIN, 0);
	if (!channel->sendMessage(msg, entry->conid)) {
		entry->conid = 0;
		entry->status = CONNECTION_ERROR;
		return false;
	}
	return true;
}

bool Networking::waitForNodeNetwork(uint32 ms) {
	localNodeEntry->type = NODE_OPTIONAL;
	if (!waitForAllNodesStatus(0, ms))
		return false;
	node->join(localNodeID);
	LogPrint(localNodeID, LOG_NETWORK, 1, "*** Node network connected, continuing system configuration ***");
	setLocalNodeStatus(NODESTATUS_NETREADY, true);
	return true;
}

bool Networking::updateNodeNetwork(uint16 remoteNodeID, NodeMap* map, uint64 conid) {
	if (!map || !conid)
		return false;

	if (getLocalNodeStatus() < NODESTATUS_INIT)
		return false;

	nodeEntriesMutex.enter();

	if (localNodeEntry->status < NODESTATUS_MAPPING)
		localNodeEntry->status = NODESTATUS_MAPPING;

	NodeMap* newMap = (NodeMap*) malloc(map->size);
	memcpy(newMap, map, map->size);

	// Fill in nodeEntries from map
	uint32 address;
	uint32 n;

	// Point to first NodeEntry in map
	NodeEntry* entry, *mapEntry = (NodeEntry*)(((char*)newMap)+sizeof(NodeMap));
	for (n=0; n<newMap->count; n++) {
		if (!(entry = nodeEntries[mapEntry->nodeID])) {
			// is this information about our local node from someone else
			if (mapEntry->nodeID == localNodeID) {
				if (mapEntry->name[0]) {
					utils::strcpyavail(localNodeName, mapEntry->name, 255, true);
					utils::strcpyavail(localNodeEntry->name, mapEntry->name, 255, true);
				}
				if (mapEntry->addr[0])
					utils::strcpyavail(localNodeEntry->addr, mapEntry->addr, 255, true);
				localNodeEntry->type = mapEntry->type;
				localNodeEntry->timeout = mapEntry->timeout;
				localNodeEntry->lastContact = GetTimeNow();
				// localNodeEntry->instanceID++;
				nodeEntries[localNodeID] = localNodeEntry;
				mapEntry++;
				continue;
			}
			else {
				entry = new NodeEntry;
				memcpy(entry, mapEntry, sizeof(NodeEntry));
				nodeEntries[entry->nodeID] = entry;
				entry->conid = 0;
				entry->location = 0;
				entry->constatus = 0;
				entry->status = NODESTATUS_IDLE;
			}
		}

		// is this information about the node who sent us this info
		if (mapEntry->nodeID == remoteNodeID) {
			if (entry->conid != conid) {
				if (entry->conid)
					manager->endConnection(entry->conid);
				entry->conid = conid;
			}
			if (entry->ipAddressCount < mapEntry->ipAddressCount)
				entry->addIPAddresses(mapEntry->ipAddresses, mapEntry->ipAddressCount);
			if (!entry->hostname[0])
				utils::strcpyavail(entry->hostname, mapEntry->hostname, 255, true);

			entry->status = mapEntry->status;
			entry->constatus = CONNECTION_READY;
			entry->lastContact = GetTimeNow();
		}

		//if (!entry->timeout)
		//	entry->timeout = reqNodeTimeout;

		if (!entry->location)
			if (utils::LookupIPAddress(entry->addr, address))
				entry->location = GETIPADDRESSPORT(address, entry->port);

		mapEntry++;
	}

	//LogPrint(0, 0, 0, "Joining existing Node network as %u '%s'\nConnecting to %u other Node(s), %u required, %u optional, %u external",
	//	localNodeID, localNodeName, nodeEntries.size() - 1, requiredCount, optionalCount, externalCount);
	nodeEntriesMutex.leave();

	nodeMapsMutex.enter();
	delete(nodeMaps[remoteNodeID]);
	nodeMaps[remoteNodeID] = newMap;
	nodeMapsMutex.leave();

	return true;
}


bool Networking::setupNodeNetwork(uint16 mainPort, XMLNode& mainNode) {

	localNodeEntry->type = NODE_TIMENODE;
	nodeEntriesMutex.enter();
	// Set status to NETWORK
	localNodeEntry->status = NODESTATUS_INIT;

	localNodeEntry->configNodeID = localNodeEntry->nodeID = localNodeID = 1;
	utils::strcpyavail(localNodeEntry->name, "Main", 255, true);
	uint32 ip;
	uint8* ipp;
	if (!utils::GetLocalIPAddress(ip))
		utils::strcpyavail(localNodeEntry->addr, "localhost", 255, true);
	else {
		ipp = (uint8*)&ip;
		snprintf(localNodeEntry->addr, 256, "%u.%u.%u.%u", ipp[0], ipp[1], ipp[2], ipp[3]);
	}
	localNodeEntry->port = mainPort;
//	entry->portUDP = mainPort;
	localNodeEntry->type = NODE_TIMENODE;
	nodeEntries[localNodeID] = localNodeEntry;

	uint16 port, id;
	uint32 timeout;
	NodeEntry* entry = NULL;
	uint32 requiredCount = 0, optionalCount = 0, externalCount = 0;
	int i = 0;
	XMLNode node;
	const char* name, *addr, *type, *sport, *stimeout;
	while (!(node = mainNode.getChildNode("node", i++)).isEmpty()) {
		name = node.getAttribute("name");
		addr = node.getAttribute("addr");
		if (!addr)
			addr = node.getAttribute("address");
		type = node.getAttribute("type");
		sport = node.getAttribute("port");
		if (sport)
			port = (uint16)utils::Ascii2Int32(sport);
		else
			port = 0;
		stimeout = node.getAttribute("timeout");
		if (stimeout)
			timeout = (uint16)utils::Ascii2Int32(stimeout);
		else
			timeout = 0;

		// Reconfigure local node
		if (!addr || !strlen(addr) || (stricmp(name, "Main") == 0) || (utils::IsLocalIPAddress(addr) && (port == mainPort))) {
			if (name)
				utils::strcpyavail(localNodeEntry->name, name, 255, true);
			if (addr)
				utils::strcpyavail(localNodeEntry->addr, addr, 255, true);
			if (port && (port != mainPort)) {
				if (!addInterface("Main", port, INTERFACE_PROTO_TCP, NOENC, INTERFACE_MESSAGE, 5000, true, NULL, NULL)) {
					LogPrint(localNodeID, LOG_NETWORK, 0, "Could not use PsySpec-defined local port '%u'...", port);
					return false;
				}
				if (!addInterface("PsyProbe", port, INTERFACE_PROTO_TCP, NOENC, INTERFACE_HTTP, 5000, false, "", "PsyProbe")) {
					LogPrint(localNodeID, LOG_NETWORK, 0, "Could not start PsyProbe on port '%u'...", port);
					return false;
				}
				if (!addInterface("Console", port, INTERFACE_PROTO_TCP, NOENC, INTERFACE_TELNET, 5000, false, "", "Console")) {
					LogPrint(localNodeID, LOG_NETWORK, 0, "Could not start Console on port '%u'...", port);
					return false;
				}
				localNodeEntry->port = port;
				altLocalPort = port;
			}
		}
		else {
			entry = new NodeEntry;
			memset(entry, 0, sizeof(NodeEntry));
			if (!type || !strlen(type) || (stricmp(type, "required") == 0)) {
				entry->type = NODE_REQUIRED;
				requiredCount++;
			}
			else if (stricmp(type, "optional") == 0) {
				entry->type = NODE_OPTIONAL;
				optionalCount++;
			}
			else if (stricmp(type, "external") == 0) {
				entry->type = NODE_EXTERNAL;
				externalCount++;
			}

			utils::strcpyavail(entry->name, name, 255, true);

			if (entry->type != NODE_EXTERNAL) {
				if (!sport) {
					LogPrint(localNodeID, LOG_NETWORK, 0, "Couldn't add Node '%s', no network port provided...",
						name, sport);
					delete(entry);
					return false;
				}
				else if (!port) {
					LogPrint(localNodeID, LOG_NETWORK, 0, "Couldn't add Node '%s', port number '%s' invalid...",
						name, sport);
					delete(entry);
					return false;
				}

				uint32 address;
				if (!utils::LookupIPAddress(addr, address)) {
					LogPrint(localNodeID, LOG_NETWORK, 0, "Couldn't resolve Node '%s' address '%s'...", name, addr);
					delete(entry);
					return false;
				}

				utils::strcpyavail(entry->addr, addr, 255, true);
				entry->port = port;
			//	entry->portUDP = port;
				entry->location = GETIPADDRESSPORT(address, port);
			}

			if (timeout)
				entry->timeout = timeout;
			else
				entry->timeout = reqNodeTimeout;

			if (!(id = getAvailableNodeID())) {
				LogPrint(localNodeID, LOG_NETWORK, 0, "Couldn't add Node '%s', maximum number of Nodes added already...", name);
				delete(entry);
				return false;
			}
			entry->nodeID = id;
			nodeEntries[id] = entry;
		}
	}

	utils::strcpyavail(localNodeName, localNodeEntry->name, 256, true);

	if (nodeEntries.size() == 1) {
		LogPrint(localNodeID, LOG_NETWORK, 1, "*** Single Node ready, continuing system setup ***");
		nodeEntriesMutex.leave();
		return true;
	}

	LogPrint(localNodeID, LOG_NETWORK, 1, "Connecting to %u other Node(s), %u required, %u optional, %u external...",
		nodeEntries.size() - 1, requiredCount, optionalCount, externalCount);
	// Now we have all the nodes registered, handover to the run() thread...

	// Send welcome to each node, if possible
	std::map<uint16, NodeEntry*>::iterator it, itEnd;
	for (it = nodeEntries.begin(), itEnd = nodeEntries.end(); it != itEnd; it++) {
		if (it->first != localNodeID) {
			if (!sendMessageToNode(it->first, CTRL_NODE_WELCOME))
				LogPrint(localNodeID,LOG_NETWORK,2,"Couldn't send welcome message to Node '%s', will keep trying...", it->second->name);
		}
	}
	if (localNodeEntry->status < NODESTATUS_MAPPING)
		localNodeEntry->status = NODESTATUS_MAPPING;
	nodeEntriesMutex.leave();

	if (!waitForAllNodesStatus(NODESTATUS_NETREADY))
		return false;

	setLocalNodeStatus(NODESTATUS_NETREADY, true);

	LogPrint(localNodeID, LOG_NETWORK, 1, "*** Node network connected, continuing system setup ***");
	// localNodeEntry->status = NODESTATUS_NETREADY;

	return true;
}

uint16 Networking::getAvailableNodeID() {
	for (uint16 n=2; n<65535; n++) {
		if (nodeEntries.find(n) == nodeEntries.end())
			return n;
	}
	return 0;
}

bool Networking::waitForAllNodesStatus(uint16 status, uint32 ms) {

	// This is only called if we know that there is more than one node in the system

	std::string statusString;
	NodeEntry* entry;
	uint64 start = GetTimeNow();
	int32 printCycleMS = 3000;
	uint64 lastPrint = start;
	// Now cycle until all nodes have responded back or until the timeout
	std::map<uint16, NodeEntry*>::iterator it, itEnd;
	bool allReqConnected;
	while (true) {
		statusString = "";
		allReqConnected = true;
		nodeEntriesMutex.enter();

		if (localNodeEntry->status >= NODESTATUS_MAPPING) {
			for (it = nodeEntries.begin(), itEnd = nodeEntries.end(); it != itEnd; it++) {
				if (entry = it->second) {
					if ((stricmp(entry->name, localNodeName) == 0)) {
						// It's me
						statusString += utils::StringFormat("[%u x] ", entry->nodeID);
					}
					else if (((entry->constatus >= CONNECTION_READY) && (entry->status >= status))
						|| (entry->type >= NODE_OPTIONAL) ) {
						statusString += utils::StringFormat("[%u v] ", entry->nodeID);
						// Great
					}
					else if (entry->timeout && (GetTimeAgeMS(start) > (int32)entry->timeout)) {
						LogPrint(localNodeID,LOG_NETWORK,0,"Timed out waiting for Node ID %u to connect (%s on %s:%u)",
						entry->nodeID, entry->name, entry->addr, entry->port);
						nodeEntriesMutex.leave();
						return false;
					}
					else {
						statusString += utils::StringFormat("[%u -] ", entry->nodeID);
						// Continue waiting
						allReqConnected = false;
						// break;
					}
				}
			}
			nodeEntriesMutex.leave();
			if (allReqConnected) {
				LogPrint(localNodeID, LOG_NETWORK, 1, "Success waiting for nodes %s", statusString.c_str());
				return true;
			}
		}
		else
			nodeEntriesMutex.leave();

		if (ms && (GetTimeAgeMS(start) > (int32)ms))
			return false;
		if (GetTimeAgeMS(lastPrint) > printCycleMS) {
			LogPrint(localNodeID, LOG_NETWORK, 1, "Waiting for nodes %s", statusString.c_str());
			lastPrint = GetTimeNow();
		}
		utils::Sleep(20);
	}
	return true;
}



bool Networking::receiveNetworkEvent(NetworkEvent* evt, NetworkChannel* channel, uint64 conid) {
	// Check if it is a Node that disconnected
	NodeEntry* entry = NULL;
	std::map<uint16, NodeEntry*>::iterator it, itEnd;
	for (it = nodeEntries.begin(), itEnd = nodeEntries.end(); it != itEnd; it++)
		if (it->second && (it->second->conid == conid) && (it->second->nodeID != localNodeID))
			entry = it->second;

	switch(evt->type) {
		case NETWORKEVENT_DISCONNECT:
			if (getLocalNodeStatus() < NODESTATUS_READY) {
				entry->lastContact = GetTimeNow();
				entry->constatus = CONNECTION_DISCONNECTED;
			}
			else if (entry) {
				if (entry->type == NODE_REQUIRED)
					LogPrint(localNodeID, LOG_NETWORK, 1, "Required Node '%s' disconnected...", entry->name);
				else
					LogPrint(localNodeID, LOG_NETWORK, 1, "AdHoc Node '%s' disconnected...", entry->name);
				entry->lastContact = GetTimeNow();
				entry->constatus = CONNECTION_DISCONNECTED;
				PsyType type = node->getPsyType(utils::StringFormat("Psyclone.%s.Disconnect", entry->name).c_str());
				if (type.isValid()) {
					DataMessage* msg = new DataMessage();
					msg->setType(type);
					node->processMessage(msg);
					delete(msg);
				}
			}
			break;
		case NETWORKEVENT_DISCONNECT_RETRYING:
			if (getLocalNodeStatus() < NODESTATUS_READY) {
				entry->lastContact = GetTimeNow();
				entry->constatus = CONNECTION_DISCONNECTED;
			}
			else if (entry) {
				if (entry->type == NODE_REQUIRED) {
					// Put whole system into pause mode until the required node comes back
					entry->constatus = CONNECTION_RETRYING;
					entry->status = NODESTATUS_ERROR;
					LogPrint(localNodeID, LOG_NETWORK, 1, "Required Node '%s' disconnected, retrying to connect...", entry->name);
				}
				else {
					// Allow whole system to continue running, node can rejoin at any time
					entry->constatus = CONNECTION_RETRYING;
					entry->status = NODESTATUS_ERROR;
					LogPrint(localNodeID, LOG_NETWORK, 1, "AdHoc Node '%s' disconnected, retrying to connect...", entry->name);
				}
				PsyType type = node->getPsyType(utils::StringFormat("Psyclone.%s.Disconnect", entry->name).c_str());
				if (type.isValid()) {
					DataMessage* msg = new DataMessage();
					msg->setType(type);
					node->processMessage(msg);
					delete(msg);
				}
			}
			break;
		case NETWORKEVENT_RECONNECT:
			if (entry) {
				if (entry->type == NODE_REQUIRED) {
					entry->constatus = CONNECTION_READY;
					entry->status = NODESTATUS_IDLE;
					if (sendMessageToNode(entry->nodeID, CTRL_NODE_WELCOME)) {
						LogPrint(localNodeID, LOG_NETWORK, 1, "Required Node '%s' reconnected, rejoining node...", entry->name);
					}
					else {
						LogPrint(localNodeID, LOG_NETWORK, 2, "Couldn't send welcome message to Node '%s', will keep trying...", entry->name);
						channel->endConnection(conid);
					}
				}
				else {
					entry->constatus = CONNECTION_READY;
					entry->status = NODESTATUS_IDLE;
					if (sendMessageToNode(entry->nodeID, CTRL_NODE_WELCOME)) {
						LogPrint(localNodeID, LOG_NETWORK, 1, "AdHoc Node '%s' reconnected, rejoining node...", entry->name);
					}
					else {
						LogPrint(localNodeID, LOG_NETWORK, 2, "Couldn't send welcome message to Node '%s', will keep trying...", entry->name);
						channel->endConnection(conid);
					}
				}
			}
			break;
	}

	delete(evt);
	return true;
}

uint32 Networking::findServiceID(uint8 type, NetworkChannel* channel, uint64 conid) {
	// One Channel one Port
	// Lookup channel to find port number
	std::map<NetworkChannel*, uint16>::iterator it = channelPorts.find(channel);
	if (it == channelPorts.end())
		return 0;
	uint16 port = it->second;
	if (port == 0)
		return 0;

	std::map<uint16, InterfacePack>::iterator pit = portInterfacePacks.find(port);
	if (pit == portInterfacePacks.end())
		return 0;
	Interface* iface = pit->second.interfaces[type];
	if (!iface)
		return 0;
	return iface->serviceID;
}

bool Networking::receiveHTTPRequest(HTTPRequest* req, NetworkChannel* channel, uint64 conid) {
	uint32 serviceID = findServiceID(INTERFACE_HTTP, channel, conid);
	// Blocking call to service, service owns the connection until it has replied
	if (!serviceID || !serviceManager->runService(serviceID, req, channel, conid)) {
		delete(req);
	}
	return true;
}

bool Networking::receiveMessage(DataMessage* msg, NetworkChannel* channel, uint64 conid) {
	if (!msg || !channel || !node)
		return false;

//	printf("*%u-", msg->getTag()); fflush(stdout);

	uint64 now = GetTimeNow();
	msg->setRecvTime(now);

	const TimeSyncData* syncRecv;
	TimeSyncData* syncSend;
	int64 total, msgtime;
	uint64 t1, t2, t3, t4;
	uint32 s, n;
	const char* data, *data2;
	int64 val;
	//const char* cdata;
	DataMessage* msg2, *dmsg;
	bool allReady = true;
	std::map<uint16, NodeEntry*>::iterator it, itEnd;
	TriggerSpec* trigger;
	std::list<TriggerSpec*> triggers;

	uint64 memID;
	uint32 procID;

	char* label;

	uint32 serviceID = findServiceID(INTERFACE_MESSAGE, channel, conid);
	if (serviceID) {
		// Blocking call to service, service owns the connection until it has replied
		if (!serviceManager->runService(serviceID, msg, channel, conid)) {
			// ...
		}
		delete(msg);
		return true;
	}

	PsyType type = msg->getType();
	if (type == PsyAPI::CTRL_INTERSYSTEM_QUERY) {
		node->interSystemManager->processQuery(msg, conid);
		delete(msg);
		return true;
	}
	else if (type == PsyAPI::CTRL_INTERSYSTEM_QUERY_REPLY) {
		node->interSystemManager->processQueryReply(msg);
		delete(msg);
		return true;
	}

	uint16 remoteNodeID = msg->getOrigin();
	uint32 size = msg->getSize();

	nodeEntriesMutex.enter();
	//printf("-%u*", msg->getTag()); fflush(stdout);
	NodeEntry* entry = nodeEntries[remoteNodeID];

	LogPrint(0, LOG_NODE, 5, "Received message type %s from Node %u", node->manager->typeToText(type).c_str(), remoteNodeID);

	// *******************************************************
	// CTRL_NODE_WELCOME
	// Invitation from any node to connect
	// *******************************************************

	if (type == CTRL_NODE_WELCOME) {
		LogPrint(localNodeID, LOG_NODE, 3, "Received welcome message from Node %u", remoteNodeID);
		// Invitation from any node to connect with assigned id,
		//  config node id and timeserver id

		if (!checkNodeInstanceID(remoteNodeID, msg->getTime("Instance"), conid)) {
			if (remoteNodeID == localNodeEntry->configNodeID) {
				LogPrint(localNodeID, LOG_NODE, 0, "Main Node %u appears to have restarted", remoteNodeID);
				node->shouldRestart = true;
			}
			else
				LogPrint(localNodeID, LOG_NODE, 0, "Other Node %u appears to have restarted", remoteNodeID);
		}

		// Are we busy in another system???
		// or have we already received the welcome msg from someone else
		if (localNodeEntry->status >= NODESTATUS_TIMESYNC) {
			LogPrint(localNodeID, LOG_NODE, 0, "Busy while receiving welcome message from Node %u", remoteNodeID);
			delete(msg);
			nodeEntriesMutex.leave();
			return true;
		}

		int64 assignedNodeID;
		int64 configNodeID;
		uint32 configNodeIPAddressCount, remoteNodeIPAddressCount;
		uint32* configNodeIPAddresses, *remoteNodeIPAddresses;
		int64 configNodePort, remoteNodePort;
		const char* configNodeAddress, *remoteNodeAddress;
		if ( 
			(!msg->getInt("AssignedNodeID", assignedNodeID)) ||
			(!msg->getInt("ConfigNodeID", configNodeID)) ||
			(!(configNodeAddress = msg->getString("ConfigNodeAddress"))) ||
			(!(configNodeIPAddresses = (uint32*)msg->getData("ConfigNodeIPAddresses", configNodeIPAddressCount))) ||
			(!msg->getInt("ConfigNodePort", configNodePort)) ||
			(!(remoteNodeAddress = msg->getString("RemoteNodeAddress"))) ||
			(!(remoteNodeIPAddresses = (uint32*)msg->getData("RemoteNodeIPAddresses", remoteNodeIPAddressCount))) ||
			(!msg->getInt("RemoteNodePort", remoteNodePort))
			) {
			LogPrint(localNodeID, LOG_NODE, 0, "Received malformed invitation from unknown node [%u]", remoteNodeID);
			delete(msg);
			nodeEntriesMutex.leave();
			return true;
		}
		configNodeIPAddressCount /= 4;
		remoteNodeIPAddressCount /= 4;
		if (!entry) {
			entry = new NodeEntry;
			memset(entry, 0, sizeof(NodeEntry));
			nodeEntries[remoteNodeID] = entry;
		}
		entry->nodeID = remoteNodeID;
		if (!entry->port)
			entry->port = (uint16)remoteNodePort;
		entry->addIPAddresses(remoteNodeIPAddresses, remoteNodeIPAddressCount);
		if (!entry->addr[0])
			utils::strcpyavail(entry->addr, remoteNodeAddress, 255, true);
		if (!entry->timeout)
			entry->timeout = reqNodeTimeout;
		entry->lastContact = now;
		if (entry->conid && (entry->conid != conid))
			channel->endConnection(entry->conid);
		entry->conid = conid;
		entry->constatus = CONNECTION_INITIALISING;

		// If we are already speaking to the config node
		//if (localNodeEntry->status >= NODESTATUS_NETSETUP) {
		//	LogPrint(0, LOG_NODE, 0, "Node %u connected...", remoteNodeID);
		//	delete(msg);
		//	nodeEntriesMutex.leave();
		//	return true;
		//}

		if (remoteNodeID == configNodeID) {
			entry->type = NODE_TIMENODE;
			LogPrint(localNodeID, LOG_NODE, 2, "Config Node %u connected, starting time synchronisation...", remoteNodeID);
		}
		else {
			NodeEntry* configNodeEntry = nodeEntries[(uint16)configNodeID];
			if (!configNodeEntry) {
				configNodeEntry = new NodeEntry;
				memset(configNodeEntry, 0, sizeof(NodeEntry));
				nodeEntries[(uint16)configNodeID] = configNodeEntry;
			}

			if (!configNodeEntry->conid) {
				LogPrint(localNodeID, LOG_NODE, 3, "Node %u connected before Config Node...", remoteNodeID);

				if (!configNodeEntry->nodeID || configNodeEntry->port) {
					configNodeEntry->nodeID = (uint16)configNodeID;
					if (!configNodeEntry->port)
						configNodeEntry->port = (uint16)configNodePort;
					configNodeEntry->addIPAddresses(configNodeIPAddresses, configNodeIPAddressCount);
					if (!configNodeEntry->addr[0])
						utils::strcpyavail(entry->addr, configNodeAddress, 255, true);
					if (!configNodeEntry->timeout)
						entry->timeout = reqNodeTimeout;
				}
			}
		}

		localNodeEntry->configNodeID = (uint16)configNodeID;

		LogPrint(localNodeID, LOG_NODE, 1, "Joining Node Network as Node ID %u...", assignedNodeID);

		localNodeID = localNodeEntry->nodeID = (uint16)assignedNodeID;
		localNodeEntry->status = NODESTATUS_TIMESYNC;
		timesyncNodeID = (uint16)configNodeID;
		if (!startTimesync()) {
			if (configNodeID == remoteNodeID) {
				LogPrint(localNodeID, LOG_NODE, 0, "Cannot connect to Config Node [%u], giving up...", remoteNodeID);
				delete(msg);
				nodeEntriesMutex.leave();
				return true;
			}
			else {
				timesyncNodeID = remoteNodeID;
				if (!startTimesync()) {
					LogPrint(localNodeID, LOG_NODE, 0, "Cannot connect to Config Node [%u] or remote node [%u], giving up...", (uint16)configNodeID, remoteNodeID);
					delete(msg);
					nodeEntriesMutex.leave();
					return true;
				}
				LogPrint(localNodeID, LOG_NODE, 1, "Cannot connect to Time Node [%u], connected via Node %u instead...", (uint16)configNodeID, remoteNodeID);
			}
		}
		delete(msg);
		nodeEntriesMutex.leave();
		LogPrint(localNodeID, LOG_NODE, 1, "Synchronising time with Node Network...");
		return true;
	}

	// other than welcome, if we are not ready we cannot handle any other type of message
	if (localNodeEntry->status < NODESTATUS_TIMESYNC) {
		LogPrint(localNodeID, LOG_NODE, 0, "Received message from node [%u] before local node initialised", remoteNodeID);
		delete(msg);
		nodeEntriesMutex.leave();
		return true;
	}

	// *******************************************************
	// CTRL_NODE_JOIN
	// Request to join, please send time/config node info
	// *******************************************************

	// ########################

	// Try to figure out if node is on the current NodeMap based on addresses and port
	// if yes, send it welcome
	// if no, block new ID and send it welcome
	// it will then broadcast to all others to let them know that a new node has joined

	if (type[0] == 1) {

		// *******************************************************
		// CTRL_NODE_STATUSMAP
		// All nodes broadcast the status map to all other nodes
		// *******************************************************

		if (type == CTRL_NODE_STATUSMAP) {
			LogPrint(localNodeID, LOG_NODE, 8, "Received heartbeat from Node %u", remoteNodeID);
			NodeMap* newMap = (NodeMap*) msg->getData("NodeStatusMap", s);
			if (newMap)
				updateNodeNetwork(newMap->nodeID, newMap, conid);
			delete(msg);
			nodeEntriesMutex.leave();
			return true;
		}

		// *******************************************************
		// CTRL_NODE_TIMESYNC & CTRL_NODE_TIMESYNC_REPLY
		// *******************************************************

		else if (type == CTRL_NODE_TIMESYNC) {
			checkNodeInstanceID(remoteNodeID, msg->getTime("Instance"), conid);
			if (syncRecv = (TimeSyncData*) msg->getData("TimeSyncData", s)) {
				LogPrint(localNodeID, LOG_NODE, 3, "Received Timesync step %u from Node %u", syncRecv->count, remoteNodeID);
				syncSend = new TimeSyncData;
				memcpy(syncSend, syncRecv, sizeof(TimeSyncData));
				syncSend->t1 = msg->getSendTime();
				syncSend->t2 = msg->getRecvTime();
				msg2 = new DataMessage(CTRL_NODE_TIMESYNC_REPLY, localNodeID);
				msg->setTime("Instance", localNodeEntry->instanceID);
				msg2->setData("TimeSyncData", (char*)syncSend, syncSend->size);
				sendMessageToNode(remoteNodeID, msg2, true);
				delete(syncSend);
			}
			nodeEntriesMutex.leave();
			delete(msg);
			return true;
		}
		else if (type == CTRL_NODE_TIMESYNC_REPLY) {
			LogPrint(localNodeID, LOG_NODE, 3, "Received timesync reply from Node %u", remoteNodeID);
			checkNodeInstanceID(remoteNodeID, msg->getTime("Instance"), conid);
			if (syncRecv = (TimeSyncData*) msg->getData("TimeSyncData", s)) {
				t3 = msg->getSendTime();
				t4 = msg->getRecvTime();
				total = syncRecv->total +
					(syncRecv->t1 - syncRecv->t2 - (t4 - syncRecv->t1 - t3 + syncRecv->t2)/2);
				msgtime = syncRecv->msgtime +
					((t4 - syncRecv->t1 - t3 + syncRecv->t2)/2);

				if (syncRecv->count < timeSyncCount)
					// continue timesync roundabout
					startTimesync(syncRecv->count+1, total, msgtime);
				else {
					// We are done and have the results
					msgtime /= timeSyncCount;
					nodeEntries[remoteNodeID]->msgtime = msgtime;
					total /= timeSyncCount * (-1);
					if (total > 0)
						LogPrint(localNodeID,LOG_NODE,1,"TimeSync we are %s behind (msg %lld total %lld)",
							PrintTimeDifString(total).c_str(), msgtime, total);
							// PrintTimeDifString(total).c_str(), msgtime, total, syncRecv->t1, syncRecv->t2, t3, t4);
					else if (total < 0)
						LogPrint(localNodeID,LOG_NODE,1,"TimeSync we are %s ahead (msg %lld total %lld)",
							PrintTimeDifString(total*(-1)).c_str(), msgtime, total);
					else
						LogPrint(localNodeID,LOG_NODE,1,"TimeSync exactly in sync (msg %lld total %lld)",
							msgtime, total);
					if (localNodeEntry->status < NODESTATUS_READY) {
						// Adjust time now in one go
						LogPrint(localNodeID,LOG_NODE,3,"Adjusting local time by %lld", total);
						if (!adjustTimesync(total))
							LogPrint(localNodeID,LOG_NODE,0, "Unable to adjust timesync");
						// utils::Sleep(1000);
					}
					else {
						timeAdjustRemain = total;
						timeStepSize = total/20;
					}
					if (localNodeEntry->status < NODESTATUS_MAPPING) {
						sendMessageToNode(localNodeEntry->configNodeID, CTRL_NODE_NODEMAP_REQUEST);
						localNodeEntry->status = NODESTATUS_MAPPING;
					}
				}
			}
			nodeEntriesMutex.leave();
			delete(msg);
			return true;
		}
	}










	// *******************************************************
	// Beyond this point we only deal with known nodes
	// *******************************************************

	if (!entry) {
		LogPrint(localNodeID,LOG_NODE,0, "Received unauthorised message from unknown node [%u]", remoteNodeID);
		nodeEntriesMutex.leave();
		delete(msg);
		return true;
	}

	if (remoteNodeID && (entry->nodeID != remoteNodeID))
		entry->nodeID = remoteNodeID;
		
	entry->lastContact = now;

	// Check if incoming connection should override the existing one
	if (manager->getConnectionType(conid) == UDPCON) {
		if (entry->udpConid && (entry->udpConid != conid) )
			channel->endConnection(entry->conid);
		entry->udpConid = conid;
	}

	addToConnectionStats(remoteNodeID, size, 0, now);

	uint16 destinationNodeID = msg->getDestination();
	if (destinationNodeID && (destinationNodeID != localNodeID)) {
		LogPrint(localNodeID, LOG_NODE, 3, "Forwarding message from Node %u to Node %u", remoteNodeID, destinationNodeID);
		sendMessageToNode(destinationNodeID, msg, true);
		nodeEntriesMutex.leave();
		return true;
	}

	nodeEntriesMutex.leave();

	if (type[0] == 1) {
		// *******************************************************
		// CTRL_NODE_NODEMAP_REQUEST
		// Node request that config node sends fresh NodeMap
		// *******************************************************

		if (type == CTRL_NODE_NODEMAP_REQUEST) {
			LogPrint(localNodeID, LOG_NODE, 3, "Received nodemap request from Node %u", remoteNodeID);
			delete(msg);
			return sendMessageToNode(remoteNodeID, CTRL_NODE_STATUSMAP);
		}

	//	utils::Sleep(5);
		// LogPrint(0, LOG_NODE, 0, "--- Received control message type %s from Node %u  (tag: %u)", node->manager->typeToText(msg->getType()).c_str(), remoteNodeID, msg->getTag());
		if (type == CTRL_NODE_SIGNAL) {
			if (msg2 = msg->getAttachedMessageCopy("Message")) {
				node->manager->processMemory->addToAllSignalQsExcept(msg2, 0);
				delete(msg2);
			}
		}
		else if (type == CTRL_NODE_CONTEXT_ACTIVE) {
			if ( (data = msg->getData("ActiveContext", size)) && (data2 = msg->getData("InactiveContext", size)) )
				node->subscriptionEngine->setContextActive(*(PsyContext*)data, *(PsyContext*)data2);
			//return true;
		}
		else if (type == CTRL_PING) {
			LogPrint(localNodeID, LOG_NODE, 3, "Received ping from Node %u", remoteNodeID);
			checkNodeInstanceID(remoteNodeID, msg->getTime("Instance"), conid);
			msg2 = new DataMessage(CTRL_PING_REPLY, localNodeID);
			msg2->setTime("Start", msg->getSendTime());
			msg2->setTime("Instance", localNodeEntry->instanceID);
			sendMessageToNode(remoteNodeID, msg2, true);
		}
		else if (type == CTRL_PING_REPLY) {
			LogPrint(localNodeID, LOG_NODE, 3, "Received ping reply from Node %u", remoteNodeID);
			checkNodeInstanceID(remoteNodeID, msg->getTime("Instance"), conid);
			t1 = msg->getTime("Start");
			t2 = msg->getRecvTime();
			nodeEntries[remoteNodeID]->msgtime = (t2-t1)/2;
		}
		//else if (type == CTRL_NODE_GREETING) {
		//	checkNodeInstanceID(remoteNodeID, msg->getTime("Instance"), conid);
		//	map = (NodeMap*) msg->getData("NodeMap", s);
		//	joinNodeNetwork(msg->getTo(), map, conid);
		//}
		//else if (type == CTRL_NODE_JOIN_SUCCESS) {
		//	checkNodeInstanceID(remoteNodeID, msg->getTime("Instance"), conid);
		//	map = (NodeMap*) msg->getData("NodeMap", s);
		//	joinNodeNetwork(msg->getTo(), map, conid);
		//}
		//else if (type == CTRL_NODE_JOIN_SUCCESS) {
		//	LogPrint(0,0,0,"Could not join existing node network");
		//}
		else if (type == CTRL_NODE_SYNC_REQ) {
			LogPrint(localNodeID, LOG_NODE, 5, "Received sync request from Node %u", remoteNodeID);
			data = generateNodeSyncData(s);
			msg2 = new DataMessage(CTRL_NODE_SYNC_REPLY, localNodeID);
			msg2->setTime("Instance", localNodeEntry->instanceID);
			msg2->setData("NodeSyncData", data, s);
			sendMessageToNode(remoteNodeID, msg2, true);
		}
		else if (type == CTRL_NODE_SYNC_COMPONENT) {
			LogPrint(localNodeID, LOG_NODE, 5, "Received sync component loc (%llu) from Node %u", msg->getReference(), remoteNodeID);
			//if (!node->enterOtherComponentLocation(*(ComponentID*)msg->getData("ComponentLocation", s)))
			if ( (data = msg->getData("ComponentLocation", s)) &&
				(node->manager->componentMemory->updateComponentLocation(((ComponentID*)data)->id, ((ComponentID*)data)->nodeID, ((ComponentID*)data)->procID)) ) {
				msg2 = new DataMessage(CTRL_NODE_SYNC_COMPONENT_SUCCESS, localNodeID);
			}
			else {
				msg2 = new DataMessage(CTRL_NODE_SYNC_COMPONENT_FAILED, localNodeID);
				LogPrint(localNodeID,LOG_SYNC,0, "Failed component loc SYNC %u, failed to Node %u...", msg->getTag(), remoteNodeID);
			}
			msg2->setTag(msg->getTag());
			//LogPrint(localNodeID,LOG_SYNC,0, "Reply CONFIRM ID SYNC %u to Node %u...", msg->getTag(), remoteNodeID);
			sendMessageToNode(remoteNodeID, msg2, true);
			//LogPrint(localNodeID,LOG_SYNC,0, "Done CONFIRM ID SYNC %u to Node %u...", msg->getTag(), remoteNodeID);
		}
		else if (type == CTRL_NODE_SYNC_COMPONENT_SUCCESS) {
			LogPrint(localNodeID,LOG_SYNC,5, "Received component loc SUCCESS SYNC %u from Node %u...", msg->getTag(), remoteNodeID);
			replyCheck.addReply(msg->getTag(), remoteNodeID, true, msg);
			msg = NULL;
		}
		else if (type == CTRL_NODE_SYNC_COMPONENT_FAILED) {
			LogPrint(localNodeID,LOG_SYNC,0, "Received component loc FAILED SYNC %u from Node %u...", msg->getTag(), remoteNodeID);
			replyCheck.addReply(msg->getTag(), remoteNodeID, false, msg);
			msg = NULL;
		}
		else if (type == CTRL_NODE_SYNC_SUBSCRIPTION) {
			msg2 = NULL;
			LogPrint(localNodeID, LOG_NODE, 5, "Received sync subscription (%llu) from Node %u", msg->getReference(), remoteNodeID);
			if (msg->getInt("CompID", val) && val) {
				n = 0;
				label = new char[MAXKEYNAMELEN+1];
				snprintf(label, MAXKEYNAMELEN+1, "Trigger%u", n);
				while (trigger = (TriggerSpec*) msg->getData(label, s)) {
					triggers.push_back(trigger);
					snprintf(label, MAXKEYNAMELEN+1, "Trigger%u", ++n);
				}
				delete [] label;
				if (node->subscriptionEngine->addSubscription((uint32)val, triggers))
					msg2 = new DataMessage(CTRL_NODE_SYNC_SUBSCRIPTION_SUCCESS, localNodeID);
				triggers.clear();
			}
			if (!msg2) {
				msg2 = new DataMessage(CTRL_NODE_SYNC_SUBSCRIPTION_FAILED, localNodeID);
				LogPrint(localNodeID,LOG_SYNC,0, "Failed subscription SYNC %u, failed to Node %u...", msg->getTag(), remoteNodeID);
			}
			msg2->setTag(msg->getTag());
			//LogPrint(localNodeID,LOG_SYNC,0, "Reply CONFIRM ID SYNC %u to Node %u...", msg->getTag(), remoteNodeID);
			sendMessageToNode(remoteNodeID, msg2, true);
			//LogPrint(localNodeID,LOG_SYNC,0, "Done CONFIRM ID SYNC %u to Node %u...", msg->getTag(), remoteNodeID);
		}
		else if (type == CTRL_NODE_SYNC_SUBSCRIPTION_SUCCESS) {
			LogPrint(localNodeID,LOG_SYNC,5, "Received Subscription SUCCESS SYNC %u from Node %u...", msg->getTag(), remoteNodeID);
			replyCheck.addReply(msg->getTag(), remoteNodeID, true, msg);
			msg = NULL;
		}
		else if (type == CTRL_NODE_SYNC_SUBSCRIPTION_FAILED) {
			LogPrint(localNodeID,LOG_SYNC,0, "Received Subscription FAILED SYNC %u from Node %u...", msg->getTag(), remoteNodeID);
			replyCheck.addReply(msg->getTag(), remoteNodeID, false, msg);
			msg = NULL;
		}
		else if (type == CTRL_NODE_SYNC_ID) {
			//LogPrint(0, 0, 0, "Start ID SYNC %u from Node %u...\n%s\n", msg->getTag(), remoteNodeID, msg->toJSON().c_str());
			uint32 existingID = 0;
			//if (node->processIDSync((uint8)msg->getReference(), msg->getData("IDEntry", s), remoteNodeID, msg->getTag(), existingID)) {
			if (node->processIDSync(msg, remoteNodeID, msg->getTag(), existingID)) {
				msg2 = new DataMessage(CTRL_NODE_SYNC_ID_SUCCESS, localNodeID);
			}
			else {
				msg2 = new DataMessage(CTRL_NODE_SYNC_ID_FAILED, localNodeID);
				msg2->setInt("ExistingID", existingID);
			}
			msg2->setTag(msg->getTag());
			LogPrint(localNodeID,LOG_SYNC,5, "Reply ID SYNC %u to Node %u...", msg->getTag(), remoteNodeID);
			sendMessageToNode(remoteNodeID, msg2, true);
			LogPrint(localNodeID,LOG_SYNC,5, "Done ID SYNC %u to Node %u...", msg->getTag(), remoteNodeID);
		}
		else if (type == CTRL_NODE_SYNC_ID_SUCCESS) {
			LogPrint(localNodeID,LOG_SYNC,5, "Received ID SUCCESS SYNC %u from Node %u...", msg->getTag(), remoteNodeID);
			replyCheck.addReply(msg->getTag(), remoteNodeID, true, msg);
			msg = NULL;
		}
		else if (type == CTRL_NODE_SYNC_ID_FAILED) {
			LogPrint(localNodeID,LOG_SYNC,0, "Received ID FAILED SYNC %u from Node %u...", msg->getTag(), remoteNodeID);
			replyCheck.addReply(msg->getTag(), remoteNodeID, false, msg);
			msg = NULL;
		}
		else if (type == CTRL_NODE_CONFIRM_ID) {
			// LogPrint(0, 0, 0, "Start ID SYNC %u to Node %u...", msg->getTag(), remoteNodeID);
			if (msg->getInt("ID", val) && node->processConfirmIDSync((uint8)msg->getReference(), (uint32)val, remoteNodeID)) {
				msg2 = new DataMessage(CTRL_NODE_CONFIRM_ID_SUCCESS, localNodeID);
				LogPrint(localNodeID,LOG_SYNC,5, "Received CONFIRM ID SYNC %u, success to Node %u...", msg->getTag(), remoteNodeID);
			}
			else {
				msg2 = new DataMessage(CTRL_NODE_CONFIRM_ID_FAILED, localNodeID);
				LogPrint(localNodeID,LOG_SYNC,0, "Received CONFIRM ID SYNC %u, failed to Node %u...", msg->getTag(), remoteNodeID);
			}
			msg2->setTag(msg->getTag());
			//LogPrint(localNodeID,LOG_SYNC,0, "Reply CONFIRM ID SYNC %u to Node %u...", msg->getTag(), remoteNodeID);
			sendMessageToNode(remoteNodeID, msg2, true);
			//LogPrint(localNodeID,LOG_SYNC,0, "Done CONFIRM ID SYNC %u to Node %u...", msg->getTag(), remoteNodeID);
		}
		else if (type == CTRL_NODE_CONFIRM_ID_SUCCESS) {
			LogPrint(localNodeID,LOG_SYNC,5, "Received CONFIRM ID SUCCESS SYNC %u from Node %u...", msg->getTag(), remoteNodeID);
			replyCheck.addReply(msg->getTag(), remoteNodeID, true, msg);
			msg = NULL;
		}
		else if (type == CTRL_NODE_CONFIRM_ID_FAILED) {
			LogPrint(localNodeID,LOG_SYNC,0, "Received CONFIRM ID FAILED SYNC %u from Node %u...", msg->getTag(), remoteNodeID);
			replyCheck.addReply(msg->getTag(), remoteNodeID, false, msg);
			msg = NULL;
		}
		else if (type == CTRL_NODE_CANCEL_ID) {
			// LogPrint(0, 0, 0, "Start ID SYNC %u to Node %u...", msg->getTag(), remoteNodeID);
			if (msg->getInt("ID", val) && node->processCancelIDSync((uint8)msg->getReference(), (uint32)val, remoteNodeID)) {
				msg2 = new DataMessage(CTRL_NODE_CANCEL_ID_SUCCESS, localNodeID);
				LogPrint(localNodeID,LOG_SYNC,5, "Cancelled ID SYNC %u, success to Node %u...", msg->getTag(), remoteNodeID);
			}
			else {
				msg2 = new DataMessage(CTRL_NODE_CANCEL_ID_FAILED, localNodeID);
				LogPrint(localNodeID,LOG_SYNC,0, "Cancelled ID SYNC %u, failed to Node %u...", msg->getTag(), remoteNodeID);
			}
			msg2->setTag(msg->getTag());
			//LogPrint(localNodeID,LOG_SYNC,5, "Reply CANCEL ID SYNC %u to Node %u...", msg->getTag(), remoteNodeID);
			sendMessageToNode(remoteNodeID, msg2, true);
			//LogPrint(localNodeID,LOG_SYNC,5, "Done CANCEL ID SYNC %u to Node %u...", msg->getTag(), remoteNodeID);
		}
		else if (type == CTRL_NODE_CANCEL_ID_SUCCESS) {
			LogPrint(localNodeID,LOG_SYNC,5, "Received CANCEL ID SUCCESS SYNC %u from Node %u...", msg->getTag(), remoteNodeID);
			replyCheck.addReply(msg->getTag(), remoteNodeID, true, msg);
			msg = NULL;
		}
		else if (type == CTRL_NODE_CANCEL_ID_FAILED) {
			LogPrint(localNodeID,LOG_SYNC,0, "Received CANCEL ID FAILED SYNC %u from Node %u...", msg->getTag(), remoteNodeID);
			replyCheck.addReply(msg->getTag(), remoteNodeID, false, msg);
			msg = NULL;
		}
		else if (type == CTRL_PSYPROBE_CREATECUSTOMPAGE) {
			int64 compID = 0;
			if (msg->getInt("ComponentID", compID) && compID) {
				if (node->psyProbe->addCustomView((uint32)compID, msg->getString("Name"), msg->getString("Template")))
					msg2 = new DataMessage(CTRL_PSYPROBE_CREATECUSTOMPAGE_SUCCESS, localNodeID);
				else
					msg2 = new DataMessage(CTRL_PSYPROBE_CREATECUSTOMPAGE_FAILED, localNodeID);
			}
			else
				msg2 = new DataMessage(CTRL_PSYPROBE_CREATECUSTOMPAGE_FAILED, localNodeID);
			msg2->setTag(msg->getTag());
			sendMessageToNode(remoteNodeID, msg2, true);
		}
		else if (type == CTRL_PSYPROBE_CREATECUSTOMPAGE_SUCCESS) {
			replyCheck.addReply(msg->getTag(), remoteNodeID, true, msg);
			msg = NULL;
		}
		else if (type == CTRL_PSYPROBE_CREATECUSTOMPAGE_FAILED) {
			replyCheck.addReply(msg->getTag(), remoteNodeID, false, msg);
			msg = NULL;
		}


		else if (type == CTRL_NODE_PERFSYNC) {
			if ((data = msg->getData("AveragePerfStats", size)) && size)
				node->monitoring->setPerfDataFromOtherNode(data, size, remoteNodeID);
		}

		else if (type == CTRL_NODE_QUERY) {
			if (msg2 = node->processQueryMsg(msg)) {
				msg2->setTag(msg->getTag());
				sendMessageToNode(remoteNodeID, msg2, true);
			}
		}
		else if (type == CTRL_NODE_QUERY_SUCCESS) {
			LogPrint(localNodeID,LOG_NETWORK,5, "Received QUERY SUCCESS %u from Node %u...", msg->getTag(), remoteNodeID);
			replyCheck.addReply(msg->getTag(), remoteNodeID, true, msg);
			msg = NULL;
		}
		else if (type == CTRL_NODE_QUERY_FAILED) {
			LogPrint(localNodeID,LOG_NETWORK,0, "Received FAILED QUERY %u from Node %u...", msg->getTag(), remoteNodeID);
			replyCheck.addReply(msg->getTag(), remoteNodeID, false, msg);
			msg = NULL;
		}
		else if (type == CTRL_NODE_CONFIG) {
			LogPrint(localNodeID, LOG_NODE, 2, "Received node config from Node %u", remoteNodeID);
			node->startConfigNode(msg);
			msg = NULL;
		}
		//else if (type == CTRL_NODE_CONFIG_SUCCESS) {
		//	LogPrint(0, 0, 0, "Node %u configured successfully...", remoteNodeID);
		//	if (statusOtherNodesMutex.enter()) {
		//		statusOtherNodes[remoteNodeID]->status = NODESTATUS_READY;
		//		//allReady = true;
		//		//for (it = statusOtherNodes.begin(), itEnd = statusOtherNodes.end(); it != itEnd; it++) {
		//		//	if (it->second->status < NODESTATUS_READY) {
		//		//		allReady = false;
		//		//		statusOtherNodesMutex.leave();
		//		//		break;
		//		//	}
		//		//}
		//		//if (allReady)
		//		//	localNodeEntry->status = NODESTATUS_READY;
		//		statusOtherNodesMutex.leave();
		//	}
		//}
		else if (type == CTRL_NODE_CONFIG_FAILED) {
			LogPrint(0, 0, 0, "Setup of node %u failed: %s", msg->getFrom(), msg->getString("ErrorText"));
			setLocalNodeStatus(NODESTATUS_ERROR);
		}
		else if (type == PsyAPI::CTRL_SYSTEM_READY) {
			node->psyProbe->setSystemReady();
			LogPrint(localNodeID,LOG_NODE,1, "************ SYSTEM READY *************");
			LogPrint(localNodeID, LOG_NODE, 2, "Current full registration:\n%s\n", node->subscriptionEngine->printFriendly().c_str());
			LogPrint(localNodeID, LOG_NODE, 2, "Component list\n%s...", node->manager->componentMemory->printFriendly().c_str());
			LogPrint(localNodeID, LOG_NODE, 2, "Types list\n%s...", node->manager->dataMapsMemory->printAllTypes().c_str());
			LogPrint(localNodeID, LOG_NODE, 2, "Contexts list\n%s...", node->manager->dataMapsMemory->printAllContexts().c_str());
		}
		else if (type == PsyAPI::CTRL_SYSTEM_SHUTDOWN) {
			LogPrint(localNodeID,LOG_NODE,1, "Received system shutdown from Node %u, shutting down...", msg->getFrom());
			node->shutdown(0);
		}
		else if (type == PsyAPI::CTRL_QUERY) {
			LogPrint(localNodeID, LOG_NODE, 5, "Received query from Node %u", remoteNodeID);
			DataMessage* replyMsg;
			uint32 reqID;
			uint32 remoteReqID = (uint32)msg->getReference();
			uint32 to = msg->getTo();
			uint16 otherNode = 0;
			uint16 otherProcID = 0;
			if ((!node->manager->componentMemory->getComponentLocation(msg->getTo(), otherNode, otherProcID)) ||
				(!node->manager->dataMapsMemory->createNewRequest(msg->getFrom(), to, remoteReqID, reqID)) ) {
				replyMsg = new DataMessage(PsyAPI::CTRL_QUERY_REPLY, msg->getTo(), msg->getFrom(), 10000);
				replyMsg->setReference(remoteReqID);
				replyMsg->setStatus(QUERY_FAILED);
				if (otherNode)
					sendMessageToNode(otherNode, replyMsg, true);
			}
			msg->setReference(reqID);
			if (!node->manager->processMemory->addToReqQ(otherProcID, msg)) {
				node->manager->dataMapsMemory->deleteRequest(reqID);
				replyMsg = new DataMessage(PsyAPI::CTRL_QUERY_REPLY, msg->getTo(), msg->getFrom(), 10000);
				replyMsg->setReference(remoteReqID);
				replyMsg->setStatus(QUERY_FAILED);
				if (otherNode)
					sendMessageToNode(otherNode, replyMsg, true);
			}
		}
		else if (type == PsyAPI::CTRL_QUERY_REPLY) {
			LogPrint(localNodeID, LOG_NODE, 5, "Received query reply from Node %u", remoteNodeID);
			uint64 msgID = 0;
			uint64 msgEOL = 0;
			uint8 reqStatus = 0;
			uint32 reqID = (uint32)msg->getReference();
			uint16 status = msg->getStatus();
			DataMessage* resultMsg = msg->getAttachedMessageCopy("DataMessage");
			if (resultMsg) {
				resultMsg->setTTL(10000000);
				msgEOL = resultMsg->getEOL();
				// decide on EOL ###################
				node->manager->temporalMemory->insertMessage(resultMsg, msgID);
				delete(resultMsg);
			}
			if (status == QUERY_SUCCESS) {
				if (msgID)
					reqStatus = REQ_SUCCESS_DATA_EOL;
				else
					reqStatus = REQ_SUCCESS;
			}
			else {
				if (msgID)
					reqStatus = REQ_FAILED_DATA_EOL;
				else
					reqStatus = REQ_FAILED;
			}
			LogPrint(localNodeID,LOG_NETWORK,5, "Got Query Reply reqID %u status %u...", reqID, reqStatus);
			node->manager->dataMapsMemory->setRequestStatus(reqID, reqStatus, msgID, msgEOL);
		}
		else if (type == PsyAPI::CTRL_TRIGGER) {
			LogPrint(localNodeID, LOG_NODE, 3, "Received trigger from Node %u", remoteNodeID);
			procID = node->manager->componentMemory->getComponentProcessID(msg->getTo());
			if (procID) {
				if (dmsg = msg->getAttachedMessageCopy("Message")) {
					if (dmsg->getTTL()) {
						// Add datamessage to memory
						if (!node->manager->insertMessage(dmsg, memID))
							LogPrint(localNodeID,LOG_MEMORY,0, "Could not add message from %u to shared memory for incoming network message from %u...", msg->getFrom());
						msg->setReference(memID);
					}
					delete(dmsg);
				}
				// Send ctrlmsg with trigger & message
				node->manager->processMemory->addToMsgQ(procID, msg);
			}
			else
				LogPrint(localNodeID,LOG_MEMORY,3, "Could not find component %u queue for incoming network message from %u...", msg->getTo(), msg->getFrom());
		}
	}
	else {
		procID = node->manager->componentMemory->getComponentProcessID(msg->getTo());
		LogPrint(0, 0, 0, "Adding message from %u to %u to shared memory for incoming network message from %u...",
			msg->getFrom(), msg->getTo(), remoteNodeID);
		if (procID) {
			//if (msg->getTTL()) {
				//if (!node->manager->insertMessage(msg, memID))
				//	LogPrint(0, 0, 0, "Could not add message from %u to shared memory for incoming network message from %u...", msg->getFrom());
			//}
			node->manager->processMemory->addToMsgQ(procID, msg);
		}
	}
	delete(msg);
	return true;
}

bool Networking::receiveTelnetLine(TelnetLine* line, NetworkChannel* channel, uint64 conid) {
	uint32 serviceID = findServiceID(INTERFACE_TELNET, channel, conid);
	// Blocking call to service, service owns the connection until it has replied
	if (!serviceID || !serviceManager->runService(serviceID, line, channel, conid)) {
		delete(line);
	}
	return true;
}

bool Networking::receiveHTTPReply(HTTPReply* reply, HTTPRequest* req, NetworkChannel* channel, uint64 conid) {
	// Not used currently
	return false;
}




bool Networking::checkNodeInstanceID(uint16 nodeID, uint64 instID, uint64 conID) {
	if (!instID || !nodeID)
		return false;

	NodeEntry* entry = nodeEntries[nodeID];
	if (!entry)
		return false;
	if (!entry->instanceID) {
		entry->instanceID = instID;
		return true;
	}

	if (entry->instanceID != instID) {
		// Remote Node has restarted
		// ####################
		return false;
	}

	return true;
}

char* Networking::generateNodeSyncData(uint32& size) {
	// ####################
	return NULL;
}


NodeEntry* Networking::getNodeEntry(uint16 nodeID) {
	if (!nodeID) return NULL;
	NodeEntry* entry = nodeEntries[nodeID];
	if (!entry || entry->nodeID != nodeID)
		return NULL;
	return entry;
}

uint16 Networking::getNodeCount() {
	return (getAvailableNodeID() - 1);
}

uint64 Networking::openConnectionToHost(uint32 host, uint16 port) {
	uint64 location = GETIPADDRESSPORT(host, port);
	uint64 conid = externalConnections[location];
	if (!conid)
		NetworkChannel* channel = manager->createTCPConnection(location, NOENC, PROTOCOL_MESSAGE, true, true, 0, this, conid);
	if (conid)
		externalConnections[location] = conid;
	return conid;
}

bool Networking::sendMessageToHost(DataMessage* msg, uint32 host, uint16 port) {
	uint64 location = GETIPADDRESSPORT(host, port);
	uint64 conid = externalConnections[location];
	if (!conid)
		conid = openConnectionToHost(host, port);
	if (conid)
		return sendMessageToConnection(msg, conid);
	else
		return false;
}

bool Networking::sendMessageToConnection(DataMessage* msg, uint64 conid) {
	NetworkChannel* channel = manager->getConnection(conid);
	if (!channel)
		return false;
	return channel->sendMessage(msg, conid);
}


bool Networking::sendMessage(DataMessage* msg, bool deleteWhenDone) {
	if (!node) {
		if (deleteWhenDone)
			delete msg;
		return false;
	}
	return sendMessageToNode(node->getNodeID(msg->getTo()), msg, deleteWhenDone);
}

bool Networking::sendMessageToNode(uint16 id, PsyType controltype) {
	
	DataMessage* msg;
	if (controltype == CTRL_PING) {
		LogPrint(localNodeID, LOG_NETWORK, 3, "Sending ping to Node %u...", id);
		msg = new DataMessage(CTRL_PING, localNodeID);
		msg->setTime("Instance", localNodeEntry->instanceID);
		return sendMessageToNode(id, msg, true);
	}
	else if (controltype == CTRL_NODE_WELCOME) {
		LogPrint(localNodeID, LOG_NETWORK, 9, "Sending welcome to Node %u...", id);
		nodeEntriesMutex.enter();
		NodeEntry* configNodeEntry;
		if (localNodeEntry->configNodeID == localNodeID)
			configNodeEntry = localNodeEntry;
		else {
			if (!(configNodeEntry = nodeEntries[localNodeEntry->configNodeID])) {
				nodeEntriesMutex.leave();
				return false;
			}
		}

		msg = new DataMessage(CTRL_NODE_WELCOME, localNodeID);
		msg->setInt("AssignedNodeID", id);
		msg->setInt("ConfigNodeID", localNodeEntry->configNodeID);
		msg->setString("ConfigNodeAddress", configNodeEntry->addr);
		msg->setData("ConfigNodeIPAddresses", (char*)(configNodeEntry->ipAddresses), configNodeEntry->ipAddressCount*sizeof(uint32));
		msg->setInt("ConfigNodePort", configNodeEntry->port);
		msg->setString("RemoteNodeAddress", localNodeEntry->addr);
		msg->setData("RemoteNodeIPAddresses", (char*)(localNodeEntry->ipAddresses), localNodeEntry->ipAddressCount*sizeof(uint32));
		msg->setInt("RemoteNodePort", localNodeEntry->port);
		msg->setTime("Instance", localNodeEntry->instanceID);
		nodeEntriesMutex.leave();
		return sendMessageToNode(id, msg, true);
	}
	else if (controltype == CTRL_NODE_STATUSMAP) {
		LogPrint(localNodeID, LOG_NETWORK, 8, "Sending heartbeat to Node %u...", id);
		msg = createHeartbeatMessage();
		return sendMessageToNode(id, msg, true);
	}
	else {
		msg = new DataMessage(controltype, localNodeID);
		msg->setTime("Instance", localNodeEntry->instanceID);
		return sendMessageToNode(id, msg, true);
	}
}

bool Networking::sendMessageToNode(uint16 id, DataMessage* msg, bool deleteWhenDone) {

	NodeEntry* entry = getNodeEntry(id);
	if (!entry)
		return false;

	uint16 sendTo;
	if (!calcViaNodeID(id, sendTo))
		sendTo = id;
	if (sendTo != id) {
		if (!(entry = getNodeEntry(sendTo)))
			return false;
	}

	uint64 now = GetTimeNow();

	// If we are not forwarding the message
	if ((msg->getDestination() != id) || !msg->getOrigin()) {
		msg->setSendTime(now);
		msg->setOrigin(localNodeID);
		msg->setDestination(id);
	}

	bool msgSent = false;
	if (msg->getPolicy() & MESSAGE_NON_GUARANTEED)
		msgSent = manager->sendUDPMessage(msg, entry->location);

	uint64 location;
	if (!msgSent) {
		NetworkChannel* channel = manager->getConnection(entry->conid);
		if (!channel) {
			// Try to reconnect
			uint64 conid;
			if (entry->location && (channel = manager->createTCPConnection(entry->location, NOENC, PROTOCOL_MESSAGE, true, true, 0, this, conid))) {
				entry->conid = conid;
				entry->constatus = CONNECTION_INITIALISING;
			}
			else if (entry->ipAddressCount && (channel = manager->createTCPConnection(entry->ipAddresses, entry->ipAddressCount, entry->port, NOENC, PROTOCOL_MESSAGE, true, true, 0, this, conid, location))) {
				entry->location = location;
				entry->conid = conid;
				entry->constatus = CONNECTION_INITIALISING;
			}
			else if (channel = manager->createTCPConnection(entry->addr, entry->port, NOENC, PROTOCOL_MESSAGE, true, true, 0, this, conid, location)) {
				entry->location = location;
				entry->conid = conid;
				entry->constatus = CONNECTION_INITIALISING;
			}
			else {
				entry->conid = 0;
				entry->status = CONNECTION_DISCONNECTED;
				return false;
			}

			//if (entry->type <= NODE_REQUIRED)
			//	LogPrint(0, 0, 0, "Required Node '%s' reconnected...", entry->name);
			//else
			//	LogPrint(0, 0, 0, "AdHoc Node '%s' connected...", entry->name);
		}

		if (!channel->sendMessage(msg, entry->conid))
			return false;
		entry->constatus = CONNECTION_READY;
	}
	LogPrint(localNodeID,LOG_NETWORK,5, "Sent message type %s to Node %u", node->manager->typeToText(msg->getType()).c_str(), id);

//	printf("************ Net Send %lld us old...\n", GetTimeAge(msg->getCreatedTime()));
	addToConnectionStats(sendTo, 0, msg->getSize(), now);
	entry->lastContact = now;

	if (deleteWhenDone)
		delete(msg);

	return true;
}

uint16 Networking::sendMessageToAllOtherNodes(PsyType controltype) {
	uint16 count = 0;
	nodeEntriesMutex.enter();
	std::map<uint16, NodeEntry*>::iterator it, itEnd;
	for (it = nodeEntries.begin(), itEnd = nodeEntries.end(); it != itEnd; it++) {
		if (it->first != localNodeID) {
			if (sendMessageToNode(it->first, controltype))
				count++;
		}
	}
	nodeEntriesMutex.leave();
	return count;
}

uint16 Networking::sendMessageToAllOtherNodes(DataMessage* msg, bool deleteWhenDone) {
	uint16 count = 0;
	std::map<uint16, NodeEntry*>::iterator it, itEnd;
	for (it = nodeEntries.begin(), itEnd = nodeEntries.end(); it != itEnd; it++) {
		if (it->first != localNodeID) {
			if (sendMessageToNode(it->first, msg, false))
				count++;
		}
	}
	if (deleteWhenDone)
		delete(msg);
	return count;
}

uint16 Networking::sendMessageToNodes(uint16* ids, uint16 count, DataMessage* msg, bool deleteWhenDone) {
	uint16 c = 0;
	for (uint16 n=0; n<count; n++) {
		if (sendMessageToNode(ids[n], msg, false))
			c++;
	}
	if (deleteWhenDone)
		delete(msg);
	return c;
}

DataMessage* Networking::waitForIncomingMessage(uint32 timeout) {

	uint64 start = GetTimeNow();
	int32 spent = 0;

	while (inputMessageQueue.empty()) {
		inputMessageQueueSemaphore.wait(timeout - spent);
		if ( inputMessageQueue.empty() && ( (spent = GetTimeAgeMS(start)) > (int32)timeout) )
			return NULL;
	}

	DataMessage* msg = inputMessageQueue.front();
	inputMessageQueue.pop();

	if (!msg->isValid()) {
		delete(msg);
		return NULL;
	}

	return msg;
}


bool Networking::updateAllConnectionStats() {
	std::map<uint16, NodeEntry*>::iterator it, itEnd;
	for (it = nodeEntries.begin(), itEnd = nodeEntries.end(); it != itEnd; it++)
		if (it->second)
			updateConnectionStats(it->second);
	return true;
}

bool Networking::updateConnectionStats(NodeEntry* entry) {

	if (entry == NULL)
		return false;

	// {1,10,60,All} sec
	MovingAverage* nodeStatIn = nodeInStats[entry->nodeID];
	MovingAverage* nodeStatOut = nodeOutStats[entry->nodeID];

	uint64 count;
	if (nodeStatIn) {
		nodeStatIn->getSum(1000, entry->dataInBytes[0], count);
		nodeStatIn->getSum(10000, entry->dataInBytes[1], count);
		nodeStatIn->getSum(60000, entry->dataInBytes[2], count);
		nodeStatIn->getTotal(entry->dataInBytes[3], count);
	}
	if (nodeStatOut) {
		nodeStatOut->getSum(1000, entry->dataOutBytes[0], count);
		nodeStatOut->getSum(10000, entry->dataOutBytes[1], count);
		nodeStatOut->getSum(60000, entry->dataOutBytes[2], count);
		nodeStatOut->getTotal(entry->dataOutBytes[3], count);
	}

	return true;
}

bool Networking::addToConnectionStats(uint16 id, uint32 inBytes, uint32 outBytes, uint64 time) {
	
	MovingAverage* stat;

	if (inBytes > 0) {
		if (id != localNodeID) {
			if (!(stat = nodeInStats[id])) {
				stat = new MovingAverage();
				nodeInStats[id] = stat;
			}
			stat->add(inBytes, 1, time);
		}
		if (!(stat = nodeInStats[localNodeID])) {
			stat = new MovingAverage();
			nodeInStats[localNodeID] = stat;
		}
		stat->add(inBytes);
	}

	if (outBytes > 0) {
		if (id != localNodeID) {
			if (!(stat = nodeOutStats[id])) {
				stat = new MovingAverage();
				nodeOutStats[id] = stat;
			}
			stat->add(outBytes, 1, time);
		}
		if (!(stat = nodeOutStats[localNodeID])) {
			stat = new MovingAverage();
			nodeOutStats[localNodeID] = stat;
		}
		stat->add(outBytes);
	}

	return true;
}

uint8 Networking::queryNode(uint16 otherNodeID, DataMessage* msg, uint32 timeout, bool deleteWhenDone, DataMessage** replyMsg) {
	if (this->localNodeID == otherNodeID)
		return SYNC_FAILED;

	// Create sync id
	uint32 syncID = replyCheck.newEntry();
	LogPrint(localNodeID,LOG_SYNC,5,"Starting syncNodes (id %u)...", syncID);
	msg->setTag(syncID);

	replyCheck.addReplier(syncID, otherNodeID);
	if (!sendMessageToNode(otherNodeID, msg, deleteWhenDone))
		return SYNC_FAILED;

	LogPrint(localNodeID,LOG_SYNC,5,"Waiting syncNodes (id %u)...", syncID);

	// Wait for all other nodes to reply back
	std::map<uint16, DataMessage*> replyMap;
	uint8 result = replyCheck.waitForReplies(syncID, timeout, &replyMap);

	DataMessage* tmsg;
	if (replyMsg && (tmsg = replyMap[otherNodeID])) {
		*replyMsg = tmsg;
		replyMap[otherNodeID] = NULL;
	}

	std::map<uint16, DataMessage*>::iterator i = replyMap.begin(), e = replyMap.end();
	while (i != e) {
		delete(i->second);
		i++;
	}

	if (result == SYNC_SUCCESS)
		LogPrint(localNodeID,LOG_SYNC,5,"Success syncNodes (id %u)...", syncID);
	else if (result == SYNC_FAILED)
		LogPrint(localNodeID,LOG_SYNC,0,"Failed syncNodes (id %u)...", syncID);
	else if (result == SYNC_TIMEOUT)
		LogPrint(localNodeID,LOG_SYNC,0,"Timeout syncNodes (id %u)...", syncID);
	else
		LogPrint(localNodeID,LOG_SYNC,0,"?????? syncNodes (id %u)...", syncID);

	return result;
}

uint8 Networking::syncNodes(DataMessage* msg, uint32 timeout, bool deleteWhenDone, std::map<uint16, DataMessage*>* replyMap) {

	if (nodeEntries.size() <= 1) {
		if (deleteWhenDone)
			delete(msg);
		return SYNC_SUCCESS;
	}

	// Create sync id
	uint32 syncID = replyCheck.newEntry();
	LogPrint(localNodeID,LOG_SYNC,5,"Starting syncNodes (id %u)...", syncID);
	msg->setTag(syncID);
	// Add all other node IDs as we send the message
	std::map<uint16, NodeEntry*>::iterator it, itEnd;
	for (it = nodeEntries.begin(), itEnd = nodeEntries.end(); it != itEnd; it++) {
		if ( it->first && it->second && ((it->first != localNodeID))) {
			replyCheck.addReplier(syncID, it->first);
			if (!sendMessageToNode(it->first, msg, false))
				// ########################
				if (it->second->type <= NODE_REQUIRED)
					return SYNC_ERROR;
		}
	}
	if (deleteWhenDone)
		delete(msg);

	LogPrint(localNodeID,LOG_SYNC,5,"Waiting syncNodes (id %u)...", syncID);

	// Wait for all other nodes to reply back
	uint8 result = replyCheck.waitForReplies(syncID, timeout, replyMap);

	if (result == SYNC_SUCCESS)
		LogPrint(localNodeID,LOG_SYNC,5,"Success syncNodes (id %u)...", syncID);
	else if (result == SYNC_FAILED)
		LogPrint(localNodeID,LOG_SYNC,0,"Failed syncNodes (id %u)...", syncID);
	else if (result == SYNC_TIMEOUT) {
		uint32 total, successCount, failedCount;
		replyCheck.getReplyStatus(syncID, total, successCount, failedCount);
		LogPrint(localNodeID,LOG_SYNC,0,"Timeout syncNodes (id %u): Total: %u, Success: %u, Failed: %u, Timed out: %u...",
			syncID, total, successCount, failedCount, total-successCount-failedCount);
	}
	else
		LogPrint(localNodeID,LOG_SYNC,0,"Error (%u) syncNodes (id %u)...", result, syncID);

	return result;
}

uint8 Networking::syncNode(uint16 nodeID, DataMessage* msg, uint32 timeout, bool deleteWhenDone, std::map<uint16, DataMessage*>* replyMap) {

	if (nodeEntries.size() <= 1) {
		if (deleteWhenDone)
			delete(msg);
		return SYNC_SUCCESS;
	}

	// Create sync id
	uint32 syncID = replyCheck.newEntry();
	LogPrint(localNodeID, LOG_SYNC, 5, "Starting syncNode (id %u, nodeID %u)...", syncID, nodeID);
	msg->setTag(syncID);
	// Add all other node IDs as we send the message
	std::map<uint16, NodeEntry*>::iterator it, itEnd;
	replyCheck.addReplier(syncID, nodeID);
	if (!sendMessageToNode(it->first, msg, false))
		if (it->second->type <= NODE_REQUIRED)
			return SYNC_ERROR;
	if (deleteWhenDone)
		delete(msg);

	LogPrint(localNodeID, LOG_SYNC, 5, "Waiting syncNode (id %u, nodeID %u)...", syncID, nodeID);

	// Wait for all other nodes to reply back
	uint8 result = replyCheck.waitForReplies(syncID, timeout, replyMap);

	if (result == SYNC_SUCCESS)
		LogPrint(localNodeID, LOG_SYNC, 5, "Success syncNode (id %u, nodeID %u)...", syncID, nodeID);
	else if (result == SYNC_FAILED)
		LogPrint(localNodeID, LOG_SYNC, 0, "Failed syncNode (id %u, nodeID %u)...", syncID, nodeID);
	else if (result == SYNC_TIMEOUT) {
		uint32 total, successCount, failedCount;
		replyCheck.getReplyStatus(syncID, total, successCount, failedCount);
		LogPrint(localNodeID, LOG_SYNC, 0, "Timeout syncNode (id %u, nodeID %u): Total: %u, Success: %u, Failed: %u, Timed out: %u...",
			syncID, nodeID, total, successCount, failedCount, total - successCount - failedCount);
	}
	else
		LogPrint(localNodeID, LOG_SYNC, 0, "Error (%u) syncNode (id %u, nodeID %u)...", result, syncID, nodeID);
	return result;
}


DataMessage* Networking::waitForOutgoingMessage(uint32 timeout){

	if (outputMessageQueue.empty()) {
		outputMessageQueueSemaphore.wait(timeout);
		if (outputMessageQueue.empty())
			return NULL;
	}

	//uint64 start = GetTimeNow();
	//int32 spent = 0;
	//while (outputMessageQueue.empty()) {
	//	outputMessageQueueSemaphore.wait(timeout - spent);
	//	if ( outputMessageQueue.empty() && ( (spent = GetTimeAgeMS(start)) > (int32)timeout) )
	//		return NULL;
	//}

	DataMessage* msg = outputMessageQueue.front();
	outputMessageQueue.pop();

	if (!msg->isValid()) {
		delete(msg);
		return NULL;
	}
	return msg;
}


DataMessage* Networking::createHeartbeatMessage() {
	nodeEntriesMutex.enter();

	// Create heartbeat data
	uint32 size = sizeof(NodeMap) + (uint32)nodeEntries.size() * sizeof(NodeEntry);
	char* data = new char[size];
	NodeMap* map = (NodeMap*) data;
	// Copy data
	map->size = size;
	map->cid = NODEMAPID;
	map->count = 0; // counting below
	map->time = GetTimeNow();
	map->nodeID = localNodeID;
	map->timesyncID = timesyncNodeID;
	map->instanceID = localNodeEntry->instanceID;
	map->nodeStatus = localNodeEntry->status;

	NodeEntry* srcEntry, *destEntry = (NodeEntry*)(data+sizeof(NodeMap));
	std::map<uint16, NodeEntry*>::iterator it = nodeEntries.begin(), itEnd = nodeEntries.end();
	while (it != itEnd) {
		if (srcEntry = it->second)
			memcpy(destEntry, srcEntry, sizeof(NodeEntry));
		else
			memset(destEntry, 0, sizeof(NodeEntry));
		map->count++;
		destEntry++;
		it++;
	}

	nodeEntriesMutex.leave();

	// Send data to all Nodes
//	DataMessage* msg = new DataMessage(CTRL_NODE_STATUSMAP, localNodeID, 0, 0, GetTimeNow(), 0, 0);
	DataMessage* msg = new DataMessage(CTRL_NODE_STATUSMAP, localNodeID);
	msg->setData("NodeStatusMap", data, size);
	msg->setTime("Instance", localNodeEntry->instanceID);
	delete [] data;
	return msg;
}

bool Networking::sendHeartbeat() {
	LogPrint(localNodeID, LOG_NETWORK, 8, "Sending heartbeat...");
	DataMessage* msg = createHeartbeatMessage();
	sendMessageToAllOtherNodes(msg, true);
	return true;
}

bool Networking::startTimesync(uint32 n, uint64 total, uint64 msgtime) {
	if (timesyncNodeID == localNodeID)
		return true;

	// Initiate time sync protocol
	TimeSyncData* data = new TimeSyncData;
	memset(data, 0, sizeof(TimeSyncData));
	data->cid = TIMESYNCDATA;
	data->size = sizeof(TimeSyncData);
	data->total = total;
	data->msgtime = msgtime;
	data->count = n;
	data->t1 = GetTimeNow();

	DataMessage* msg = new DataMessage(CTRL_NODE_TIMESYNC, localNodeID);
	msg->setTime("Instance", localNodeEntry->instanceID);
	msg->setData("TimeSyncData", (char*)data, data->size);
	delete(data);
	if (sendMessageToNode(timesyncNodeID, msg, true)) {
		LogPrint(localNodeID, LOG_NETWORK, 3, "Timesync step %u with node %u", n, timesyncNodeID);
		return true;
	}
	else {
		LogPrint(localNodeID, LOG_NETWORK, 0, "Couldn't send timesync step %u with node %u", n, timesyncNodeID);
		return false;
	}

	// Reply received and handled in receiveMessage()
}

bool Networking::adjustTimesync(int64 amount) {
	SetCurrentNetSyncDif(amount);
	return true;
}



	//	DataMessage* msg = new DataMessage(CTRL_NODE_GREETING, localNodeID);
	//	msg->setTime("Instance", instanceID);
	//	uint32 size = sizeof(NodeMap) + ((uint32)nodeEntries.size() * sizeof(NodeEntry));
	//	NodeMap* map = (NodeMap*) malloc(size);
	//	entry = (NodeEntry*) (((char*)map) + sizeof(NodeMap));
	//	map->cid = NODEMAP;
	//	map->time = GetTimeNow();
	//
	//	uint32 count = 0;
	//	std::map<uint16, NodeEntry*>::iterator it, itEnd;
	//	for (it = nodeEntries.begin(), itEnd = nodeEntries.end(); it != itEnd; it++) {
	//		if (it->second) {
	//			*entry++ = *it->second;
	//			count++;
	//		}
	//	}
	//	map->count = count;
	//	map->size = sizeof(NodeMap) + (count * sizeof(NodeEntry));
	//	msg->setData("NodeMap", map, map->size);
	//	free(map);

	//	uint64 conid = 0;
	//	NetworkChannel* channel;
	//	NodeCon* con;
	//	for (it = nodeEntries.begin(), itEnd = nodeEntries.end(); it != itEnd; it++) {
	//		if (it->first && (it->first != localNodeID) && it->second) {
	//			con = new NodeCon;
	//			memset(con, 0, sizeof(NodeCon));
	//			con->nodeID = it->second->nodeID;
	//			con->location = it->second->location;
	//			con->timeout = it->second->timeout;
	//			con->type = it->second->type;
	//			con->status = CONNECTION_IDLE;
	//			nodeCons[con->nodeID] = con;

	//			if (channel = manager->createTCPConnection(it->second->addr, it->second->port, PROTOCOL_MESSAGE, true, true, 0, this, conid)) {
	//				con->lastContact = GetTimeNow();
	//				con->conid = conid;
	//				con->status = CONNECTION_CONNECTED;
	//				LogPrint(0, 0, 0, "Connected to Node %u '%s'...", it->second->nodeID, it->second->name);
	//				msg->setOrigin(localNodeID);
	//				msg->setTo(con->nodeID);
	//				if (!channel->sendMessage(msg, conid)) {
	//					con->conid = 0;
	//					con->status = CONNECTION_ERROR;
	//				}
	//			}
	//			else {
	//				con->status = CONNECTION_DISCONNECTED;
	//			}
	//		}
	//	}

	//	delete(msg);
	//}


		//DataMessage* msg = new DataMessage(CTRL_PING, localNodeID);
		//msg->setTime("Instance", instanceID);

		//if (!con->conid) {
		//	if (channel = manager->createTCPConnection(entry->addr, entry->port, PROTOCOL_MESSAGE, true, true, 0, this, conid)) {
		//		con->lastContact = GetTimeNow();
		//		con->conid = conid;
		//		con->status = CONNECTION_CONNECTED;
		//	}
		//	else {
		//		con->status = CONNECTION_DISCONNECTED;
		//		channel = NULL;
		//	}
		//}
		//else
		//	channel = manager->getConnection(con->conid);

		//if (channel && (con->status >= CONNECTION_CONNECTED)) {
		//	msg->setOrigin(localNodeID);
		//	if (!channel->sendMessage(msg, con->conid)) {
		//		con->conid = 0;
		//		con->status = CONNECTION_ERROR;
		//	}
		//}


bool Networking::run() {

	std::map<uint16, NodeEntry*>::iterator it, itEnd;
	DataMessage* msg = NULL;
	isRunning = true;

	uint64 lastHeartbeat = 0;
	uint64 lastTimesync = GetTimeNow();
	uint64 lastTimestep = 0;
	uint64 lastPing = 0;
	uint64 lastConCheck = 0;

	uint16 localNodeStatus;

	bool allConnected = false;

	while (shouldContinue) {

		if (!node)
			return true;

		if (!localNodeEntry || (nodeEntries.size() <= 1)) {
			utils::Sleep(50);
			continue;
		}
		
		localNodeStatus = getLocalNodeStatus();
		// Connection checks
		if (!allConnected && (localNodeStatus == NODESTATUS_MAPPING)) {
			allConnected = true;
			nodeEntriesMutex.enter();
			for (it = nodeEntries.begin(), itEnd = nodeEntries.end(); it != itEnd; it++) {
				if ((it->first != localNodeID) && it->second && (it->second->constatus <= CONNECTION_DISCONNECTED) ) {
					if (sendMessageToNode(it->second->nodeID, CTRL_NODE_WELCOME)) {
						if (it->second->type <= NODE_REQUIRED)
							LogPrint(localNodeID,LOG_NETWORK,1, "Required Node '%s' connected...", it->second->name);
						else
							LogPrint(localNodeID,LOG_NETWORK,1, "AdHoc Node '%s' connected...", it->second->name);
					}
					else
						if (it->second->type <= NODE_REQUIRED)
							allConnected = false;
				}
			}
			nodeEntriesMutex.leave();
			if (allConnected) {
				// localNodeEntry->status = NODESTATUS_NETREADY;
				// Great
			}
			else {
				utils::Sleep(50);
				continue;
			}
		}

		if (localNodeStatus >= NODESTATUS_NETREADY) {
			// Send status heartbeat
			if (!lastHeartbeat || (GetTimeAgeMS(lastHeartbeat) > (int32)heartbeatInterval)) {
				sendHeartbeat();
				lastHeartbeat = GetTimeNow();
			}
		}

		if (localNodeStatus == NODESTATUS_READY) {
			// Check output queue and send
			if (msg = waitForOutgoingMessage(50))
				sendMessage(msg, true);

			// Gradual TimeSync update
			if (!timeAdjustRemain && (!lastTimestep || GetTimeAgeMS(lastTimestep) > (int32)timestepInterval)) {
				if (timeAdjustRemain > timeStepSize) {
					// Adjust time with timeStepSize
					if (!adjustTimesync(timeStepSize)) {
						LogPrint(localNodeID,LOG_NETWORK,0, "Unable to adjust timesync");
						timeAdjustRemain = 0;
					}
					timeAdjustRemain -= timeStepSize;
				}
				else {
					// Adjust time with timeAdjustRemain
					if (!adjustTimesync(timeAdjustRemain))
						LogPrint(localNodeID,LOG_NETWORK,0, "Unable to adjust timesync");
					timeAdjustRemain = 0;
					timeStepSize = 0;
				}
				lastTimestep = GetTimeNow();
			}

			//if (!lastConCheck || GetTimeAgeMS(lastConCheck) > (int32)conCheckInterval) {
			//	for (it = nodeEntries.begin(), itEnd = nodeEntries.end(); it != itEnd; it++) {
			//		if (it->second && (it->second->status == CONNECTION_DISCONNECTED) ) {
			//			if (sendMessageToNode(it->second->nodeID, CTRL_PING)) {
			//				if (it->second->type <= NODE_REQUIRED)
			//					LogPrint(0, 0, 0, "Required Node '%s' reconnected...", it->second->name);
			//				else
			//					LogPrint(0, 0, 0, "AdHoc Node '%s' connected...", it->second->name);
			//			}
			//		}
			//	}
			//	lastConCheck = GetTimeNow();
			//}

			// Measure link to each of the other Nodes
			if (!lastPing || GetTimeAgeMS(lastPing) > (int32)pingInterval) {
				sendMessageToAllOtherNodes(CTRL_PING);
				lastPing = GetTimeNow();
			}
		}
		else {
			utils::Sleep(50);
			continue;
		}
	}
	LogPrint(localNodeID,LOG_NODE,1, "Node Networking is shutting down...");
	isRunning = false;
	return true;
}









THREAD_RET THREAD_FUNCTION_CALL NodeNetworkingRun(THREAD_ARG arg) {
	if (arg == NULL) thread_ret_val(1);
	thread_ret_val((int)(((Networking*)arg)->run() ? 0 : 1));
}




}
