Remove dependency on Xinerama

Improve camfinder a little
Add data_recorder.c
Closes #1 making libsurvive much more pluggable.

10 files changed, 293 insertions, 232 deletions

diff --git a/Makefile b/Makefile
index 29c05a1..d688287 100644
--- a/Makefile
+++ b/Makefile
@@ -1,10 +1,12 @@
-all : test
+all : data_recorder test
 
 CFLAGS:=-Iinclude -fPIC -g -Os -Iredist
-LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lXinerama
-DEBUGSTUFF:=redist/os_generic.o redist/DrawFunctions.o redist/XDriver.o
+LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11
 
-test : test.c lib/libsurvive.so
+test : test.c lib/libsurvive.so redist/os_generic.o
+	gcc -o $@ $^ $(LDFLAGS) $(CFLAGS)
+
+data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.o redist/DrawFunctions.o redist/XDriver.o
 	gcc -o $@ $^ $(LDFLAGS) $(CFLAGS)
 
 lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o $(DEBUGSTUFF)
diff --git a/data_recorder.c b/data_recorder.c
new file mode 100644
index 0000000..24c8bc1
--- /dev/null
+++ b/data_recorder.c
@@ -0,0 +1,160 @@
+//Data recorder mod with GUI showing light positions.
+
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <survive.h>
+#include <string.h>
+#include <os_generic.h>
+#include <DrawFunctions.h>
+
+struct SurviveContext * ctx;
+
+void HandleKey( int keycode, int bDown )
+{
+	if( !bDown ) return;
+
+	if( keycode == 'O' || keycode == 'o' )
+	{
+		survive_usb_send_magic(ctx,1);
+	}
+	if( keycode == 'F' || keycode == 'f' )
+	{
+		survive_usb_send_magic(ctx,0);
+	}
+}
+
+void HandleButton( int x, int y, int button, int bDown )
+{
+}
+
+void HandleMotion( int x, int y, int mask )
+{
+}
+
+int bufferpts[32*2];
+char buffermts[32*128];
+int buffertimeto[32];
+
+void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
+{
+	if( acode == -1 ) return;
+
+	if( acode == 0 || acode == 2 ) //data = 0
+	{
+		printf( "L X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length );
+		if( strcmp( so->codename, "HED" ) == 0 )
+		{
+			bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500;
+			buffertimeto[sensor_id] = 0;
+		}
+	}
+	if( acode == 1 || acode == 3 ) //data = 1
+	{
+		printf( "L Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length );
+		if( strcmp( so->codename, "HED" ) == 0 )
+		{
+			bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500;
+			buffertimeto[sensor_id] = 0;
+		}
+	}
+
+
+	if( acode == 4 || acode == 6 ) //data = 0
+	{
+		printf( "R X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length );
+		if( strcmp( so->codename, "HED" ) == 0 )
+		{
+			bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500;
+			buffertimeto[sensor_id] = 0;
+		}
+	}
+	if( acode == 5 || acode == 7 ) //data = 1
+	{
+		printf( "R Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length );
+		if( strcmp( so->codename, "HED" ) == 0 )
+		{
+			bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500;
+			buffertimeto[sensor_id] = 0;
+		}
+	}
+
+}
+
+void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
+{
+	//if( so->codename[0] == 'H' )
+	if( 1 )
+	{
+		printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id );
+	}
+}
+
+
+
+
+void * GuiThread( void * v )
+{
+	short screenx, screeny;
+	while(1)
+	{
+		CNFGHandleInput();
+		CNFGClearFrame();
+		CNFGColor( 0xFFFFFF );
+		CNFGGetDimensions( &screenx, &screeny );
+
+		int i;
+		for( i = 0; i < 32; i++ )
+		{
+			if( buffertimeto[i] < 5 )
+			{
+				uint32_t color = i * 3231349;
+				uint8_t r = color & 0xff;
+				uint8_t g = (color>>8) & 0xff;
+				uint8_t b = (color>>16) & 0xff;
+				r = (r * (5-buffertimeto[i])) / 5 ;
+				g = (g * (5-buffertimeto[i])) / 5 ;
+				b = (b * (5-buffertimeto[i])) / 5 ;
+				CNFGColor( (b<<16) | (g<<8) | r );
+				CNFGTackRectangle( bufferpts[i*2+0], bufferpts[i*2+1], bufferpts[i*2+0] + 5, bufferpts[i*2+1] + 5 );
+				CNFGPenX = bufferpts[i*2+0]; CNFGPenY = bufferpts[i*2+1];
+				CNFGDrawText( buffermts, 2 );			
+				buffertimeto[i]++;
+			}
+		}
+
+
+		CNFGSwapBuffers();
+		OGUSleep( 10000 );
+	}
+}
+
+
+
+int main()
+{
+	ctx = survive_init( );
+
+	survive_install_light_fn( ctx,  my_light_process );
+	survive_install_imu_fn( ctx,  my_imu_process );
+
+
+	CNFGBGColor = 0x000000;
+	CNFGDialogColor = 0x444444;
+	CNFGSetup( "Survive GUI Debug", 640, 480 );
+	OGCreateThread( GuiThread, 0 );
+	
+
+	if( !ctx )
+	{
+		fprintf( stderr, "Fatal. Could not start\n" );
+		return 1;
+	}
+
+	while(survive_poll(ctx) == 0)
+	{
+		//Do stuff.
+	}
+}
+
diff --git a/include/survive.h b/include/survive.h
index 637bd09..61a6620 100644
--- a/include/survive.h
+++ b/include/survive.h
@@ -1,17 +1,53 @@
 #ifndef _SURVIVE_H
 #define _SURVIVE_H
 
+#include <stdint.h>
+
+#define SV_FLOAT  		double
+
 struct SurviveContext;
 
-struct SurviveContext * survive_init( void(*faultfunction)( struct SurviveContext * ctx, const char * fault ),
-	void(*notefunction)( struct SurviveContext * ctx, const char * note )  );
+//DANGER: This structure may be redefined 
+
+struct SurviveObject
+{
+	struct SurviveContext * ctx;
+	char    codename[4];
+	int16_t buttonmask;
+	int16_t axis1;
+	int16_t axis2;
+	int16_t axis3;
+	int8_t  charge;
+	int8_t  charging:1;
+	int8_t  ison:1;
+	int sensors;
+
+	int nr_locations;
+	SV_FLOAT * sensor_locations;
+};
+
+typedef void (*text_feedback_fnptr)( struct SurviveContext * ctx, const char * fault );
+typedef void (*light_process_func)( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
+typedef void (*imu_process_func)( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id );
+
+struct SurviveContext * survive_init();
+
+//For any of these, you may pass in 0 for the function pointer to use default behavior.
+void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_fnptr fbp );
+void survive_install_error_fn( struct SurviveContext * ctx,  text_feedback_fnptr fbp );
+void survive_install_light_fn( struct SurviveContext * ctx,  light_process_func fbp );
+void survive_install_imu_fn( struct SurviveContext * ctx,  imu_process_func fbp );
 
 void survive_close( struct SurviveContext * ctx );
 int survive_poll();
 
 
+
+//Utilitiy functions.
 int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen );
-int survive_usb_send_magic(struct SurviveContext * ctx, int on );
+
+//TODO: Need to make this do haptic responses for hands.
+int survive_usb_send_magic( struct SurviveContext * ctx, int on );
 
 #endif
 
diff --git a/redist/XDriver.c b/redist/XDriver.c
index 942fe0f..507ca95 100644
--- a/redist/XDriver.c
+++ b/redist/XDriver.c
@@ -2,7 +2,7 @@
 //portions from 
 //http://www.xmission.com/~georgeps/documentation/tutorials/Xlib_Beginner.html
 
-#define HAS_XINERAMA
+//#define HAS_XINERAMA
 
 #include "DrawFunctions.h"
 
diff --git a/src/survive.c b/src/survive.c
index edcbb86..773f02a 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -17,13 +17,29 @@ static int jsoneq(const char *json, jsmntok_t *tok, const char *s) {
 	return -1;
 }
 
-struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, const char * fault ), void(*notefunction)( struct SurviveContext * ctx, const char * note ) )
+
+static void survivefault( struct SurviveContext * ctx, const char * fault )
+{
+	fprintf( stderr, "Error: %s\n", fault );
+	exit( -1 );
+}
+
+static void survivenote( struct SurviveContext * ctx, const char * fault )
+{
+	fprintf( stderr, "Info: %s\n", fault );
+}
+
+
+struct SurviveContext * survive_init()
 {
 	int r = 0;
-	struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext  ) );
+	struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext ) );
+
+	ctx->faultfunction = survivefault;
+	ctx->notefunction = survivenote;
 
