Merge branch 'master' of https://github.com/cnlohr/libsurvive

18 files changed, 519 insertions, 1120 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 7ae1402..61a6620 100644
--- a/include/survive.h
+++ b/include/survive.h
@@ -1,16 +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 );
 
+//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/redist/linmath.c b/redist/linmath.c
index 808bfbf..60fbc21 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -63,7 +63,22 @@ void copy3d( FLT * out, const FLT * in )
 	out[2] = in[2];
 }
 
+FLT magnitude3d( FLT * a )
+{
+	return sqrt( a[0]*a[0] + a[1]*a[1] + a[2]*a[2] );
+}
 
+FLT anglebetween3d( FLT * a, FLT * b )
+{
+	FLT an[3];
+	FLT bn[3];
+	normalize3d( an, a );
+	normalize3d( bn, b );
+	FLT dot = dot3d( a, b );
+	if( dot < -0.9999999 ) return LINMATHPI;
+	if( dot >  0.9999999 ) return 0;
+	return acos( dot );
+}
 
 /////////////////////////////////////QUATERNIONS//////////////////////////////////////////
 //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman)
@@ -74,7 +89,7 @@ void copy3d( FLT * out, const FLT * in )
 
 void quatsetnone( FLT * q )
 {
-	q[0] = 0; q[1] = 0; q[2] = 0; q[3] = 1;
+	q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0;
 }
 
 void quatcopy( FLT * qout, const FLT * qin )
diff --git a/redist/linmath.h b/redist/linmath.h
index 9b21347..5cc7b7d 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -6,6 +6,12 @@
 //Yes, I know it's kind of arbitrary.
 #define DEFAULT_EPSILON 0.001
 
+//For printf
+#define PFTHREE(x) x[0], x[1], x[2]
+#define PFFOUR(x) x[0], x[1], x[2], x[3]
+
+#define LINMATHPI 3.141592653589
+
 //If you want, you can define FLT to be double for double precision.
 #ifndef FLT
 #define FLT float
@@ -30,8 +36,9 @@ int compare3d( const FLT * a, const FLT * b, FLT epsilon );
 
 void copy3d( FLT * out, const FLT * in );
 
+FLT magnitude3d( FLT * a );
 
-
+FLT anglebetween3d( FLT * a, FLT * b );
 
 //Quaternion things...
 
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 11677e0..88c9e7d 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -41,11 +41,14 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 		int32_t deltat = (uint32_t)le->timestamp - (uint32_t)ct->last_photo_time;
 		if( deltat > 2000 || deltat < -2000 )		//New pulse. (may be inverted)
 		{
-			ct->last_photo_time = le->timestamp;
-			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 );
+			if( le->timestamp - ct->last_photo_time > 80000 )
+			{
+				ct->last_photo_time = le->timestamp;
+				ct->total_photo_time = 0;
+				ct->total_photos = 0;
+				ct->total_pulsecode_time = 0;
+				ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat );
+			}
 		}
 		else
 		{
@@ -54,7 +57,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 			ct->total_photos++;
 		}
 	}
-	else if( le->length < 900 && le->length > 50 && ct->total_photos )
+	else if( le->length < 1200 && le->length > 40 && ct->total_photos )
 	{
 		int32_t dl = (ct->total_photo_time/ct->total_photos);
 		int32_t tpco = (ct->total_pulsecode_time/ct->total_photos);
@@ -70,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 );
+			ct->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length );
 		}
 	}
 	else
@@ -155,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;
@@ -175,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);
 
@@ -286,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;
@@ -342,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;
@@ -360,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 d6168b2..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 );
-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 268fdfc..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  )
+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\n", so->codename, timecode, sensor_id, acode, timeinsweep );
-		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\n", so->codename, timecode, sensor_id, acode, timeinsweep );
-		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\n", so->codename, timecode, sensor_id, acode, timeinsweep );
-		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\n", so->codename, timecode, sensor_id, acode, timeinsweep );
-		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/src/survive_usb.c b/src/survive_usb.c
index 07e5cb4..4f341ec 100644
--- a/src/survive_usb.c
+++ b/src/survive_usb.c
@@ -233,12 +233,22 @@ int survive_usb_init( struct SurviveContext * ctx )
 
 	SV_INFO( "All devices attached." );
 
-#if 1
+
+	//libUSB initialized.  Continue.
+	return 0;
+}
+
+int survive_usb_send_magic(struct SurviveContext * ctx, int turnon )
+{
+	int r;
+
+
+	if( turnon )
 	{
 		//Magic from vl_magic.h, originally copywritten under LGPL. 
 		// * Copyright (C) 2013 Fredrik Hultin
 		// * Copyright (C) 2013 Jakob Bornecrantz
-
+#if 0
 		static uint8_t vive_magic_power_on[] = {
 			0x04, 0x78, 0x29, 0x38, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x01,
 			0xa8, 0x0d, 0x76, 0x00, 0x40, 0xfc, 0x01, 0x05, 0xfa, 0xec, 0xd1, 0x6d, 0x00,
@@ -246,15 +256,14 @@ int survive_usb_init( struct SurviveContext * ctx )
 			0x01, 0x05, 0x2c, 0xb0, 0x2e, 0x65, 0x7a, 0x0d, 0x76, 0x00, 0x68, 0x54, 0x72,
 			0x00, 0x18, 0x54, 0x72, 0x00, 0x00, 0x6a, 0x72, 0x00, 0x00, 0x00, 0x00,
 		};
-
-		//From actual use. ((THIS SEEMS TO BREAK IT))
-/*
+#else
+		//From actual steam.
 		static uint8_t vive_magic_power_on[64] = {   0x04, 0x78, 0x29, 0x38,
 			0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00,	 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,  0x00, 0x7a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 			0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
-*/
+#endif
 		r = update_feature_report( ctx->udev[USB_DEV_HMD], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
 		SV_INFO( "UCR: %d", r );
 		if( r != sizeof( vive_magic_power_on ) ) return 5;
@@ -274,17 +283,38 @@ int survive_usb_init( struct SurviveContext * ctx )
 			usleep( 1000 );
 		}
 #endif
+		SV_INFO( "Powered unit on." );
 	}
-#endif
+	else
+	{
 
-	SV_INFO( "Powered unit on." );
+		static uint8_t vive_magic_power_off1[] = {
+			0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
+			0x30, 0x05, 0x77, 0x00, 0x30, 0x05, 0x77, 0x00, 0x6c, 0x4d, 0x37, 0x65, 0x40,
+			0xf9, 0x33, 0x00, 0x04, 0xf8, 0xa3, 0x04, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0,
+			0x72, 0x00, 0xf4, 0xf7, 0xa3, 0x04, 0x7c, 0xf8, 0x33, 0x00, 0x0c, 0xf8, 0xa3,
+			0x04, 0x0a, 0x6e, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00,
+		};
 
+		static uint8_t vive_magic_power_off2[] = {
+			0x04, 0x78, 0x29, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00,
+			0x30, 0x05, 0x77, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0xe4, 0xf7, 0x33, 0x00, 0x60,
+			0x6e, 0x72, 0x00, 0xb4, 0xf7, 0x33, 0x00, 0x04, 0x00, 0x00, 0x00, 0x70, 0xb0,
+			0x72, 0x00, 0x90, 0xf7, 0x33, 0x00, 0x7c, 0xf8, 0x33, 0x00, 0xd0, 0xf7, 0x33,
+			0x00, 0x3c, 0x68, 0x29, 0x65, 0x24, 0xf9, 0x33, 0x00, 0x00, 0x00, 0x00,
+		};
 
+//		r = update_feature_report( ctx->udev[USB_DEV_HMD], 0, vive_magic_power_off1, sizeof( vive_magic_power_off1 ) );
+//		SV_INFO( "UCR: %d", r );
+//		if( r != sizeof( vive_magic_power_off1 ) ) return 5;
 
-	//libUSB initialized.  Continue.
-	return 0;
-}
+		r = update_feature_report( ctx->udev[USB_DEV_HMD], 0, vive_magic_power_off2, sizeof( vive_magic_power_off2 ) );
+		SV_INFO( "UCR: %d", r );
+		if( r != sizeof( vive_magic_power_off2 ) ) return 5;
 
+	}
+
+}
 
 void survive_usb_close( struct SurviveContext * t )
 {
diff --git a/test.c b/test.c
index 5eff185..537b2e5 100644
--- a/test.c
+++ b/test.c
@@ -7,97 +7,14 @@
 #include <os_generic.h>
 #include <DrawFunctions.h>
 
-
-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 )
-{
-}
-
-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 );
-	}
-}
-
-
+struct SurviveContext * ctx;
 
 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 magicon = 0;
