/*

"bangup" - a small testbed for ode networking.

The code is broken up into a number of pieces,

	1) Network - a network abstraction layer (runs under cygwin only - not ported to windows)
	2) SimObject - a generic type of simulation object
	3) Dynamics  - the basic ODE dynamics (world, space, ground )
	4) Car		 - a car
	5) Main		 - handles network traffic and overall event stuff

In general the networking scheme here is:

	1) There is a server and a number of clients.
	2) A server is similar to a client in that it also has a window and a pov
	3) Each machine is locally authoritative for some objects
	4) Other objects on a machine are considered to be driven by some remote authority
	5) In the tests so far the clients are authoritative for the local participants pov.
	6) In the tests so far the server is authoritative for its pov
	7) The clients and server echo to each other any state that they are authoritative for.
	8) The server additionally re-echoes out all state so that other clients can be advised of changes.
	9) Rather than publishing a database on new client connections; clients dynamically create missing objects.

This is extremely crude... and one will see that it does not behave well.  Not much state is networked so things tend to jump around and misbehave terribly.  It may improve over time.

*/

//#if defined( WIN32 ) || defined( __CYGWIN32__ )
//#include <winsock.h>
//#else
#include <sys/types.h>
#include <sys/time.h>
#include <sys/socket.h>
#include <netinet/in.h>
#define _USE_BSD
#include <netdb.h>
#include <string.h>
#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/fcntl.h>
#include <errno.h>
#include <sys/signal.h>
#include <sys/wait.h>
#include <arpa/inet.h>
#include <stdarg.h>
typedef int SOCKET;
//#endif

#include <errno.h>

#ifdef USE_WIN32
#define __USE_W32_SOCKETS 1
#include <windows.h>
#include <timer.h>
#ifdef MSVC
#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
#endif
#endif


#include <ode/ode.h>
#include <drawstuff/drawstuff.h>

// select correct drawing functions

#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCappedCylinder dsDrawCappedCylinderD
#endif

int headless = 0;

/**********************************************************************/
// dynamics
/**********************************************************************/

class Dynamics {
public:
	dWorldID world;
	dSpaceID space;
	dGeomID ground;
	dJointGroupID contactgroup;
	Dynamics* next;

	// this is called by dSpaceCollide when two objects in space are potentially colliding.

	void nearCallback2 (void *data, dGeomID o1, dGeomID o2) {
		int i,n;
		const int N = 10;
		dContact contact[N];
		n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
		if (n > 0) {
			for (i=0; i<n; i++) {
			contact[i].surface.mode = dContactSlip1 | dContactSlip2 | 
							dContactSoftERP | dContactSoftCFM | dContactApprox1;
			contact[i].surface.mu = dInfinity;
			contact[i].surface.slip1 = 0.1;
			contact[i].surface.slip2 = 0.1;
			contact[i].surface.soft_erp = 0.5;
			contact[i].surface.soft_cfm = 0.3;
			dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
			dJointAttach (c,dGeomGetBody(contact[i].geom.g1),dGeomGetBody(contact[i].geom.g2));
			}
		}
	}

	static void nearCallback(void* data, dGeomID o1, dGeomID o2) {
		((Dynamics*)data)->nearCallback2(0,o1,o2);
	}

	Dynamics() {
		world = dWorldCreate();
		space = dHashSpaceCreate();
		contactgroup = dJointGroupCreate (0);
		dWorldSetGravity (world,0,0,-0.5);
		ground = dCreatePlane (space,0,0,1,0);
	}

	void draw() {}

	void update() {
		dSpaceCollide (space,this,&nearCallback);
		dWorldStep (world,0.05);
		dJointGroupEmpty (contactgroup);
	}

	void command(char* cmd) {}

	~Dynamics() {
		dJointGroupDestroy (contactgroup);
		dSpaceDestroy (space);
		dWorldDestroy (world);
	}
};

/**********************************************************************/
// simobject - game object management
/**********************************************************************/

class SimObject {
public:

	SimObject* next;
	int key;
	static class SimObject* objects;

