

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

namespace	cmlabs{

/////////////////////////////////////////////////////////////
// SSL Encryption Protocol
/////////////////////////////////////////////////////////////

bool SSLCheckBufferForCompatibility(const char* buffer, uint32 length) {
	if (length < 14)
		return false;
	return ( utils::stristr(buffer, "ClientVersion ") == buffer );
}

bool AESCheckBufferForCompatibility(const char* buffer, uint32 length) {
	if (length < 14)
		return false;
	return ( utils::stristr(buffer, "xxxxxxxxx ") == buffer );
}


TCPListener::TCPListener() {
	CHECKNETWORKINIT

	encryption = NOENC;
	localAddress = 0;
	threadID = 0;
	lastActivity = 0;
	socket = INVALID_SOCKET;
	receiver = NULL;
	dataReceiver = NULL;
}

TCPListener::~TCPListener() {
	disconnect();
	localAddress = 0;
	threadID = 0;
	lastActivity = 0;
	socket = INVALID_SOCKET;
	receiver = NULL;
	dataReceiver = NULL;
}


bool TCPListener::disconnect(uint16 error) {
	mutex.enter();
	stop();
	shutdown(socket, SD_BOTH);
	closesocket(socket);
	socket = INVALID_SOCKET;
	if ( receiver && error )
		receiver->registerError(error, this);
	mutex.leave();
	return true;
}

bool TCPListener::isConnected() {
	return (socket != INVALID_SOCKET);
}

bool TCPListener::init(uint16 port, uint8 encryption, NetworkConnectionReceiver* receiver, NetworkDataReceiver* dataReceiver) {
	if (port == 0) {
		LogPrint(0, LOG_NETWORK, 0, "Could not start TCPListener on port 0...");
		return false;
	}

	mutex.enter();
	this->encryption = encryption;
	memcpy(((char*)&localAddress)+sizeof(uint32), &port, sizeof(uint16));
	// Open TCP port

	// Setup listening on port node->networkPort
	if((socket=::socket(AF_INET,SOCK_STREAM,IPPROTO_TCP))==INVALID_SOCKET){
		LogPrint(0, LOG_NETWORK, 0, "Could not create TCPListener socket (%d)...", utils::GetLastOSErrorNumber());
		mutex.leave();
		return false;
	}
  
	#ifdef WINDOWS
		// Set the exclusive address option, preventing other software binding to
		// non-INADDR_ANY (i.e. interface addresses such as localhost directly)
		int one = 1;
		setsockopt(socket, SOL_SOCKET, SO_EXCLUSIVEADDRUSE, (char *) &one, sizeof(one));
	#else
		/*
			This socket option tells the kernel that even if this port is busy (in
			the TIME_WAIT state), go ahead and reuse it anyway.  If it is busy,
			but with another state, you will still get an address already in use
			error.  It is useful if your server has been shut down, and then
			restarted right away while sockets are still active on its port.  You
			should be aware that if any unexpected data comes in, it may confuse
			your server, but while this is possible, it is not likely.
		*/
		int one = 1;
		setsockopt(socket,SOL_SOCKET,SO_REUSEADDR,&one,sizeof(one));
	#endif

	struct	sockaddr_in	addr;
	addr.sin_family= AF_INET;
	addr.sin_addr.s_addr=INADDR_ANY;
	addr.sin_port=htons(port);

	if(bind(socket,(SOCKADDR*)&addr,sizeof(struct sockaddr_in))==SOCKET_ERROR){
		LogPrint(0, LOG_NETWORK, 0, "Could not create TCPListener on port %u...", port);
		disconnect();
		mutex.leave();
		return false;
	}

	// Set blocking mode
	utils::SetSocketNonBlockingMode(socket);

	if(listen(socket,SOMAXCONN)==SOCKET_ERROR){
		disconnect();
		mutex.leave();
		return false;
	}

	if (receiver != NULL) {
		this->receiver = receiver;
		// Start networking thread
		if (!ThreadManager::CreateThread(TCPListenerRun, this, threadID)) {
			LogPrint(0, LOG_NETWORK, 0, "Could not start TCPListener thread...");
			disconnect();
			mutex.leave();
			return false;
		}
	}
	this->dataReceiver = dataReceiver;

	mutex.leave();
	return true;
}

uint64 TCPListener::getLocalAddress() {
	return localAddress;
}

bool TCPListener::setSSLCertificate(const char* sslCertPath, const char* sslKeyPath) {
	this->sslCertPath = sslCertPath;
	this->sslKeyPath = sslKeyPath;
	return true;
}

NetworkConnection* TCPListener::acceptConnection(uint32 timeout) {
	SOCKET new_sock;
	TCPConnection* newCon;
	#ifdef _USE_SSL_
		SSLConnection* sslCon;
	#endif // _USE_SSL_
	uint64 start = GetTimeNow(), timespent;

	while ( (timespent = (GetTimeNow() - start)/1000) < timeout) {
		// Listen for incoming connections
		mutex.enter();
		new_sock = accept(socket, NULL, NULL);

		if ((int) new_sock < 0) {
			int err = utils::GetLastOSErrorNumber();
			mutex.leave();
			if ( (err == SOCKETWOULDBLOCK) || (err == SOCKETTRYAGAIN) ) {
				// No connection available right now, continue waiting
				utils::WaitForSocketReadability(socket, (uint32)(timeout-timespent));
			}
			else {
				// Socket error, disconnect and exit
				disconnect(NETWORKERROR_ACCEPT);
				return NULL;
			}
		}
		else {
			mutex.leave();
			// We have a winner!
			// Check for encryption
			#ifdef _USE_SSL_
			if (encryption == SSLENC) {
				sslCon = new SSLConnection();
				if (!sslCon->init(sslCertPath.c_str(), sslKeyPath.c_str())) {
					shutdown(new_sock, SD_BOTH);
					closesocket(new_sock);
					delete(sslCon);
					sslCon = NULL;
					continue;
				}
				if (!sslCon->connect(new_sock, localAddress, dataReceiver)) {
					delete(sslCon);
					sslCon = NULL;
					continue;
				}
				return sslCon;
			}
			else if (encryption == AESENC) {
			}
			else {
			#endif // _USE_SSL_
				lastActivity = GetTimeNow();
				newCon = new TCPConnection();
				// LogPrint(0, LOG_NETWORK, 1, "Incoming connection: %d... ", (int)new_sock);
				if (!newCon->connect(new_sock, localAddress, dataReceiver)) {
					//printf("FAILED\n");
					delete(newCon);
				}
				else {
					//printf("SUCCESS\n");
					return newCon;
				}
			#ifdef _USE_SSL_
			}
			#endif // _USE_SSL_
		}
	}
	return NULL;
}


bool TCPListener::run() {
	NetworkConnection* con;

	if (receiver == NULL)
		return false;

	isRunning = true;
	while (shouldContinue) {
		if ( (con = acceptConnection(100)) != NULL) {
			if (receiver != NULL)
				receiver->receiveNetworkConnection(con);
			else
				delete(con);
		}
	}
	isRunning = false;
	return true;
}








NetworkConnection::NetworkConnection() {
	CHECKNETWORKINIT

	type = 0;
	localAddress = 0;
	remoteAddress = 0;
	threadID = 0;
	lastActivity = 0;
	socket = INVALID_SOCKET;
	remote = false;
	bufferLen = 0;
	buffer = NULL;
	bufferContentLen = 0;
	bufferContentPos = 0;
	receiver = NULL;
	resizeBuffer(INITIALBUFFERSIZE);
	inputSpeed = outputSpeed = 0;
	inputBytes = outputBytes = 0;
}

NetworkConnection::~NetworkConnection() {
	disconnect();
	localAddress = 0;
	remoteAddress = 0;
	threadID = 0;
	lastActivity = 0;
	socket = INVALID_SOCKET;
	remote = false;
	if (buffer != NULL)
		delete(buffer);
	bufferLen = 0;
	bufferContentLen = 0;
	bufferContentPos = 0;
	receiver = NULL;
	buffer = NULL;
}

uint64 NetworkConnection::getRemoteAddress() {
	return remoteAddress;
}

bool NetworkConnection::isRemote() {
	return remote;
}

bool NetworkConnection::disconnect(uint16 error) {
//	LogPrint(0,0,0,"*** DISCONNECT ***");
	mutex.enter();
	stop();
	shutdown(socket, SD_BOTH);
	closesocket(socket);
	socket = INVALID_SOCKET;
	bufferContentLen = 0;
	bufferContentPos = 0;
	if (receiver && error)
		receiver->registerError(error, this);
	mutex.leave();
	return true;
}

bool NetworkConnection::resizeBuffer(uint32 len) {
	if (len < 128) return false;
	char* newBuffer = new char[len];
	if (bufferContentLen != bufferContentPos) {
		memcpy(newBuffer, buffer+bufferContentPos, bufferContentLen-bufferContentPos);
		bufferContentLen -= bufferContentPos;
		bufferContentPos = 0;
	}
	else {
		bufferContentLen = 0;
		bufferContentPos = 0;
	}

	if (buffer != NULL)
		delete(buffer);
	bufferLen = len;
	buffer = newBuffer;
	return true;
}

//int32 NetworkConnection::peekStream() {
//
//	mutex.enter();
//
//	// and read from the socket
//	int count = ::recvfrom(socket,buffer+bufferContentLen,bufferLen-bufferContentLen,MSG_PEEK,NULL,0);
//	if(count==SOCKET_ERROR) {
//		int err = utils::GetLastOSErrorNumber();
//		mutex.leave();
//		if ( (err == SOCKETWOULDBLOCK) || (err == SOCKETTRYAGAIN) )
//			return 0;
//		else {
//			disconnect(NETWORKERROR_RECEIVE);
//			return -1;
//		}
//	}
//	mutex.leave();
//	return count;
//}

int32 NetworkConnection::readIntoBuffer() {

	// Assume that the mutex is locked
	// LogPrint(0,0,0,"<<<<<<< READINTOBUFFER <<<<<<<<\n");

	int c = 0;
	int32 count = 0;
	// Read from the socket while data is still available
	do {
		//LogPrint(0,0,0,"<<<<<<< READINTOBUFFER RECVFROM start <<<<<<<<\n");
		c = ::recvfrom(socket,buffer+bufferContentLen,bufferLen-bufferContentLen,0,NULL,0);
	//	c = ::recv(socket,buffer+bufferContentLen,bufferLen-bufferContentLen,0);
		//LogPrint(0,0,0,"<<<<<<< READINTOBUFFER RECVFROM end <<<<<<<<\n");
		if(c==SOCKET_ERROR) {
			int err = utils::GetLastOSErrorNumber();
			if ( (err == SOCKETWOULDBLOCK) || (err == SOCKETTRYAGAIN) )
				break;
			else {
				disconnect(NETWORKERROR_RECEIVE);
				return -1;
			}
		}

		if (c) {
			count += c;
			bufferContentLen += c;

			if (bufferLen-bufferContentLen < 512) {
				if (bufferLen-bufferContentLen + bufferContentPos > 512) {
					memmove(buffer, buffer+bufferContentPos, bufferContentLen-bufferContentPos);
					bufferContentLen -= bufferContentPos;
					bufferContentPos = 0;
				}
				else
					resizeBuffer(bufferLen + INITIALBUFFERSIZE);
			}
		}
	} while (c > 0);

	#ifdef TCPCON_PRINT_DEBUG
		if (this->type == TCPCON) {
			if (count) {
				char* tmp = new char[count+1];
				memcpy(tmp, buffer+bufferContentLen-count, count);
				tmp[count] = 0;
				LogPrint(0,LOG_NETWORK,0,"<<<<<<< READINTOBUFFER <<<<<<<< TCP RECV BUF %u bytes '%s'\n", count, tmp);
				delete [] tmp;
			}
			else {
				// LogPrint(0,LOG_NETWORK,0,"<<<<<<< READINTOBUFFER <<<<<<<< NOTHING\n");
			}
		}
	#endif

	#ifdef UDPCON_PRINT_DEBUG
		if (this->type == UDPCON) {
			if (count) {
				char* tmp = new char[count+1];
				memcpy(tmp, buffer+bufferContentLen-count, count);
				tmp[count] = 0;
				LogPrint(0,LOG_NETWORK,0,"<<<<<<< READINTOBUFFER <<<<<<<< UDP RECV BUF %u bytes '%s'\n", count, tmp);
				delete [] tmp;
			}
			else {
			//	LogPrint(0,LOG_NETWORK,0,"<<<<<<< READINTOBUFFER <<<<<<<< UDP NOTHING\n");
			}
		}
	#endif

	return count;
}

bool NetworkConnection::receiveAvailable(char* data, uint32& size, uint32 maxSize, uint32 timeout, bool peek) {

	mutex.enter();
	if (maxSize > bufferLen-bufferContentLen) {
		if (bufferContentLen + bufferContentPos > maxSize) {
			memmove(buffer, buffer+bufferContentPos, bufferContentLen-bufferContentPos);
			bufferContentLen -= bufferContentPos;
			bufferContentPos = 0;
		}
		else
			resizeBuffer(bufferLen + maxSize + INITIALBUFFERSIZE);
	}

	uint64 start = GetTimeNow();
	uint32 count = 0;

	int32 c = readIntoBuffer();
	if (c < 0) {
		mutex.leave();
		return false;
	}

	if (timeout > 0) {

		// Wait for full size to be available, if not already
		int32 timespent = 0;
		if (bufferContentLen - bufferContentPos < maxSize) {
			while (true) {
				mutex.leave();
				utils::WaitForSocketReadability(socket, (uint32)(timeout-timespent));
				mutex.enter();
				if ((c = readIntoBuffer()) < 0) {
					mutex.leave();
					return false;
				}
				if (bufferContentLen - bufferContentPos >= maxSize)
					break;
				if ((timespent = GetTimeAgeMS(start)) >= (int32)timeout)
					break;
			}
		}
	}

	size = bufferContentLen - bufferContentPos;
	if (size > maxSize)
		size = maxSize;

	inputBytes += size;
	if ( size > 0 ) {
		memcpy(data, buffer+bufferContentPos, size);
		if (!peek) {
			bufferContentPos += size;
			if (bufferContentPos == bufferContentLen)
				bufferContentPos = bufferContentLen = 0;
		}
	}
	mutex.leave();
	return true;
}

bool NetworkConnection::receive(char* data, uint32 size, uint32 timeout, bool peek) {

	mutex.enter();

	uint64 start = GetTimeNow();

	// Read all available data into buffer
	//LogPrint(0,LOG_NETWORK,0,"************** receive before readIntoBuffer ******************");
	int32 c = readIntoBuffer();
	//LogPrint(0,LOG_NETWORK,0,"************** receive after readIntoBuffer ******************");
	if (c < 0)
		return false;

	// Wait for full size to be available, if not already
	uint32 count = 0;
	int32 timespent = 0;
	if (bufferContentLen - bufferContentPos < size) {
		while (true) {
			mutex.leave();
			//LogPrint(0,LOG_NETWORK,0,"************** receive before waitforsocketreadability ******************");
			utils::WaitForSocketReadability(socket, (uint32)(timeout-timespent));
			//LogPrint(0,LOG_NETWORK,0,"************** receive after waitforsocketreadability ******************");
			mutex.enter();
			//LogPrint(0,LOG_NETWORK,0,"************** receive before readIntoBuffer 2 ******************");
			if ((c = readIntoBuffer()) < 0) {
				mutex.leave();
				return false;
			}
			//LogPrint(0,LOG_NETWORK,0,"************** receive after readIntoBuffer 2 ******************");
			if (bufferContentLen - bufferContentPos >= size)
				break;
			if ((timespent = GetTimeAgeMS(start)) >= (int32)timeout) {
				mutex.leave();
				return false;
			}
		}
	}

	// LogPrint(0,LOG_NETWORK,0,"Receiving %u (buffer %u) bytes via network...", size, bufferContentLen - bufferContentPos);
	//printf("**************** Buffer ok (%u, %u = %u->%u), left (%u), size (%u), next %u, %u ****************\n",
	//	bufferLen, bufferContentLen - bufferContentPos, bufferContentPos, bufferContentLen,
	//	bufferLen-bufferContentLen+bufferContentPos, size,
	//	*(uint32*)(buffer+bufferContentPos), *(uint32*)(buffer+bufferContentPos+4));

	int64 t = GetTimeAge(start);
	if (t > 0)
		inputSpeed = (uint32)(size*1000000.0/(double)t);
	inputBytes += size;

	memcpy(data, buffer+bufferContentPos, size);
	if (!peek) {
		bufferContentPos += size;
		if (bufferContentPos == bufferContentLen)
			bufferContentPos = bufferContentLen = 0;
	}
	mutex.leave();
	return true;
}

bool NetworkConnection::discard(uint32 size) {
	mutex.enter();
	readIntoBuffer();
	if (bufferContentLen - bufferContentPos >= size) {
		bufferContentPos += size;
		if (bufferContentPos == bufferContentLen)
			bufferContentPos = bufferContentLen = 0;
		inputBytes += size;
		mutex.leave();
		return true;
	}
	else {
		mutex.leave();
		return false;
	}
}

uint32 NetworkConnection::clearBuffer() {
	mutex.enter();
	readIntoBuffer();
	uint32 c = bufferContentLen - bufferContentPos;
	if (c) {
		LogPrint(0,LOG_NETWORK,2,"Cleared %u chars from buffer...", c);
	//	if (c > 1)
	//		utils::PrintBinary(buffer+bufferContentPos, c, false, "Cleared Buffer Content");
	}
	bufferContentPos = bufferContentLen = 0;
	inputBytes += c;
	mutex.leave();
	return c;
}


bool NetworkConnection::waitForDataToRead(uint32 timeout) {
	if (bufferContentLen != bufferContentPos)
		return true;
	else {
		return utils::WaitForSocketReadability(socket, timeout);
	}
}

bool NetworkConnection::waitForDataToBeWritten(uint32 timeout) {
	return utils::WaitForSocketWriteability(socket, timeout);
}


bool NetworkConnection::isConnected(int timeout) {

	if (socket == INVALID_SOCKET)
		return false;

	char peekBuffer[1];
	// First use the socket a bit
	mutex.enter();
	int res = recv(socket, peekBuffer, 1, MSG_PEEK);
	if (res > 0) {
		mutex.leave();
		return true;
	}
	else if (res < 0) {
		int err = utils::GetLastOSErrorNumber();
		// UDP needs TRYAGAIN
		if (err == SOCKETWOULDBLOCK) {
			mutex.leave();
			return true;
		}

		#ifdef WINDOWS
			else if ((type == UDPCON) && (err == SOCKETTRYAGAIN)) {
				mutex.leave();
				return true;
			}
			else if (err == WSAENOTCONN) {} // Windows needs this one...
			else if (err == WSAEOPNOTSUPP) {} // WinCE may need this one...???.
		#else
			else if (err == SOCKETTRYAGAIN) {
				mutex.leave();
				return true;
			}
		#endif //WINDOWS
		else {
			// Error will be handled by the caller
			disconnect(0);
			mutex.leave();
			return false;
		}
	//	mutex.leave();
	}
	else { // if err == 0
		// Error will be handled by the caller
		disconnect(0);
		mutex.leave();
		return false;
	}

	struct timeval tv;
	tv.tv_sec = 0;
	tv.tv_usec = 10;

	int maxfd = 0;
	fd_set wfds;
	// create a list of sockets to check for activity
	FD_ZERO(&wfds);
	// specify socket
	FD_SET(socket, &wfds);

	mutex.leave();

	#ifdef WINDOWS
		int len;
	#else
		#ifdef Darwin
			#if GCC_VERSION < 40000
				int len;
				maxfd = socket + 1;
			#else
				socklen_t len;
				maxfd = socket + 1;
			#endif // GCC_VERSION < 40000
		#else
			socklen_t len;
			maxfd = socket + 1;
		#endif
	#endif

	if (timeout > 0) {
		ldiv_t d = ldiv(timeout*1000, 1000000);
		tv.tv_sec = d.quot;
		tv.tv_usec = d.rem;
	}

	// Check for writability
	res = select(maxfd, NULL, &wfds, NULL, &tv);

	if (res <= 0)
		return false;

	//printf("wait res > 0\n");

	int error;
	len = sizeof(error);

	mutex.enter();
	if (FD_ISSET(socket, &wfds) != 0) {
		if (getsockopt(socket, SOL_SOCKET, SO_ERROR, (char*)&error, &len) != 0) {

			int wsaError = utils::GetLastOSErrorNumber();
			mutex.leave();
			if (wsaError == 0) {
				// No error, just unable to send...
				return true;
			}
			#ifdef WINDOWS
				else if (wsaError == WSAENETDOWN ) {
					return false;
				}
				else if (wsaError == WSAEFAULT  ) {
					return false;
				}
				else if (wsaError == WSAEINPROGRESS  ) {
					return false;
				}
				else if (wsaError == WSAEINVAL  ) {
					return false;
				}
				else if (wsaError == WSAENOPROTOOPT  ) {
					if (error == 0) {
						return true;
					}
				}
				else if (wsaError == WSAENOTSOCK  ) {
					return false;
				}
			#endif			
			return false;
		}
		mutex.leave();
		if (error == 0)
			return true;
	}
	else
		mutex.leave();

	return false;
}

bool NetworkConnection::run() {
	if (receiver == NULL)
		return false;

	uint32 size;
	uint32 buflen = INITIALBUFFERSIZE;
	char* myBuffer = (char*) malloc(buflen);
	if (myBuffer == NULL)
		return false;

	LogPrint(0, LOG_NETWORK, 2, "Incoming TCP connection from %u.%u.%u.%u:%u, started run...",
		GETIPADDRESSQUAD(remoteAddress), GETIPPORT(remoteAddress));

	isRunning = true;
	while (shouldContinue) {
		if (receive((char*)&size, sizeof(size), 50, true)) {

			if (buflen < size) {
				buflen = size;
				myBuffer = (char*) realloc(myBuffer, buflen);
				if (myBuffer == NULL) {
					isRunning = false;
					disconnect(NETWORKERROR_MEMORYFULL);
					return false;
				}
			}

			// Receive the full data structure
			if (!receive(myBuffer, size, 500)) {
				isRunning = false;
				free(myBuffer);
				// Error already reported & already disconnected
				return false;
			}

			if (receiver) {
				receiver->receiveData(myBuffer, size, this);
			}
		}
	}
	isRunning = false;
	free(myBuffer);
	return true;
}

uint32 NetworkConnection::getOutputSpeed() {
	return outputSpeed;
}

uint32 NetworkConnection::getInputSpeed() {
	return inputSpeed;
}

uint8 NetworkConnection::getConnectionType() {
	return type;
}










UDPConnection::UDPConnection() : NetworkConnection() {
	type = UDPCON;
	defaultReceiver = 0L;
}

UDPConnection::~UDPConnection() {
}

bool UDPConnection::initForOutputOnly() {
	// Setup socket
	mutex.enter();
	if((socket=::socket(AF_INET,SOCK_DGRAM,IPPROTO_UDP))==INVALID_SOCKET){
		LogPrint(0, LOG_NETWORK, 0, "Could not create UDPConnection socket...");
		disconnect();
		mutex.leave();
		return false;
	}
  
	#ifdef WINDOWS
		// Set the exclusive address option, preventing other software binding to
		// non-INADDR_ANY (i.e. interface addresses such as localhost directly)
		int one = 1;
		setsockopt(socket, SOL_SOCKET, SO_EXCLUSIVEADDRUSE, (char *) &one, sizeof(one));
	#else
		/*
			This socket option tells the kernel that even if this port is busy (in
			the TIME_WAIT state), go ahead and reuse it anyway.  If it is busy,
			but with another state, you will still get an address already in use
			error.  It is useful if your server has been shut down, and then
			restarted right away while sockets are still active on its port.  You
			should be aware that if any unexpected data comes in, it may confuse
			your server, but while this is possible, it is not likely.
		*/
		int one = 1;
		setsockopt(socket,SOL_SOCKET,SO_REUSEADDR,&one,sizeof(one));
	#endif

	setsockopt(socket,SOL_SOCKET,SO_BROADCAST,(char*)&one,sizeof(one));

	// Set blocking mode
	utils::SetSocketNonBlockingMode(socket);

	mutex.leave();
	return true;
}

bool UDPConnection::connect(uint16 port, NetworkDataReceiver* receiver) {
	if (port == 0) {
		LogPrint(0, LOG_NETWORK, 0, "Could not start UDPConnection on port 0...");
		return false;
	}

	memcpy(((char*)&localAddress)+sizeof(uint32), &port, sizeof(uint16));
	// Open UDP port

	// Setup listening on port node->networkPort
	mutex.enter();
	if((socket=::socket(AF_INET,SOCK_DGRAM,IPPROTO_UDP))==INVALID_SOCKET){
		LogPrint(0, LOG_NETWORK, 0, "Could not create UDPConnection socket...");
		disconnect();
		mutex.leave();
		return false;
	}
  
	#ifdef WINDOWS
		// Set the exclusive address option, preventing other software binding to
		// non-INADDR_ANY (i.e. interface addresses such as localhost directly)
		int one = 1;
		setsockopt(socket, SOL_SOCKET, SO_EXCLUSIVEADDRUSE, (char *) &one, sizeof(one));
	#else
		/*
			This socket option tells the kernel that even if this port is busy (in
			the TIME_WAIT state), go ahead and reuse it anyway.  If it is busy,
			but with another state, you will still get an address already in use
			error.  It is useful if your server has been shut down, and then
			restarted right away while sockets are still active on its port.  You
			should be aware that if any unexpected data comes in, it may confuse
			your server, but while this is possible, it is not likely.
		*/
		int one = 1;
		setsockopt(socket,SOL_SOCKET,SO_REUSEADDR,&one,sizeof(one));
	#endif

	setsockopt(socket,SOL_SOCKET,SO_BROADCAST,(char*)&one,sizeof(one));

	struct	sockaddr_in	addr;
	addr.sin_family= AF_INET;
	addr.sin_addr.s_addr=INADDR_ANY;
	addr.sin_port=htons(port);

	if(bind(socket,(SOCKADDR*)&addr,sizeof(struct sockaddr_in))==SOCKET_ERROR){
		LogPrint(0, LOG_NETWORK, 0, "Could not create UDPConnection on port %u...", port);
		disconnect();
		mutex.leave();
		return false;
	}
	LogPrint(0, LOG_NETWORK, 2, "Created UDPConnection on port %u...", port);

	// Set blocking mode
	utils::SetSocketNonBlockingMode(socket);

	if (receiver != NULL) {
		this->receiver = receiver;
		// Start networking thread
		if (!ThreadManager::CreateThread(UDPConnectionRun, this, threadID)) {
			LogPrint(0, LOG_NETWORK, 0, "Could not start UDPConnection thread...");
			disconnect();
			mutex.leave();
			return false;
		}
	}

	mutex.leave();
	return true;
}

bool UDPConnection::send(char* data, uint32 size, uint64 receiver) {
	uint64 start;
	sockaddr_in recvAddr;
	recvAddr.sin_family = AF_INET;
	int64 t;

	if (!receiver && !defaultReceiver) {
		// this should broadcast ##############
		return false;
	}
	else {
		uint64 rec = receiver ? receiver : defaultReceiver;
		uint16 p = GETIPPORT(rec);
		recvAddr.sin_port = htons(p);
		memcpy(&recvAddr.sin_addr.s_addr, &rec, 4);
		sendMutex.enter();
		start = GetTimeNow();

		uint32 pos = 0;
		int32 n;

		while (true) {
			n = ::sendto(socket, data+pos, size-pos, 0,(SOCKADDR*)&recvAddr,sizeof(sockaddr_in));
			if (n == SOCKET_ERROR) {
				// printf("--- UDP error sending reply (%u)...\n", size);
				disconnect(NETWORKERROR_SEND_ERROR);
				sendMutex.leave();
				return false;
			}
			pos += n;
			// Have we written everything?
			if (pos >= size)
				break;
			// else wait for writeability
			utils::WaitForSocketWriteability(socket, 500);
			// check for ridiculous timeout
			if ((t = GetTimeAgeMS(start)) > 10000) {
				disconnect(NETWORKERROR_SEND_TIMEOUT);
				sendMutex.leave();
				return false;
			}
			// and go again
		}
	}
	t = GetTimeAge(start);
	if (t > 0)
		outputSpeed = (uint32)(size*1000000.0/(double)t);
	outputBytes += size;
	sendMutex.leave();

	#ifdef UDPCON_PRINT_DEBUG
		if (size < 1024) {
			char* tmp = new char[size+1];
			memcpy(tmp, data, size);
			tmp[size] = 0;
			LogPrint(0,LOG_NETWORK,0,">>>>>>> SEND >>>>>>>> UDP Sent %u bytes '%s' to %u.%u.%u.%u:%u\n", size, tmp, GETIPADDRESSQUAD(receiver), GETIPPORT(receiver));
			delete [] tmp;
		}
		else {
			#ifdef UDPCON_PRINTBINARY_DEBUG
				char* tmp = new char[size+1];
				memcpy(tmp, data, size);
				tmp[size] = 0;
				LogPrint(0,LOG_NETWORK,0,">>>>>>> SEND >>>>>>>> UDP Sent %u bytes '%s'\n", size, tmp);
				delete [] tmp;
			#else
				char* tmp = new char[1024];
				memcpy(tmp, data, 1023);
				tmp[1023] = 0;
				LogPrint(0,LOG_NETWORK,0,">>>>>>> SEND >>>>>>>> UDP Sent %u bytes '%s' to %u.%u.%u.%u:%u\n", size, tmp, GETIPADDRESSQUAD(receiver), GETIPPORT(receiver));
				delete [] tmp;
			#endif
		}
	#endif

	return true;
}

bool UDPConnection::reconnect() {
	disconnect();
	return connect(GETIPPORT(localAddress), receiver);
}

bool UDPConnection::setDefaultReceiver(uint64 receiver) {
	defaultReceiver = receiver;
	return true;
}

uint64 UDPConnection::getLocalAddress() {
	return localAddress;
}













TCPConnection::TCPConnection() : NetworkConnection() {
	type = TCPCON;
}

TCPConnection::~TCPConnection() {
}

bool TCPConnection::connect(SOCKET s, uint64 localAddr, NetworkDataReceiver* receiver) {
	mutex.enter();
	bufferContentLen = 0;
	bufferContentPos = 0;
	localAddress = localAddr;

	socket = s;
	findRemoteAddress(remoteAddress);

	// Set blocking mode
	utils::SetSocketNonBlockingMode(socket);

	struct linger tmp = {1, 0};
	setsockopt(socket, SOL_SOCKET, SO_LINGER, (char *)&tmp, sizeof(tmp));
	int delay = 1;
	setsockopt(socket, IPPROTO_TCP, TCP_NODELAY, (char*) &delay, sizeof(delay));
	//char buffsize = 1;
	//setsockopt(socket, SOL_SOCKET, SO_SNDBUF, &buffsize, sizeof(buffsize));
	//buffsize = 1;
	//setsockopt(socket, SOL_SOCKET, SO_RCVBUF, &buffsize, sizeof(buffsize));

	LogPrint(0, LOG_NETWORK, 2, "Incoming TCP connection from %u.%u.%u.%u:%u, starting run...",
		GETIPADDRESSQUAD(remoteAddress), GETIPPORT(remoteAddress));

	if (receiver != NULL) {
		this->receiver = receiver;
		// Start networking thread
		if (!ThreadManager::CreateThread(TCPConnectionRun, this, threadID)) {
			LogPrint(0, LOG_NETWORK, 0, "Could not start TCPConnection thread...");
			disconnect();
			mutex.leave();
			return false;
		}
	}

	remote = true;
	mutex.leave();
	return true;
}

bool TCPConnection::connect(uint64 addr, NetworkDataReceiver* receiver) {
	mutex.enter();
	if((socket=::socket(AF_INET,SOCK_STREAM,IPPROTO_TCP))==INVALID_SOCKET){
		LogPrint(0, LOG_NETWORK, 0, "Could not create TCPListener socket (%d)...", utils::GetLastOSErrorNumber());
		mutex.leave();
		return false;
	}

	bufferContentLen = 0;
	bufferContentPos = 0;

	// Set blocking mode
	// utils::SetSocketBlockingMode(socket);

	struct sockaddr_in saServer;
	saServer.sin_family = AF_INET;
	saServer.sin_port = htons(GETIPPORT(addr));
	memcpy(&saServer.sin_addr.s_addr, &addr, 4);

	// Connect to the server
	int res;
	if ((res = ::connect(socket, (struct sockaddr*)&saServer, sizeof(struct sockaddr))) != 0) {

		int err = utils::GetLastOSErrorNumber();
		if ( (err == SOCKETWOULDBLOCK) || (err == SOCKETTRYAGAIN) ) {
			if (!isConnected(1000)) {
				disconnect();
				mutex.leave();
				return false;
			}
		}
		else {
			disconnect();
			mutex.leave();
			return false;
		}
	}

	// Set blocking mode
	utils::SetSocketNonBlockingMode(socket);

	struct linger tmp = {1, 0};
	setsockopt(socket, SOL_SOCKET, SO_LINGER, (char *)&tmp, sizeof(tmp));
	int delay = 1;
	setsockopt(socket, IPPROTO_TCP, TCP_NODELAY, (char*) &delay, sizeof(delay));
	//char buffsize = 1;
	//setsockopt(socket, SOL_SOCKET, SO_SNDBUF, &buffsize, sizeof(buffsize));
	//buffsize = 1;
	//setsockopt(socket, SOL_SOCKET, SO_RCVBUF, &buffsize, sizeof(buffsize));

	if (receiver != NULL) {
		this->receiver = receiver;
		// Start networking thread
		if (!ThreadManager::CreateThread(TCPConnectionRun, this, threadID)) {
			LogPrint(0, LOG_NETWORK, 0, "Could not start TCPConnection thread...");
			disconnect();
			mutex.leave();
			return false;
		}
	}

	remoteAddress = addr;
	remote = false;

	mutex.leave();
	return true;
}

bool TCPConnection::connect(const char* addr, uint16 port, uint64& location, NetworkDataReceiver* receiver) {
	// Resolve address
	uint32 address;
	if (!utils::LookupIPAddress(addr, address))
		return false;
	return connect(location = GETIPADDRESSPORT(address, port), receiver);
}

bool TCPConnection::connect(const uint32* addresses, uint16 addressCount, uint16 port, uint64& location, NetworkDataReceiver* receiver) {
	// Try all addresses
	for (uint16 n=0; n<addressCount; n++) {
		if (connect(location = GETIPADDRESSPORT(addresses[n], port), receiver))
			return true;
	}
	return false;
}

bool TCPConnection::send(char* data, uint32 size, uint64 receiver) {
	// Ignore receiver, only for UDP
	// ### Consider splitting large data up into smaller chunks ###
	if (!size)
		return false;
	sendMutex.enter();
	uint64 start = GetTimeNow();
//	utils::SetSocketBlockingMode(socket);

	// Charles:
			//const char *data = (const char *) tdata;
			//	IntT done = 0;
			//	do {
			//	  int n = write(fd,&(data[done]),length - done);
			//	  if(n < 0) {
			// if(errno == EAGAIN || errno == EINTR) // Recoverable error?
			//   continue;
			// return -1;
			//	  }
			//	  done += n;
			//	} while(done < length);
			//	return done;

	int64 t;
	uint32 pos = 0;
	int32 n;

	while (true) {
		n = ::send(socket, data+pos, size-pos, 0);
		if(n == SOCKET_ERROR) {
			// printf("--- TCP error sending reply (%u)...\n", size);
			disconnect(NETWORKERROR_SEND_ERROR);
			sendMutex.leave();
			return false;
		}
		pos += n;
		// Have we written everything?
		if (pos >= size)
			break;
		// else wait for writeability
		utils::WaitForSocketWriteability(socket, 1000);
		// check for ridiculous timeout
		if ((t = GetTimeAgeMS(start)) > 10000) {
			disconnect(NETWORKERROR_SEND_TIMEOUT);
			sendMutex.leave();
			return false;
		}
		// and go again
	}

//	utils::SetSocketNonBlockingMode(socket);
	utils::WaitForSocketWriteability(socket, 1000);

	t = GetTimeAge(start);
	if (t > 0)
		outputSpeed = (uint32)(size*1000000.0/(double)t);
	outputBytes += size;
	sendMutex.leave();

	#ifdef TCPCON_PRINT_DEBUG
		if (size < 1024) {
			char* tmp = new char[size+1];
			memcpy(tmp, data, size);
			tmp[size] = 0;
			LogPrint(0,LOG_NETWORK,0,">>>>>>> SEND >>>>>>>> TCP Sent %u bytes '%s'\n", size, tmp);
			delete [] tmp;
		}
		else {
			#ifdef TCPCON_PRINTBINARY_DEBUG
				char* tmp = new char[size+1];
				memcpy(tmp, data, size);
				tmp[size] = 0;
				LogPrint(0,LOG_NETWORK,0,">>>>>>> SEND >>>>>>>> TCP Sent %u bytes '%s'\n", size, tmp);
				delete [] tmp;
			#else
				char* tmp = new char[1024];
				memcpy(tmp, data, 1023);
				tmp[1023] = 0;
				LogPrint(0,LOG_NETWORK,0,">>>>>>> SEND >>>>>>>> TCP Sent %u bytes '%s'\n", size, tmp);
				delete [] tmp;
			#endif
		}
	#endif


//	utils::Sleep(100);
	return true;
}



bool TCPConnection::reconnect() {
	disconnect();
	return connect(remoteAddress, receiver);
}


bool TCPConnection::findRemoteAddress(uint64& addr) {

	if (socket == INVALID_SOCKET)
		return false;

	struct sockaddr_in remoteAddr;
	
	#ifdef WINDOWS
		int remoteAddrLen;
	#else
		#ifdef Darwin
			#if GCC_VERSION < 40000
				int remoteAddrLen;
			#else
				socklen_t remoteAddrLen;
			#endif // GCC_VERSION < 40000
		#else
			socklen_t remoteAddrLen;
		#endif
	#endif // WINDOWS

	remoteAddrLen = sizeof(struct sockaddr_in);

	if (getpeername(socket, (struct sockaddr*) &remoteAddr, &remoteAddrLen) != 0)
		return false;

	uint32 address = remoteAddr.sin_addr.s_addr;
	if ( address == LOCALHOSTIP ) {
		// Get the actual local ip address, if possible
		utils::GetLocalIPAddress(address);
	}
	addr = GETIPADDRESSPORT(address, (uint16)remoteAddr.sin_port);
	return true;
}
















bool IsSSLInitialised = false;

#ifdef _USE_SSL_
#ifdef WINDOWS
	#include "openssl/applink.c"
#endif
#endif


SSLConnection::SSLConnection() : NetworkConnection() {
	type = SSLCON;
	#ifdef _USE_SSL_
		if (!IsSSLInitialised) {
			// Init SSL
			//CRYPTO_malloc_init();
			OpenSSL_add_all_algorithms();
			ERR_load_BIO_strings();
			ERR_load_crypto_strings();
			SSL_load_error_strings();

			certbio = BIO_new(BIO_s_file());
			outbio  = BIO_new_fp(stdout, BIO_NOCLOSE);

			SSL_library_init();

			IsSSLInitialised = true;
		}
		ctx = NULL;
		ssl = NULL;
	#endif // _USE_SSL_
}

SSLConnection::~SSLConnection() {
	#ifdef _USE_SSL_
		mutex.enter();
		if (ctx) {
			SSL_CTX_free(ctx);
			ctx = NULL;
		}
		mutex.leave();
	#else // _USE_SSL_
	#endif // _USE_SSL_
}

bool SSLConnection::init(const char *certFile, const char *keyFile) {
	mutex.enter();
	#ifdef _USE_SSL_
		// Compatible with SSLv2, SSLv3 and TLSv1
		const SSL_METHOD *method = SSLv23_server_method();
		// Create new context from method.
		ctx = SSL_CTX_new(method);
		if(!ctx) {
			BIO_printf(outbio, "Unable to create a new SSL context structure.\n");
			mutex.leave();
			return false;
		}

		if ( SSL_CTX_use_certificate_chain_file(ctx, certFile) <= 0) {
			ERR_print_errors_fp(stderr);
			mutex.leave();
			return false;
		}
		if ( SSL_CTX_use_PrivateKey_file(ctx, keyFile, SSL_FILETYPE_PEM) <= 0) {
			ERR_print_errors_fp(stderr);
			mutex.leave();
			return false;
		}

		// Verify that the two keys goto together.
		if ( !SSL_CTX_check_private_key(ctx) ) {
			fprintf(stderr, "Private key is invalid.\n");
			mutex.leave();
			return false;
		}
		mutex.leave();
		return true;
	#else // _USE_SSL_
		mutex.leave();
		return false;
	#endif // _USE_SSL_
}

bool SSLConnection::init() {
	mutex.enter();
	#ifdef _USE_SSL_
		// Compatible with SSLv2, SSLv3 and TLSv1
		const SSL_METHOD *method = SSLv23_method();
		// Create new context from method.
		ctx = SSL_CTX_new(method);
		if(!ctx) {
			BIO_printf(outbio, "Unable to create a new SSL context structure.\n");
			mutex.leave();
			return false;
		}

		//SSL_CTX_set_options(ctx, SSL_OP_NO_SSLv2);

		mutex.leave();
		return true;
	#else // _USE_SSL_
		mutex.leave();
		return false;
	#endif // _USE_SSL_
}

bool SSLConnection::isConnected(int timeout) {
	
	// ######################
	//return true;
	
	//printf("ISCON1\n");fflush(stdout);

#ifdef _USE_SSL_

	if ((socket == INVALID_SOCKET) || !ssl)
		return false;

	char peekBuffer[1];
	// First use the socket a bit

	//utils::Sleep(100);

	if (!mutex.enter(200, __FUNCTION__))
		return false;
	//printf("ISCON2\n");fflush(stdout);
	int res = recv(socket, peekBuffer, 1, MSG_PEEK);
	if (res > 0) {
		mutex.leave();
		//printf("ISCON3\n");fflush(stdout);
		return true;
	}
	else if (res < 0) {
		int err = utils::GetLastOSErrorNumber();
		// UDP needs TRYAGAIN
		if (err == SOCKETWOULDBLOCK) {
			mutex.leave();
			//printf("ISCON4\n");fflush(stdout);
			return true;
		}

		#ifdef WINDOWS
			else if ((type == UDPCON) && (err == SOCKETTRYAGAIN)) {
				mutex.leave();
				//printf("ISCON5\n");fflush(stdout);
				return true;
			}
			else if (err == WSAENOTCONN) {} // Windows needs this one...
			else if (err == WSAEOPNOTSUPP) {} // WinCE may need this one...???.
		#else
			else if (err == SOCKETTRYAGAIN) {
				mutex.leave();
				return true;
			}
		#endif //WINDOWS
		else {
			// Error will be handled by the caller
			//printf("ISCON6\n");fflush(stdout);
			disconnect(0);
			mutex.leave();
			return false;
		}
	//	mutex.leave();
	}
	else { // if err == 0
		// Error will be handled by the caller
		//printf("ISCON7\n");fflush(stdout);
		disconnect(0);
		mutex.leave();
		return false;
	}

	//printf("ISCON10\n");fflush(stdout);

	struct timeval tv;
	tv.tv_sec = 0;
	tv.tv_usec = 10;

	int maxfd = 0;
	fd_set wfds;
	// create a list of sockets to check for activity
	FD_ZERO(&wfds);
	// specify socket
	FD_SET(socket, &wfds);

	mutex.leave();

	#ifdef WINDOWS
		int len;
	#else
		#ifdef Darwin
			#if GCC_VERSION < 40000
				int len;
				maxfd = socket + 1;
			#else
				socklen_t len;
				maxfd = socket + 1;
			#endif // GCC_VERSION < 40000
		#else
			socklen_t len;
			maxfd = socket + 1;
		#endif
	#endif

	if (timeout > 0) {
		ldiv_t d = ldiv(timeout*1000, 1000000);
		tv.tv_sec = d.quot;
		tv.tv_usec = d.rem;
	}

	// Check for writability
	//printf("ISCON11\n");fflush(stdout);
	res = select(maxfd, NULL, &wfds, NULL, &tv);
	//printf("ISCON12\n");fflush(stdout);

	if (res <= 0)
		return false;

	//printf("wait res > 0\n");

	int error;
	len = sizeof(error);

	mutex.enter();
	//printf("ISCON13\n");fflush(stdout);
	if (FD_ISSET(socket, &wfds) != 0) {
		if (getsockopt(socket, SOL_SOCKET, SO_ERROR, (char*)&error, &len) != 0) {

			int wsaError = utils::GetLastOSErrorNumber();
			mutex.leave();
			if (wsaError == 0) {
				// No error, just unable to send...
				return true;
			}
			#ifdef WINDOWS
				else if (wsaError == WSAENETDOWN ) {
					return false;
				}
				else if (wsaError == WSAEFAULT  ) {
					return false;
				}
				else if (wsaError == WSAEINPROGRESS  ) {
					return false;
				}
				else if (wsaError == WSAEINVAL  ) {
					return false;
				}
				else if (wsaError == WSAENOPROTOOPT  ) {
					if (error == 0) {
						return true;
					}
				}
				else if (wsaError == WSAENOTSOCK  ) {
					return false;
				}
			#endif			
			return false;
		}
		mutex.leave();
		if (error == 0)
			return true;
	}
	else
		mutex.leave();

#endif //_USE_SSL_
	return false;
}

bool SSLConnection::disconnect(uint16 error) {
	#ifdef _USE_SSL_
		mutex.enter();
		if (!ctx || !ssl)
			return false;
		NetworkConnection::disconnect(error);
		SSL_shutdown(ssl);
		SSL_free(ssl);
		ssl = NULL;
		mutex.leave();
		return true;
	#else // _USE_SSL_
		return false;
	#endif // _USE_SSL_
}

bool SSLConnection::connect(SOCKET s, uint64 localAddr, NetworkDataReceiver* receiver) {
	#ifdef _USE_SSL_
		if (!ctx || !(ssl = SSL_new(ctx)))
			return false;

		mutex.enter();
		bufferContentLen = 0;
		bufferContentPos = 0;
		localAddress = localAddr;

		socket = s;
		findRemoteAddress(remoteAddress);

		// Set blocking mode
		utils::SetSocketNonBlockingMode(socket);

		struct linger tmp = {1, 0};
		setsockopt(s, SOL_SOCKET, SO_LINGER, (char *)&tmp, sizeof(tmp));
		int delay = 1;
		setsockopt(s, IPPROTO_TCP, TCP_NODELAY, (char*) &delay, sizeof(delay));

		remote = true;
		SSL_set_fd(ssl, (int)socket);
		int ret = SSL_accept(ssl);
		int err;
		
		while (ret == -1) {
			err = SSL_get_error(ssl, ret);
			if (err == SSL_ERROR_WANT_READ || err == SSL_ERROR_WANT_WRITE) {
				utils::WaitForSocketReadability(s, 20);
				ret = SSL_accept(ssl);
			}
			else {
				disconnect();
				mutex.leave();
				return false;
			}
		}

		if (receiver != NULL) {
			this->receiver = receiver;
			// Start networking thread
			if (!ThreadManager::CreateThread(SSLConnectionRun, this, threadID)) {
				LogPrint(0, LOG_NETWORK, 0, "Could not start SSLConnection thread...");
				disconnect();
				mutex.leave();
				return false;
			}
		}

		mutex.leave();
		return true;
	#else // _USE_SSL_
		return false;
	#endif // _USE_SSL_
}

bool SSLConnection::connect(uint64 addr, NetworkDataReceiver* receiver) {
	#ifdef _USE_SSL_
		// Make client SSL connection
		if (!ctx || !(ssl = SSL_new(ctx))) {
			ERR_print_errors_fp(stdout);
			return false;
		}

		mutex.enter();
		if((socket=::socket(AF_INET,SOCK_STREAM,IPPROTO_TCP))==INVALID_SOCKET){
			LogPrint(0, LOG_NETWORK, 0, "Could not create SSLListener socket (%d)...", utils::GetLastOSErrorNumber());
			mutex.leave();
			return false;
		}

		bufferContentLen = 0;
		bufferContentPos = 0;

		struct sockaddr_in saServer;
		saServer.sin_family = AF_INET;
		saServer.sin_port = htons(GETIPPORT(addr));
		memcpy(&saServer.sin_addr.s_addr, &addr, 4);

		// Connect to the server
		int res;
		if ((res = ::connect(socket, (struct sockaddr*)&saServer, sizeof(struct sockaddr))) != 0) {

			int err = utils::GetLastOSErrorNumber();
			if ( (err == SOCKETWOULDBLOCK) || (err == SOCKETTRYAGAIN) ) {
				if (!isConnected(1000)) {
					disconnect();
					mutex.leave();
					return false;
				}
			}
			else {
				disconnect();
				mutex.leave();
				return false;
			}
		}

		struct linger tmp = {1, 0};
		setsockopt(socket, SOL_SOCKET, SO_LINGER, (char *)&tmp, sizeof(tmp));
		int delay = 1;
		setsockopt(socket, IPPROTO_TCP, TCP_NODELAY, (char*) &delay, sizeof(delay));

		// Connect the SSL struct to our connection
		if (!SSL_set_fd (ssl, (int)socket)) {
			ERR_print_errors_fp(stdout);
			disconnect();
			mutex.leave();
			return false;
		}

		//SSL_set_connect_state(ssl);
		SSL_set_mode(ssl, SSL_MODE_AUTO_RETRY);

		// Set blocking mode
		utils::SetSocketNonBlockingMode(socket);

		int ret; //, err;
		// Initiate SSL handshake
		while ( (ret = SSL_connect(ssl)) != 1) {
			switch (SSL_get_error(ssl, ret)) {
				case SSL_ERROR_WANT_READ:
					utils::WaitForSocketReadability(socket, 200);
					break;
				case SSL_ERROR_WANT_WRITE:
					utils::WaitForSocketWriteability(socket, 200);
					break;
				default:
					ERR_print_errors_fp(stderr);
					disconnect();
					mutex.leave();
					return false;
			}
			//err = SSL_get_error(ssl, ret);
			//if (err == SSL_ERROR_WANT_READ || err == SSL_ERROR_WANT_WRITE) {
			//	utils::WaitForSocketWriteability(socket, 200);
			//	if ( (ret = SSL_connect(ssl)) != 1) {
			//		ERR_print_errors_fp(stderr);
			//		disconnect();
			//		mutex.leave();
			//		return false;
			//	}
			//}
		}

		uint32 certnamemax = 1000;
		char *certname;
		X509 *cert = NULL;

		cert = SSL_get_peer_certificate(ssl);
		if (cert != NULL) {
			certname = new char[certnamemax+1];
			certinfo = X509_NAME_oneline(X509_get_subject_name(cert), certname, certnamemax);
			delete [] certname;
			X509_free(cert);
		}

		if (receiver != NULL) {
			this->receiver = receiver;
			// Start networking thread
			if (!ThreadManager::CreateThread(SSLConnectionRun, this, threadID)) {
				LogPrint(0, LOG_NETWORK, 0, "Could not start SSLConnection thread...");
				disconnect();
				mutex.leave();
				return false;
			}
		}

		remoteAddress = addr;
		remote = false;

		mutex.leave();
		return true;
	#else // _USE_SSL_
		return false;
	#endif // _USE_SSL_
}

bool SSLConnection::connect(const char* addr, uint16 port, uint64& location, NetworkDataReceiver* receiver) {
	#ifdef _USE_SSL_
		uint32 address;
		if (!utils::LookupIPAddress(addr, address))
			return false;
		return connect(location = GETIPADDRESSPORT(address, port), receiver);
	#else // _USE_SSL_
		return false;
	#endif // _USE_SSL_
}

bool SSLConnection::connect(const uint32* addresses, uint16 addressCount, uint16 port, uint64& location, NetworkDataReceiver* receiver) {
	// Try all addresses
	for (uint16 n=0; n<addressCount; n++) {
		if (connect(location = GETIPADDRESSPORT(addresses[n], port), receiver))
			return true;
	}
	return false;
}

bool SSLConnection::send(char* data, uint32 size, uint64 receiver) {
	#ifdef _USE_SSL_
		if (!size)
			return false;
		// Ignore receiver, only for UDP
		sendMutex.enter();
		if (!ssl) {
			sendMutex.leave();
			return false;
		}
		uint64 start = GetTimeNow();
		//utils::SetSocketBlockingMode(socket);

		int ret;
		while ( (ret = SSL_write(ssl, data, size)) <= 0) {
			switch (SSL_get_error(ssl, ret)) {
				case SSL_ERROR_WANT_READ:
					utils::WaitForSocketReadability(socket, 200);
					break;
				case SSL_ERROR_WANT_WRITE:
					utils::WaitForSocketWriteability(socket, 200);
					break;
				default:
					mutex.leave();
					disconnect(NETWORKERROR_RECEIVE);
					return false;
			}
		}

		if (ret < (int)size) {
			disconnect(NETWORKERROR_SEND_TIMEOUT);
			sendMutex.leave();
			return	false;
		}
		utils::WaitForSocketWriteability(socket, 1000);
		//utils::SetSocketNonBlockingMode(socket);

		int64 t = GetTimeAge(start);
		if (t > 0)
			outputSpeed = (uint32)(size*1000000.0/(double)t);
		outputBytes += size;
		sendMutex.leave();

		#ifdef TCPCON_PRINT_DEBUG
			if (size < 1024) {
				char* tmp = new char[size+1];
				memcpy(tmp, data, size);
				tmp[size] = 0;
				LogPrint(0,LOG_NETWORK,0,">>>>>>> SEND >>>>>>>> SSL Sent %u bytes '%s'\n", size, tmp);
				delete [] tmp;
			}
			else {
				#ifdef TCPCON_PRINTBINARY_DEBUG
					char* tmp = new char[size+1];
					memcpy(tmp, data, size);
					tmp[size] = 0;
					LogPrint(0,LOG_NETWORK,0,">>>>>>> SEND >>>>>>>> SSL Sent %u bytes '%s'\n", size, tmp);
					delete [] tmp;
				#endif
			}
		#endif
		return true;
	#else // _USE_SSL_
		return false;
	#endif // _USE_SSL_
}



bool SSLConnection::reconnect() {
	#ifdef _USE_SSL_
		disconnect();
		return connect(remoteAddress, receiver);
	#else // _USE_SSL_
		return false;
	#endif // _USE_SSL_
}


bool SSLConnection::findRemoteAddress(uint64& addr) {

	if (socket == INVALID_SOCKET)
		return false;

	struct sockaddr_in remoteAddr;
	
	#ifdef WINDOWS
		int remoteAddrLen;
	#else
		#ifdef Darwin
			#if GCC_VERSION < 40000
				int remoteAddrLen;
			#else
				socklen_t remoteAddrLen;
			#endif // GCC_VERSION < 40000
		#else
			socklen_t remoteAddrLen;
		#endif
	#endif // WINDOWS

	remoteAddrLen = sizeof(struct sockaddr_in);

	if (getpeername(socket, (struct sockaddr*) &remoteAddr, &remoteAddrLen) != 0)
		return false;

	uint32 address = remoteAddr.sin_addr.s_addr;
	if ( address == LOCALHOSTIP ) {
		// Get the actual local ip address, if possible
		utils::GetLocalIPAddress(address);
	}
	addr = GETIPADDRESSPORT(address, (uint16)remoteAddr.sin_port);
	return true;
}

int32 SSLConnection::peekStream() {
	#ifdef _USE_SSL_
		mutex.enter();
		if (!ssl) {
			mutex.leave();
			return -1;
		}
		// and read from the socket
		// int count = ::recvfrom(socket,buffer+bufferContentLen,bufferLen-bufferContentLen,MSG_PEEK,NULL,0);
		int count = SSL_pending(ssl);
		mutex.leave();
		return count;
	#else // _USE_SSL_
		return -1;
	#endif // _USE_SSL_
}

int32 SSLConnection::readIntoBuffer() {

	// Assume that the mutex is locked

	#ifdef _USE_SSL_

		if (!ssl)
			return -1;

		int32 count = 0;
		int pending = SSL_pending(ssl);
		if (pending <= 0)
			return 0;

		// Check for buffer resize
		if ((int32)bufferLen-(int32)bufferContentLen < pending) {
			if ((int32)bufferLen-(int32)bufferContentLen + (int32)bufferContentPos > pending) {
				memmove(buffer, buffer+bufferContentPos, bufferContentLen-bufferContentPos);
				bufferContentLen -= bufferContentPos;
				bufferContentPos = 0;
			}
			else
				resizeBuffer(bufferLen + pending + INITIALBUFFERSIZE);
		}

		// Read pending bytes from the socket
		count = SSL_read(ssl, buffer+bufferContentLen, pending);

		if (count < 0)
			return 0;
		else if (count = 0) {
			disconnect(NETWORKERROR_RECEIVE);
			return -1;
		}

		#ifdef TCPCON_PRINT_DEBUG
			char* tmp = new char[count+1];
			memcpy(tmp, buffer+bufferContentLen, count);
			tmp[count] = 0;
			LogPrint(0,LOG_NETWORK,0,"<<<<<<< SSL READINTOBUFFER <<<<<<<< TCP RECV BUF %u bytes '%s'\n", count, tmp);
			delete [] tmp;
		#endif

		bufferContentLen += count;

		return count;
	#else // _USE_SSL_
		return -1;
	#endif // _USE_SSL_
}

bool SSLConnection::receiveAvailable(char* data, uint32& size, uint32 maxSize, uint32 timeout, bool peek) {

	#ifdef _USE_SSL_
		mutex.enter();

		if (!ssl) {
			mutex.leave();
			return false;
		}

		if (maxSize > bufferLen-bufferContentLen+bufferContentPos) {
			if (bufferContentPos > maxSize) {
				memmove(buffer, buffer+bufferContentPos, bufferContentLen-bufferContentPos);
				bufferContentLen -= bufferContentPos;
				bufferContentPos = 0;
			}
			else
				resizeBuffer((bufferLen+maxSize) * 2);
		}

		uint64 start = GetTimeNow(), timespent;

		int count, err, timeleft;
		// Do we have enough data in the buffer already
		while (bufferContentLen - bufferContentPos < maxSize) {
			// and read from the socket
			count = SSL_read(ssl,buffer+bufferContentLen,bufferLen-bufferContentLen);
			if(count <= 0) {
				err = SSL_get_error(ssl, count);
				if (err == SSL_ERROR_WANT_READ || err == SSL_ERROR_WANT_WRITE) {
				}
				else {
					mutex.leave();
					disconnect(NETWORKERROR_RECEIVE);
					return	false;
				}
			}
			else if (count > 0) {
				#ifdef TCPCON_PRINT_DEBUG
					char* tmp = new char[count+1];
					memcpy(tmp, buffer+bufferContentLen, count);
					tmp[count] = 0;
					LogPrint(0,LOG_NETWORK,0,"<<<<<<< RECEIVE <<<<<<<< TCP RECV AVAIL %u bytes '%s'\n", count, tmp);
					delete [] tmp;
				#endif
				bufferContentLen += count;
				if (bufferContentLen - bufferContentPos >= maxSize)
					break;
			}
			if ((timespent = (GetTimeNow() - start)/1000) >= timeout) {
			//	if (timeout > 0)
			//		printf("**** TCP::receive available %u ***\n", bufferContentLen - bufferContentPos);
				break;
			}
			else {
				mutex.leave();
				timeleft = (int)(timeout-timespent);
				utils::WaitForSocketReadability(socket, timeleft < 50 ? timeleft : 50);
			}
			mutex.enter();
		}

		size = bufferContentLen - bufferContentPos;
		if (size > maxSize)
			size = maxSize;

		inputBytes += size;
		if ( size > 0 ) {
			memcpy(data, buffer+bufferContentPos, size);
			if (!peek) {
				bufferContentPos += size;
				if (bufferContentPos == bufferContentLen)
					bufferContentPos = bufferContentLen = 0;
			}
		}
		mutex.leave();
		return true;
	#else // _USE_SSL_
		return false;
	#endif // _USE_SSL_
}

bool SSLConnection::receive(char* data, uint32 size, uint32 timeout, bool peek) {

	#ifdef _USE_SSL_
		mutex.enter();

		if (!ssl) {
			mutex.leave();
			return false;
		}

		if (size > bufferLen-bufferContentLen+bufferContentPos) {
			if (bufferContentPos > size) {
				memmove(buffer, buffer+bufferContentPos, bufferContentLen-bufferContentPos);
				bufferContentLen -= bufferContentPos;
				bufferContentPos = 0;
			}
			else
				resizeBuffer((bufferLen+size) * 2);
		}

		uint64 start = GetTimeNow();
		int32 timespent;

		int count, err, timeleft;
		// Do we have enough data in the buffer already
		while (bufferContentLen - bufferContentPos < size) {
			if (!ssl) {
				mutex.leave();
				return false;
			}

			// and read from the socket
			
			// int count = ::recvfrom(socket,buffer+bufferContentLen,bufferLen-bufferContentLen,0,NULL,0);
			
			count = SSL_read((SSL *)ssl, buffer+bufferContentLen,size);

			if(count <= 0) {
				err = SSL_get_error(ssl, count);
				if (err == SSL_ERROR_WANT_READ || err == SSL_ERROR_WANT_WRITE) {
				}
				else {
					mutex.leave();
					disconnect(NETWORKERROR_RECEIVE);
					return false;
				}
			}
			else if (count > 0) {
				#ifdef TCPCON_PRINTBINARY_DEBUG
					char* tmp = new char[count+1];
					memcpy(tmp, buffer+bufferContentLen, count);
					tmp[count] = 0;
					LogPrint(0,LOG_NETWORK,0,"<<<<<<<< RECEIVE <<<<<<< TCP RECV %u bytes '%s'", count, tmp);
					delete [] tmp;
				#endif
				bufferContentLen += count;
				if (bufferContentLen - bufferContentPos >= size)
					break;
			}
			if ((timespent = GetTimeAgeMS(start)) >= (int32)timeout) {
			//	printf("**** TCP::receive only got %u out of %u bytes ***\n", bufferContentLen - bufferContentPos, size);
				mutex.leave();
				return false;
			}
			else {
				//utils::Sleep(5);
				//uint64 t = GetTimeNow();
				mutex.leave();
				timeleft = (int)(timeout-timespent);
				utils::WaitForSocketReadability(socket, timeleft < 50 ? timeleft : 50);
				//printf("WaitForRead:      %lu    (%lu)\n", GetTimeAgeMS(t), timeout-timespent);
			}
			mutex.enter();
		}

		int64 t = GetTimeAge(start);
		if (t > 0)
			inputSpeed = (uint32)(size*1000000.0/(double)t);
		inputBytes += size;

		memcpy(data, buffer+bufferContentPos, size);
		if (!peek) {
			bufferContentPos += size;
			if (bufferContentPos == bufferContentLen)
				bufferContentPos = bufferContentLen = 0;
		}
		mutex.leave();
		return true;
	#else // _USE_SSL_
		return false;
	#endif // _USE_SSL_
}










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

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

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

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



bool NetworkTest_TCPServer(uint16 port, uint32 count2) {
	uint32 period = 100000;

	TCPListener* listener = new TCPListener();
	if (!listener->init(port, NOENC)) {
		delete(listener);
		printf("Could not bind to port %u, exiting... \n\n", port);
		return false;
	}

	char* startdata = new char[12], *data;
	uint32 size, count;
	NetworkConnection* con;
	uint32 c = 0;
	while (true) {
		c = 0;
		printf("\n\nListening to port %u for a new connection... ", port);
		while ( (con = listener->acceptConnection(10000)) == NULL) {}

		while (con->isConnected()) {
			// Reading count and size
			if (!con->receive(startdata, 12, 10000)) {
				printf("[%u] Could not receive start data, exiting...\n", c);
				break;
			}
			if ((*(uint32*)startdata) != 123456789) {
				printf("[%u] Start data wrong, exiting...\n", c);
				break;
			}
			size = *(((uint32*)startdata)+1);
			count = *(((uint32*)startdata)+2);
			c = 0;
			data = new char[size];

			printf("Got it - starting test with size %u and count %u...\n\n", size, count);

			while (con->isConnected() && (c++ < count) ) {
				if (!con->receive(data, size, 10000)) {
					printf("[%u] Could not receive data %u, exiting...\n", port, c);
					break;
				}
				if (!con->send(data, size)) {
					printf("[%u] Could not send data %u, exiting...\n", port, c);
					break;
				}
			}
			delete [] data;
		}
		delete(con);


		//while (con->isConnected()) {
		//	t1 = GetTimeNow();
		//	if (!NetworkTest_SendReceiveData(con, data, dataLen, c, true)) {
		//		delete(con);
		//		break;
		//	}
		//	t2 = GetTimeNow();
		//	d += t2-t1;
		//	if ((c > 0) && (c % period == 0)) {
		//		printf("Round time: %.3f us...\n", (double)d/period);
		//		d = 0;
		//	}
		//	c++;
		//	if ( (count > 0) && (c > count) ) {
		//		delete(con);
		//		delete(listener);
		//		return true;;
		//	}
		//}

	}
	delete [] startdata;
	return true;
}


bool NetworkTest_TCPClient(const char* address, uint16 port, uint32 count2) {
	TCPConnection* con = new TCPConnection();
	uint64 location;
	if (!con->connect(address, port, location)) {
		delete(con);
		printf("Could not connect to '%s' on port %u, exiting...\n", address, port);
		return false;
	}
	printf("Connected to '%s' on port %u, starting test...\n", address, port);

	char* startdata = new char[12];
	memset(startdata, 0, 12);
	*((uint32*)startdata) = 123456789;

	uint32 maxSize = 1024*64, innercount = 20, count = 1000, steps = 64, s, c, step;
	char* data = new char[maxSize];
	double* vals = new double[count];
	double* avgvals = new double[steps];
	double* maxvals = new double[steps];
	double* minvals = new double[steps];
	double* stdvals = new double[steps];
	double sum, avg, mx, mn, std;

	uint64 t1, t2;

	while (con->isConnected()) {
	
		for (step=0; step<steps; step++) {
			s = (maxSize/steps*(step+1));
			*(((uint32*)startdata)+1) = s;
			*(((uint32*)startdata)+2) = count * innercount;
			if (!con->send(startdata, 12)) {
				printf("[%u] Could not send startdata, exiting...\n", port);
				break;
			}
			printf("Step %u size %u...\n", step, s);
			for (c=0; c<count; c++) {
				t1 = GetTimeNow();
				for (uint32 i=0; i<innercount; i++) {
					if (!con->send(data, s)) {
						printf("[%u] Could not send data %u, exiting...\n", port, c);
						break;
					}
					if (!con->receive(data, s, 10000)) {
						printf("[%u] Could not receive data %u, exiting...\n", port, c);
						break;
					}
				}
				t2 = GetTimeNow();
				vals[c] = (t2-t1)/(double)innercount;
			}

			sum = avg = std = 0;
			mx = mn = vals[0];
			for (c=0; c<count; c++) {
				sum += vals[c];
				if (vals[c] > mx) mx = vals[c];
				if (vals[c] < mn) mn = vals[c];
			}
			avg = sum/count;

			sum = 0;
			for (c=0; c<count; c++)
				sum += pow((vals[c] - avg), 2);
			std = sqrt(sum/count-1);

			avgvals[step] = avg;
			maxvals[step] = mx;
			minvals[step] = mn;
			stdvals[step] = std;
		}
	
		printf("Test results (size, avg, min, max, std in us):\n");
		for (step=0; step<steps; step++) {
			s = (maxSize/steps*(step+1));
			printf("%u  %u  %.3f  %.3f  %.3f  %.3f\n",
				step, s, avgvals[step], minvals[step], maxvals[step], stdvals[step]);
		}

		break;
	}

	delete [] startdata;
	delete [] data;
	delete [] vals;
	delete [] avgvals;
	delete [] maxvals;
	delete [] minvals;
	delete [] stdvals;




//	strcpy(data+(2*sizeof(uint32)), "Testing");

//	utils::PrintBinary(data, dataLen, true, "Initial Structure");

//	uint32 c = 0;
//	uint64 t1, t2, d = 0;






	//while (con->isConnected()) {
	//	t1 = GetTimeNow();
	//	if (!NetworkTest_SendReceiveData(con, data, dataLen, c)) {
	//		delete(con);
	//		return false;
	//	}
	//	t2 = GetTimeNow();
	//	d += t2-t1;
	//	if ((c > 0) && (c % period == 0)) {
	//		printf("Round time: %.3f us...\n", (double)d/period);
	//		d = 0;
	//	}
	//	c++;
	//	if ( (count > 0) && (c > count) )
	//		break;
	//}
	delete(con);
	return true;
}

bool NetworkTest_SendReceiveData(TCPConnection* con, char* data, uint32 dataLen, uint32 c, bool receiveFirst) {

	uint32 expectC = c*2;
	if (!receiveFirst) {
		expectC++;
		// Send data
		if (!con->send(data, dataLen)) {
			printf("[%u] Could not send data, exiting...\n", c);
			return false;
		}
		//utils::PrintBinary(data, dataLen, true, "Send1");
	}

	memset(data, 0, dataLen);
	// Wait for reply data
	if (!con->receive(data, dataLen, 10000)) {
		printf("[%u] Could not receive data, exiting...\n", c);
		return false;
	}		
	//utils::PrintBinary(data, dataLen, true, "Receive");
	// Check reply data
	if (*(uint32*)data != dataLen) {
		printf("[%u] Did not receive correct length (%u != %u), exiting...\n", c, *(uint32*)data, dataLen);
	//	utils::PrintBinary(data, dataLen, true, "Structure");
		return false;
	}		
	// Check reply data
	if (*((uint32*)data+1) != expectC) {
		printf("[%u] Did not receive correct count (%u != %u), exiting...\n", c, *((uint32*)data+1), expectC);
	//	utils::PrintBinary(data, dataLen, true, "Structure");
		return false;
	}		
	// Check reply data
	if (strcmp(data+(2*sizeof(uint32)), "Testing") != 0) {
		data[dataLen-1] = 0;
		printf("[%u] Did not receive correct text ('%s' != 'Testing'), exiting...\n", c, data+(2*sizeof(uint32)));
	//	utils::PrintBinary(data, dataLen, true, "Structure");
		return false;
	}
	*((uint32*)data+1) = *((uint32*)data+1) + 1;

//	printf("[%u] Receive OK\n", c);
	if (receiveFirst) {
		// Send data
		if (!con->send(data, dataLen)) {
			printf("[%u] Could not send data, exiting...\n", c);
			return false;
		}
		//utils::PrintBinary(data, dataLen, true, "Send2");
	}
	return true;
}


}
