#include "NetWorker.h"
#include <cstdio>
#include <cstdarg>
#include <cstring>
#include "SDL_net.h"
#include <signal.h>
#include <cassert>


/* Yucky, yes.  Here are NET2's queue.h and set.h */




//
// simple fixed length queue
//

#define QUEUETYPE(NAME, TYPE, SIZE)  \
  \
typedef struct  \
{  \
  int length;  \
  int head;  \
  int tail;  \
  TYPE buf[(SIZE)];  \
} NAME##Que;

#define QUEUEFORWARD(MODIFIER, NAME, TYPE)  \
  \
MODIFIER void Init##NAME##Que(NAME##Que *q);  \
MODIFIER int NAME##QueFull(NAME##Que *q);  \
MODIFIER int NAME##QueEmpty(NAME##Que *q);  \
MODIFIER int Enque##NAME(NAME##Que *q, TYPE *val);  \
MODIFIER int Deque##NAME(NAME##Que *q, TYPE *val);  \

#define QUEUECODE(MODIFIER, NAME, TYPE, SIZE)  \
  \
MODIFIER void Init##NAME##Que(NAME##Que *q)  \
{  \
  q->length = 0;  \
  q->head = 0;  \
  q->tail = 0;  \
}  \
  \
MODIFIER int NAME##QueFull(NAME##Que *q)  \
{  \
  return ((SIZE) == q->length);  \
}  \
  \
MODIFIER int NAME##QueEmpty(NAME##Que *q)  \
{  \
  return (0 == q->length);  \
}  \
  \
MODIFIER int Enque##NAME(NAME##Que *q, TYPE *val)  \
{  \
  if ((SIZE) == q->length)  \
  {  \
    return -1;  \
  }  \
  \
  q->length++;  \
  q->buf[q->tail] = *val;  \
  q->tail = ((q->tail + 1) % (SIZE));  \
    \
  return 0;  \
}  \
  \
MODIFIER int Deque##NAME(NAME##Que *q, TYPE *val)  \
{  \
  if (0 == q->length)  \
  {  \
    return -1;  \
  }  \
  \
  q->length--;  \
  *val = q->buf[q->head];  \
  q->head = (q->head + 1) % (SIZE);  \
  \
  return 0;  \
}













//
// simple set data type
//

#define SETTYPE(NAME, TYPE)  \
typedef struct  \
{  \
  int itr;  \
  int last;  \
  int size;  \
  TYPE *set;  \
} NAME##Set;

#define SETFORWARD(MODIFIER, NAME, TYPE)  \
  \
MODIFIER int init##NAME##Set(NAME##Set *s, int size);  \
MODIFIER void finit##NAME##Set(NAME##Set *s);  \
MODIFIER int member##NAME##Set(NAME##Set *s, TYPE *v);  \
MODIFIER TYPE *item##NAME##Set(NAME##Set *s, TYPE *v);  \
MODIFIER int add##NAME##Set(NAME##Set *s, TYPE *v);  \
MODIFIER int del##NAME##Set(NAME##Set *s, TYPE *v);  \
MODIFIER int first##NAME##Set(NAME##Set *s, TYPE *v);  \
MODIFIER int next##NAME##Set(NAME##Set *s, TYPE *v);  \

#define SETCODE(MODIFIER, NAME, TYPE, INCREMENT, TESTEQ)  \
  \
MODIFIER int init##NAME##Set(NAME##Set *s, int size)  \
{  \
  s->itr = 0;  \
  s->last = 0;  \
  s->size = size;  \
  s->set = malloc(size * sizeof(TYPE));  \
  if (NULL == s->set)  \
  {  \
    return -1;  \
  }  \
  \
  return 0;  \
}  \
  \
MODIFIER void finit##NAME##Set(NAME##Set *s)  \
{  \
  s->itr = 0;  \
  s->last = 0;  \
  s->size = 0;  \
  if (NULL != s->set)  \
  {  \
    free(s->set);  \
  }  \
  s->set = NULL;  \
}  \
  \
MODIFIER int member##NAME##Set(NAME##Set *s, TYPE *v)  \
{  \
  int i;  \
  for (i = 0; i < s->last; i++)  \
  {  \
    TYPE *a = v;  \
    TYPE *b = &(s->set[i]);  \
    if (TESTEQ)  \
    {  \
      return i;  \
    }  \
  }  \
  return -1;  \
}  \
  \
  \
MODIFIER TYPE *item##NAME##Set(NAME##Set *s, TYPE *v)  \
{  \
  int index = member##NAME##Set(s, v);  \
  if (-1 != index)  \
  {  \
    return &(s->set[index]);  \
  }  \
  \
  return NULL;  \
}  \
  \