	SimObject() {
		next = objects; objects = this;
	}
	virtual ~SimObject() {
		for(SimObject** obj = &objects; *obj; obj = &((*obj)->next)) {
			if( *obj == this ) {
				*obj = (*obj)->next;
				break;
			}
		}
	}
	virtual void draw() {}
	virtual void update() {} 
	virtual void command(char *cmd) {}
	virtual void emit(char* buff,int max) {}
	virtual void fill(char* buff,int max) {}
};

SimObject* SimObject::objects = 0;

/**********************************************************************/
// a car
/**********************************************************************/

class Car: public SimObject {
public:
	// some constants

	#define LENGTH 0.7	// chassis length
	#define WIDTH 0.5		// chassis width
	#define HEIGHT 0.2	// chassis height
	#define RADIUS 0.18	// wheel radius
	#define STARTZ 0.5	// starting height of chassis
	#define CMASS 1		// chassis mass
	#define WMASS 0.2		// wheel mass

	// dynamics and collision objects (chassis, 3 wheels )

	dBodyID body[4];
	dJointID joint[3];	// joint[0] is the front wheel
	dGeomID geom_group;
	dGeomID box[1];
	dGeomID sphere[3];
	dReal speed,steer;
	Dynamics* parent;
	int chase;

	Car(Dynamics* parent, dReal x, dReal y, dReal z);
	virtual ~Car();
	virtual void update();
	virtual void draw();
	virtual void command(char *);
	virtual void emit(char* buf,int max);
	virtual void fill(char* b,int max);
};

Car::Car(Dynamics* parent, dReal x, dReal y, dReal z): SimObject() {

	this->parent = parent;

  int i;
  dMass m;

  chase = 0;

  // chassis body
  body[0] = dBodyCreate (parent->world);
  dBodySetPosition (body[0],x+0,y+0,z+STARTZ);
  dMassSetBox (&m,1,LENGTH,WIDTH,HEIGHT);
  dMassAdjust (&m,CMASS);
  dBodySetMass (body[0],&m);

  box[0] = dCreateBox (0,LENGTH,WIDTH,HEIGHT);
  dGeomSetBody (box[0],body[0]);
  // wheel bodies
  for (i=1; i<=3; i++) {
    body[i] = dBodyCreate ( parent->world);
    dQuaternion q;
    dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
    dBodySetQuaternion (body[i],q);
    dMassSetSphere (&m,1,RADIUS);
    dMassAdjust (&m,WMASS);
    dBodySetMass (body[i],&m);
    sphere[i-1] = dCreateSphere (0,RADIUS);
    dGeomSetBody (sphere[i-1],body[i]);
  }
  dBodySetPosition (body[1],x+ 0.5*LENGTH,y+ 0,z + STARTZ-HEIGHT*0.5);
  dBodySetPosition (body[2],x+ -0.5*LENGTH,y+ WIDTH*0.5,z + STARTZ-HEIGHT*0.5);
  dBodySetPosition (body[3],x+ -0.5*LENGTH,y+ -WIDTH*0.5,z + STARTZ-HEIGHT*0.5);

  // front and back wheel hinges
  for (i=0; i<3; i++) {
    joint[i] = dJointCreateHinge2 (parent->world,0);
    dJointAttach (joint[i],body[0],body[i+1]);
    const dReal *a = dBodyGetPosition (body[i+1]);
    dJointSetHinge2Anchor (joint[i],a[0],a[1],a[2]);
    dJointSetHinge2Axis1 (joint[i],0,0,1);
    dJointSetHinge2Axis2 (joint[i],0,1,0);
  }

  // set joint suspension
  for (i=0; i<3; i++) {
    dJointSetHinge2Param (joint[i],dParamSuspensionERP,0.4);
    dJointSetHinge2Param (joint[i],dParamSuspensionCFM,0.8);
  }

  // lock back wheels along the steering axis
  for (i=1; i<3; i++) {
    // set stops to make sure wheels always stay in alignment
    dJointSetHinge2Param (joint[i],dParamLoStop,0);
    dJointSetHinge2Param (joint[i],dParamHiStop,0);
    // the following alternative method is no good as the wheels may get out
    // of alignment:
    //   dJointSetHinge2Param (joint[i],dParamVel,0);
    //   dJointSetHinge2Param (joint[i],dParamFMax,dInfinity);
  }

  // create geometry group and add it to the space
  geom_group = dCreateGeomGroup ( parent->space );
  dGeomGroupAdd (geom_group,box[0]);
  dGeomGroupAdd (geom_group,sphere[0]);
  dGeomGroupAdd (geom_group,sphere[1]);
  dGeomGroupAdd (geom_group,sphere[2]);

  speed = 0;
  steer = 0;
}