+	double Start = OGGetAbsoluteTime();
 
-	int r = survive_simple_inflate( 0, input, sizeof( input ), output, sizeof(output) );
-
-	printf( "%d: ", r );
-	for( i = 0 ;i < r; i++ )
-	{
-		printf( "%02x ", output[i] );
-	}
-	return 0;*/
-	struct SurviveContext * ctx = survive_init( &survivefault, &survivenote );
-
-	CNFGBGColor = 0x000000;
-	CNFGDialogColor = 0x444444;
-	CNFGSetup( "Survive GUI Debug", 640, 480 );
-	OGCreateThread( GuiThread, 0 );
-	
+	ctx = survive_init( );
 
 	if( !ctx )
 	{
@@ -107,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/planetest/Makefile b/tools/planetest/Makefile
deleted file mode 100644
index 37f02d7..0000000
--- a/tools/planetest/Makefile
+++ /dev/null
@@ -1,10 +0,0 @@
-all : camfind
-
-CFLAGS:=-g -O4 -DFLT=float -ffast-math
-LDFLAGS:=$(CFLAGS) -lm
-
-camfind : camfind.o linmath.o
-	gcc -o $@ $^  $(LDFLAGS)
-
-clean :
-	rm -rf *.o *~ camfind
diff --git a/tools/planetest/camfind.c b/tools/planetest/camfind.c
deleted file mode 100644
index 1e9b2b9..0000000
--- a/tools/planetest/camfind.c
+++ /dev/null
@@ -1,385 +0,0 @@
-#include <stdio.h>
-#include <stdlib.h>
-#include "linmath.h"
-#include <string.h>
-#include <math.h>
-
-#define PTS 32
-#define MAX_CHECKS 20000
-
-FLT hmd_points[PTS*3];
-FLT hmd_norms[PTS*3];
-FLT hmd_point_angles[PTS*2];
-int    hmd_point_counts[PTS*2];
-int LoadData( char Camera );
-
-//Values used for RunTest()
-FLT LighthousePos[3] = { 0, 0, 0 };
-FLT LighthouseQuat[4] = { 1, 0, 0, 0 };
-
-//Actual values
-FLT xyzypr[6] = { 0, 2, 2, 0, 0, 0 };
-
-
-FLT RunOpti( int axis );
-FLT RunTest( int print );
-void PrintOpti();
-
-int main()
-{
-	int i;
-
-	//Load either 'L' (LH1) or 'R' (LH2) data.
-	if( LoadData( 'R' ) ) return 5;
-
-	int opti = 0;
-	int cycle = 0;
-	int axis = 0;
-
-	FLT dx, dy, dz;
-
-	FLT bestxyzypr[6];
-	memcpy( bestxyzypr, xyzypr, sizeof( xyzypr ) );
-
-	FLT fullrange = 5; //Maximum search space for positions.  (Relative to HMD)
-	for( cycle = 0; cycle < 20; cycle ++ )
-	{
-
-		//Adjust position, one axis at a time, over and over until we zero in.
-#define FULLSEARCH
-
-#ifndef FULLSEARCH
-		for( axis = 0; axis < 3; axis++ )
-#endif
-		{
-			FLT bestxyzyprrunning[6];
-			FLT beste = 1e20;
-
-			//Try a bunch of points in this axis.
-#ifdef FULLSEARCH
-			for( dz = -fullrange; dz <= fullrange; dz += fullrange/3 )
-			for( dy = -fullrange; dy <= fullrange; dy += fullrange/3 )
-			for( dx = -fullrange; dx <= fullrange; dx += fullrange/3 )
-			{
-#else
-			for( dx = -fullrange; dx <= fullrange; dx += fullrange/5 )
-			{
-#endif
-				//Specificially adjust one axis at a time, searching for the best.
-				memcpy( xyzypr, bestxyzypr, sizeof( xyzypr ) );
-#ifdef FULLSEARCH
-				xyzypr[0] += dx;
-				xyzypr[1] += dy;
-				xyzypr[2] += dz;
-#else
-				xyzypr[axis] += dx;
-#endif
-				//if( axis == 2 && xyzypr[2] < 0 ) continue;
-
-				xyzypr[3] = 0;
-				xyzypr[4] = 0;
-				xyzypr[5] = 0;
-				FLT ft;
-				int reopt = 0;
-
-				//Try refining the search for the best orientation several times.
-				for( reopt = 0; reopt < 4; reopt++ )
-				{
-					//XXX This search mechanism is insufficient :(
-					//Search through each axis every time.
-					for( opti = 3; opti < 6; opti++ )
-					{
-						ft = RunOpti( opti );
-					}
-					if( ft < beste ) { beste = ft; memcpy( bestxyzyprrunning, xyzypr, sizeof( xyzypr ) ); }
-				}
-				//printf( "  %f %f %f %f\n", xyzypr[0], xyzypr[1], xyzypr[2], ft );
-#ifndef FULLSEARCH
-				memcpy( bestxyzypr, bestxyzyprrunning, sizeof( bestxyzypr ) );
-#endif
-			}
-#ifdef FULLSEARCH
-				memcpy( bestxyzypr, bestxyzyprrunning, sizeof( bestxyzypr ) );
-#endif
-
-			//Print out the quality of the lock this time.
-			FLT dist = sqrt(bestxyzypr[0]*bestxyzypr[0] + bestxyzypr[1]*bestxyzypr[1] + bestxyzypr[2]*bestxyzypr[2]);
-			printf( "%f %f %f (%f) %f %f %f = %f\n", bestxyzypr[0], bestxyzypr[1], bestxyzypr[2], dist, bestxyzypr[3], bestxyzypr[4], bestxyzypr[5], beste );
-		}
-/*
-		if( bestxyzypr[2] < 0 )
-		{
-			bestxyzypr[0] *= -1;
-			bestxyzypr[1] *= -1;
-			bestxyzypr[2] *= -1;
-		}
-*/
-		//Every cycle, tighten up the search area.
-		fullrange *= 0.6;
-	}
-
-	//Use bestxyzypr
-	memcpy( xyzypr, bestxyzypr, sizeof( xyzypr ) );
-
-
-	//Print out plane accuracies with these settings.
-	PrintOpti();
-}
-
-void PrintOpti()
-{
-	FLT xyzyprchange[6];
-	memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) );
-	quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
-	memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
-	FLT ft = RunTest(1);
-	printf( "Final RMS: %f\n", ft );
-}
-
-
-FLT RunOpti( int axis )
-{
-	FLT xyzyprchange[6];
-	memcpy( xyzyprchange, xyzypr, sizeof( xyzypr ) );
-	int i;
-	FLT minv = 1e20;
-	FLT fv = -3.3;
-	FLT bv = 0;
-
-
-	//Coarse linear search first, try to figure out about where
-	for( i = 0; i < 33*2; i++ )
-	{
-		xyzyprchange[axis] = fv;
-		quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
-		memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
-		FLT ft = RunTest(0);
-		//printf( " %d / %f = %f\n", ft, fv );
-		if( ft < minv ) { minv = ft; bv = fv; }
-		fv += 0.1;
-	}
-
-	xyzyprchange[axis] = bv;
-#if 1
-	//Now, do a more precise binary-ish search.
-	FLT jumpamount = 0.15;
-	for( i = 0; i < 20; i++ )
-	{
-		FLT orig = xyzyprchange[axis];
-
-		minv = 1e20;
-
-		//When searching, consider 'less' 'this' and 'more'
-
-		fv = xyzyprchange[axis] = orig;
-		quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
-		memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
-		FLT ft = RunTest(0);
-		if( ft < minv ) { minv = ft; bv = fv; }
-
-		fv = xyzyprchange[axis] = orig + jumpamount;
-		quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
-		memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
-		ft = RunTest(0);
-		if( ft < minv ) { minv = ft; bv = fv; }
-
-		fv = xyzyprchange[axis] = orig - jumpamount;
-		quatfromeuler( LighthouseQuat, &xyzyprchange[3] );
-		memcpy( LighthousePos, &xyzyprchange[0], sizeof( LighthousePos ) );
-		ft = RunTest(0);
-		if( ft < minv ) { minv = ft; bv = fv; }
-
-		xyzyprchange[axis] = bv;
-
-		//Tighten up search area.  Doing by 0.5 can cause unusual instabilities.
-		jumpamount *= 0.6;
-	}
-#endif
-	xyzypr[axis] = bv;
-	return minv;
-}
-
-
-FLT RunTest( int print )
-{
-	int k;
-	FLT totprob = 0.0;
-	int ict = 0;
-	for( k = 0; k < 64; k++ )
-	{
-		if( hmd_point_counts[k] == 0 ) continue;
-		int axis = k%2;
-		int pt = k/2;
-		FLT angle = hmd_point_angles[k];
-		if( print ) printf( "%d %d : angle: %f / ", axis, pt, angle );
-
-		//XXX TODO: This is critical.  We need to properly define the planes. 
-		FLT plane_normal[3] = { 0, 0, 0 };
-		if( axis == 0 )
-		{
-			plane_normal[1] = cos(angle);
-			plane_normal[2] =-sin(angle);
-		}
-		else
-		{
-			plane_normal[0] =-cos(angle);
-			plane_normal[2] =-sin(angle);
-		}
-
-		quatrotatevector( plane_normal, LighthouseQuat, plane_normal ); //Rotate plane normal by concatenated rotation.
-
-		//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.
-
-		FLT delta[] = { LighthousePos[0]-hmd_points[pt*3+0],LighthousePos[1]-hmd_points[pt*3+1],LighthousePos[2]-hmd_points[pt*3+2] };
-		FLT dot = delta[0] * hmd_norms[pt*3+0] + delta[1] * hmd_norms[pt*3+1] + delta[2] * hmd_norms[pt*3+2];
-//		if( dot < -0.04 ) totprob+=10000000000;
-		if( print ) printf( " %f %f N:%f\n", d, D, dot );
-		totprob += (D*D);  //Calculate RMS distance of incorrectitude.
-
-		ict++;
-	}
-	return sqrt(totprob/ict);
-}
-
-
-int LoadData( char Camera )
-{
-	int calpts[MAX_CHECKS*4]; //X (0) or Y (1), ID, offset
-	int calptscount;
-
-	FILE * f = fopen( "correct_hmd_points.csv", "r" );
-	int pt = 0;
-	if( !f ) { fprintf( stderr, "error: can't open hmd points.\n" ); return -5; }
-	while(!feof(f) && !ferror(f) && pt < PTS)
-	{
-		float fa, fb, fc;
-		int r = fscanf( f,"%g,%g,%g\n", &fa, &fb, &fc );
-		hmd_points[pt*3+0] = fa;
-		hmd_points[pt*3+1] = fb;
-		hmd_points[pt*3+2] = fc;
-		pt++;
-		if( r != 3 )
-		{
-			fprintf( stderr, "Not enough entries on line %d\n", pt );
-			return -8;
-		}
-	}
-	if( pt < PTS )
-	{
-		fprintf( stderr, "Not enough points.\n" );
-		return -9;
-	}
-	fclose( f );
-	printf( "Loaded %d points\n", pt );
-
-
-	f = fopen( "hmd_normals.csv", "r" );
-	int nrm = 0;
-	if( !f ) { fprintf( stderr, "error: can't open hmd points.\n" ); return -5; }
-	while(!feof(f) && !ferror(f) && nrm < PTS)
-	{
-		float fa, fb, fc;
-		int r = fscanf( f,"%g,%g,%g\n", &fa, &fb, &fc );
-		hmd_norms[nrm*3+0] = fa;
-		hmd_norms[nrm*3+1] = fb;
-		hmd_norms[nrm*3+2] = fc;
-		nrm++;
-		if( r != 3 )
-		{
-			fprintf( stderr, "Not enough entries on line %d\n", nrm );
-			return -8;
-		}
-	}
-	if( nrm < PTS )
-	{
-		fprintf( stderr, "Not enough points.\n" );
-		return -9;
-	}
-	if( nrm != pt )
-	{
-		fprintf( stderr, "point/normal counts disagree.\n" );
-		return -9;
-	}
-	fclose( f );
-	printf( "Loaded %d norms\n", nrm );
-
-
-	int xck = 0;
-	f = fopen( "third_test_with_time_lengths.csv", "r" );
-	if( !f ) { fprintf( stderr, "Error: can't open two lighthouses test data.\n" ); return -11; }
-	while( !feof(f) && !ferror(f) )
-	{
-		char * lineptr;
-		size_t n;
-		lineptr = 0;
-		n = 0;
-		ssize_t r = getline( &lineptr, &n, f );
-		char lf[10];
-		char xory[10];
-		char dev[10];
-		int timestamp;
-		int sensorid;
-		int offset;
-		int code;
-		int length;
-		int rk = sscanf( lineptr, "%9s %9s %9s %d %d %d %d %d\n", lf, xory, dev, &timestamp, &sensorid, &code, &offset, &length );
-		if( lf[0] == Camera && rk == 8 )
-		{
-			//calpts
-			if( xory[0] == 'X' )
-			{
-				calpts[xck*3+0] = 0;
-			}
-			else if( xory[0] == 'Y' )
-			{
-				calpts[xck*3+0] = 1;
-			}
-			else
-			{
-				printf( "Confusing line\n" );
-				continue;
-			}
-
-			calpts[xck*3+1] = sensorid;
-			calpts[xck*3+2] = offset;
-			xck++;
-		}
-		if( lineptr ) free( lineptr );
-	}
-	printf( "Cal points: %d\n", xck );
-	calptscount = xck;
-
-	int i;
-	for( i = 0; i < calptscount; i++ )
-	{
-		int isy = calpts[i*3+0];
-		int pt = calpts[i*3+1];
-		int ofs = calpts[i*3+2];
-		hmd_point_counts[pt*2+isy]++;
-		hmd_point_angles[pt*2+isy]+=ofs;
-	}
-	for( i = 0; i < 32; i++ )
-	{
-		if( hmd_point_counts[i*2+0] < 100 ) hmd_point_counts[i*2+0] = 0;
-		if( hmd_point_counts[i*2+1] < 100 ) hmd_point_counts[i*2+1] = 0;
-
-		hmd_point_angles[i*2+0]/=hmd_point_counts[i*2+0];
-		hmd_point_angles[i*2+1]/=hmd_point_counts[i*2+1];
-		hmd_point_angles[i*2+0] = (hmd_point_angles[i*2+0] - 200000) / 200000 * 3.1415926535/2; // Is it really 800,000 ticks per revolution?
-		hmd_point_angles[i*2+1] = (hmd_point_angles[i*2+1] - 200000) / 200000 * 3.1415926535/2; // Is it really 800,000 ticks per revolution?
-	}
-
-
-
-
-	return 0;
-}
diff --git a/tools/planetest/linmath.c b/tools/planetest/linmath.c
deleted file mode 100644
index 88643b4..0000000
--- a/tools/planetest/linmath.c
+++ /dev/null
@@ -1,326 +0,0 @@
-//Copyright 2013 <>< C. N. Lohr.  This file licensed under the terms of the MIT license.
-
-#include "linmath.h"
-#include <math.h>
-
-void cross3d( FLT * out, const FLT * a, const FLT * b )
-{
-	out[0] = a[1]*b[2] - a[2]*b[1];
-	out[1] = a[2]*b[0] - a[0]*b[2];
-	out[2] = a[0]*b[1] - a[1]*b[0];
-}
-
-void sub3d( FLT * out, const FLT * a, const FLT * b )
-{
-	out[0] = a[0] - b[0];
-	out[1] = a[1] - b[1];
-	out[2] = a[2] - b[2];
-}
-
-void add3d( FLT * out, const FLT * a, const FLT * b )
-{
-	out[0] = a[0] + b[0];
-	out[1] = a[1] + b[1];
-	out[2] = a[2] + b[2];
-}
-
-void scale3d( FLT * out, const FLT * a, FLT scalar )
-{
-	out[0] = a[0] * scalar;
-	out[1] = a[1] * scalar;
-	out[2] = a[2] * scalar;
-}
-
-void normalize3d( FLT * out, const FLT * in )
-{
-	FLT r = 1./sqrtf( in[0] * in[0] + in[1] * in[1] + in[2] * in[2] );
-	out[0] = in[0] * r;
-	out[1] = in[1] * r;
-	out[2] = in[2] * r;
-}
-
-FLT dot3d( const FLT * a, const FLT * b )
-{
-	return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
-}
-
-int compare3d( const FLT * a, const FLT * b, FLT epsilon )
-{
-	if( !a || !b ) return 0;
-	if( a[2] - b[2] > epsilon ) return 1;
-	if( b[2] - a[2] > epsilon ) return -1;
-	if( a[1] - b[1] > epsilon ) return 1;
-	if( b[1] - a[1] > epsilon ) return -1;
-	if( a[0] - b[0] > epsilon ) return 1;
-	if( b[0] - a[0] > epsilon ) return -1;
-	return 0;
-}
-
-void copy3d( FLT * out, const FLT * in )
-{
-	out[0] = in[0];
-	out[1] = in[1];
-	out[2] = in[2];
-}
-
-
-
-/////////////////////////////////////QUATERNIONS//////////////////////////////////////////
-//Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman)
-//Under the mit/X11 license.
-
-
-
-
-void quatsetnone( FLT * q )
-{
-	q[0] = 0; q[1] = 0; q[2] = 0; q[3] = 1;
-}
-
-void quatcopy( FLT * qout, const FLT * qin )
-{
-	qout[0] = qin[0];
-	qout[1] = qin[1];
-	qout[2] = qin[2];
-	qout[3] = qin[3];
-}
-
-void quatfromeuler( FLT * q, const FLT * euler )
-{
-	FLT X = euler[0]/2.0f; //roll
-	FLT Y = euler[1]/2.0f; //pitch
-	FLT Z = euler[2]/2.0f; //yaw
-
-	FLT cx = cosf(X);
-	FLT sx = sinf(X);
-	FLT cy = cosf(Y);
-	FLT sy = sinf(Y);
-	FLT cz = cosf(Z);
-	FLT sz = sinf(Z);
-
-	//Correct according to
-	//http://en.wikipedia.org/wiki/Conversion_between_MQuaternions_and_Euler_angles
-	q[0] = cx*cy*cz+sx*sy*sz;//q1
-	q[1] = sx*cy*cz-cx*sy*sz;//q2
-	q[2] = cx*sy*cz+sx*cy*sz;//q3
-	q[3] = cx*cy*sz-sx*sy*cz;//q4
-	quatnormalize( q, q );
-}
-
-void quattoeuler( FLT * euler, const FLT * q )
-{
-	//According to http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles (Oct 26, 2009)
-	euler[0] = atan2( 2 * (q[0]*q[1] + q[2]*q[3]), 1 - 2 * (q[1]*q[1] + q[2]*q[2] ) );
-	euler[1] = asin( 2 * (q[0] *q[2] - q[3]*q[1] ) );
-	euler[2] = atan2( 2 * (q[0]*q[3] + q[1]*q[2]), 1 - 2 * (q[2]*q[2] + q[3]*q[3] ) );
-}
-
-void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians )
-{
-	FLT v[3];
-	normalize3d( v, axis );
-	
-	FLT sn = sin(radians/2.0f);
-	q[0] = cos(radians/2.0f);
-	q[1] = sn * v[0];
-	q[2] = sn * v[1];
-	q[3] = sn * v[2];
-
-	quatnormalize( q, q );
-}
-
-FLT quatmagnitude( const FLT * q )
-{
-	return sqrt((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3]));
-}
-
-FLT quatinvsqmagnitude( const FLT * q )
-{
-	return 1./((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3]));
-}
-
-
-void quatnormalize( FLT * qout, const FLT * qin )
-{
-	FLT imag = quatinvsqmagnitude( qin );
-	quatscale( qout, qin, imag );
-}
-
-void quattomatrix( FLT * matrix44, const FLT * qin )
-{
-	FLT q[4];
-	quatnormalize( q, qin );
-	
-	//Reduced calulation for speed
-	FLT xx = 2*q[0]*q[0];
-	FLT xy = 2*q[0]*q[1];
-	FLT xz = 2*q[0]*q[2];
-	FLT xw = 2*q[0]*q[3];
-	
-	FLT yy = 2*q[1]*q[1];
-	FLT yz = 2*q[1]*q[2];
-	FLT yw = 2*q[1]*q[3];
-	
-	FLT zz = 2*q[2]*q[2];
-	FLT zw = 2*q[2]*q[3];
-
-	//opengl major
-	matrix44[0] = 1-yy-zz;
-	matrix44[1] = xy-zw;
-	matrix44[2] = xz+yw;
-	matrix44[3] = 0;
-
-	matrix44[4] = xy+zw;
-	matrix44[5] = 1-xx-zz;
-	matrix44[6] = yz-xw;
-	matrix44[7] = 0;
-
-	matrix44[8] = xz-yw;
-	matrix44[9] = yz+xw;
-	matrix44[10] = 1-xx-yy;
-	matrix44[11] = 0;
-
-	matrix44[12] = 0;
-	matrix44[13] = 0;
-	matrix44[14] = 0;
-	matrix44[15] = 1;
-}
-
-void quatgetconjugate( FLT * qout, const FLT * qin )
-{
-	qout[0] = qin[0];
-	qout[1] = -qin[1];
-	qout[2] = -qin[2];
-	qout[3] = -qin[3];
-}
-
-void quatgetreciprocal( FLT * qout, const FLT * qin )
-{
-	FLT m = quatinvsqmagnitude(qin);
-	quatgetconjugate( qout, qin );
-	quatscale( qout, qout, m );
-}
-
-void quatsub( FLT * qout, const FLT * a, const FLT * b )
-{
-	qout[0] = a[0] - b[0];
-	qout[1] = a[1] - b[1];
-	qout[2] = a[2] - b[2];
-	qout[3] = a[3] - b[3];
-}
-
-void quatadd( FLT * qout, const FLT * a, const FLT * b )
-{
-	qout[0] = a[0] + b[0];
-	qout[1] = a[1] + b[1];
-	qout[2] = a[2] + b[2];
-	qout[3] = a[3] + b[3];
-}
-
-void quatrotateabout( FLT * qout, const FLT * a, const FLT * b )
-{
-	FLT q1[4];
-	FLT q2[4];
-
-	quatnormalize( q1, a );
-	quatnormalize( q2, b );
-
-	qout[0] = (q1[0]*q2[0])-(q1[1]*q2[1])-(q1[2]*q2[2])-(q1[3]*q2[3]);
-	qout[1] = (q1[0]*q2[1])+(q1[1]*q2[0])+(q1[2]*q2[3])-(q1[3]*q2[2]);
-	qout[2] = (q1[0]*q2[2])-(q1[1]*q2[3])+(q1[2]*q2[0])+(q1[3]*q2[1]);
-	qout[3] = (q1[0]*q2[3])+(q1[1]*q2[2])-(q1[2]*q2[1])+(q1[3]*q2[0]);
-}
-
-void quatscale( FLT * qout, const FLT * qin, FLT s )
-{
-	qout[0] = qin[0] * s;
-	qout[1] = qin[1] * s;
-	qout[2] = qin[2] * s;
-	qout[3] = qin[3] * s;
-}
-
-
-FLT quatinnerproduct( const FLT * qa, const FLT * qb )
-{
-	return (qa[0]*qb[0])+(qa[1]*qb[1])+(qa[2]*qb[2])+(qa[3]*qb[3]);
-}
-
-void quatouterproduct( FLT * outvec3, FLT * qa, FLT * qb )
-{
-	outvec3[0] = (qa[0]*qb[1])-(qa[1]*qb[0])-(qa[2]*qb[3])+(qa[3]*qb[2]);
-	outvec3[1] = (qa[0]*qb[2])+(qa[1]*qb[3])-(qa[2]*qb[0])-(qa[3]*qb[1]);
-	outvec3[2] = (qa[0]*qb[3])-(qa[1]*qb[2])+(qa[2]*qb[1])-(qa[3]*qb[0]);
-}
-
-void quatevenproduct( FLT * q, FLT * qa, FLT * qb )
-{
-	q[0] = (qa[0]*qb[0])-(qa[1]*qb[1])-(qa[2]*qb[2])-(qa[3]*qb[3]);
-	q[1] = (qa[0]*qb[1])+(qa[1]*qb[0]);
-	q[2] = (qa[0]*qb[2])+(qa[2]*qb[0]);
-	q[3] = (qa[0]*qb[3])+(qa[3]*qb[0]);
-}
-
-void quatoddproduct( FLT * outvec3, FLT * qa, FLT * qb )
-{
-	outvec3[0] = (qa[2]*qb[3])-(qa[3]*qb[2]);
-	outvec3[1] = (qa[3]*qb[1])-(qa[1]*qb[3]);
-	outvec3[2] = (qa[1]*qb[2])-(qa[2]*qb[1]);
-}
-
-void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t )
-{
-	FLT an[4];
-	FLT bn[4];
-	quatnormalize( an, qa );
-	quatnormalize( bn, qb );
-	FLT cosTheta = quatinnerproduct(an,bn);
-	FLT sinTheta;
-
-	//Careful: If cosTheta is exactly one, or even if it's infinitesimally over, it'll
-	// cause SQRT to produce not a number, and screw everything up.
-	if ( 1 - (cosTheta*cosTheta) <= 0 )
-		sinTheta = 0;
-	else
-		sinTheta = sqrt(1 - (cosTheta*cosTheta));
-
-	FLT Theta = acos(cosTheta); //Theta is half the angle between the 2 MQuaternions
-
-	if(fabs(Theta) < DEFAULT_EPSILON )
-		quatcopy( q, qa );
-	else if(fabs(sinTheta) < DEFAULT_EPSILON )
-	{
-		quatadd( q, qa, qb );
-		quatscale( q, q, 0.5 );
-	}
-	else
-	{
-		FLT aside[4];
-		FLT bside[4];
-		quatscale( bside, qb, sin( t * Theta ) );
-		quatscale( aside, qa, sin((1-t)*Theta) );
-		quatadd( q, aside, bside );
-		quatscale( q, q, 1./sinTheta );
-	}
-}
-
-void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in )
-{
-	FLT tquat[4];
-	FLT vquat[4];
-	FLT qrecp[4];
-	vquat[0] = 0;
-	vquat[1] = vec3in[0];
-	vquat[2] = vec3in[1];
-	vquat[3] = vec3in[2];
-
-	quatrotateabout( tquat, quat, vquat );
-	quatgetreciprocal( qrecp, quat );
-	quatrotateabout( vquat, tquat, qrecp );
-
-	vec3out[0] = vquat[1];
-	vec3out[1] = vquat[2];
-	vec3out[2] = vquat[3];
-}
-
-
-
diff --git a/tools/planetest/linmath.h b/tools/planetest/linmath.h
deleted file mode 100644
index 881ca70..0000000
--- a/tools/planetest/linmath.h
+++ /dev/null
@@ -1,59 +0,0 @@
-//Copyright 2013 <>< C. N. Lohr.  This file licensed under the terms of the MIT license.
-
-#ifndef _LINMATH_H
-#define _LINMATH_H
-
-//Yes, I know it's kind of arbitrary.
-#define DEFAULT_EPSILON 0.001
-
-
-//NOTE: Inputs may never be output with cross product.
-void cross3d( FLT * out, const FLT * a, const FLT * b );
-
-void sub3d( FLT * out, const FLT * a, const FLT * b );
-
-void add3d( FLT * out, const FLT * a, const FLT * b );
-
-void scale3d( FLT * out, const FLT * a, FLT scalar );
-
-void normalize3d( FLT * out, const FLT * in );
-
-FLT dot3d( const FLT * a, const FLT * b );
-
-//Returns 0 if equal.  If either argument is null, 0 will ALWAYS be returned.
-int compare3d( const FLT * a, const FLT * b, FLT epsilon );
-
-void copy3d( FLT * out, const FLT * in );
-
-
-
-
-//Quaternion things...
-
-void quatsetnone( FLT * q );
-void quatcopy( FLT * qout, const FLT * qin );
-void quatfromeuler( FLT * q, const FLT * euler );
-void quattoeuler( FLT * euler, const FLT * q );
-void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians );
-FLT quatmagnitude( const FLT * q );
-FLT quatinvsqmagnitude( const FLT * q );
-void quatnormalize( FLT * qout, const FLT * qin );  //Safe for in to be same as out.
-void quattomatrix( FLT * matrix44, const FLT * q );
-void quatgetconjugate( FLT * qout, const FLT * qin );
-void quatgetreciprocal( FLT * qout, const FLT * qin );
-void quatsub( FLT * qout, const FLT * a, const FLT * b );
-void quatadd( FLT * qout, const FLT * a, const FLT * b );
-void quatrotateabout( FLT * qout, const FLT * a, const FLT * b );  //same as quat multiply, not piecewise multiply.
-void quatscale( FLT * qout, const FLT * qin, FLT s );
-FLT quatinnerproduct( const FLT * qa, const FLT * qb );
-void quatouterproduct( FLT * outvec3, FLT * qa, FLT * qb );
-void quatevenproduct( FLT * q, FLT * qa, FLT * qb );
-void quatoddproduct( FLT * outvec3, FLT * qa, FLT * qb );
-void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t );
-void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in );
-
-
-#endif
-
-
-
diff --git a/tools/planetest2/Makefile b/tools/planetest2/Makefile
index dd19480..b48b099 100644
--- a/tools/planetest2/Makefile
+++ b/tools/planetest2/Makefile
@@ -1,6 +1,6 @@
 all : camfind
 