-	ctx->faultfunction = ff;
-	ctx->notefunction = notefunction;
+	ctx->lightproc = survive_default_light_process;
+	ctx->imuproc = survive_default_imu_process;
 
 	ctx->headset.sensors = 32;
 	ctx->headset.ctx = ctx;
@@ -44,8 +60,6 @@ struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, co
 		return 0;
 	}
 
-
-#if 1
 	//Next, pull out the config stuff.
 	{
 		char * ct0conf = 0;
@@ -123,12 +137,42 @@ struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, co
 		}
 
 	}
-#endif
 
 	//ctx->headset->photos = malloc( ctx->headset->sensors * sizeof(struct SurvivePhoto) );
 	return ctx;
 }
 
+void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_fnptr fbp )
+{
+	if( fbp )
+		ctx->notefunction = fbp;
+	else
+		ctx->notefunction = survivenote;
+}
+
+void survive_install_error_fn( struct SurviveContext * ctx,  text_feedback_fnptr fbp )
+{
+	if( fbp )
+		ctx->faultfunction = fbp;
+	else
+		ctx->faultfunction = survivefault;
+}
+
+void survive_install_light_fn( struct SurviveContext * ctx, light_process_func fbp )
+{
+	if( fbp )
+		ctx->lightproc = fbp;
+	else
+		ctx->lightproc = survive_default_light_process;
+}
+
+void survive_install_imu_fn( struct SurviveContext * ctx,  imu_process_func fbp )
+{
+	if( fbp )
+		ctx->imuproc = fbp;
+	else
+		ctx->imuproc = survive_default_imu_process;
+}
 
 void survive_close( struct SurviveContext * ctx )
 {
diff --git a/src/survive_data.c b/src/survive_data.c
index c84ab09..88c9e7d 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -47,7 +47,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 				ct->total_photo_time = 0;
 				ct->total_photos = 0;
 				ct->total_pulsecode_time = 0;
-				survive_light_process( so, le->sensor_id, -1, 0, le->timestamp, deltat );
+				ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat );
 			}
 		}
 		else