Car::~Car() {
	// destroy body xxx
	dGeomDestroy (box[0]);
	dGeomDestroy (sphere[0]);
	dGeomDestroy (sphere[1]);
	dGeomDestroy (sphere[2]);
}

void Car::update() {

    // motor
    dJointSetHinge2Param (joint[0],dParamVel2,-speed);
    dJointSetHinge2Param (joint[0],dParamFMax2,0.1);

    // steering
    dReal v = steer - dJointGetHinge2Angle1 (joint[0]);
    if (v > 0.1) v = 0.1;
    if (v < -0.1) v = -0.1;
    v *= 10.0;
    dJointSetHinge2Param (joint[0],dParamVel,v);
    dJointSetHinge2Param (joint[0],dParamFMax,0.2);
    dJointSetHinge2Param (joint[0],dParamLoStop,-0.75);
    dJointSetHinge2Param (joint[0],dParamHiStop,0.75);
    dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1);

}

void Car::draw() {
  dsSetColor (0,1,1);
  dsSetTexture (DS_WOOD);
  dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
  dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides);
  dsSetColor (1,1,1);
  int i;
  for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]),dBodyGetRotation(body[i]),0.02f,RADIUS);

	if( chase )
	{
		const dReal* pos = dBodyGetPosition(body[0]);
		// xxx move viewpoint behind car - xxx wrong - do dynamically
		static float xyz[3] = {0.8317f,-0.9817f,0.8000f};
		static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
		xyz[0] = pos[0] + 0.8317f + 3;
		xyz[1] = pos[1] - 0.9817f - 3;
		xyz[2] = pos[2] + 0.8000f + 2;
		dsSetViewpoint (xyz,hpr);
	}

}

void Car::command(char* command) {
	int cmd;
	float x,y,z;
	sscanf(command,"%d %f %f %f",&cmd,&x,&y,&z);
	switch( cmd ) {
	  case 'a': case 'A':
	    speed += 0.3;
	    break;
	  case 'z': case 'Z':
	    speed -= 0.3;
	    break;
	  case ',':
	    steer -= 0.5;
	    break;
	  case '.':
	    steer += 0.5;
	    break;
	  case 'c':
		chase = 1-chase;
		break;
	  case 'r':
		{

		   float x = 0, y = 0; z = 0;
		  dBodySetPosition (body[0],x+0,y+0,z+STARTZ);
		  dBodySetPosition (body[1],x+ 0.5*LENGTH,y+ 0,z + STARTZ-HEIGHT*0.5);
		  dBodySetPosition (body[2],x+ -0.5*LENGTH,y+ WIDTH*0.5,z + STARTZ-HEIGHT*0.5);
		  dBodySetPosition (body[3],x+ -0.5*LENGTH,y+ -WIDTH*0.5,z + STARTZ-HEIGHT*0.5);
		  dBodyAddForce(body[0],10,15,20);


		}
	  case ' ':
		{

		const dReal* rot = dBodyGetQuaternion( body[0] );
		printf("%f,%f,%f,%f\n",(float)rot[0],(float)rot[1],(float)rot[2],(float)rot[3]);

		dQuaternion q;
		q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0;
		dBodySetQuaternion( body[0], q );
		dBodySetQuaternion( body[1], q );
		dBodySetQuaternion( body[2], q );
		dBodySetQuaternion( body[3], q );

		}
	    speed = 0.0;
	    steer = 0.0;
	default:
		break;
	}
}