-CFLAGS:=-g -O4 -DFLT=float -ffast-math -I../../redist -flto
+CFLAGS:=-g -O4 -DFLT=float -I../../redist -flto
 LDFLAGS:=$(CFLAGS) -lm 
 
 camfind : camfind.o ../../redist/linmath.c
diff --git a/tools/planetest2/camfind.c b/tools/planetest2/camfind.c
index 54fd8a1..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];
@@ -18,8 +18,6 @@ int LoadData( char Camera );
 //Values used for RunTest()
 FLT LighthousePos[3] = { 0, 0, 0 };
 FLT LighthouseQuat[4] = { 1, 0, 0, 0 };
-FLT BarrelRotate[4];  //XXX TODO: Concatenate these.
-
 
 FLT RunOpti( int print );
 FLT RunTest( int print );
@@ -30,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;
@@ -52,10 +50,14 @@ int main()
 			FLT bestxyzrunning[3];
 			FLT beste = 1e20;
 
-			FLT splits = 2;
-			if( cycle == 0 ) splits = 25;
 
-			//XXX TODO: Switch to polar coordinate search
+			FLT splits = 4;
+			if( cycle == 0 ) splits = 24;
+			if( cycle == 1 ) splits = 13;
+			if( cycle == 2 ) splits = 10;
+			if( cycle == 3 ) splits = 8;
+			if( cycle == 4 ) splits = 5;
+
 			for( dz = 0; dz <= fullrange; dz += fullrange/splits )
 			for( dy = -fullrange; dy <= fullrange; dy += fullrange/splits )
 			for( dx = -fullrange; dx <= fullrange; dx += fullrange/splits )