@@ -73,7 +73,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 
 		if( offset_from < 380000 )
 		{
-			survive_light_process( so, le->sensor_id, acode, offset_from, le->timestamp, le->length );
+			ct->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length );
 		}
 	}
 	else
@@ -158,7 +158,7 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 	if( ( ( type & 0xe8 ) == 0xe8 ) || doimu ) //Hmm, this looks kind of yucky... we can get e8's that are accelgyro's but, cleared by first propset.
 	{
 		propset |= 2;
-		survive_imu_process( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 );
+		w->ctx->imuproc( w, (int16_t *)&readdata[1], (time1<<24)|(time2<<16)|readdata[0], 0 );
 		int16_t * k = (int16_t *)readdata+1;
 		//printf( "Match8 %d %d %d %d %d %3d %3d\n", qty, k[0], k[1], k[2], k[3], k[4], k[5] );
 		readdata += 13; qty -= 13;
@@ -178,16 +178,6 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 		readdata--;
 		*readdata = type;
 
-#if 1
-		//             good        good          maybe?         probably wrong
-		printf( "POST %d: %4d (%02x%02x) - ", propset, qty, time1, time2 );
-		for( i = 0; i < qty + 4; i++ )
-		{
-			printf( "%02x ", readdata[i] );
-		}
-		printf("\n");
-#endif
-
 		uint8_t * end = &readdata[qty];
 		uint32_t mytime = (end[1] << 0)|(end[2] << 8)|(end[3] << 16);
 
@@ -289,7 +279,8 @@ static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
 		int i;
 		for( i = lese-1; i >= 0; i-- )
 		{
-			printf( "%d: %d [%d]\n", les[i].sensor_id, les[i].length, les[i].timestamp );
+			//printf( "%d: %d [%d]\n", les[i].sensor_id, les[i].length, les[i].timestamp );
+			handle_lightcap( w, &les[i] );
 		}
 
 		return;
@@ -345,14 +336,6 @@ void survive_data_cb( struct SurviveUSBInterface * si )
 		//printf( "%d -> ", size );
 		for( i = 0; i < 3; i++ )
 		{
-/*
-			for( i = 0; i < 17; i++ )
-			{
-				printf( "%02x ", readdata[i] );
-			}
-			printf( "\n" );
-*/
-			//handle_lightdata( (struct LightpulseStructure *)readdata );
 			int16_t * acceldata = (int16_t*)readdata;
 			readdata += 12;
 			uint32_t timecode = POP4;
@@ -363,7 +346,7 @@ void survive_data_cb( struct SurviveUSBInterface * si )
 			if( cd > 0 )
 			{
 				ctx->oldcode = code;
-				survive_imu_process( &ctx->headset, acceldata, timecode, code );
+				ctx->imuproc( &ctx->headset, acceldata, timecode, code );
 			}
 		}
 
diff --git a/src/survive_internal.h b/src/survive_internal.h
index 40659e8..ac378a2 100644
--- a/src/survive_internal.h
+++ b/src/survive_internal.h
@@ -19,6 +19,7 @@
 #include <stdint.h>
 #include <libusb-1.0/libusb.h>
 #include <zlib.h>
+#include <survive.h>
 
 #define SV_INFO( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->notefunction( ctx, stbuff ); }
 #define SV_ERROR( x... ) { char stbuff[1024]; sprintf( stbuff, x ); ctx->faultfunction( ctx, stbuff ); }
@@ -26,8 +27,6 @@
 //XXX TODO This one needs to be rewritten.
 #define SV_KILL()		exit(0)
 
-#define SV_FLOAT  		double
-
 #define USB_DEV_HMD			0
 #define USB_DEV_LIGHTHOUSE	1
 #define USB_DEV_WATCHMAN1	2
@@ -61,33 +60,20 @@ struct SurviveUSBInterface
 	const char * hname;			//human-readable names
 };
 