void Car::emit(char* buffer,int max) {
	/*
	we've decided to emit state about this car
	we should emit the most useful information we can...

	filters:
	 distance attenuate
	 lack of total change (csum?)
	 lack of change seen by client

	compression:
	 msbs, translations of related hierarchies
	 can't really deliver deltas to client due to udp missing traffic
	
	other:
	 do publish entire id periodically and do publish alive/dead periodically too
	 attenuate on distance
	 get position of body and all parts
	 get forces of body and all parts
	 compress away translations on leaf nodes
	 compress away rotations relative to a normative rotation
	 publish touching systems as bundles

	on receiving end:
	 server may *only* use blending rules.... never hammer.
	 use blending rules
	 what force must be applied to get translation/rotation/forces to fix up?
	 car is at 0,0,0 - must be at 0,10,0 moving 5mph west in 0.1 ms - calc force.
	*/

	dReal b[256];
	dReal* p = b;
	Car* car = this;

	for(int z = 0; z < 3; z++ ) {
		const dReal* pos = dBodyGetPosition( car->body[z] );
		const dReal* rot = dBodyGetQuaternion( car->body[z] );
		const dReal* lve = dBodyGetLinearVel( car->body[z] );
		const dReal* ave = dBodyGetAngularVel( car->body[z] );
		const dReal* force = dBodyGetForce( car->body[z] );
		const dReal* tor = dBodyGetTorque( car->body[z] );
		// blend if possib
		*p++ = pos[0]; *p++ = pos[1]; *p++ = pos[2];
		// blend if posib
		*p++ = rot[0]; *p++ = rot[1]; *p++ = rot[2]; *p++ = rot[3];
		 // set + add blend ontop of
		*p++ = lve[0]; *p++ = lve[1]; *p++ = lve[2];
		*p++ = ave[0]; *p++ = ave[1]; *p++ = ave[2];
		 // set is ok; since is one frame only
		*p++ = force[0]; *p++ = force[1]; *p++ = force[2];
		*p++ = tor[0]; *p++ = tor[1]; *p++ = tor[2];
	}
	// set only
	*p++ = dJointGetHinge2Param (car->joint[0],dParamVel2);
	*p++ = dJointGetHinge2Param (car->joint[0],dParamFMax2);
	*p++ = dJointGetHinge2Param (car->joint[0],dParamVel);
	*p++ = dJointGetHinge2Param (car->joint[0],dParamFMax);
	*p++ = dJointGetHinge2Param (car->joint[0],dParamLoStop);
	*p++ = dJointGetHinge2Param (car->joint[0],dParamHiStop);
	*p++ = dJointGetHinge2Param (car->joint[0],dParamFudgeFactor);
	*p++ = speed;
	*p++ = steer;

	sprintf(buffer,"update %x",car->key);
	for(int i = 0; i < p - b; i++ ) {
		int* val2 = (int*)(b+i);
		sprintf(buffer+strlen(buffer)," %x %x",*val2,*(val2+1));
	}
	sprintf(buffer+strlen(buffer),"\n");
	if( strlen(buffer) >= (unsigned int)max ) {
		printf("this is bad\n");
		exit(0);
	}
}