@@ -70,8 +72,6 @@ int main()
 				//Try refining the search for the best orientation several times.
 				ft = RunOpti(0);
 				if( ft < beste ) { beste = ft; memcpy( bestxyzrunning, LighthousePos, sizeof( LighthousePos ) ); }
-
-				//printf( "  %f %f %f %f\n", LighthousePos[0], LighthousePos[1], LighthousePos[2], ft );
 			}
 			memcpy( bestxyz, bestxyzrunning, sizeof( bestxyz ) );
 
@@ -81,7 +81,7 @@ int main()
 		}
 
 		//Every cycle, tighten up the search area.
-		fullrange *= 0.6;
+		fullrange *= 0.25;
 	}
 
 	//Use bestxyz
@@ -90,29 +90,7 @@ int main()
 	//Optimize the quaternion for lighthouse rotation
 	RunOpti(1);
 
-	//STAGE 2: Determine optimal distance from target
-	{
-		FLT dist = 0.1;
-		FLT best_dist = 0;
-		FLT best_err = 1e20;
-		for( ; dist < 10; dist+=0.01 )
-		{
-			FLT nrmvect[3];
-			normalize3d( nrmvect, bestxyz );
-			scale3d( LighthousePos, nrmvect, dist );
-			FLT res = RunTest( 0 );
-			if( res < best_err )
-			{
-				best_dist = dist;
-				best_err = res;
-			}
-		}
-
-		//Apply the best res.
-		normalize3d( LighthousePos, bestxyz );
-		scale3d( LighthousePos, LighthousePos, best_dist );
-		printf( "Best distance: %f\n", best_dist );
-	}
+	printf( "Best Quat: %f %f %f %f\n", PFFOUR( LighthouseQuat ) );
 
 	//Print out plane accuracies with these settings.
 	FLT ft = RunTest(1);