-
-struct SurviveObject
-{
-	struct SurviveContext * ctx;
-	char    codename[4];
-	int16_t buttonmask;
-	int16_t axis1;
-	int16_t axis2;
-	int16_t axis3;
-	int8_t  charge;
-	int8_t  charging:1;
-	int8_t  ison:1;
-	int sensors;
-
-	int nr_locations;
-	SV_FLOAT * sensor_locations;
-};
+//This is defined in survive.h
+struct SurviveObject;
 
 struct SurviveContext
 {
 	//USB Subsystem
     struct libusb_context* usbctx;
-	void(*faultfunction)( struct SurviveContext * ctx, const char * fault );
-	void(*notefunction)( struct SurviveContext * ctx, const char * fault );
 	struct libusb_device_handle * udev[MAX_USB_DEVS];
 	struct SurviveUSBInterface uiface[MAX_INTERFACES];
 
+	text_feedback_fnptr faultfunction;
+	text_feedback_fnptr notefunction;
+	light_process_func lightproc;
+	imu_process_func imuproc;
 
 	//Flood info, for calculating which laser is currently sweeping.
 	int8_t oldcode;
@@ -113,13 +99,8 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno,
 void survive_data_cb( struct SurviveUSBInterface * si );
 
 //Accept higher-level data.
-void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
-void survive_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id );
-
-
-//Util
-int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen );
-
+void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length );
+void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id );
 
 #endif
 
diff --git a/src/survive_process.c b/src/survive_process.c
index 046971f..d3a8c4a 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -8,56 +8,14 @@ int bufferpts[32*2];
 char buffermts[32*128];
 int buffertimeto[32];
 