void Car::fill(char* buffer,int max) {

	int key = 0;
	double r[256];
	int pos = 0;
	double* p = r;

	for(char* c = buffer; *c; pos++ ) {
		while(*c && *c==' ')c++;
		if( pos == 0 ) {
			sscanf(c,"%x",&key);
		} else {
			int* val = (int*)(r+pos-1);
			sscanf(c,"%x %x",val,val+1);
			//r[pos-1] = val;
			while(*c && *c!=' ')c++;
			while(*c && *c==' ')c++;
		}
		while(*c && *c!=' ')c++;
	}

	Car* car = this;
	for( int z = 0; z < 3 ; z++ ) {
		dBodySetPosition( car->body[z], p[0], p[1],p[2] ); p+=3;
		dQuaternion q;
		q[0] = p[0]; q[1] = p[1]; q[2] = p[2]; q[3] = p[3];
		dBodySetQuaternion( car->body[z], q ); p+=4;
		dBodySetLinearVel( car->body[z], p[0], p[1], p[2] ); p+=3;
		dBodySetAngularVel( car->body[z], p[0], p[1], p[2] ); p+=3;
		dBodySetForce( car->body[z], p[0], p[1], p[2] ); p+=3;
		dBodySetTorque( car->body[z], p[0], p[1], p[2] ); p+=3;
	}
	dJointSetHinge2Param (car->joint[0],dParamVel2,*p); p++;
	dJointSetHinge2Param (car->joint[0],dParamFMax2,*p); p++;
	dJointSetHinge2Param (car->joint[0],dParamVel,*p); p++;
	dJointSetHinge2Param (car->joint[0],dParamFMax,*p); p++;
	dJointSetHinge2Param (car->joint[0],dParamLoStop,*p); p++;
	dJointSetHinge2Param (car->joint[0],dParamHiStop,*p); p++;
	dJointSetHinge2Param (car->joint[0],dParamFudgeFactor,*p); p++;
	speed = *p++;
	steer = *p++;
}

/**************************************************************************************/
// networking base
/**************************************************************************************/

static class NetworkClass* head = 0;
static SOCKET udptest = -1;
struct sockaddr_in satest;

class NetworkClass {
private:
	SOCKET udp;
	SOCKET udpreply;
	SOCKET sock;
	FILE* sockin;
	FILE* sockout;
	struct sockaddr_in  ua;
	struct sockaddr_in  sa;
	struct sockaddr_in  ra;
	struct hostent *hp;
	#define MAXHOSTNAME 64
	char	myname[MAXHOSTNAME+1];
	int connectfreq;
	int validreply;

public:
	NetworkClass* next;
	int isserver;
	int key;

	void init() {
		connectfreq = 0;
		key = 0;
		udp = -1;
		udpreply = -1;
		sock = -1;
		sockin = 0;
		sockout = 0;
		myname[0] = 0;
		isserver = 0;
		validreply = 0;
		next = head;
		head = this;
	}

	NetworkClass() {
		init();

		#ifdef USEWIN32
		WSAData ws;
		if( WSAStartup(0x0101,&ws) < 0 ) {
			printf("TcpServer: tcp does not seem to be supported");
			return 0;
		}
		#endif
	}

	NetworkClass(SOCKET sock, int key = 0) {
		init();
		this->key = key;
		openfilehandles(sock);
	}

	virtual ~NetworkClass() {
		stop();
		/*if( isserver ) {
			for(NetworkClass* n = head; n ; ) {
				NetworkClass* temp = n->next;
				if( n != this ) delete n;
				n = temp;
			}
		}*/
	}

	int build(int client,SOCKET& s, int style,struct sockaddr_in& sad, char* hostname, int portnum ) {
		int souse = 1;
		int opts;
		if( !hostname ) {
			myname[0] = 0;
			gethostname(myname, MAXHOSTNAME);
			hostname = myname;
		}

		if ((hp= gethostbyname(hostname)) == NULL) {
			printf("conn failed to resolve hostname\n");
			stop();
			return 0;
		}

		s = socket(AF_INET,style,0);
		if( s < 0 ) {
			printf("no open socket");
			stop();
			return 0;
		}
		if (setsockopt(s, SOL_SOCKET, SO_REUSEADDR, (char *) &souse, sizeof(souse)) < 0) {
			printf("failed on setsockopt(): %s", strerror(errno));
			stop();
			return 0;
		}
		memset(&sad, 0, sizeof(struct sockaddr_in));
		if( client ) {
			memcpy((char *)&sad.sin_addr,hp->h_addr,hp->h_length);
		} else {
			sa.sin_addr.s_addr = htonl(INADDR_ANY);
		}
		sad.sin_port = htons(portnum);
		sad.sin_family = AF_INET;

		if( client ) {
			if( connect(s,(struct sockaddr *)&sad,sizeof(sad)) == -1 ) {
				printf("client no connect?");
				stop();
				return 0;
			}
		} else {
			if( bind(s,(struct sockaddr *)&sad,sizeof(sad)) == -1 ) {
				printf("server no bind tcp");
				stop();
				return 0;
			}
			if( style == SOCK_STREAM ) {
				if( listen(s,100) < 0 ) {
					printf("no listen\n");
					stop();
					return 0;
				}
			}
		}
			
		opts = fcntl(s,F_GETFL);
		opts = opts | O_NONBLOCK;
		fcntl(s,F_SETFL,opts);

		return 1;
	}