@@ -121,97 +99,131 @@ int main()
 
 FLT RunOpti( int print )
 {
-	int i;
+	int i, p;
 	FLT UsToTarget[3];
+	FLT LastUsToTarget[3];
+	FLT mux = .9;
+	quatsetnone( LighthouseQuat );
 
-	{
-		//XXX TODO: We should use several points to determine initial rotation optimization to object.
-		//By using only one point the "best" we could be rotating somewhere wrong.  We do use
-		//several points for the rotate-about-this-vector rotation in stage 2.
-
-		//Find out where our ray shoots forth from.
-		FLT ax = hmd_point_angles[best_hmd_target*2+0];
-		FLT ay = hmd_point_angles[best_hmd_target*2+1];
+	int first = 1, second = 0;
 
-		//NOTE: Inputs may never be output with cross product.
-		//Create a fictitious normalized ray.  Imagine the lighthouse is pointed
-		//straight in the +z direction, this is the lighthouse ray to the point.
-		FLT RayShootOut[3] = { sin(ax), sin(ay), 0 };
-		RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
+	//First check to see if this is a valid viewpoint.
 
-		//Find a ray from us to the target point.
-		sub3d( UsToTarget, &hmd_points[best_hmd_target*3], LighthousePos );
-		normalize3d( UsToTarget, UsToTarget );
-
-		FLT AxisToRotate[3];
-		cross3d( AxisToRotate, RayShootOut, UsToTarget );
-		//Rotate the lighthouse around this axis to point at the HMD.
-
-		FLT RotateAmount = -acos( dot3d( RayShootOut, UsToTarget ) );  //XXX TODO How to determine if negative.
-		quatfromaxisangle( LighthouseQuat, AxisToRotate, RotateAmount ); //Tested, working!
+	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;
+		FLT me_to_dot[3];
+		sub3d( me_to_dot, LighthousePos, &hmd_points[p*3] );
+		float dot = dot3d( &hmd_norms[p*3], me_to_dot );
+		if( dot < -.01 ) { 	return 1000; }
 	}
 
-	//Now our lighthouse's ray is pointed at the HMD's dot, but, we need to
-	//rotate the lighthouse such that it is oriented optimally.  We do this
-	//by finding what the optimal rotation aroud our face would be for all
-	//remaining points.
-	FLT rotate_radians = 0;
-	int points_found_to_rotate = 0;
+	int iters = 6;
 
-	float rots[PTS];
-	for( i = 0; i < PTS; i++ )
+	for( i = 0; i < iters; i++ )
 	{
-		if( i == best_hmd_target ) continue;
-		int xhits = hmd_point_counts[i*2+0];
-		int yhits = hmd_point_counts[i*2+1];
-		int xyhits = (xhits<yhits)?xhits:yhits;
-		if( xyhits < MIN_HITS_FOR_VALID ) continue;
-
-		//Find a point on the object to point at.
-		FLT HMDRayPoint[3];
-		sub3d( HMDRayPoint, &hmd_points[i*3], LighthousePos );
-		normalize3d( HMDRayPoint, HMDRayPoint );
-
-		FLT RayShootOut[3] = { sin(hmd_point_angles[i*2+0]), sin(hmd_point_angles[i*2+1]), 0 };
-		RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
-		//Find the corresponding vector from camera out if it was rotated by "RotateAmount"
-		quatrotatevector( RayShootOut, LighthouseQuat, RayShootOut );
-
-		//Figure out rotation to rotate around UsToTarget, from RayShootOut to HMDRayPoint
-		FLT SphSurf_Point[3]; //Relative to our fixed point, how much to rotate?
-		FLT SphSurf_Ray[3];
-		sub3d( SphSurf_Point, HMDRayPoint, UsToTarget  );
-		sub3d( SphSurf_Ray,   RayShootOut, UsToTarget  );
-		normalize3d( SphSurf_Point, SphSurf_Point );
-		normalize3d( SphSurf_Ray, SphSurf_Ray );
-
-		FLT rotate = acos( dot3d( SphSurf_Point, SphSurf_Ray ) );
+		first = 1;
+		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;
+
+			//Find out where our ray shoots forth from.
+			FLT ax = hmd_point_angles[p*2+0];
+			FLT ay = hmd_point_angles[p*2+1];
+
+			//NOTE: Inputs may never be output with cross product.
+			//Create a fictitious normalized ray.  Imagine the lighthouse is pointed
+			//straight in the +z direction, this is the lighthouse ray to the point.
+			FLT RayShootOut[3] = { sin(ax), sin(ay), 0 };
+			RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
+			FLT RayShootOutWorld[3];
+
+			//Rotate that ray by the current rotation estimation.
+			quatrotatevector( RayShootOutWorld, LighthouseQuat, RayShootOut );
+
+			//Find a ray from us to the target point.
+			sub3d( UsToTarget, &hmd_points[p*3], LighthousePos );
+			if( magnitude3d( UsToTarget ) < 0.0001 ) { continue; }
+			normalize3d( UsToTarget, UsToTarget );
+
+			FLT RotatedLastUs[3];
+			quatrotatevector( RotatedLastUs, LighthouseQuat, LastUsToTarget );
+
+			//Rotate the lighthouse around this axis to point at the HMD.
+			//If it's the first time, the axis is synthesized, if it's after that, use most recent point.
+			FLT ConcatQuat[4];
+			FLT AxisToRotate[3];
+			if( first )
+			{
+				cross3d( AxisToRotate, RayShootOutWorld, UsToTarget );
+				if( magnitude3d(AxisToRotate) < 0.0001 ) break;
+				normalize3d( AxisToRotate, AxisToRotate );
+				//Don't need to worry about being negative, cross product will fix it.
+				FLT RotateAmount = anglebetween3d( RayShootOutWorld, UsToTarget );
+				quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount );
+			}
+			else
+			{
+				FLT Target[3];
+				FLT Actual[3];
+
+				copy3d( AxisToRotate, LastUsToTarget );
+				//Us to target = normalized ray from us to where we should be.
+				//RayShootOut = where we would be pointing.
+				sub3d( Target, UsToTarget, AxisToRotate );  //XXX XXX XXX WARNING THIS MESSES STUFF UP.
+				sub3d( Actual, RayShootOutWorld, AxisToRotate );
+				if( magnitude3d( Actual ) < 0.0001 || magnitude3d( Target ) < 0.0001 ) { continue; }
+				normalize3d( Target, Target );
+				normalize3d( Actual, Actual );
+
+				cross3d( AxisToRotate, Actual, Target );  //XXX Check: AxisToRotate should be equal to LastUsToTarget.
+				if( magnitude3d( AxisToRotate ) < 0.000001 ) { continue; }
+				normalize3d( AxisToRotate,AxisToRotate );
+
+				//printf( "%f %f %f === %f %f %f : ", PFTHREE( AxisToRotate ), PFTHREE( LastUsToTarget ) );
+				FLT RotateAmount = anglebetween3d( Actual, Target ) * mux;
+				//printf( "FA: %f (O:%f)\n", acos( dot3d( Actual, Target ) ), RotateAmount );
+				quatfromaxisangle( ConcatQuat, AxisToRotate, RotateAmount );
+			}
 
-		//XXX TODO: How to determine if rotate amount should be negative!!!
+			quatrotateabout( LighthouseQuat, ConcatQuat, LighthouseQuat );  //Chekcked.  This appears to be 
 
-		if( print ) printf( " %f ", rotate );
-		rotate_radians += rotate;
-		rots[points_found_to_rotate++] = rotate;
+			mux = mux * 0.94;
+			if( second ) { second = 0; }
+			if( first ) { first = 0; second = 1; }
+			copy3d( LastUsToTarget, RayShootOutWorld );
+		}
 	}
 