-void survive_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
+void survive_default_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
 {
-	if( acode == -1 ) return;
-
-	if( acode == 0 || acode == 2 ) //data = 0
-	{
-		printf( "L X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length );
-		bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500;
-		buffertimeto[sensor_id] = 0;
-		//printf( "X: %d\n",bufferpts[sensor_id*2+0] );
-			//480-(timeinsweep)/1000; // Full scan
-	}
-	if( acode == 1 || acode == 3 ) //data = 1
-	{
-		printf( "L Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length );
-		bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500;
-		//printf( "Y: %d\n",bufferpts[sensor_id*2+1] );
-		buffertimeto[sensor_id] = 0;
-
-			//480-(timeinsweep)/1000; //Full scan
-	}
-
-
-	if( acode == 4 || acode == 6 ) //data = 0
-	{
-		printf( "R X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length );
-		bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500;
-		buffertimeto[sensor_id] = 0;
-		//printf( "X: %d\n",bufferpts[sensor_id*2+0] );
-			//480-(timeinsweep)/1000; // Full scan
-	}
-	if( acode == 5 || acode == 7 ) //data = 1
-	{
-		printf( "R Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length );
-		bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500;
-		//printf( "Y: %d\n",bufferpts[sensor_id*2+1] );
-		buffertimeto[sensor_id] = 0;
-
-			//480-(timeinsweep)/1000; //Full scan
-	}
-
-
-	//timeinsweep = 200,000 1/48,000,000ths of a second is "center-of-image"
+	//TODO: Writeme!
 }
 
-void survive_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
+void survive_default_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
 {
-	//if( so->codename[0] == 'H' )
-	if( 1 )
-	{
-		printf( "I %s %d %d %d %d %d %d %d %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id );
-	}
+	//TODO: Writeme!
 }
+
+
diff --git a/test.c b/test.c
index 1f6778a..537b2e5 100644
--- a/test.c
+++ b/test.c
@@ -9,106 +9,12 @@
 
 struct SurviveContext * ctx;
 
-void survivefault( struct SurviveContext * ctx, const char * fault )
-{
-	fprintf( stderr, "Error: %s\n", fault );
-	exit( -1 );
-}
-
-void survivenote( struct SurviveContext * ctx, const char * fault )
-{
-	fprintf( stderr, "Info: %s\n", fault );
-}
-
-
-void HandleKey( int keycode, int bDown )
-{
-	if( !bDown ) return;
-
-	if( keycode == 'O' || keycode == 'o' )
-	{
-		survive_usb_send_magic(ctx,1);
-	}
-	if( keycode == 'F' || keycode == 'f' )
-	{
-		survive_usb_send_magic(ctx,0);
-	}
-}
-
-void HandleButton( int x, int y, int button, int bDown )
-{
-}
-
-void HandleMotion( int x, int y, int mask )
-{
-}
-
-extern int bufferpts[32*2];
-extern char buffermts[32*128];
-extern int buffertimeto[32];
-
-void * GuiThread( void * v )
-{
-	short screenx, screeny;
-	while(1)
-	{
-		CNFGHandleInput();
-		CNFGClearFrame();
-		CNFGColor( 0xFFFFFF );
-		CNFGGetDimensions( &screenx, &screeny );
-
-		int i;
-		for( i = 0; i < 32; i++ )
-		{
-			if( buffertimeto[i] < 5 )
-			{
-				uint32_t color = i * 3231349;
-				uint8_t r = color & 0xff;
-				uint8_t g = (color>>8) & 0xff;
-				uint8_t b = (color>>16) & 0xff;
-				r = (r * (5-buffertimeto[i])) / 5 ;
-				g = (g * (5-buffertimeto[i])) / 5 ;
-				b = (b * (5-buffertimeto[i])) / 5 ;
-				CNFGColor( (b<<16) | (g<<8) | r );
-				CNFGTackRectangle( bufferpts[i*2+0], bufferpts[i*2+1], bufferpts[i*2+0] + 5, bufferpts[i*2+1] + 5 );
-				CNFGPenX = bufferpts[i*2+0]; CNFGPenY = bufferpts[i*2+1];
-				CNFGDrawText( buffermts, 2 );			
-				buffertimeto[i]++;
-			}
-		}
-
-
-		CNFGSwapBuffers();
-		OGUSleep( 10000 );
-	}
-}
-
-
-
 int main()
 {
-/*	int i;
-	uint8_t input[] = {  0x7a, 0x01, 0x48, 0xc4, 0x1e, 0x1a, 0xfe, 0x6f, 0x6a, 0xf7, 0x25, 0x34 };
-//	uint8_t input[] = {  0x1f, 0x8b, 0x08, 0x00, 0xc2, 0x45, 0x43, 0x58, 0x00, 0x03, 0xcb, 0xc8, 0xe4, 0x02, 0x00, 0x7a, 0x7a, 0x6f, 0xed, 0x03, 0x00, 0x00, 0x00 };
-//	uint8_t input[] = { 0x78, 0xda, 0xcb, 0xc8, 0x04, 0x00, 0x01, 0x3b, 0x00, 0xd2 };
-
-	uint8_t output[1024];
-
-	int r = survive_simple_inflate( 0, input, sizeof( input ), output, sizeof(output) );
+	int magicon = 0;
+	double Start = OGGetAbsoluteTime();
 
-	printf( "%d: ", r );
-	for( i = 0 ;i < r; i++ )
-	{
-		printf( "%02x ", output[i] );
-	}
-	return 0;*/
-	ctx = survive_init( &survivefault, &survivenote );
-
-	CNFGBGColor = 0x000000;
-	CNFGDialogColor = 0x444444;
-	CNFGSetup( "Survive GUI Debug", 640, 480 );
-	OGCreateThread( GuiThread, 0 );
-	
+	ctx = survive_init( );
 
 	if( !ctx )
 	{
@@ -118,6 +24,12 @@ int main()
 
 	while(survive_poll(ctx) == 0)
 	{
+		double Now = OGGetAbsoluteTime();
+		if( Now > (Start+1) && !magicon )
+		{
+			survive_usb_send_magic(ctx,1);
+			magicon = 1;
+		}
 		//Do stuff.
 	}
 }
diff --git a/tools/planetest2/camfind.c b/tools/planetest2/camfind.c
index 6caee1a..c987e46 100644
--- a/tools/planetest2/camfind.c
+++ b/tools/planetest2/camfind.c
@@ -5,7 +5,7 @@
 #include <math.h>
 
 #define PTS 32
-#define MAX_CHECKS 20000
+#define MAX_CHECKS 30000
 #define MIN_HITS_FOR_VALID 10
 
 FLT hmd_points[PTS*3];
@@ -28,7 +28,7 @@ int main()
 	int i;
 
 	//Load either 'L' (LH1) or 'R' (LH2) data.
-	if( LoadData( 'R' ) ) return 5;
+	if( LoadData( 'L' ) ) return 5;
 
 	int opti = 0;
 	int cycle = 0;
@@ -39,17 +39,6 @@ int main()
 	FLT bestxyz[3];
 	memcpy( bestxyz, LighthousePos, sizeof( LighthousePos ) );
 
-	if( 0 )
-	{
-		LighthousePos[0] = .0531311;
-		LighthousePos[1] = 1.2911;
-		LighthousePos[2] = 2.902;
-		RunOpti(1);
-		FLT ft = RunTest(1);
-		printf( "Final RMS: %f\n", ft );
-		return 0;
-	}
-
 	//STAGE1 1: Detemine vectoral position from lighthouse to target.  Does not determine lighthouse-target distance.
 	//This also is constantly optimizing the lighthouse quaternion for optimal spotting.
 	FLT fullrange = 5; //Maximum search space for positions.  (Relative to HMD)
@@ -231,7 +220,7 @@ FLT RunOpti( int print )
 		cross3d( xproduct, UsToTarget, RayShootOut );
 		FLT dist = magnitude3d( xproduct );
 		errorsq += dist*dist;
-		if( print ) printf( "%f (%d) ", dist, p );
+		if( print ) printf( "%f (%d(%d/%d))\n", dist, p, hmd_point_counts[p*2+0], hmd_point_counts[p*2+1] );
 	}
 	if( print ) printf( " = %f\n", sqrt( errorsq ) );
 	return sqrt(errorsq);
@@ -270,11 +259,6 @@ FLT RunTest( int print )
 
 		//plane_normal is our normal / LighthousePos is our point.  
 		FLT w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+1], hmd_points[pt*3+2] };
-
-//May be able to remove this.
-//		FLT w0[] = { hmd_points[pt*3+0], hmd_points[pt*3+2],-hmd_points[pt*3+1] };  //XXX WRONG Why does this produce the right answers?
-
-		//FLT w0[] = { 0, 0, 0 };
 		FLT d = -(plane_normal[0] * LighthousePos[0] + plane_normal[1] * LighthousePos[1] + plane_normal[2] * LighthousePos[2]);
 		FLT D =   plane_normal[0] * w0[0]            + plane_normal[1] * w0[1]            + plane_normal[2] * w0[2] + d;
 			//Point line distance assuming ||normal|| = 1.
@@ -291,8 +275,9 @@ FLT RunTest( int print )
 	if( print )
 	{
 		int p;
-		printf( "Imagespace comparison:\n" );
+		printf( "POS:  %f %f %f %f\n", PFFOUR(LighthousePos ) );
 		printf( "QUAT: %f %f %f %f\n", PFFOUR(LighthouseQuat ) );
+		printf( "Imagespace comparison:\n" );
 		for( p = 0; p < 32; p++ )
 		{
 			if( hmd_point_counts[p*2+0] < MIN_HITS_FOR_VALID || hmd_point_counts[p*2+1] < MIN_HITS_FOR_VALID ) continue;
@@ -381,7 +366,7 @@ int LoadData( char Camera )
 
 
 	int xck = 0;
-	f = fopen( "third_test_with_time_lengths.csv", "r" );
+	f = fopen( "testfive.csv", "r" );
 	if( !f ) { fprintf( stderr, "Error: can't open two lighthouses test data.\n" ); return -11; }
 	while( !feof(f) && !ferror(f) )
 	{