	int clientconnect(char* hostname, unsigned int portnum ) {

		isserver = 0;

		// tcp
		if(!build(1,sock,SOCK_STREAM,sa,hostname,portnum)) {
			return 0;
		}

		// udp
		if(!build(1,udp,SOCK_DGRAM,ua,hostname,portnum+1)) {
			return 0;
		}

		openfilehandles(sock);
		printf("client connected to host %s:%d\n",hostname,portnum);
		return 1;
	}

	bool serve(int portnum = 28000 ) {

		isserver = 1;

		if(!build(0,sock,SOCK_STREAM,sa,0,portnum)) {
			return 0;
		}

		if(!build(0,udp,SOCK_DGRAM,ua,0,portnum+1)) {
			return 0;
		}

		printf("server connected\n");
		return 1;
	}

	void serverupdates() {

		if(!isserver) return;

		if( sock < 0 ) {
			return;
		}

		if( connectfreq < 100 ) {
			connectfreq++;
			return;
		}
		connectfreq = 0;

		timeval tv; tv.tv_sec = 0; tv.tv_usec = 1;
		fd_set readmask;
		FD_ZERO( &readmask );
		FD_SET( sock, &readmask );
		int results = select( FD_SETSIZE, &readmask, 0, 0, &tv );
		if( !results ) {
			return;
		}

		#ifdef USEWIN32
		WSAAsyncSelect(sock,GetForegroundWindow(),WM_USER+1,FD_ACCEPT | FD_CLOSE | FD_READ | FD_WRITE );
		#endif

		int conn = accept(sock,NULL,NULL);
		if( conn < 0 ) {
			printf("failed conn\n");
			return;
		}

		connection(conn);
	}

	void connection( int conn ) {
		NetworkClass* client = new NetworkClass( conn, 0 );
		printf("server tracking new conn\n");
		if(!client ) {
			#ifdef USEWIN32
			closesocket(conn);
			#else
			close(conn);
			#endif
		}
	}

	void openfilehandles(SOCKET sock) {
		this->sock = sock;
		int opts = fcntl(sock,F_GETFL);
		opts = opts | O_NONBLOCK;
		fcntl(sock,F_SETFL,opts);
		sockin = fdopen(sock,"r");
		sockout = fdopen(sock,"w");
		printf("conned %x,%x,%x\n",sock,(int)sockin,(int)sockout);
	}

	void stop() {
		printf("stopped network\n");
		if( sockout ) {
			fflush(sockout);
			fclose(sockout);
			sockout = 0;
		}
		if( sockin ) {
			fclose(sockin);
			sockin = 0;
		}
		if( sock >= 0) {
			#ifdef USEWIN32
			closesocket(sock);
			#else
			close(sock);
			#endif
			sock = 0;
		}
		if( udp >= 0 ) {
			close(udp);
		}
	}

	void publish(char* str, int size = 0) {
		if(!sockout ) return;
		fputs(str,sockout);
		fflush(sockout);
	}

	int get(char* buf,int size) {
		buf[0] = 0;
		if(!sockin ) return 0;
		fgets(buf,size,sockin);
		return(strlen(buf));
	}

	void send_udp(char* str,int len) {
		// sending a datagram...
/*
clients have no bound udp port....?
struct sockaddr_in sad;
memset(&sad, 0, sizeof(struct sockaddr_in));
memcpy((char *)&sad.sin_addr,hp->h_addr,hp->h_length);
sad.sin_port = ua.sin_port;
sad.sin_family = AF_INET;
*/
		if( udp > 0 ) {
			sendto(udp,str,strlen(str),0,(struct sockaddr *)&ua,sizeof(ua));
		} else {
			if( udptest != -1 ) { // validreply != 0 && udpreply != -1 ) {
				sendto(udptest,str,strlen(str),0,(struct sockaddr *)&satest,sizeof(satest));
			}
		}
	}