-	//Get average barrel rotation
-	rotate_radians/=points_found_to_rotate;
-
-	//Find the standard of deviation of our data.
-	float stddev = 0.0;
-	for( i = 0; i < points_found_to_rotate; i++ )
+	//Step 2: Determine error.
+	float errorsq = 0.0;
+	int count = 0;
+	for( p = 0; p < 32; p++ )
 	{
-		float dr = rots[i]-rotate_radians;
-		stddev += dr*dr;
-	}
-	stddev/=points_found_to_rotate;
-	stddev = sqrt(stddev);
-	if( print ) printf( " = %f\n", stddev );
+		if( hmd_point_counts[p*2+0] < MIN_HITS_FOR_VALID || hmd_point_counts[p*2+1] < MIN_HITS_FOR_VALID ) continue;
+
+		//Find out where our ray shoots forth from.
+		FLT ax = hmd_point_angles[p*2+0];
+		FLT ay = hmd_point_angles[p*2+1];
+		FLT RayShootOut[3] = { sin(ax), sin(ay), 0 };
+		RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
 
-	quatfromaxisangle( BarrelRotate, UsToTarget, rotate_radians ); //Rotate around barrel
-	//quatrotateabout( LighthouseQuat, LighthouseQuat, BarrelRotate ); //Concatenate the rotation into the lighthouse quat rotation.
+		//Rotate that ray by the current rotation estimation.
+		quatrotatevector( RayShootOut, LighthouseQuat, RayShootOut );
 