MODIFIER int add##NAME##Set(NAME##Set *s, TYPE *v)  \
{  \
  TYPE *nset = NULL;  \
  int nsize = 0;  \
  int i;  \
  \
  if (-1 != member##NAME##Set(s, v))  \
  {  \
    return 0;  \
  }  \
  \
  if (s->last < s->size)  \
  {  \
    s->set[s->last] = *v;  \
    s->last++;  \
  \
    return 0;  \
  }  \
  \
  nsize = s->size + INCREMENT;  \
  nset = malloc(nsize * sizeof(TYPE));  \
  if (NULL == nset)  \
  {  \
    return -1;  \
  }  \
  \
  for (i = 0; i < s->last; i++)  \
  {  \
    nset[i] = s->set[i];  \
  }  \
  \
  free(s->set);  \
  s->set = nset;  \
  s->size = nsize;  \
  \
  return add##NAME##Set(s, v);  \
}  \
  \
MODIFIER int del##NAME##Set(NAME##Set *s, TYPE *v)  \
{  \
  int index = member##NAME##Set(s, v);  \
  if (-1 != index)  \
  {  \
    (s->last)--;  \
    s->set[index] = s->set[s->last];  \
    return 0;  \
  }  \
  \
  return -1;  \
}  \
  \
MODIFIER int first##NAME##Set(NAME##Set *s, TYPE *v)  \
{  \
  s->itr = 0;  \
  if (s->itr < s->last)  \
  {  \
    *v = s->set[s->itr];  \
    return 0;  \
  }  \
  \
  return -1;  \
}  \
  \
MODIFIER int next##NAME##Set(NAME##Set *s, TYPE *v)  \
{  \
  (s->itr)++;  \
  if (s->itr < s->last)  \
  {  \
    *v = s->set[s->itr];  \
    return 0;  \
  }  \
  \
  return -1;  \
}



using namespace std;

// Look at NET2
// I need to use a thread to wait for network events and set error messages.
// I need to wrap SDL_net functions with mutexes

/*
I want:
NetWorker::init();
NetWorker networker;
NetConnection someGuy(ip, port);
someGuy.connect();
NetData d("lhl", mylong, myshort, mylong);
someGuy.send(d);
d.free();

NetEvent net_event;
while(networker.popEvent(net_event))
{
	if(net_event.type == NW_ERROR)
	{
		printf("%s\n", net_event.message.c_str());
	}
	else if(net_event.type == NW_ACCEPT)
	{
		net_event.connection.accept();
	}
	else if(net_event.type == NW_CLOSE)
	{
		net_event.connection.close();
	}
	else if(net_event.type == NW_RECEIVE)
	{
		net_event.data.unpack("lhl", &mylong, &myshort, &mylong);
		net_event.data.free();
	}
}
someGuy.close();
networker.quit();

*/

// Connections indexed by a unique integer (the socket index)
map<int, NetWorker::ConnectionInfo> NetWorker::connections;
// Connections which have not been accepted yet, indexed by a unique integer (counting)
map<int, NetWorker::ConnectionInfo> NetWorker::pendingConnections;
// Ports that are being listened to for new connections
vector<NetSocket> NetWorker::listening;
// Event queue
NetQueue<NetEvent> NetWorker::events;
unsigned int NetWorker::maxSizeUDP = 2000;

void packi16(unsigned char *buf, unsigned int i);
void packi32(unsigned char *buf, unsigned long i);
unsigned int unpacki16(unsigned char *buf);
unsigned long unpacki32(unsigned char *buf);

long long pack754(long double f, unsigned bits, unsigned expbits);
long double unpack754(long long i, unsigned bits, unsigned expbits);

// macros for packing floats and doubles:
#define pack754_32(f) (pack754((f), 32, 8))
#define pack754_64(f) (pack754((f), 64, 11))
#define unpack754_32(i) (unpack754((i), 32, 8))
#define unpack754_64(i) (unpack754((i), 64, 11))

int NET2_TCPAcceptOn(int port);
int NET2_TCPConnectToIP(IPaddress *ip);
void NET2_TCPClose(int socket);
int NET2_TCPSend(int socket, unsigned char *buf, int len);
int NET2_TCPRead(int socket, unsigned char *buf, int len);
IPaddress *NET2_TCPGetPeerAddress(int s);
int NET2_UDPAcceptOn(int port, int size);
void NET2_UDPClose(int socket);
int PumpNetworkEvents(void *nothing);



#define maxSockets    (1024)
#define tcpQueLen     (1024)
#define udpQueLen     (tcpQueLen / (sizeof(UDPpacket *)))

//----------------------------------------
//
// handy stuff
//
#ifndef max
#define max(a,b) (((a) > (b)) ? (a) : (b))
#endif
#ifndef min
#define min(a,b) (((a) < (b)) ? (a) : (b))
#endif
#ifndef abs
#define abs(a) (((a)<0) ? -(a) : (a))
#endif
#ifndef sign
#define sign(a) (((a)<0) ? -1 : (a)>0 ? 1 : 0)
#endif

//----------------------------------------
// 
// Forward Declarations
//

__inline__ int PumpNetworkEvents(); // thread

static __inline__ int InitSockets(int incr);
static __inline__ void FinitSockets();

static __inline__ int AllocSocket(int type);
static __inline__ void FreeSocket(int socket);

static __inline__ int raw_NET2_TCPAcceptOn(int port);
static __inline__ int raw_NET2_TCPAcceptOnIP(IPaddress *ip);
static __inline__ void raw_NET2_TCPClose(int socket);
static __inline__ int raw_NET2_TCPSend(int socket, unsigned char *buf, int len);
static __inline__ int raw_NET2_TCPRead(int socket, unsigned char *buf, int len);
static __inline__ IPaddress *raw_NET2_TCPGetPeerAddress(int socket);

static __inline__ void setError(const string& err);
static __inline__ void setError(const string& err, int socket);
__inline__ int sendError(const string& err, int socket = -1);
static __inline__ int sendEvent(const NetEvent& event);

static __inline__ void lockData();
static __inline__ void unlockData();
static __inline__ void lockSDLNet();
static __inline__ void unlockSDLNet();


static __inline__ int snResolveHost(IPaddress *ip, const char *name, int port);
static __inline__ TCPsocket snTCPOpen(IPaddress *ip);

static __inline__ UDPsocket snUDPOpen(int port);
static __inline__ int snUDPSend(UDPsocket s, int c, UDPpacket *p);
static __inline__ void snUDPClose(UDPsocket s);


static __inline__ void signalRead();




  /* NET2 event types */
  enum 
  {
    NET2_ERROREVENT,

    NET2_TCPACCEPTEVENT,
    NET2_TCPRECEIVEEVENT,
    NET2_TCPCLOSEEVENT,

    NET2_UDPRECEIVEEVENT,
  };

  typedef struct
  {
    int itr;
    int last;
    int size;
    int *set;
  } SocketSet;
  
  
  typedef struct
  {
    int itr;
    int last;
    int size;
    IPaddress *set;
  } IPSet;




//----------------------------------------
// 
// Private Types
//

enum // socket types
{
  unusedSocket,        // ready for allocation

  TCPServerSocket,     // accept connections on these
  TCPClientSocket,     // send and receive data on these

  UDPServerSocket,     // accept packets on these
};

enum // socket states
{
  unusedState =  1,     // socket is not in use
  readyState  =  2,     // socket is ready for use
  dyingState  =  4,     // connection closed but input still available
  addState    =  8,     // needs to be added to the socket set
  delState    = 16,     // needs to be deleted from the socket set
};

typedef struct
{
  int len;
  Uint8 buf[tcpQueLen];
} CharQue;

QUEUETYPE(Packet, UDPpacket *, udpQueLen);            // create type PacketQue
QUEUECODE(static __inline__, Packet, UDPpacket *, udpQueLen);

typedef struct
{
  Uint8 type;
  Uint8 state;
  union
  {
    int tcpPort;
    int udpLen;
  }p;
  union
  {
    TCPsocket            tcpSocket;
    UDPsocket            udpSocket;
    SDLNet_GenericSocket genSocket;
  }s;
  union
  {
    CharQue tb;
    PacketQue ub;
  }q;
} NET2_Socket;


//----------------------------------------
// 
// Threads, mutexes, thread utils, and 
// thread safe wrappers
//

static int dataLocked = 0;
static SDL_mutex *dataLock = NULL;
static SDL_cond *dataWait = NULL;
static SDL_mutex *sdlNetLock = NULL;
static SDL_Thread *processSockets = NULL;
static int doneYet = 0;
static int waitForRead = 0;


//----------------------------------------
// 
// Static Variables
//

static int lastHeapSocket = 0;
static int freeHeapSocket = 0;

static NET2_Socket *socketHeap[maxSockets];
static SDLNet_SocketSet socketSet = NULL;

static int initialized = 0;

static UDPsocket udpSendSocket = NULL;

static string error;
static int errorSocket = -1;



static __inline__ void lockMutex(SDL_mutex* mutex)
{
  if (-1 == SDL_LockMutex(mutex))
  {
    setError(string("Failed to lock mutex (SDL: ") + SDL_GetError() + "%s)");
  }
}

static __inline__ void unlockMutex(SDL_mutex* mutex)
{
  if (-1 == SDL_UnlockMutex(mutex))
  {
    setError(string("Failed to unlock mutex (SDL: ") + SDL_GetError() + "%s)");
  }
}




NetIPv4::NetIPv4()
	: a(127), b(0), c(0), d(1)
{}
NetIPv4::NetIPv4(unsigned char a, unsigned char b, unsigned char c, unsigned char d)
	: a(a), b(b), c(c), d(d)
{}
NetIPv4::NetIPv4(const std::string& ip_address)
	: a(127), b(0), c(0), d(1)
{
	// Read string
	string temp = ip_address;
	size_t dot = temp.find_first_of(".");
	if(dot == string::npos)
		return;
	sscanf(temp.substr(dot).c_str(), "%c", &a);
	
	temp = temp.substr(dot+1, string::npos);
	
	dot = temp.find_first_of(".");
	if(dot == string::npos)
		return;
	sscanf(temp.substr(dot).c_str(), "%c", &b);
	
	temp = temp.substr(dot+1, string::npos);
	
	dot = temp.find_first_of(".");
	if(dot == string::npos)
		return;
	sscanf(temp.substr(dot).c_str(), "%c", &c);
	
	temp = temp.substr(dot+1, string::npos);
	
	dot = temp.find_first_of(".");
	if(dot == string::npos)
		return;
	sscanf(temp.substr(dot).c_str(), "%c", &d);
	
	temp = temp.substr(dot+1, string::npos);
	
}

std::string NetIPv4::toString() const
{
	char buff[16];
	sprintf(buff, "%d.%d.%d.%d", a, b, c, d);
	return buff;
}







// NetData

void countAndAlloc(NetData& d, const char* format, va_list arglist)
{
	size_t dataLength = 0;
	
	va_list ap;
	va_copy(ap, arglist);
	
	
	if(d.data == NULL)
	{
		// Count the needed space
		// First, add the size of the packet.
		d.bytes += 2;
		
		for(; *format != '\0'; format++)
		{
			switch(*format)
			{
				case 'h': // 16-bit
					d.bytes += 2;
					break;

				case 'l': // 32-bit
					d.bytes += 4;
					break;

				case 'c': // 8-bit
					d.bytes += 1;
					break;

				case 'f': // float
					d.bytes += 4;
					break;

				case 's': // string
					d.bytes += strlen(va_arg(ap, char*)) + 2; // Actual length plus two bytes which tell the length
					break;

				case 'v': // void*
					d.bytes += dataLength + 2; // Actual length plus two bytes which tell the length
					break;

				case 'o': // offset
					d.bytes += dataLength; // Skipping some bytes for later packing
					break;
					
				default:
					if (isdigit(*format)) { // track len
						dataLength = dataLength * 10 + (*format-'0');
					}
				}

				if (!isdigit(*format)) dataLength = 0;
		}
		
		va_end(ap);
		
		d.data = new unsigned char[d.bytes];
	}
}

void packIt(NetData& d, const char* format, va_list arglist)
{
	assert(d.data != NULL);
	
	// Packing (Beej)
	int h;
	int l;
	char c;
	float f;
	char *s;
	void* v;
	size_t len, dataLength = 0;
	
	
	unsigned char* buf = d.data;
	
	// First, pack the size of the packet (not including the size of the size :)
	packi16(buf, d.bytes-2);
	buf += 2;

	va_list ap;
	va_copy(ap, arglist);
	
	for(; *format != '\0'; format++)
	{
		switch(*format)
		{
			case 'h': // 16-bit
				//bytes += 2;
				h = va_arg(ap, int); // promoted
				packi16(buf, h);
				buf += 2;
				break;

			case 'l': // 32-bit
				//bytes += 4;
				l = va_arg(ap, int);
				packi32(buf, l);
				buf += 4;
				break;

			case 'c': // 8-bit
				//bytes += 1;
				c = va_arg(ap, int); // promoted
				*buf++ = (c>>0)&0xff;
				break;

			case 'f': // float
				//bytes += 4;
				f = va_arg(ap, double); // promoted
				l = pack754_32(f); // convert to IEEE 754
				packi32(buf, l);
				buf += 4;
				break;

			case 's': // string
				s = va_arg(ap, char*);
				len = strlen(s);
				//bytes += len + 2;
				packi16(buf, len);
				buf += 2;
				memcpy(buf, s, len);
				buf += len;
				break;

			case 'v': // void*
				v = va_arg(ap, void*);
				//bytes += len + 2;
				packi16(buf, dataLength);
				buf += 2;
				memcpy(buf, v, dataLength);
				buf += dataLength;
				break;

			case 'o': // offset
				buf += dataLength;  // Skip these bytes for future packing
				break;
				
			default:
				if (isdigit(*format)) { // track len
					dataLength = dataLength * 10 + (*format-'0');
				}
			}

			if (!isdigit(*format)) dataLength = 0;
	}
	
	va_end(ap);
	
}







NetData::NetData()
	: bytes(0), data(NULL)
{}
NetData::NetData(const NetData& src)
	: bytes(src.bytes), data(src.data)
{}
NetData::NetData(const char* format, ...)
	: bytes(0), data(NULL)
{
	va_list ap;
	va_start(ap, format);
	
	countAndAlloc(*this, format, ap);
	packIt(*this, format, ap);
	
	va_end(ap);
}
NetData::NetData(unsigned int bytes, unsigned char* data)
	: bytes(bytes), data(NULL)
{
	this->data = new unsigned char[bytes];
	memcpy(this->data, data, bytes);
}
NetData::~NetData()
{}



void NetData::pack(const char* format, ...)
{
	va_list ap;
	va_start(ap, format);
	
	countAndAlloc(*this, format, ap);
	packIt(*this, format, ap);
	
	va_end(ap);
}

bool NetData::unpack(const char* format, ...)
{
	if(data == NULL)
	{
		setError("Failed to unpack NULL data");
		return false;
	}
	// Unpacking (Beej)
	short *h;
	int *l;
	int pf;
	char *c;
	float *f;
	char *s;
	void* v;
	size_t len, count, maxstrlen=0;
	
	unsigned char* buf = data;
	
	// Skip the packet size
	buf += 2;

	va_list ap;
	va_start(ap, format);

	for(; *format != '\0'; format++) {
		switch(*format) {
		case 'h': // 16-bit
			h = va_arg(ap, short*);
			*h = unpacki16(buf);
			buf += 2;
			break;

		case 'l': // 32-bit
			l = va_arg(ap, int*);
			*l = unpacki32(buf);
			buf += 4;
			break;

		case 'c': // 8-bit
			c = va_arg(ap, char*);
			*c = *buf++;
			break;

		case 'f': // float
			f = va_arg(ap, float*);
			pf = unpacki32(buf);
			buf += 4;
			*f = unpack754_32(pf);
			break;

		case 's': // string
			s = va_arg(ap, char*);
			len = unpacki16(buf);
			buf += 2;
			if (maxstrlen > 0 && len > maxstrlen)
				count = maxstrlen - 1;
			else
				count = len;
			memcpy(s, buf, count);
			s[count] = '\0';
			buf += len;
			break;

		case 'v': // void*
			v = va_arg(ap, void*);
			len = unpacki16(buf);
			buf += 2;
			if (maxstrlen > 0 && len > maxstrlen)
				count = maxstrlen - 1;
			else
				count = len;
			memcpy(v, buf, count);
			buf += len;
			break;

		case 'o': // offset
			buf += maxstrlen;
			break;

		default:
			if (isdigit(*format)) { // track max str len
				maxstrlen = maxstrlen * 10 + (*format-'0');
			}
		}

		if (!isdigit(*format)) maxstrlen = 0;
	}

	va_end(ap);
	return true;
}

unsigned int NetData::size() const
{
	return bytes;
}

NetData NetData::copy() const
{
	NetData result(*this);
	result.data = new unsigned char[bytes];
	memcpy(result.data, data, bytes);
	return result;
}

void NetData::free()
{
	delete[] data;
	data = NULL;
	bytes = 0;
}







NetConnection::NetConnection()
	: id(-1), port(9999)
{}

NetConnection::NetConnection(int id)
	: id(id), port(9999)
{
	if(!bad() && NetWorker::connections.find(id) != NetWorker::connections.end())
	{
		lockMutex(NetWorker::connectionMutex);
		hostname = NetWorker::connections[id].hostname;
		port = NetWorker::connections[id].port;
		unlockMutex(NetWorker::connectionMutex);
	}
}

NetConnection::NetConnection(const NetConnection& copy)
	: id(copy.id), hostname(copy.hostname), port(copy.port)
{}

NetConnection::NetConnection(const std::string& hostname, unsigned int port)
	: id(-1), hostname(hostname), port(port)
{}

NetConnection::NetConnection(const NetIPv4& ip_address, unsigned int port)
	: id(-1), hostname(ip_address.toString()), port(port)
{}

bool NetConnection::connect()
{
	lockMutex(NetWorker::connectionMutex);
	if(bad())
	{
		NetWorker::ConnectionInfo c(hostname, port);
		id = c.connect();
		if(id >= 0)
			NetWorker::connections.insert(make_pair(c.socket, c));
	}
	else
	{
		// FIXME: This doesn't look like it works right
		id = NetWorker::connections[id].connect();
	}
	unlockMutex(NetWorker::connectionMutex);
	return !bad();
}

bool NetConnection::connect(NetTypeEnum type)
{
	lockMutex(NetWorker::connectionMutex);
	if(bad())
	{
		NetWorker::ConnectionInfo c(hostname, port);
		id = c.connect(type);
		if(id >= 0)
			NetWorker::connections.insert(make_pair(c.socket, c));
	}
	else
	{
		// FIXME: This doesn't look like it works right
		id = NetWorker::connections[id].connect();
	}
	unlockMutex(NetWorker::connectionMutex);
	return !bad();
}

bool NetConnection::accept()
{
	if(bad())
	{
		setError("Tried to accept a bad connection");
		return false;
	}
	
	// FIXME: Move this mutex stuff into ConnectionInfo::accept()
	lockMutex(NetWorker::pendingMutex);
	lockMutex(NetWorker::connectionMutex);  // ConnectionInfo::accept() messes with this data
	id = NetWorker::pendingConnections[id].accept();
	unlockMutex(NetWorker::connectionMutex);
	unlockMutex(NetWorker::pendingMutex);
	return !bad();
}

void NetConnection::deny()
{
	if(bad())
		return;  // FIXME: This could use a verbose message
	
	lockMutex(NetWorker::pendingMutex);
	NetWorker::pendingConnections[id].deny();
	unlockMutex(NetWorker::pendingMutex);
}

bool NetConnection::bad() const
{
	return (id < 0);
}

void NetConnection::close()
{
	if(bad())
		return;  // This could use a verbose message
	
	lockMutex(NetWorker::connectionMutex);
	NetWorker::connections[id].close();
	unlockMutex(NetWorker::connectionMutex);
	id = -1;
}

bool NetConnection::isServer() const
{
	if(bad())
	{
		setError("Used isServer on bad connection");
		return false;
	}
	
	lockMutex(NetWorker::connectionMutex);
	bool result = NetWorker::connections[id].isServer();
	unlockMutex(NetWorker::connectionMutex);
	return result;
}

bool NetConnection::isConnected() const
{
	if(bad())
		return false;  // FIXME: This could use a verbose message
	
	lockMutex(NetWorker::connectionMutex);
	//bool result = (NetWorker::connections.find(id) != NetWorker::connections.end());
	bool result = NetWorker::connections[id].isConnected();
	unlockMutex(NetWorker::connectionMutex);
	return result;
}


bool NetConnection::send(const NetData& data) const
{
	if(!isConnected())
	{
		setError("Tried to send on a closed connection");
		return false;
	}
	
	lockMutex(NetWorker::connectionMutex);
	bool result = NetWorker::connections[id].send(data);
	unlockMutex(NetWorker::connectionMutex);
	return result;
}


std::string NetConnection::getHost() const
{
	return hostname;
}

int NetConnection::getPort() const
{
	return port;
}

int NetConnection::getID() const
{
	return id;
}

void NetConnection::setHost(const std::string& hostname)
{
	this->hostname = hostname;
	
	if(!bad() && NetWorker::connections.find(id) != NetWorker::connections.end())
	{
		lockMutex(NetWorker::connectionMutex);
		NetWorker::connections[id].hostname = hostname;
		unlockMutex(NetWorker::connectionMutex);
	}
}

void NetConnection::setPort(int port)
{
	this->port = port;
	
	if(!bad() && NetWorker::connections.find(id) != NetWorker::connections.end())
	{
		lockMutex(NetWorker::connectionMutex);
		NetWorker::connections[id].port = port;
		unlockMutex(NetWorker::connectionMutex);
	}
}

bool NetConnection::operator==(const NetConnection& A) const
{
	if(bad() || A.bad())
		return false;
	return (id == A.id);
}

bool NetConnection::operator!=(const NetConnection& A) const
{
	if(bad() || A.bad())
		return true;
	return (id != A.id);
}




// ConnectionInfo (private nested class of NetWorker)

int NetWorker::ConnectionInfo::idCounter = 0;
SDL_mutex* NetWorker::eventMutex = NULL;
SDL_mutex* NetWorker::pendingMutex = NULL;
SDL_mutex* NetWorker::connectionMutex = NULL;

NetWorker::ConnectionInfo::ConnectionInfo()
	: hostname("localhost"), port(9999), type(NW_TCP), remoteIsServer(false), socket(-1), accepted(false), acceptingIndex(-1)
{}

NetWorker::ConnectionInfo::ConnectionInfo(const ConnectionInfo& copy)
	: hostname(copy.hostname), port(copy.port), type(copy.type), remoteIsServer(copy.remoteIsServer), socket(copy.socket), accepted(false), acceptingIndex(copy.acceptingIndex), ip(copy.ip)
{}
NetWorker::ConnectionInfo::ConnectionInfo(const std::string& hostname, unsigned int port)
	: hostname(hostname), port(port), type(NW_TCP), remoteIsServer(false), socket(-1), accepted(false), acceptingIndex(-1)
{}
NetWorker::ConnectionInfo::ConnectionInfo(const NetIPv4& ip_address, unsigned int port)
	: hostname(ip_address.toString()), port(port), type(NW_TCP), remoteIsServer(false), socket(-1), accepted(false), acceptingIndex(-1)
{}

int NetWorker::ConnectionInfo::connect()
{
	if(!isConnected())
	{
		remoteIsServer = true;
		
		if(type == NW_TCP)
		{
			lockData();

			if(-1 == snResolveHost(&ip, hostname.c_str(), port))
			{
				//setError("NET2: can't find that host name or address", -1);
				printf("NET2: can't find that host name or address\n");
				unlockData();
				return -1;
			}
			//printIPaddress(&ip);

			socket = AllocSocket(TCPClientSocket);
			if (-1 == socket)
			{
				//setError("NET2: out of memory", -1);
				printf("NET2: out of memory\n");
				unlockData();
				return -1;
			}

			socketHeap[socket]->s.tcpSocket = snTCPOpen(&ip);
			if (NULL == socketHeap[socket]->s.tcpSocket)
			{
				//setError("NET2: can't open a socket", -1);
				printf("NET2: can't open a socket\n");
				FreeSocket(socket);
				unlockData();
				return -1;
			}

			socketHeap[socket]->state = addState;
			unlockData();
			
			if(socket == -1)
			{
				// FIXME: Check deeper error messages?
				char buf[8];
				sprintf(buf, "%d", port);
				setError(string("Failed to connect to '") + hostname + "' on port " + buf);
			}
			/*else
			{
				//printf("NetWorker::ConnectionInfo: Connected!\n");
				//connected = true;
				//connectTime = SDL_GetTicks();
			}*/
		}
		else // NW_UDP
		{
			
			lockData();  // Is this really needed?
			
			if(-1 == snResolveHost(&ip, hostname.c_str(), port))
			{
				//setError("NET2: can't find that host name or address", -1);
				printf("NET2: can't find that host name or address\n");
				unlockData();
				return -1;
			}
			
			unlockData();
			//socket = udpSendSocket;  // This is a UDPsocket, not an int...
			socket = 0;  // FIXME: Is there a better value?
			//printf("UDP resolved\n");
		}
	}
	
	return socket;
}

int NetWorker::ConnectionInfo::connect(NetTypeEnum type)
{
	if(!isConnected())
	{
		this->type = type;
		connect();
	}
	return socket;
}


static __inline__ TCPsocket snTCPAccept(TCPsocket s);
static __inline__ void snTCPClose(TCPsocket s);

int NetWorker::ConnectionInfo::accept()
{
	// Remove from pendingConnections and add to connections.  Return socket.
	//printf("ConnectionInfo::accept(): %d, %d, %d\n", accepted, socket, acceptingIndex);
	if(accepted)
		return socket;  // FIXME: This could use a verbose message
	
	int i = acceptingIndex;
	if(i < 0)
	{
		// FIXME: Remove from pendingConnections
		socket = -1;
		return socket;
	}
	
	lockData();
	TCPsocket socketPtr = NULL;
	socketPtr = snTCPAccept(socketHeap[i]->s.tcpSocket);

	if(socketPtr != NULL)
	{
	  int s = -1;

	  s = AllocSocket(TCPClientSocket);
	  if (-1 != s)
	  {
		socketHeap[s]->s.tcpSocket = socketPtr;
		socketHeap[s]->state = addState;
		
		accepted = true;
		
		// Remove from the pending connections
		NetWorker::pendingConnections.erase(socket);
		
		// Set new ID and add to the connections
		socket = s;
		
		NetWorker::connections.insert(make_pair(socket, *this));
	   }
	   else // can't handle the connection, so close it.
	   {
	     snTCPClose(socketPtr);
		 // Should I remove it from the pendingConnections?  I think so.
		 NetWorker::pendingConnections.erase(socket);
		 socket = -1;
		 
	     //sendError("NET2: a TCP accept failed", i); // let the app know
		setError("Failed to accept connection");
	   }
	 }
	 
	 unlockData();
	 
	 return socket;
}

void NetWorker::ConnectionInfo::deny()
{
	// Remove from pendingConnections.
	if(!accepted)
	{
		lockMutex(pendingMutex);
		NetWorker::pendingConnections.erase(socket);
		socket = -1;
		unlockMutex(pendingMutex);
	}
}

void NetWorker::ConnectionInfo::close()
{
	if(type == NW_TCP)
		NET2_TCPClose(socket);
	else
		NET2_UDPClose(socket);
	socket = -1;
}

bool NetWorker::ConnectionInfo::isServer() const
{
	return remoteIsServer;
}

bool NetWorker::ConnectionInfo::isConnected() const
{
	return (socket >= 0);
}


bool NetWorker::ConnectionInfo::send(const NetData& data) const
{
	if(type == NW_TCP)
		return (NET2_TCPSend(socket, data.data, data.bytes) >= 0);
	else
	{
		UDPpacket p;

		p.channel = -1;
		p.data = data.data+2; // Skip the size bytes
		p.len = data.bytes;
		p.maxlen = data.bytes;
		p.address = ip;
		
		lockData();  // FIXME: Do I really need to lock here?
		if (0 == snUDPSend(udpSendSocket, -1, &p))
		{
			//setError("NET2: UDP send failed", -1);
			printf("NET2: UDP send failed (%s)\n", SDLNet_GetError());
			unlockData();
			return false;
		}

		unlockData();
		return true;
	}
}







bool NetWorker::init()
{
	if(SDLNet_Init() < 0)
	{
		setError("Failed to initialize SDLNet");
		return false;
	}
	
	if(initialized)
	{
		return true;
	}

	#ifndef WIN32

	// SIGPIPE has to be ignored so that NET2 can handle broken
	// connections without raising a program wide exception. SDL_net
	// "should" do this so that it can handle exception properly, but it
	// doesn't.

	signal(SIGPIPE, SIG_IGN); // work around for bug in SDL_net
	#endif

	error.clear();
	errorSocket = -1;

	doneYet = 0;

	dataLock = SDL_CreateMutex();
	if(NULL == dataLock)
	{
		setError("Failed to create a mutex");
		return false;
	}

	sdlNetLock = SDL_CreateMutex();
	if(NULL == sdlNetLock)
	{
		setError("Failed to create a mutex");
		return false;
	}

	dataWait = SDL_CreateCond();
	if(NULL == dataWait)
	{
		setError("Failed to create a thread condition variable");
		return false;
	}
	
	
	eventMutex = SDL_CreateMutex();
	if(!eventMutex)
	{
		setError("Failed to create a mutex");
		return false;
	}
	pendingMutex = SDL_CreateMutex();
	if(!pendingMutex)
	{
		setError("Failed to create a mutex");
		return false;
	}
	connectionMutex = SDL_CreateMutex();
	if(!connectionMutex)
	{
		setError("Failed to create a mutex");
		return false;
	}

	InitSockets(maxSockets);

	udpSendSocket = snUDPOpen(0);
	if(NULL == udpSendSocket)
	{
		setError("NET2: can't open the UDP send socket", -1);
		return false;
	}

	processSockets = SDL_CreateThread(PumpNetworkEvents, NULL);
	if(NULL == processSockets)
	{
		//setError("NET2: can't start the network thread", -1);
		setError("Failed to create network thread");
		return false;
	}
	
	// FIXME: This info could be in a verbose message
	//printf("root=%d thread=%d\n", SDL_ThreadID(), SDL_GetThreadID(processSockets)); fflush(NULL);

	initialized = 1;
	return true;
}


void NetWorker::quit()
{
	SDL_Event event;

	if (!initialized)
		return;
	
	// FIXME: Add mutex to doneYet
	// Tell pumpNetworkEvents thread to quit
	doneYet = 1;
	
	// Clear the event queue - Is this necessary anymore?
	while (0 < SDL_PollEvent(&event));
	
	signalRead();
	
	// Wait for the network thread to quit
	SDL_WaitThread(processSockets, NULL);

	FinitSockets();

	snUDPClose(udpSendSocket);

	SDL_DestroyMutex(dataLock);
	SDL_DestroyMutex(sdlNetLock);
	SDL_DestroyCond(dataWait);

	dataLock = NULL;
	sdlNetLock = NULL;
	dataWait = NULL;
	
	SDL_DestroyMutex(eventMutex);
	SDL_DestroyMutex(pendingMutex);
	SDL_DestroyMutex(connectionMutex);
	
	eventMutex = NULL;
	pendingMutex = NULL;
	connectionMutex = NULL;

	error.clear();
	errorSocket = -1;

	initialized = 0;
}

vector<NetConnection> NetWorker::getConnections()
{
	vector<NetConnection> result;
	
	lockMutex(connectionMutex);
	for(map<int, NetWorker::ConnectionInfo>::const_iterator e = connections.begin(); e != connections.end(); e++)
	{
		result.push_back(NetConnection(e->second.socket));
	}
	unlockMutex(connectionMutex);
	return result;
}

unsigned int NetWorker::numConnections()
{
	return connections.size();
}

// If enabled, the NetWorker starts putting new incoming connections into pendingConnections.  If disabled, incoming connections are rejected.
void NetWorker::listen(bool enable, unsigned int port, NetTypeEnum type)
{
	if(enable)
	{
		// Open a socket
		int socket;
		if(type == NW_TCP)
			socket = NET2_TCPAcceptOn(port);
		else
			socket = NET2_UDPAcceptOn(port, maxSizeUDP);
		if(socket >= 0)
			listening.push_back(NetSocket(port, socket, type));
	}
	else
	{
		// Close a socket
		// Loop over all accepting connections, looking for the port.
		for(vector<NetSocket>::iterator e = listening.begin(); e != listening.end(); e++)
		{
			if(e->port >= 0 && (unsigned int)e->port == port)
			{
				// Close the associated socket.
				if(type == NW_TCP)
					NET2_TCPClose(e->socket);
				else
					NET2_UDPClose(e->socket);
				
				listening.erase(e);
				break;
			}
		}
	}
}

bool NetWorker::isListening()
{
	return (listening.size() > 0);
}

bool NetWorker::isListening(unsigned int port)
{
	for(vector<NetSocket>::iterator e = listening.begin(); e != listening.end(); e++)
	{
		if(e->port >= 0 && (unsigned int)e->port == port)
			return true;
	}
	
	return false;
}


vector<NetSocket> NetWorker::getListening()
{
	return vector<NetSocket>(listening);
}

bool NetWorker::popEvent(NetEvent& event)
{
	// Deny unaccepted connections
	if(event.type == NW_ACCEPT)
	{
		// FIXME: Check if this works okay
		/*lockMutex(NetWorker::pendingMutex);
		if(NetWorker::pendingConnections.find(event.connection.id) != NetWorker::pendingConnections.end())
			event.connection.deny();
		unlockMutex(NetWorker::pendingMutex);*/
	}
	// Free leftover data
	else if(event.type == NW_RECEIVE)
		event.data.free();
	// Close a closed connection (remove it from lists, etc.)
	else if(event.type == NW_CLOSE)
		event.connection.close();
	// Reset the message
	event.message.clear();
	
	// Retrieve the next event
	lockMutex(eventMutex);
	if(events.empty())
	{
		unlockMutex(eventMutex);
		return false;
	}
	event = events.pop();
	unlockMutex(eventMutex);
	return true;
}

void NetWorker::pushEvent(const NetEvent& event)
{
	lockMutex(eventMutex);
	events.push(event);
	unlockMutex(eventMutex);
}

string NetWorker::getError()
{
	return error;
}

unsigned int NetWorker::getUDPMax()
{
	return maxSizeUDP;
}

void NetWorker::setUDPMax(unsigned int maxBytes)
{
	maxSizeUDP = maxBytes;
}













// PACKING

/*
** pack() -- store data dictated by the format string in the buffer
**
**  h - 16-bit              l - 32-bit
**  c - 8-bit char          f - float, 32-bit
**  s - string (16-bit length is automatically prepended)
*/


/*
** pack754() -- pack a floating point number into IEEE-754 format
*/
long long pack754(long double f, unsigned bits, unsigned expbits)
{
	long double fnorm;
	int shift;
	long long sign, exp, significand;
	unsigned significandbits = bits - expbits - 1; // -1 for sign bit

	if (f == 0.0) return 0; // get this special case out of the way

	// check sign and begin normalization
	if (f < 0) { sign = 1; fnorm = -f; }
	else { sign = 0; fnorm = f; }

	// get the normalized form of f and track the exponent
	shift = 0;
	while(fnorm >= 2.0) { fnorm /= 2.0; shift++; }
	while(fnorm < 1.0) { fnorm *= 2.0; shift--; }
	fnorm = fnorm - 1.0;

	// calculate the binary form (non-float) of the significand data
	significand = (long long)(fnorm * ((1LL<<significandbits) + 0.5f));

	// get the biased exponent
	exp = shift + ((1<<(expbits-1)) - 1); // shift + bias

	// return the final answer
	return (sign<<(bits-1)) | (exp<<(bits-expbits-1)) | significand;
}

/*
** unpack754() -- unpack a floating point number from IEEE-754 format
*/
long double unpack754(long long i, unsigned bits, unsigned expbits)
{
	long double result;
	long long shift;
	unsigned bias;
	unsigned significandbits = bits - expbits - 1; // -1 for sign bit

	if (i == 0) return 0.0;

	// pull the significand
	result = (i&((1LL<<significandbits)-1)); // mask
	result /= (1LL<<significandbits); // convert back to float
	result += 1.0f; // add the one back on

	// deal with the exponent
	bias = (1<<(expbits-1)) - 1;
	shift = ((i>>significandbits)&((1LL<<expbits)-1)) - bias;
	while(shift > 0) { result *= 2.0; shift--; }
	while(shift < 0) { result /= 2.0; shift++; }

	// sign it
	result *= (i>>(bits-1))&1? -1.0: 1.0;

	return result;
}

/*
** packi16() -- store a 16-bit int into a char buffer (like htons())
*/
void packi16(unsigned char *buf, unsigned int i)
{
	*buf++ = i>>8; *buf++ = i;
}

/*
** packi32() -- store a 32-bit int into a char buffer (like htonl())
*/
void packi32(unsigned char *buf, unsigned long i)
{
	*buf++ = i>>24; *buf++ = i>>16;
	*buf++ = i>>8;  *buf++ = i;
}

/*
** unpacki16() -- unpack a 16-bit int from a char buffer (like ntohs())
*/
unsigned int unpacki16(unsigned char *buf)
{
	return (buf[0]<<8) | buf[1];
}

/*
** unpacki32() -- unpack a 32-bit int from a char buffer (like ntohl())
*/
unsigned long unpacki32(unsigned char *buf)
{
	return (buf[0]<<24) | (buf[1]<<16) | (buf[2]<<8) | buf[3];
}


/* END packing code */






















//----------------------------------------
//
//
//


// Use this for errors immediately due to user function calls.
static __inline__ void setError(const string& err)
{
	error = err;
	errorSocket = -1;
}

static __inline__ void setError(const string& err, int socket)
{
	error = err;
	errorSocket = socket;
}

// Use this for errors due to internal calls
__inline__ int sendError(const string& err, int socket)
{
	if(socket == -1)
		return sendEvent(NetEvent(NW_ERROR, err));
	else
		return sendEvent(NetEvent(NW_ERROR, err, NetConnection(socket)));
}

//----------------------------------------
// 
// Threads, mutexs, thread utils, and 
// thread safe wrappers
//

static __inline__ int sendEvent(const NetEvent& event)
{
	NetWorker::pushEvent(event);
	return 0;
}

static __inline__ void signalRead()
{
  waitForRead = 0;
  SDL_CondSignal(dataWait);
}

static __inline void waitUntilRead()
{
  dataLocked = 0;
  SDL_CondWait(dataWait, dataLock);
  dataLocked = 1;
}

static __inline__ void lockData()
{
  if (-1 == SDL_LockMutex(dataLock))
  {
    setError("NET2: can't lock Data mutex", -1);
  }
  dataLocked = 1;
}

static __inline__ void unlockData()
{
  dataLocked = 0;
  if (-1 == SDL_UnlockMutex(dataLock))
  {
    setError("NET2: can't unlock Data mutex", -1);
  }
}

static __inline__ void lockSDLNet()
{
  if (-1 == SDL_LockMutex(sdlNetLock))
  {
    setError("NET2: can't lock SDLNet mutex", -1);
  }
}

static __inline__ void unlockSDLNet()
{
  if (-1 == SDL_UnlockMutex(sdlNetLock))
  {
    setError("NET2: can't unlock SDLNet mutex", -1);
  }
}

static __inline__ void snFreeSocketSet(SDLNet_SocketSet ss)
{
  lockSDLNet();
  SDLNet_FreeSocketSet(ss);
  unlockSDLNet();
}

static __inline__ SDLNet_SocketSet snAllocSocketSet(int size)
{
  SDLNet_SocketSet ss = NULL;

  lockSDLNet();
  ss = SDLNet_AllocSocketSet(size);
  unlockSDLNet();

  return ss;
}

static __inline__ int snTCPAddSocket(TCPsocket s)
{
  int val = 0;

  lockSDLNet();
  val = SDLNet_TCP_AddSocket(socketSet, s);
  unlockSDLNet();

  return val;
}

static __inline__ int snTCPDelSocket(TCPsocket s)
{
  int val = 0;

  lockSDLNet();
  val = SDLNet_TCP_DelSocket(socketSet, s);
  unlockSDLNet();

  return val;
}

static __inline__ int snUDPAddSocket(UDPsocket s)
{
  int val = 0;

  lockSDLNet();
  val = SDLNet_UDP_AddSocket(socketSet, s);
  unlockSDLNet();

  return val;
}

static __inline__ int snUDPDelSocket(UDPsocket s)
{
  int val = 0;

  lockSDLNet();
  val = SDLNet_UDP_DelSocket(socketSet, s);
  unlockSDLNet();

  return val;
}

static __inline__ TCPsocket snTCPAccept(TCPsocket s)
{
  TCPsocket socket = NULL;

  lockSDLNet();
  socket = SDLNet_TCP_Accept(s);
  unlockSDLNet();

  return socket;
}

static __inline__ void snTCPClose(TCPsocket s)
{
  lockSDLNet();
  SDLNet_TCP_Close(s);
  unlockSDLNet();
}

static __inline__ TCPsocket snTCPOpen(IPaddress *ip)
{
  TCPsocket socket = NULL ;

  lockSDLNet();
  socket = SDLNet_TCP_Open(ip);
  // FIXME: Support better error messages by grabbing the SDLNet errors
  //printf("SDLNet_TCP_Open: %s\n", SDLNet_GetError());
  unlockSDLNet();

  return socket;
}

static __inline__ void snUDPClose(UDPsocket s)
{
  lockSDLNet();
  SDLNet_UDP_Close(s);
  unlockSDLNet();
}

static __inline__ UDPsocket snUDPOpen(int port)
{
  UDPsocket socket = NULL;

  lockSDLNet();
  socket = SDLNet_UDP_Open(port);
  unlockSDLNet();

  return socket;
}

static __inline__ int snUDPSend(UDPsocket s, int c, UDPpacket *p)
{
  int val = 0;

  lockSDLNet();
  val = SDLNet_UDP_Send(s, c, p);
  unlockSDLNet();

  return val;
}

static __inline__ int snUDPRecv(UDPsocket s, UDPpacket *p)
{
  int val = 0;

  lockSDLNet();
  val = SDLNet_UDP_Recv(s, p);
  unlockSDLNet();

  return val;
}

static __inline__ UDPpacket *snUDPAllocPacket(int size)
{
  UDPpacket *val = 0;

  lockSDLNet();
  val = SDLNet_AllocPacket(size);
  unlockSDLNet();

  return val;
}

static __inline__ void snUDPFreePacket(UDPpacket *p)
{
  lockSDLNet();
  SDLNet_FreePacket(p);
  unlockSDLNet();
}

static __inline__ int snCheckSockets(SDLNet_SocketSet ss, int wait)
{
  int val = 0;

  val = SDLNet_CheckSockets(ss, wait);
  return val;
}

static __inline__ int snTCPRead(TCPsocket s, Uint8 *buf, int max)
{
  int val = -1;

  lockSDLNet();
  val = SDLNet_TCP_Recv(s, buf, max); // read what we can
  unlockSDLNet();

  return val;
}

static __inline__ int snTCPSend(TCPsocket s, Uint8 *buf, int len)
{
  int val = -1;

  lockSDLNet();
  val = SDLNet_TCP_Send(s, buf, len);
  unlockSDLNet();

  return val;
}

static __inline__ int snResolveHost(IPaddress *ip, const char *name, int port)
{
  int val = 0;

  lockSDLNet();
  val = SDLNet_ResolveHost(ip, name, port);
  unlockSDLNet();

  return val;
}

static __inline__ int snSocketReady(SDLNet_GenericSocket s)
{
  int val = 0;

  lockSDLNet();
  val = SDLNet_SocketReady(s);
  unlockSDLNet();

  return val;
}

static __inline__ IPaddress *snTCPGetPeerAddress(TCPsocket s)
{
  IPaddress *val = NULL;

  lockSDLNet();
  val = SDLNet_TCP_GetPeerAddress(s);
  unlockSDLNet();

  return val;
}

//----------------------------------------
//
// API routines
//
// to make coding easier most APIs have a
// raw version that does the work and a
// thread safe wrapper. 
//

//----------------------------------------
//
// 
//

int NET2_ResolveHost(IPaddress *ip, char *name, int port)
{
  return snResolveHost(ip, name, port);
}


//----------------------------------------
//
// 
//

void NET2_UDPFreePacket(UDPpacket *p)
{
  snUDPFreePacket(p);
}

//----------------------------------------
//
//
//

int NET2_GetEventType(SDL_Event *e)
{
  return e->user.code;
}

//----------------------------------------
//
//
//

int NET2_GetSocket(SDL_Event *e)
{
  return (int)e->user.data1;
}

//----------------------------------------
//
//
//

static __inline__ int raw_NET2_UDPAcceptOn(int port, int size)
{
  int s = -1;

  s = AllocSocket(UDPServerSocket);
  if (-1 == s)
  {
    setError("NET2: out of memory", s);
    return -1;
  }

  socketHeap[s]->s.udpSocket = snUDPOpen(port);
  if (NULL == socketHeap[s]->s.udpSocket)
  {
    setError("NET2: can't open a socket", s);
    FreeSocket(s);
    return -1;
  }

  socketHeap[s]->p.udpLen = size;
  socketHeap[s]->state = addState;

  return s;
}

int NET2_UDPAcceptOn(int port, int size)
{
  int val = -1;

  lockData();
  val = raw_NET2_UDPAcceptOn(port, size);
  unlockData();

  return val;
}

//----------------------------------------
//
//
//

static __inline__ int raw_NET2_UDPSend(IPaddress *ip, unsigned char *buf, int len)
{
  UDPpacket p;

  p.channel = -1;
  p.data = buf;
  p.len = len;
  p.maxlen = len;
  p.address = *ip;

  if (0 == snUDPSend(udpSendSocket, -1, &p))
  {
    setError("NET2: UDP send failed", -1);
    return -1;
  }

  return 0;
}

int NET2_UDPSend(IPaddress *ip, unsigned char *buf, int len)
{
  int val = -1;

  lockData();
  val = raw_NET2_UDPSend(ip, buf, len);
  unlockData();

  return val;
}

//----------------------------------------
//
//
//

UDPpacket *raw_NET2_UDPRead(int s)
{
  UDPpacket *p = NULL;
  PacketQue *ub = NULL;

  if ((s >= 0) && 
      (s < lastHeapSocket) &&
      (socketHeap[s]->type == UDPServerSocket))
  {
    ub = &socketHeap[s]->q.ub;

    if (-1 == DequePacket(ub, &p))
    {
      return NULL;
    }
  }

  return p;
}

UDPpacket *NET2_UDPRead(int socket)
{
  UDPpacket *val = NULL;

  lockData();
  val = raw_NET2_UDPRead(socket);
  unlockData();
  signalRead();

  return val;
}

//----------------------------------------
//
//
//

static __inline__ void raw_NET2_UDPClose(int s)
{
  if ((s >= 0) && 
      (s < lastHeapSocket) &&
      (socketHeap[s]->type == UDPServerSocket))
  {
    socketHeap[s]->state = delState;
  }
}

void NET2_UDPClose(int socket)
{
  lockData();
  raw_NET2_UDPClose(socket);
  unlockData();
}

//----------------------------------------
//
//
//

int NET2_GetEventData(SDL_Event *e)
{
  return (int)e->user.data2;
}

//----------------------------------------
//
//
//


char *NET2_GetEventError(SDL_Event *e)
{
  return (char *)NET2_GetEventData(e);
}

//----------------------------------------
//
//
//

static __inline__ int raw_NET2_TCPAcceptOnIP(IPaddress *ip)
{
  int s = -1;

  s = AllocSocket(TCPServerSocket);
  if (-1 == s)
  {
    setError("NET2: out of memory", -1);
    return -1;
  }

  socketHeap[s]->s.tcpSocket = snTCPOpen(ip);
  if (NULL == socketHeap[s]->s.tcpSocket)
  {
    setError("NET2: can't open a socket", -1);
    FreeSocket(s);
    return -1;
  }

  socketHeap[s]->p.tcpPort = ip->port;
  socketHeap[s]->state = addState;

  return s;
}

static __inline__ int raw_NET2_TCPAcceptOn(int port)
{
  int s = -1;
  IPaddress ip;

  if (-1 == snResolveHost(&ip, NULL, port))
  {
    setError("NET2: can't resolve that host name or address", -1);
    return -1;
  }
  //printIPaddress(&ip);

  s = raw_NET2_TCPAcceptOnIP(&ip);
  if (-1 != s)
  {
    //printf("port=%d\n", port);
    socketHeap[s]->p.tcpPort = port;
  }

  return s;
}

int NET2_TCPAcceptOn(int port)
{
  int val = 0;

  lockData();
  val = raw_NET2_TCPAcceptOn(port);
  unlockData();

  return val;
}

int NET2_TCPAcceptOnIP(IPaddress *ip)
{
  int val = 0;

  lockData();
  val = raw_NET2_TCPAcceptOnIP(ip);
  unlockData();

  return val;
}


//----------------------------------------
//
//
//

static __inline__ void raw_NET2_TCPClose(int s)
{
  if ((s >= 0) && 
      (s < lastHeapSocket) &&
      ((socketHeap[s]->type == TCPServerSocket) || 
       (socketHeap[s]->type == TCPClientSocket)))
  {
    socketHeap[s]->state = delState; // stop reading and writing
  }
}

void NET2_TCPClose(int socket)
{
  lockData();
  raw_NET2_TCPClose(socket);
  unlockData();
}

//----------------------------------------
//
//
//

static __inline__ int raw_NET2_TCPSend(int s, unsigned char *buf, int len)
{
  int val = 0;

  if ((s >= 0) && 
      (s < lastHeapSocket) &&
      (socketHeap[s]->type == TCPClientSocket) &&
      (0 != (socketHeap[s]->state & (addState | readyState))))
  {
    if (0 < len)
    {
      val = snTCPSend(socketHeap[s]->s.tcpSocket, buf, len);
      if (val != len)
      {
        socketHeap[s]->state = dyingState;
        setError("NET2: can't send TCP data, expect a close event", s);
        //sendEvent(NET2_TCPCLOSEEVENT, s, -1);
		// FIXME: Find the appropriate connection using 's'.
		sendEvent(NetEvent(NW_ERROR, "NET2: can't send TCP data, expect a close event"));
		sendEvent(NetEvent(NW_CLOSE, "NET2: can't send TCP data"));
        return -1;
      }
    }
    return val;
  }

  setError("NET2: you can't do a TCP send on that socket", s);
  return -1;
}

int NET2_TCPSend(int socket, unsigned char *buf, int len)
{
  int val = 0;

  lockData();
  val = raw_NET2_TCPSend(socket, buf, len);
  unlockData();

  return val;
}

//----------------------------------------
//
//
//

static __inline__ int raw_NET2_TCPRead(int s, unsigned char *buf, int len)
{
  CharQue *tb = NULL;
  int nlen = 0;

  if ((0 < len) &&
      (s >= 0) && 
      (s < lastHeapSocket) &&
      (socketHeap[s]->type == TCPClientSocket))
  {
    tb = &socketHeap[s]->q.tb;

    nlen = min(tb->len, len);
    memmove(&buf[0], &tb->buf[0], nlen);
    tb->len -= nlen;
    memmove(&tb->buf[0], &tb->buf[nlen], tb->len);
  }

  return nlen;
}

int NET2_TCPRead(int socket, unsigned char *buf, int len)
{
  int val = 0;

  lockData();
  val = raw_NET2_TCPRead(socket, buf, len);
  unlockData();
  signalRead();

  return val;
}

//----------------------------------------
//
//
//

static __inline__ IPaddress *raw_NET2_TCPGetPeerAddress(int s)
{
  IPaddress *ip = NULL;

  if ((s >= 0) && 
      (s < lastHeapSocket) &&
      (socketHeap[s]->type == TCPClientSocket))
  {
    ip = snTCPGetPeerAddress(socketHeap[s]->s.tcpSocket);
  }

  return ip;
}

IPaddress *NET2_TCPGetPeerAddress(int s)
{
  IPaddress *ip = NULL;

  lockData();
  ip = raw_NET2_TCPGetPeerAddress(s);
  unlockData();

  return ip;
}

//----------------------------------------
//
// NET2 initialization and finalization
//



//----------------------------------------
// 
// Network pump thread. This handles 
// input and converts it to events.
//

int PumpNetworkEvents(void *nothing)
{
  int i = 0;

#define timeOut (10)

  while (!doneYet)
  {
	  SDL_Delay(1);
	lockMutex(NetWorker::pendingMutex);
	lockMutex(NetWorker::connectionMutex);
    if (-1 == snCheckSockets(socketSet, timeOut))
    {
      setError("NET2: the CheckSockets call failed", -1);
    }

    lockData();
	
    while ((!doneYet) && waitForRead)
    {
      waitUntilRead();
    }

    for (i = 0; ((!doneYet) && (i < lastHeapSocket)); i++)
    {
      if (addState == socketHeap[i]->state)
      {
        switch(socketHeap[i]->type)
        {
        case unusedSocket:
          sendError("NET2: trying to add an unused socket", i);
          break;

        case TCPServerSocket:
        case TCPClientSocket:
          if (-1 != snTCPAddSocket(socketHeap[i]->s.tcpSocket))
          {
            socketHeap[i]->state = readyState;
          }
          else
          {
            socketHeap[i]->state = delState;
            sendError("NET2: can't add a TCP socket to the socket set", i);
          }
          break;

        case UDPServerSocket:
          if (-1 != snUDPAddSocket(socketHeap[i]->s.udpSocket))
          {
            socketHeap[i]->state = readyState;
          }
          else
          {
            socketHeap[i]->state = delState;
            sendError("NET2: can't add a UDP socket to the socket set", i);
          }
          break;

        default:
          sendError("NET2: invalid socket type, this should never happen", i);
          break;
        }
      }
      else if (delState == socketHeap[i]->state)
      {
        switch(socketHeap[i]->type)
        {
        case unusedSocket:
          sendError("NET2: trying to delete an unused socket", i);
          break;

        case TCPServerSocket:
        case TCPClientSocket:
          if (-1 == snTCPDelSocket(socketHeap[i]->s.tcpSocket))
          {
            sendError("NET2: can't delete a TCP socket from the socket set", i);
          }
          snTCPClose(socketHeap[i]->s.tcpSocket);
          FreeSocket(i);
          break;

        case UDPServerSocket:
          if (-1 == snUDPDelSocket(socketHeap[i]->s.udpSocket))
          {
            sendError("NET2: can't delete a UDP socket from the socket set", i);
          }
          snUDPClose(socketHeap[i]->s.udpSocket);
          FreeSocket(i);
          break;

        default:
          sendError("NET2: invalid socket type, this should never happen", i);
          break;
        }
      }
      else if ((TCPServerSocket == socketHeap[i]->type) &&
               (snSocketReady(socketHeap[i]->s.genSocket)))
	  {
		  // NW_ACCEPT event

			// The connection should be sent to the programmer to accept.
			// When the programmer accepts (accept()), then it is added to the active connections.
			// Otherwise, the connection should be discarded with deny().
			
			bool skip = false;
			
			NetWorker nw;
			
			for(map<int, NetWorker::ConnectionInfo>::iterator e = nw.pendingConnections.begin(); e != nw.pendingConnections.end(); e++)
			{
				if(e->second.acceptingIndex == i)
				{
					skip = true;
					break;
				}
			}
			
			if(!skip)
			{
				NetWorker::ConnectionInfo c;
				c.hostname = "";
				c.port = -1;//socketHeap[s]->p.tcpPort;
				c.type = NW_TCP;
				c.remoteIsServer = false;
				c.accepted = false;
				c.socket = NetWorker::ConnectionInfo::idCounter++;//s;
				c.acceptingIndex = i;
				
				NetWorker::pendingConnections.insert(make_pair(c.socket, c));
				
				
				sendEvent(NetEvent(NW_ACCEPT, "Incoming TCP connection.", NetConnection(c.socket)));
				
			}
			
      }
      else if ((TCPClientSocket == socketHeap[i]->type) && 
               (readyState == socketHeap[i]->state) &&
               (snSocketReady(socketHeap[i]->s.genSocket)))
      {
		  // NW_RECEIVE or NW_CLOSE event
        int len;
        CharQue *tb = &socketHeap[i]->q.tb;

        if (tcpQueLen <= tb->len)
        {
          waitForRead = 1;
        }
        else
        {
			// Put any new data at the end of the buffer
          len = snTCPRead(socketHeap[i]->s.tcpSocket, 
                          &tb->buf[tb->len], 
                          tcpQueLen - tb->len);
						  
          if (0 < len)
          {
            if(len < 2)
			{
				tb->len += len;
			}
			else
			{
			    //int oldlen = tb->len;
				tb->len += len;
				//printf("Got a packet. Size: %d\n", len);
				
				// Read the first two bytes to get the packet length.  Ughh...  This has forced extra data into every TCP packet (max 65536 bytes, which is very reasonable).  The biggest problem may be that this can no longer interface with other software.  Maybe I can add an option to turn this off.  Then, events will be sent like Net2 and the user will have to loop over it.
				int newLen;
				while(1)
				{
					if(tb->len < 2)  // Can't do anything with this yet.  Wait for more.
						break;
					
					newLen = unpacki16(tb->buf) + 2;  // Reads the first two bytes for the packet size
					
					if(newLen > 2)
					{
						if(newLen > tb->len)  // We need to wait for more data to come in
							break;
						
						//buf += 2;  // Move the buffer ahead
						
						NetData data;
						data.data = new unsigned char[newLen];
						data.bytes = newLen;
						// Copy the data (newLen bytes)
						memcpy(data.data, tb->buf, newLen);
						// Tell us how much data is left
						tb->len -= newLen;
						// Move the remaining data to the beginning of the buffer
						memmove(&tb->buf[0], &tb->buf[newLen], tb->len);
						
						sendEvent(NetEvent(data, NetConnection(i)));
					}
					else
					{
						// Move the tb->buf data up, replacing the two worthless size bytes.  Hopefully, there's another pair of size bytes next.
						memmove(&tb->buf[0], &tb->buf[2], tb->len);
					}
				}
			}
          }
          else // no byte, must be dead.
          {
            socketHeap[i]->state = dyingState;
            //sendEvent(NET2_TCPCLOSEEVENT, i, -1);
            sendEvent(NetEvent(NW_CLOSE, "Closing connection.", NetConnection(i)));
          }
        }
      }
      else if ((UDPServerSocket == socketHeap[i]->type) && 
               (readyState == socketHeap[i]->state) &&
               (snSocketReady(socketHeap[i]->s.genSocket)))
      {
		  // NW_RECEIVE event (UDP)
        int recv = 0;
        UDPpacket *p = NULL;
        PacketQue *ub = &socketHeap[i]->q.ub;

        if (PacketQueFull(ub))
        {
          waitForRead = 1;
        }
        else
        {
          while ((!PacketQueFull(ub)) &&
                 (NULL != (p = snUDPAllocPacket(socketHeap[i]->p.udpLen))) &&
                 (1 == (recv = snUDPRecv(socketHeap[i]->s.udpSocket, p))))
          {
			  
			NetData data;
			data.bytes = p->len;
			data.data = new unsigned char[data.bytes+2];
			
			// Put in the size info
			packi16(data.data, data.bytes);
			// Copy the data
			memcpy(data.data+2, p->data, p->len);
			
			sendEvent(NetEvent(data, NetConnection(i)));
			snUDPFreePacket(p);  // FIXME: I think this will be done below...
			p = NULL;
			
            /*if (PacketQueEmpty(ub))
            {
              //EnquePacket(ub, &p);
              //sendEvent(NET2_UDPRECEIVEEVENT, i, -1);
			  // FIXME: Find the appropriate connection using 'i'
			  // FIXME: Read data into the event's NetData.
				//UDPpacket *p = NULL;
				//PacketQue *ub = NULL;

			  
              //sendEvent(NetEvent(NW_RECEIVE, "Received UDP data."));
              //sendEvent(NetEvent(NetData(len, ub->buf), NetConnection(i)));
            }
            else
            {
              EnquePacket(ub, &p);
            }*/
          }

          // unravel terminating conditions and free left over memory
          // if we need to
          if (!PacketQueFull(ub)) // if the packet que is full then everything is fine
          {
            if (NULL != p) // couldn't alloc a packet
            {
              if (0 >= recv) // ran out of packets
              {
                snUDPFreePacket(p);
              }
            }
            else
            {
              sendError("NET2: out of memory", i);
            }
          }
        }
      }
    }
    unlockData();
	unlockMutex(NetWorker::pendingMutex);
	unlockMutex(NetWorker::connectionMutex);
  }

  return 0;
}

//----------------------------------------
//
// Socket heap utilities
//

static __inline__ int InitSockets(int incr)
{
  int i = 0;

  freeHeapSocket = 0;
  lastHeapSocket = 0;

  socketSet = snAllocSocketSet(maxSockets);
  if (NULL == socketSet)
  {
    setError("NET2: out of memory", -1);
    return -1;
  }

  memset(socketHeap, 0, sizeof(socketHeap));
  for (i = 0; i < maxSockets; i++)
  {
    //printf("%d\n", i);
    socketHeap[i] = NULL;
  }

  return 0;
}

//
// close all sockets and free resources
//
// MUST NOT be called while PumpNetworkEvents
// thread is running.
//

static __inline__ void FinitSockets()
{
  int i = 0;

  for (i = 0; i < lastHeapSocket; i++)
  {
    if (NULL != socketHeap[i])
    {
      if ((TCPServerSocket == socketHeap[i]->type) ||
          (TCPClientSocket == socketHeap[i]->type))
      {
        //printf("Used=%d\n", i);
        snTCPClose(socketHeap[i]->s.tcpSocket);
      }

      if (UDPServerSocket == socketHeap[i]->type)
      {
        //printf("Used=%d\n", i);
        snUDPClose(socketHeap[i]->s.udpSocket);
      }

      free(socketHeap[i]);
      socketHeap[i] = NULL;
    }
  }

  if (NULL != socketSet)
  {
    snFreeSocketSet(socketSet);
    socketSet = NULL;
  }
}

//
// free a socket
//
static __inline__ void FreeSocket(int s)
{
  if ((s >= 0) && 
      (s < lastHeapSocket) &&
      (socketHeap[s]->type != unusedSocket))
  {
    if (UDPServerSocket == socketHeap[s]->type)
    {
      UDPpacket *p;
      PacketQue *ub = &socketHeap[s]->q.ub;

      while (-1 != DequePacket(ub, &p))
      {
        SDLNet_FreePacket(p);
      }
    }

    socketHeap[s]->type = unusedSocket;
    socketHeap[s]->state = unusedState;
    socketHeap[s]->s.genSocket = NULL;
  }
}

//
// allocate a socket 
//

static __inline__ void initBuffer(int s, int type)
{
  switch (type)
  {
  case TCPClientSocket:
    socketHeap[s]->q.tb.len = 0;
    break;

  case UDPServerSocket:
    InitPacketQue(&socketHeap[s]->q.ub);
    break;
  }
}

static __inline__ int AllocSocket(int type)
{
  int i = 0;
  int s = -1;

  //printf("last=%d\n", lastHeapSocket);
  for (i = 0; i < lastHeapSocket; i++)
  {
    //printf("(%d)\n", freeHeapSocket);
    s = freeHeapSocket;
    if (unusedSocket == socketHeap[s]->type)
    {
      initBuffer(s, type);
      socketHeap[s]->type = type;
      socketHeap[s]->state = unusedState;
      socketHeap[s]->s.genSocket = NULL;

      return s;
    }

    freeHeapSocket = (freeHeapSocket + 1) % lastHeapSocket;
  }

  if (lastHeapSocket < maxSockets)
  {
    s = lastHeapSocket;

    socketHeap[s] = (NET2_Socket*)malloc(sizeof(NET2_Socket));
    if (NULL == socketHeap[s])
    {
      return -1;
    }

    initBuffer(s, type);
    socketHeap[s]->type = type;
    socketHeap[s]->state = unusedState;
    socketHeap[s]->s.genSocket = NULL;

    lastHeapSocket++;
    return s;
  }

  return -1;
}