	int get_udp(char* buf,int size) {
		buf[0] = 0;
		if( udp < 0 ) return 0;
		struct sockaddr_in cli;
		int len = sizeof(sockaddr_in);
		int n = recvfrom(udp,(char*)buf,size,0,(struct sockaddr *)&cli,&len);
		if( n <= 0 ) {
		} else {
			if( isserver) {
				satest = cli;
				udptest = udp;
			}
/*
			// remember caller
			for( NetworkClass* find = head; find ; find = find->next ) {
				if( find->udp == 0 && find->udpreply == 0) {
					find->ra = cli; find->validreply = 1; break;
					build(1,find->udpreply,SOCK_DGRAM,ua,"localhost",28000);
				}
			}
 */
			return n;
		}
		return 0;
	}
};

/**************************************************************************************/
// networking app layer
/**************************************************************************************/

class Dynamics* dynamics;
class NetworkManager* network;
static int local_sim_key_code = 0x00010000;
static int remote_machine_code = 0x00010000;

class NetworkManager: public NetworkClass {
public:

	// start server or client
	NetworkManager(int style, char *hostname = "localhost",int portnum = 28000 ) {
		if( style ) {
			clientconnect(hostname,portnum);
			printf("client: logging into %s:%d\n",hostname,portnum);
			publish("login joe\n");
		} else {
			printf("server: waiting for connections at %s:%d\n",hostname,portnum);
			serve(portnum);
			event(this,"id 0x00010000\n");
			event(this,"new 0x00010001 0 2.0 0 0\n");
			key = 0x00010000;
		}
	}

	// locally do an event
	void event(NetworkClass* s,char* buffer) {
		if(!strncmp(buffer,"login",5)) {
			// remote has logged into me; accept their login
			remote_machine_code += 0x00010000;
			s->key = remote_machine_code;
			printf("got login for new client with given id %x\n",remote_machine_code);
			char buf[512];
			sprintf(buf,"id %x\n",remote_machine_code);
			s->publish(buf);
			// tell client to make an obj
			sprintf(buf,"new %x %d %f %f %f\n",remote_machine_code + 1, 0, 0.0f,0.0f,0.0f);
			s->publish(buf);
		}
		else if(!strncmp(buffer,"id",2)) {
			// remote sent us our own unique id
			sscanf(buffer+2,"%x",&local_sim_key_code);
			remote_machine_code = local_sim_key_code; // bang on this - not used on client.
			printf("local id is now %x\n",local_sim_key_code);
			if( s ) s->key = local_sim_key_code;
		}
		else if(!strncmp(buffer,"new",3)) {
			// remote has asked us to make something at location xyz
			int id,type;
			float x,y,z;
			sscanf(buffer+3,"%x %d %f %f %f",&id,&type,&x,&y,&z);
			printf("got new request: 0x%x,%d,%f,%f,%f\n",id,type,x,y,z);
			Car* car = new Car(dynamics,x,y,z);
			printf("done\n");
			car->key = id;
		}
		else if(!strncmp(buffer,"update",3)) {

			Car* car = 0;
			int clientkey;
			double pos[3];
			int* p = (int*)pos;
			sscanf(buffer+6,"%x %x %x %x %x %x %x",&clientkey,p,p+1,p+2,p+3,p+4,p+5);
			for( SimObject* obj = SimObject::objects; obj; obj = obj->next ) {
				if( obj->key == clientkey ) {
					car = (Car*)obj;
					break;
				}
			}
			if(!car) {
				// if not found then add
				Car* car = new Car(dynamics,pos[0],pos[1],pos[2]);
				printf("new car %x at %f %f %f\n",clientkey,(float)pos[0],(float)pos[1],(float)pos[2]);
				car->key = clientkey;
			}
			if( car ) car->fill(buffer + 6,4096);
 		}
	}

	// update server or client mode