-	return stddev;
+		//Point-line distance.
+		//Line defined by LighthousePos & Direction: RayShootOut
+
+		//Find a ray from us to the target point.
+		sub3d( UsToTarget, &hmd_points[p*3], LighthousePos );
+		FLT xproduct[3];
+		cross3d( xproduct, UsToTarget, RayShootOut );
+		FLT dist = magnitude3d( xproduct );
+		errorsq += dist*dist;
+		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);
 }
 
 
@@ -225,33 +237,28 @@ FLT RunTest( int print )
 		if( hmd_point_counts[k] == 0 ) continue;
 		int axis = k%2;
 		int pt = k/2;
+
 		FLT angle = hmd_point_angles[k];
-		if( print ) printf( "%d %d : angle: %f / ", axis, pt, angle );
+		if( print ) printf( "%3d %3d : angle: %10f / ", axis, pt, angle );
 
 		//XXX TODO: This is critical.  We need to properly define the planes. 
 		FLT plane_normal[3] = { 0, 0, 0 };
-		if( axis == 1 )
+		if( axis == 0 )
 		{
-			plane_normal[1] = cos(angle);
+			plane_normal[0] = cos(angle);
 			plane_normal[2] =-sin(angle);
 		}
 		else
 		{
-			plane_normal[0] = cos(angle);
+			plane_normal[1] = cos(angle);
 			plane_normal[2] =-sin(angle);
 		}
 
 
 		quatrotatevector( plane_normal, LighthouseQuat, plane_normal ); //Rotate plane normal by concatenated rotation.