	void update() {

		// if server do server updates
		serverupdates();

		// server and client: handle incoming traffic from anywhere
		for(NetworkClass* conn = head; conn ; conn = conn->next ) {
			char buffer[4096];

			// get all tcp traffic
			while( conn->get(buffer,4096) > 0 ) {
				event(conn,buffer);
			}

			// get all udp traffic
			while( conn->get_udp(buffer,4096) > 0 ) {
				event(conn,buffer);
			}
		}

		// hack: throttle outbound traffic here - later use attenuation

		static int counter = 0;
		counter++;
		if( counter & 15 ) {
			return;
		}

		// server and client: broadcast state that is locally owned
		for(NetworkClass* s = head; s ; s = s->next ) {
			for(SimObject* obj = SimObject::objects; obj; obj = obj->next ) {
				if(!isserver) {
					// if we are a client then emit information about our objects only
					if( (obj->key & 0xffff0000) != (local_sim_key_code & 0xffff0000) ) {
						continue;
					}
				} else {
					// if we are a server - emit all objects to all machines except real owners
					if( (obj->key & 0xffff0000) == ( s->key & 0xffff0000) ) {
						continue;
					}
				}

				char buffer[4096];
				obj->emit(buffer,4096);
				s->send_udp(buffer,4096);
				//if( counter & 63 ) ; else { s->send_udp("hi",2); }
			}
		}
	}
};

/**************************************************************************************/
// game
/**************************************************************************************/

void key_events(int cmd ) {
	int key = ( local_sim_key_code & 0xffff0000 ) + 1;
	for(SimObject* obj = SimObject::objects; obj; obj = obj->next ) {
		if( obj->key == key ) {
			char buffer[512];
			sprintf(buffer,"%d 0 0 0\n",cmd);
			obj->command(buffer);
			return;
		}
	}
}

static void mainLoop (int pause) {

	// update local requests (before integrating and before publishing)
	for(SimObject* obj = SimObject::objects; obj; obj = obj->next ) {
		obj->update();
	}

	// broadcast state and receive state corrections (soon after local user updates for client)
	if(network && !network->isserver) {
		network->update();
	}

	// update simulation (before server sends freshest state)
	if( dynamics ) {
		dynamics->update();
	}

	// broadcast state and receive state corrections (for server it is best to do it here)
	if(network && network->isserver) {
		network->update();
	}

	// only if not headless do we draw
	if( headless ) return;

	// redraw (while server network is dealing with packets being sent)
	for(SimObject* obj = SimObject::objects; obj; obj = obj->next ) {
		obj->draw();
	}
}

void start() {
	printf ("Press:\t'a' to increase speed.\n"
	  "\t'z' to decrease speed.\n"
	  "\t',' to steer left.\n"
	  "\t'.' to steer right.\n"
	  "\t' ' to reset speed and steering.\n");

	// move camera somewhere useful
	{
		//const dReal* pos = dBodyGetPosition(body[0]);
		float pos[3] = { 3,-3,2 };
		// xxx move viewpoint behind car - xxx wrong - do dynamically
		static float xyz[3] = {0.8317f,-0.9817f,0.8000f};
		static float hpr[3] = {121.0000f,-27.5000f,0.0000f};
		xyz[0] = pos[0] + 0.8317f;
		xyz[1] = pos[1] - 0.9817f;
		xyz[2] = pos[2] + 0.8000f;
		dsSetViewpoint (xyz,hpr);
	}

}

int main (int argc, char **argv) {

	dynamics = new Dynamics();
	network = new NetworkManager( !strcmp(argv[0],"test_buggy") );

	if( argc > 1 && !stricmp(argv[1],"headless")) {
		headless = 1;
		for(;;) {
			mainLoop(0);
		}
	} else {
		dsFunctions fn;
		fn.version = DS_VERSION;
		fn.start = &start;
		fn.step = &mainLoop;
		fn.command = &key_events;
		fn.stop = 0;
		fn.path_to_textures = "../../drawstuff/textures";
		dsSimulationLoop (0,0,352,288,&fn);
	}

	delete network;
	delete dynamics;
}