-		quatrotatevector( plane_normal, BarrelRotate, plane_normal ); //Rotate plane normal by concatenated rotation.
 
 		//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.
@@ -259,11 +266,39 @@ FLT RunTest( int print )
 		FLT delta[] = { LighthousePos[0]-hmd_points[pt*3+0],LighthousePos[1]-hmd_points[pt*3+1],LighthousePos[2]-hmd_points[pt*3+2] };
 		FLT dot = delta[0] * hmd_norms[pt*3+0] + delta[1] * hmd_norms[pt*3+1] + delta[2] * hmd_norms[pt*3+2];
 //		if( dot < -0.04 ) totprob+=10000000000;
-		if( print ) printf( " %f %f N:%f\n", d, D, dot );
+		if( print ) printf( " %10f %10f N:%f\n", d, D, dot );
 		totprob += (D*D);  //Calculate RMS distance of incorrectitude.
 
 		ict++;
 	}
+
+	if( print )
+	{
+		int p;
+		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;
+
+			FLT us_to_targ[3];
+			sub3d( us_to_targ, &hmd_points[p*3] , LighthousePos );
+
+			//Unrotate us_to_targ.
+			FLT unrotate[4];
+			quatgetconjugate( unrotate, LighthouseQuat );
+
+			quatrotatevector( us_to_targ, unrotate, us_to_targ );
+			normalize3d( us_to_targ, us_to_targ );
+			FLT x = asin( us_to_targ[0] );
+			FLT y = asin( us_to_targ[1] );
+
+			printf( "%f %f %f %f\n", hmd_point_angles[p*2+0], hmd_point_angles[p*2+1], x, y );
+		}
+	}
+
+
 	return sqrt(totprob/ict);
 }
 
@@ -331,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) )
 	{
@@ -415,3 +450,4 @@ int LoadData( char Camera )
 
 	return 0;
 }
+