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

26 files changed, 2654 insertions, 1629 deletions

diff --git a/81-vive.rules b/81-vive.rules
new file mode 100644
index 0000000..ab38087
--- /dev/null
+++ b/81-vive.rules
@@ -0,0 +1,14 @@
+# HTC Vive HID Sensor naming and permissioning
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="0bb4", ATTRS{idProduct}=="2c87", TAG+="uaccess"
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2101", TAG+="uaccess"
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2000", TAG+="uaccess"
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="1043", TAG+="uaccess"
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2050", TAG+="uaccess"
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2011", TAG+="uaccess"
+KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTRS{idProduct}=="2012", TAG+="uaccess"
+SUBSYSTEM=="usb", ATTRS{idVendor}=="0bb4", ATTRS{idProduct}=="2c87", TAG+="uaccess"
+# HTC Camera USB Node
+SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8328", TAG+="uaccess"
+# HTC Mass Storage Node
+SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8200", TAG+="uaccess"
+SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8a12", TAG+="uaccess"
diff --git a/Makefile b/Makefile
index ed51e30..75b5606 100644
--- a/Makefile
+++ b/Makefile
@@ -1,7 +1,10 @@
-all : lib data_recorder test calibrate
+all : lib data_recorder test calibrate calibrate_client
 
-CFLAGS:=-Iinclude -fPIC -g -Os -Iredist -flto
-LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lm -flto
+CFLAGS:=-Iinclude -I. -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99
+LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -lm -flto -g
+
+
+CALS:=src/survive_cal_lhfind.o src/survive_cal.o
 
 # unused: redist/crc32.c
 
@@ -14,14 +17,17 @@ data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.o redist/Dra
 calibrate :  calibrate.c lib/libsurvive.so redist/os_generic.c redist/DrawFunctions.c redist/XDriver.c
 	gcc -o $@ $^ $(LDFLAGS) $(CFLAGS)
 
+calibrate_client :  calibrate_client.c lib/libsurvive.so redist/os_generic.c redist/DrawFunctions.c redist/XDriver.c
+	gcc -o $@ $^ $(LDFLAGS) $(CFLAGS)
+
 lib:
 	mkdir lib
 
-lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/survive_cal.o src/ootx_decoder.o $(DEBUGSTUFF)
+lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o src/ootx_decoder.o redist/linmath.o src/survive_driverman.o src/survive_vive.o src/survive_config.o redist/json_helpers.o $(DEBUGSTUFF) $(CALS)
 	gcc -o $@ $^ $(LDFLAGS) -shared
 
 clean :
-	rm -rf *.o src/*.o *~ src/*~ test data_recorder lib/libsurvive.so
+	rm -rf *.o src/*.o *~ src/*~ test data_recorder lib/libsurvive.so redist/*.o redist/*~
 
 
 
diff --git a/calibrate.c b/calibrate.c
index 60f4316..a04c269 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -7,8 +7,11 @@
 #include <survive.h>
 #include <string.h>
 #include <os_generic.h>
+#include "src/survive_cal.h"
 #include <DrawFunctions.h>
 
+#include "src/survive_config.h"
+
 struct SurviveContext * ctx;
 
 void HandleKey( int keycode, int bDown )
@@ -17,11 +20,11 @@ void HandleKey( int keycode, int bDown )
 
 	if( keycode == 'O' || keycode == 'o' )
 	{
-		survive_usb_send_magic(ctx,1);
+		survive_send_magic(ctx,1,0,0);
 	}
 	if( keycode == 'F' || keycode == 'f' )
 	{
-		survive_usb_send_magic(ctx,0);
+		survive_send_magic(ctx,0,0,0);
 	}
 }
 
@@ -39,6 +42,7 @@ int buffertimeto[32*3];
 
 void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
 {
+//	if( timeinsweep < 0 ) return;
 	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
 
 	if( acode == -1 ) return;
@@ -94,6 +98,10 @@ void my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint
 void * GuiThread( void * v )
 {
 	short screenx, screeny;
+	CNFGBGColor = 0x000000;
+	CNFGDialogColor = 0x444444;
+	CNFGSetup( "Survive GUI Debug", 640, 480 );
+
 	while(1)
 	{
 		CNFGHandleInput();
@@ -139,7 +147,8 @@ void * GuiThread( void * v )
 
 int main()
 {
-	ctx = survive_init( );
+	ctx = survive_init( 0 );
+	config_init();
 
 	survive_install_light_fn( ctx,  my_light_process );
 	survive_install_imu_fn( ctx,  my_imu_process );
@@ -147,9 +156,6 @@ int main()
 
 	survive_cal_install( ctx );
 
-	CNFGBGColor = 0x000000;
-	CNFGDialogColor = 0x444444;
-	CNFGSetup( "Survive GUI Debug", 640, 480 );
 	OGCreateThread( GuiThread, 0 );
 	
 
@@ -163,5 +169,6 @@ int main()
 	{
 		//Do stuff.
 	}
+	printf( "Returned\n" );
 }
 
diff --git a/calibrate_client.c b/calibrate_client.c
new file mode 100644
index 0000000..08f0715
--- /dev/null
+++ b/calibrate_client.c
@@ -0,0 +1,224 @@
+//Totally hacky "client" for absorbing data from a network source of the vive data.
+
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <survive.h>
+#include <string.h>
+#include <os_generic.h>
+#include "src/survive_cal.h"
+#include <DrawFunctions.h>
+
+#include "src/survive_config.h"
+
+struct SurviveContext * ctx;
+
+void HandleKey( int keycode, int bDown )
+{
+	if( !bDown ) return;
+
+	if( keycode == 'O' || keycode == 'o' )
+	{
+		survive_send_magic(ctx,1,0,0);
+	}
+	if( keycode == 'F' || keycode == 'f' )
+	{
+		survive_send_magic(ctx,0,0,0);
+	}
+}
+
+void HandleButton( int x, int y, int button, int bDown )
+{
+}
+
+void HandleMotion( int x, int y, int mask )
+{
+}
+
+int bufferpts[32*2*3];
+char buffermts[32*128*3];
+int buffertimeto[32*3];
+
+void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
+{
+	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
+
+	if( acode == -1 ) return;
+//return;
+	int jumpoffset = sensor_id;
+	if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32;
+	else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64;
+
+
+	if( acode == 0 || acode == 2 ) //data = 0
+	{
+		bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
+	}
+	if( acode == 1 || acode == 3 ) //data = 1
+	{
+		bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
+	}
+
+
+	if( acode == 4 || acode == 6 ) //data = 0
+	{
+		bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
+	}
+	if( acode == 5 || acode == 7 ) //data = 1
+	{
+		bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
+	}
+}
+
+void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
+{
+	survive_default_imu_process( so, accelgyro, timecode, id );
+
+return;
+	//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 my_angle_process( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle )
+{
+	survive_default_angle_process( so, sensor_id, acode, timecode, length, angle );
+}
+
+
+void * GuiThread( void * v )
+{
+	short screenx, screeny;
+	while(1)
+	{
+		CNFGHandleInput();
+		CNFGClearFrame();
+		CNFGColor( 0xFFFFFF );
+		CNFGGetDimensions( &screenx, &screeny );
+
+		int i;
+		for( i = 0; i < 32*3; i++ )
+		{
+			if( buffertimeto[i] < 50 )
+			{
+				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]++;
+			}
+		}
+
+		CNFGColor( 0xffffff );
+		char caldesc[256];
+		survive_cal_get_status( ctx, caldesc, sizeof( caldesc ) );
+		CNFGPenX = 3;
+		CNFGPenY = 3;
+		CNFGDrawText( caldesc, 4 );
+
+
+		CNFGSwapBuffers();
+		OGUSleep( 10000 );
+	}
+}
+
+
+
+
+int main()
+{
+	ctx = survive_init( 1 );
+
+	survive_install_light_fn( ctx,  my_light_process );
+	survive_install_imu_fn( ctx,  my_imu_process );
+	survive_install_angle_fn( ctx, my_angle_process );
+
+	survive_cal_install( ctx );
+
+	struct SurviveObject * hmd = survive_get_so_by_name( ctx, "HMD" );
+	struct SurviveObject * wm0 = survive_get_so_by_name( ctx, "WM0" );
+	struct SurviveObject * wm1 = survive_get_so_by_name( ctx, "WM1" );
+
+	CNFGBGColor = 0x000000;
+	CNFGDialogColor = 0x444444;
+	CNFGSetup( "Survive GUI Debug", 640, 480 );
+	OGCreateThread( GuiThread, 0 );
+
+	config_init();
+	config_set_str(&global_config_values, "Hello","World!");
+	const char *s = config_read_str(&global_config_values, "TestStr","This is a test.");
+	printf("%s\n", s);
+	config_save("config.json");
+	
+
+	if( !ctx )
+	{
+		fprintf( stderr, "Fatal. Could not start\n" );
+		return 1;
+	}
+
+	while(survive_poll(ctx) == 0)
+	{
+		char * lineptr;
+		size_t n;
+		lineptr = 0;
+		n = 0;
+		ssize_t gl = getline( &lineptr, &n, stdin );
+//		printf( "%d %s\n", gl, lineptr );
+
+		switch( lineptr[0] )
+		{
+			case 'I': 
+				//IMU data
+				//I WM0 -695533550 -321 1357 -3928 -16 -2 -2 0
+				break;
+			case 'R': 
+			case 'L': 
+			{
+				//Light data
+				//R X HMD -878577652 -1 6 380498 6004
+				char lhn[10];
+				char beam[10];
+				char dev[10];
+				int timecode, sensor_id, acode, timeinsweep, length;
+
+				sscanf( lineptr, "%9s %9s %9s %d %d %d %d %d\n", 
+					lhn, beam, dev, &timecode, &sensor_id, &acode, &timeinsweep, &length );
+
+				struct SurviveObject * so = 0;
+				if( strcmp( dev, "HMD" ) == 0 )
+					so = hmd;
+				if( strcmp( dev, "WM0" ) == 0 )
+					so = wm0;
+				if( strcmp( dev, "WM1" ) == 0 )
+					so = wm1;
+
+				if( so )
+					my_light_process( so, sensor_id, acode, timeinsweep, timecode, length );
+
+				break;
+			}
+		}
+
+		free( lineptr );
+
+        //printf( "!!!\n" );
+		//Do stuff.
+	}
+}
+
diff --git a/data_recorder.c b/data_recorder.c
index ced82c4..951f234 100644
--- a/data_recorder.c
+++ b/data_recorder.c
@@ -17,11 +17,11 @@ void HandleKey( int keycode, int bDown )
 
 	if( keycode == 'O' || keycode == 'o' )
 	{
-		survive_usb_send_magic(ctx,1);
+		survive_send_magic(ctx,1,0,0);
 	}
 	if( keycode == 'F' || keycode == 'f' )
 	{
-		survive_usb_send_magic(ctx,0);
+		survive_send_magic(ctx,0,0,0);
 	}
 }
 
@@ -81,7 +81,7 @@ void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t ti
 {
 	survive_default_imu_process( so, accelgyro, timecode, id );
 
-return;
+//return;
 	//if( so->codename[0] == 'H' )
 	if( 1 )
 	{
@@ -132,7 +132,7 @@ void * GuiThread( void * v )
 
 int main()
 {
-	ctx = survive_init( );
+	ctx = survive_init( 0 );
 
 	survive_install_light_fn( ctx,  my_light_process );
 	survive_install_imu_fn( ctx,  my_imu_process );
diff --git a/include/survive.h b/include/survive.h
index 3e8dc35..51910a6 100644
--- a/include/survive.h
+++ b/include/survive.h
@@ -4,7 +4,11 @@
 #include <stdint.h>
 
 #ifndef FLT
+#ifdef USE_DOUBLE
 #define FLT double
+#else
+#define FLT float
+#endif
 #endif
 
 struct SurviveContext;
@@ -20,7 +24,8 @@ struct SurviveObject
 {
 	struct SurviveContext * ctx;
 
-	char    codename[4];  //3 letters, null-terminated.  Currently HMD, WM0, WM1.
+	char    codename[4];    //3 letters, null-terminated.  Currently HMD, WM0, WM1.
+	char    drivername[4];  //3 letters for driver.  Currently "HTC"
 	int16_t buttonmask;
 	int16_t axis1;
 
@@ -33,6 +38,7 @@ struct SurviveObject
 
 	FLT * sensor_locations;
 	FLT * sensor_normals;
+	int8_t nr_locations;
 
 	//Timing sensitive data (mostly for disambiguation)
 	int32_t timebase_hz;		//48,000,000 for normal vive hardware.  (checked)
@@ -43,7 +49,6 @@ struct SurviveObject
 	int32_t pulse_max_for_sweep; //1,800 for normal vive hardware.     (guessed)
 	int32_t pulse_synctime_offset; //20,000 for normal vive hardware.  (guessed)
 	int32_t pulse_synctime_slack; //5,000 for normal vive hardware.    (guessed)
-	int8_t nr_locations;
 
 	//Flood info, for calculating which laser is currently sweeping.
 	int8_t oldcode;
@@ -64,7 +69,7 @@ typedef void (*light_process_func)( struct SurviveObject * so, int sensor_id, in
 typedef void (*imu_process_func)( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id );
 typedef void (*angle_process_func)( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
 
-struct SurviveContext * survive_init();
+struct SurviveContext * survive_init( int headless );
 
 //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_func fbp );
@@ -81,8 +86,7 @@ struct SurviveObject * survive_get_so_by_name( struct SurviveContext * ctx, cons
 //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 );
+int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data, int datalen );
 
 //Install the calibrator.
 void survive_cal_install( struct SurviveContext * ctx );
diff --git a/redist/json_helpers.c b/redist/json_helpers.c
new file mode 100644
index 0000000..74028b2
--- /dev/null
+++ b/redist/json_helpers.c
@@ -0,0 +1,64 @@
+// (C) 2017 <>< Joshua Allen, Under MIT/x11 License.
+
+#define _GNU_SOURCE
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "json_helpers.h"
+
+void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count) {
+	uint8_t i = 0;
+	char * str1 = NULL;
+	char * str2 = NULL;
+	asprintf(&str1,"\"%s\":[", tag);
+
+	for (i=0;i<count;++i) {
+		if (i<(count-1)) {
+			asprintf(&str2, "%s\"%f\",", str1,v[i]);
+		} else {
+			asprintf(&str2, "%s\"%f\"", str1,v[i]);
+		}
+		free(str1);
+		str1=str2;
+		str2=NULL;
+	}
+	asprintf(&str2, "%s]", str1,v[i]);
+	fputs(str2,f);
+	free(str1);
+	free(str2);
+}
+
+void json_write_double_array(FILE* f, const char* tag, double* v, uint8_t count) {
+	uint8_t i = 0;
+	char * str1 = NULL;
+	char * str2 = NULL;
+	asprintf(&str1,"\"%s\":[", tag);
+
+	for (i=0;i<count;++i) {
+		if (i<(count-1)) {
+			asprintf(&str2, "%s\"%f\",", str1,v[i]);
+		} else {
+			asprintf(&str2, "%s\"%f\"", str1,v[i]);
+		}
+		free(str1);
+		str1=str2;
+		str2=NULL;
+	}
+	asprintf(&str2, "%s]", str1,v[i]);
+	fputs(str2,f);
+	free(str1);
+	free(str2);
+}
+
+void json_write_uint32(FILE* f, const char* tag, uint32_t v) {
+	fprintf(f, "\"%s\":\"%d\"", tag, v);
+}
+
+void json_write_float(FILE* f, const char* tag, float v) {
+	fprintf(f, "\"%s\":\"%f\"", tag, v);
+}
+
+void json_write_str(FILE* f, const char* tag, const char* v) {
+	fprintf(f, "\"%s\":\"%s\"", tag, v);
+}
\ No newline at end of file
diff --git a/redist/json_helpers.h b/redist/json_helpers.h
new file mode 100644
index 0000000..8d6eb64
--- /dev/null
+++ b/redist/json_helpers.h
@@ -0,0 +1,14 @@
+// (C) 2017 <>< Joshua Allen, Under MIT/x11 License.
+
+#ifndef JSON_HELPERS_H
+#define JSON_HELPERS_H
+
+#include <stdint.h>
+
+void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count);
+void json_write_double_array(FILE* f, const char* tag, double* v, uint8_t count);
+void json_write_uint32(FILE* f, const char* tag, uint32_t v);
+void json_write_float(FILE* f, const char* tag, float v);
+void json_write_str(FILE* f, const char* tag, const char* v);
+
+#endif
\ No newline at end of file
diff --git a/redist/linmath.h b/redist/linmath.h
index 20a7848..caec281 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -19,6 +19,7 @@
 
 #define FLT double
 #define FLT_SQRT sqrt
+#define FLT_TAN tan
 #define FLT_SIN  sin
 #define FLT_COS  cos
 #define FLT_ACOS  acos
@@ -30,6 +31,7 @@
 
 #define FLT float
 #define FLT_SQRT sqrtf
+#define FLT_TAN tanf
 #define FLT_SIN  sinf
 #define FLT_COS  cosf
 #define FLT_ACOS  acosf
diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c
index 8ec16f2..e35c24d 100644
--- a/src/ootx_decoder.c
+++ b/src/ootx_decoder.c
@@ -112,7 +112,7 @@ void ootx_pump_bit(ootx_decoder_context *ctx, uint8_t dbit) {
 //		printf("drop %d\n", dbit);
 		if( !dbit )
 		{
-			printf("Bad sync bit\n");
+			//printf("Bad sync bit\n");
 			ootx_reset_buffer(ctx);
 		}
 		ctx->bits_processed = 0;
diff --git a/src/survive.c b/src/survive.c
index aab2ae6..9bc1a2c 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -3,22 +3,12 @@
 
 #include <survive.h>
 #include "survive_internal.h"
+#include "survive_driverman.h"
 #include <stdio.h>
 #include <stdlib.h>
-#include <jsmn.h>
 #include <string.h>
 #include <zlib.h>
 
-
-static int jsoneq(const char *json, jsmntok_t *tok, const char *s) {
- if (tok->type == JSMN_STRING && (int) strlen(s) == tok->end - tok->start &&
-    strncmp(json + tok->start, s, tok->end - tok->start) == 0) {
-		return 0;
-	}
-	return -1;
-}
-
-
 static void survivefault( struct SurviveContext * ctx, const char * fault )
 {
 	fprintf( stderr, "Error: %s\n", fault );
@@ -30,113 +20,11 @@ static void survivenote( struct SurviveContext * ctx, const char * fault )
 	fprintf( stderr, "Info: %s\n", fault );
 }
 
-static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i )
-{
-	int k;
-	int pts = t[i+1].size;
-	jsmntok_t * tk;
-
-	so->nr_locations = 0;
-	*floats_out = malloc( sizeof( **floats_out ) * 32 * 3 );
-
-	for( k = 0; k < pts; k++ )
-	{
-		tk = &t[i+2+k*4];
-
-		FLT vals[3];
-		int m;
-		for( m = 0; m < 3; m++ )
-		{
-			char ctt[128];
-
-			tk++;
-			int elemlen = tk->end - tk->start;
-
-			if( tk->type != 4 || elemlen > sizeof( ctt )-1 )
-			{
-				SV_ERROR( "Parse error in JSON\n" );
-				return 1;
-			}
-
-			memcpy( ctt, ct0conf + tk->start, elemlen );
-			ctt[elemlen] = 0;
-			FLT f = atof( ctt );
-			int id = so->nr_locations*3+m;
-			(*floats_out)[id] = f;
-		}
-		so->nr_locations++;
-	}
-	return 0;
-}
-
-static int LoadConfig( struct SurviveContext * ctx, struct SurviveObject * so, int devno, int iface, int extra_magic )
-{
-	char * ct0conf = 0;
-	int len = survive_get_config( &ct0conf, ctx, devno, iface, extra_magic );
-
-#if 0
-	char fname[100];
-	sprintf( fname, "%s_config.json", so->codename );
-	FILE * f = fopen( fname, "w" );
-	fwrite( ct0conf, strlen(ct0conf), 1, f );
-	fclose( f );
-#endif
-
-	if( len > 0 )
-	{
-
-		//From JSMN example.
-		jsmn_parser p;
-		jsmntok_t t[4096];
-		jsmn_init(&p);
-		int i;
-		int r = jsmn_parse(&p, ct0conf, len, t, sizeof(t)/sizeof(t[0]));	
-		if (r < 0) {
-			SV_INFO("Failed to parse JSON in HMD configuration: %d\n", r);
-			return -1;
-		}
-		if (r < 1 || t[0].type != JSMN_OBJECT) {
-			SV_INFO("Object expected in HMD configuration\n");
-			return -2;
-		}
-
-		for (i = 1; i < r; i++) {
-			jsmntok_t * tk = &t[i];
-
-			char ctxo[100];
-			int ilen = tk->end - tk->start;
-			if( ilen > 99 ) ilen = 99;
-			memcpy(ctxo, ct0conf + tk->start, ilen);
-			ctxo[ilen] = 0;
-
-//				printf( "%d / %d / %d / %d %s %d\n", tk->type, tk->start, tk->end, tk->size, ctxo, jsoneq(ct0conf, &t[i], "modelPoints") );
-//				printf( "%.*s\n", ilen, ct0conf + tk->start );
-
-			if (jsoneq(ct0conf, tk, "modelPoints") == 0) {
-				if( ParsePoints( ctx, so, ct0conf, &so->sensor_locations, t, i  ) )
-				{
-					break;
-				}
-			}
-			if (jsoneq(ct0conf, tk, "modelNormals") == 0) {
-				if( ParsePoints( ctx, so, ct0conf, &so->sensor_normals, t, i  ) )
-				{
-					break;
-				}
-			}
-		}
-	}
-	else
-	{
-		//TODO: Cleanup any remaining USB stuff.
-		return 1;
-	}
-	return 0;
-}
 
-struct SurviveContext * survive_init()
+struct SurviveContext * survive_init( int headless )
 {
 	int r = 0;
+	int i = 0;
 	struct SurviveContext * ctx = calloc( 1, sizeof( struct SurviveContext ) );
 
 	ctx->faultfunction = survivefault;
@@ -146,66 +34,17 @@ struct SurviveContext * survive_init()
 	ctx->imuproc = survive_default_imu_process;
 	ctx->angleproc = survive_default_angle_process;
 
-	ctx->headset.ctx = ctx;
-	memcpy( ctx->headset.codename, "HMD", 4 );
-	ctx->watchman[0].ctx = ctx;
-	memcpy( ctx->watchman[0].codename, "WM0", 4 );
-	ctx->watchman[1].ctx = ctx;
-	memcpy( ctx->watchman[1].codename, "WM1", 4 );
+	const char * DriverName;
+	while( ( DriverName = GetDriverNameMatching( "DriverReg", i++ ) ) )
 
-	//USB must happen last.
-	if( r = survive_usb_init( ctx ) )
 	{
-		//TODO: Cleanup any libUSB stuff sitting around.
-		goto fail_gracefully;
+		DeviceDriver dd = GetDriver( DriverName );
+		printf( "Loading driver %s (%p) (%d)\n", DriverName, dd, i );
+		r = dd( ctx );
+		printf( "Driver %s reports status %d\n", DriverName, r );
 	}
 
-	//Next, pull out the config stuff.
-	if( LoadConfig( ctx, &ctx->headset, 1, 0, 0 ) ) goto fail_gracefully;
-	if( LoadConfig( ctx, &ctx->watchman[0], 2, 0, 1 ) ) { SV_INFO( "Watchman 0 config issue." ); }
-	if( LoadConfig( ctx, &ctx->watchman[1], 3, 0, 1 ) ) { SV_INFO( "Watchman 1 config issue." ); }
-
-	ctx->headset.timebase_hz = ctx->watchman[0].timebase_hz = ctx->watchman[1].timebase_hz = 48000000;
-	ctx->headset.pulsedist_max_ticks = ctx->watchman[0].pulsedist_max_ticks = ctx->watchman[1].pulsedist_max_ticks = 500000;
-	ctx->headset.pulselength_min_sync = ctx->watchman[0].pulselength_min_sync = ctx->watchman[1].pulselength_min_sync = 2200;
-	ctx->headset.pulse_in_clear_time = ctx->watchman[0].pulse_in_clear_time = ctx->watchman[1].pulse_in_clear_time = 35000;
-	ctx->headset.pulse_max_for_sweep = ctx->watchman[0].pulse_max_for_sweep = ctx->watchman[1].pulse_max_for_sweep = 1800;
-
-	ctx->headset.pulse_synctime_offset = ctx->watchman[0].pulse_synctime_offset = ctx->watchman[1].pulse_synctime_offset = 20000;
-	ctx->headset.pulse_synctime_slack = ctx->watchman[0].pulse_synctime_slack = ctx->watchman[1].pulse_synctime_slack = 5000;
-
-	ctx->headset.timecenter_ticks     = ctx->headset.timebase_hz / 240;
-	ctx->watchman[0].timecenter_ticks = ctx->watchman[0].timebase_hz / 240;
-	ctx->watchman[1].timecenter_ticks = ctx->watchman[1].timebase_hz / 240;
-/*
-	int i;
-	int locs = ctx->headset.nr_locations;
-	printf( "Locs: %d\n", locs );
-	if (ctx->headset.sensor_locations )
-	{
-		printf( "POSITIONS:\n" );
-		for( i = 0; i < locs*3; i+=3 )
-		{
-			printf( "%f %f %f\n", ctx->headset.sensor_locations[i+0], ctx->headset.sensor_locations[i+1], ctx->headset.sensor_locations[i+2] );
-		}
-	}
-	if( ctx->headset.sensor_normals )
-	{
-		printf( "NORMALS:\n" );
-		for( i = 0; i < locs*3; i+=3 )
-		{
-			printf( "%f %f %f\n", ctx->headset.sensor_normals[i+0], ctx->headset.sensor_normals[i+1], ctx->headset.sensor_normals[i+2] );
-		}
-	}
-*/
-
-	
-
 	return ctx;
-fail_gracefully:
-	survive_usb_close( ctx );
-	free( ctx );
-	return 0;
 }
 
 void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_func fbp )
@@ -249,18 +88,80 @@ void survive_install_angle_fn( struct SurviveContext * ctx,  angle_process_func
 		ctx->angleproc = survive_default_angle_process;
 }
 
+int survive_add_object( struct SurviveContext * ctx, struct SurviveObject * obj )
+{
+	int oldct = ctx->objs_ct;
+	ctx->objs = realloc( ctx->objs, sizeof( struct SurviveObject * ) * (oldct+1) );
+	ctx->objs[oldct] = obj;
+	ctx->objs_ct = oldct+1;
+}
+
+void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic )
+{
+	int oldct = ctx->driver_ct;
+	ctx->drivers = realloc( ctx->drivers, sizeof( void * ) * (oldct+1) );
+	ctx->driverpolls = realloc( ctx->driverpolls, sizeof( DeviceDriverCb * ) * (oldct+1) );
+	ctx->drivercloses = realloc( ctx->drivercloses, sizeof( DeviceDriverCb * ) * (oldct+1) );
+	ctx->drivermagics = realloc( ctx->drivermagics, sizeof( DeviceDriverMagicCb * ) * (oldct+1) );
+	ctx->drivers[oldct] = payload;
+	ctx->driverpolls[oldct] = poll;
+	ctx->drivercloses[oldct] = close;
+	ctx->drivermagics[oldct] = magic;
+	ctx->driver_ct = oldct+1;
+}
 
+int survive_send_magic( struct SurviveContext * ctx, int magic_code, void * data, int datalen )
+{
+	int oldct = ctx->driver_ct;
+	int i;
+	for( i = 0; i < oldct; i++ )
+	{
+		ctx->drivermagics[i]( ctx, ctx->drivers[i], magic_code, data, datalen );
+	}	
+}
 
 void survive_close( struct SurviveContext * ctx )
 {
-	survive_usb_close( ctx );
+	const char * DriverName;
+	int r = 0;
+	while( ( DriverName = GetDriverNameMatching( "DriverUnreg", r++ ) ) )
+	{
+		DeviceDriver dd = GetDriver( DriverName );
+		SV_INFO( "De-registering driver %s (%p)", DriverName, dd );
+		r = dd( ctx );
+		SV_INFO( "Driver %s reports status %d", DriverName, r );
+	}
+
+	int oldct = ctx->driver_ct;
+	int i;
+	for( i = 0; i < oldct; i++ )
+	{
+		ctx->drivercloses[i]( ctx, ctx->drivers[i] );
+	}
+
+	free( ctx->objs );
+	free( ctx->drivers );
+	free( ctx->driverpolls );
+	free( ctx->drivermagics );
+	free( ctx->drivercloses );
+	free( ctx );
 }
 
 int survive_poll( struct SurviveContext * ctx )
 {
-	survive_usb_poll( ctx );
+	int oldct = ctx->driver_ct;
+	int i, r;
+
+	for( i = 0; i < oldct; i++ )
+	{
+		r = ctx->driverpolls[i]( ctx, ctx->drivers[i] );
+		if( r ) return r;
+	}
+
+	return 0;
 }
 
+
 int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int inlen, char * output, int outlen )
 {
 	z_stream zs; //Zlib stream.  May only be used by configuration at beginning and by USB thread periodically.
@@ -285,9 +186,12 @@ int survive_simple_inflate( struct SurviveContext * ctx, const char * input, int
 
 struct SurviveObject * survive_get_so_by_name( struct SurviveContext * ctx, const char * name )
 {
-	if( strcmp( name, "HMD" ) == 0 ) return &ctx->headset;
-	if( strcmp( name, "WM0" ) == 0 ) return &ctx->watchman[0];
-	if( strcmp( name, "WM1" ) == 0 ) return &ctx->watchman[1];
+	int i;
+	for( i = 0; i < ctx->objs_ct; i++ )
+	{
+		if( strcmp( ctx->objs[i]->codename, name ) == 0 )
+			return ctx->objs[i];
+	}
 	return 0;
 }
 
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 7b0a824..f372309 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -2,14 +2,25 @@
 // (C) 2016, 2017 <>< C. N. Lohr, Under MIT/x11 License.
 
 // All OOTX code was written by J. Allen. Rest of the code is probably mostly CNLohr.
+//
+// This file is primarily geared to the calibration phase, to produce the world cal information.
+// Once world cal is produced, it's unlikely you will need this file at all.  The plan is
+// to not include it at all on any stripped-down versions of libsurvive.
+//
 
 #include "survive_cal.h"
 #include "survive_internal.h"
 #include <math.h>
 #include <string.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+
+#include "survive_config.h"
 
 #define PTS_BEFORE_COMMON 32
-#define NEEDED_COMMON_POINTS 20
+#define NEEDED_COMMON_POINTS 10
+#define NEEDED_TIMES_OF_COMMON 5
+#define DRPTS_NEEDED_FOR_AVG ((int)(DRPTS*3/4))
 
 static void handle_calibration( struct SurviveCalData *cd );
 static void reset_calibration( struct SurviveCalData * cd );
@@ -20,7 +31,7 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	struct SurviveCalData * cd = ctx->calptr;
 	int id = ct->user1;
 
-	SV_INFO( "Got OOTX packet %d %p\n", id, cd );
+	SV_INFO( "Got OOTX packet %d %p", id, cd );
 
 	lighthouse_info_v6 v6;
 	init_lighthouse_info_v6(&v6, packet->data);
@@ -40,6 +51,9 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	b->fcalgibmag[0] = v6.fcal_0_gibmag;
 	b->fcalgibmag[1] = v6.fcal_1_gibmag;
 	b->OOTXSet = 1;
+
+	config_set_lighthouse(b,id);
+	config_save("config.json");
 }
 
 int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length )
@@ -49,20 +63,26 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int
 	switch( cd->stage )
 	{
 	case 0:
-		return snprintf( description, description_length, "Not calibrating" );
+		return snprintf( description, description_length, "0 Not calibrating" );
 	case 1:
-		return snprintf( description, description_length, "Collecting OOTX Data (%d:%d)", cd->ootx_decoders[0].buf_offset, cd->ootx_decoders[1].buf_offset );
+		return snprintf( description, description_length, "1 Collecting OOTX Data (%d:%d)", cd->ootx_decoders[0].buf_offset, cd->ootx_decoders[1].buf_offset );
 	case 2:
+	case 3:
 		if( cd->found_common )
 		{
-			return snprintf( description, description_length, "Collecting Sweep Data %d/%d", cd->peak_counts, DRPTS );
+			return snprintf( description, description_length, "%d Collecting Sweep Data %d/%d", cd->stage, cd->peak_counts, DRPTS );
 		}
 		else
 		{
-			return snprintf( description, description_length, "Searching for common watchman cal %d/%d", cd->peak_counts, PTS_BEFORE_COMMON );
+			return snprintf( description, description_length, "%d Searching for common watchman cal %d/%d (%d/%d)", cd->stage, cd->peak_counts, PTS_BEFORE_COMMON, cd->times_found_common, NEEDED_TIMES_OF_COMMON );
 		}
+
+	case 5:
+		return snprintf( description, description_length, "%d LH Find complete.", cd->stage );
+
+	case 4:
 	default:
-		return snprintf( description, description_length, "Unkown calibration state" );
+		return snprintf( description, description_length, "%d Unkown calibration state", cd->stage );
 	}
 }
 
@@ -79,6 +99,15 @@ void survive_cal_install( struct SurviveContext * ctx )
 	}
 
 	cd->stage = 1;
+	cd->ctx = ctx;
+
+	cd->hmd = survive_get_so_by_name( ctx, "HMD" );
+	if( !cd->hmd )
+	{
+		SV_ERROR( "Error: cannot find any devices labeled HMD. Required for calibration" );
+		free( cd );
+		return;
+	}
 
 	ootx_packet_clbk = ootx_packet_clbk_d;
 
@@ -127,6 +156,17 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 
 	if( !cd ) return;
 
+	int sensid = sensor_id;
+	if( strcmp( so->codename, "WM0" ) == 0 )
+		sensid += 32;
+	if( strcmp( so->codename, "WM1" ) == 0 )
+		sensid += 64;
+
+	if( sensid >= MAX_SENSORS_TO_CAL || sensid < 0 ) return;
+
+	int lighthouse = acode>>2;
+	int axis = acode & 1;
+
 	switch( cd->stage )
 	{
 	default:
@@ -135,32 +175,21 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 		break;
 	case 2:
 	{
-		int sensid = sensor_id;
-		if( strcmp( so->codename, "WM0" ) == 0 )
-			sensid += 32;
-		if( strcmp( so->codename, "WM1" ) == 1 )
-			sensid += 64;
-
-		int lighthouse = acode>>2;
-		int axis = acode & 1;
 		int ct = cd->all_counts[sensid][lighthouse][axis]++;
 		cd->all_lengths[sensid][lighthouse][axis][ct] = length;
 		cd->all_angles[sensid][lighthouse][axis][ct] = angle;
 		if( ct > cd->peak_counts )
 		{
 			cd->peak_counts = ct;
-			if( ct >= DRPTS )
-				handle_calibration( cd ); //This will also reset all cals.
 		}
 
-		//TODO: Determine if there is a sensor on a watchman visible from both lighthouses.
-		if( sensid >= 32 && !cd->found_common )
+		//Determine if there is a sensor on a watchman visible from both lighthouses.
+		if( sensid >= 32 )
 		{
 			int k;
 			int ok = 1;
 			for( k = 0; k < NUM_LIGHTHOUSES; k++ )
 			{
-
 				if( cd->all_counts[sensid][k][0] < NEEDED_COMMON_POINTS || cd->all_counts[sensid][k][1] < NEEDED_COMMON_POINTS )
 				{
 					ok = 0;
@@ -170,13 +199,48 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 			if( ok ) cd->found_common = 1;
 		}
 
-		if( cd->peak_counts > PTS_BEFORE_COMMON && !cd->found_common )
+		if( cd->peak_counts >= PTS_BEFORE_COMMON )
 		{
-			reset_calibration( cd );
-		}
+			int tfc = cd->times_found_common;
+			if( cd->found_common )
+			{
+				if( tfc >= NEEDED_TIMES_OF_COMMON )
+				{
+					SV_INFO( "Stage 2 moving to stage 3. %d %d %d", cd->peak_counts, cd->found_common, tfc );
+					reset_calibration( cd );
+					cd->stage = 3;
+					cd->found_common = 1;
+				}
+				else
+				{
+					SV_INFO( "Stage 2 good - continuing. %d %d %d", cd->peak_counts, cd->found_common, tfc );
+					reset_calibration( cd );
+					cd->times_found_common = tfc+1;
+				}
+			}
+			else
+			{
+				SV_INFO( "Stage 2 bad - redoing. %d %d %d", cd->peak_counts, cd->found_common, tfc );
+				reset_calibration( cd );
+				cd->times_found_common = 0;
+			}
+		}			
 
 		break;
 	}
+	case 3:
+	{
+		int ct = cd->all_counts[sensid][lighthouse][axis]++;
+		cd->all_lengths[sensid][lighthouse][axis][ct] = length;
+		cd->all_angles[sensid][lighthouse][axis][ct] = angle;
+		if( ct > cd->peak_counts )
+		{
+			cd->peak_counts = ct;
+			if( ct >= DRPTS )
+				handle_calibration( cd ); //This must also reset all cals.
+		}
+		break;
+	}
 	}
 }
 
@@ -185,11 +249,206 @@ static void reset_calibration( struct SurviveCalData * cd )
 	memset( cd->all_counts, 0, sizeof( cd->all_counts ) );
 	cd->peak_counts = 0;
 	cd->found_common = 0;
+	cd->times_found_common = 0;
+	cd->stage = 2;
 }
 
 static void handle_calibration( struct SurviveCalData *cd )
 {
-	//Do stuff.
+	struct SurviveContext * ctx = cd->ctx;
+
+	#define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2)
+
+/*
+	FLT avgsweeps[MAX_CAL_PT_DAT];
+	FLT avglens[MAX_CAL_PT_DAT];
+	FLT stdsweeps[MAX_CAL_PT_DAT];
+	FLT stdlens[MAX_CAL_PT_DAT];
+	int ctsweeps[MAX_CAL_PT_DAT];
+*/
+
+	memset( cd->ctsweeps, 0, sizeof( cd->ctsweeps ) );
+
+	//Either advance to stage 4 or go resetting will go back to stage 2.
+	//What is stage 4?  Are we done then?
+
+	mkdir( "calinfo", 0755 );
+	FILE * hists = fopen( "calinfo/histograms.csv", "w" );
+	FILE * ptinfo = fopen( "calinfo/ptinfo.csv", "w" );
+	int sen, axis, lh;
+	for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ )
+	for( lh = 0; lh < NUM_LIGHTHOUSES; lh++ )
+	for( axis = 0; axis < 2; axis++ )
+	{
+		int dpmax = cd->all_counts[sen][lh][axis];
+		if( dpmax < 50 ) continue;
+		int i;
+
+		FLT sumsweepangle = 0;
+		FLT sumlentime = 0;
+
+		//Find initial guess at average
+		for( i = 0; i < dpmax; i++ )
+		{
+			FLT sweepangle = cd->all_angles[sen][lh][axis][i];
+			FLT datalen = cd->all_lengths[sen][lh][axis][i];
+			sumsweepangle += sweepangle;
+			sumlentime += datalen;
+		}
+
+		#define OUTLIER_ANGLE   0.001	//TODO: Tune
+		#define OUTLIER_LENGTH	0.001	//TODO: Tune
+		#define ANGLE_STDEV_TOO_HIGH 0.000001 //TODO: Tune
+
+		FLT avgsweep = sumsweepangle / dpmax;
+		FLT avglen = sumlentime / dpmax;
+		int count = 0;
+
+		FLT max_outlier_angle = 0;
+		FLT max_outlier_length = 0;
+
+		//Get rid of outliers
+		for( i = 0; i < dpmax; i++ )
+		{
+			FLT sweepangle = cd->all_angles[sen][lh][axis][i];
+			FLT datalen = cd->all_lengths[sen][lh][axis][i];
+			FLT Sdiff = sweepangle - avgsweep;
+			FLT Ldiff = datalen - avglen;
+			FLT Sdiff2 = Sdiff * Sdiff;
+			FLT Ldiff2 = Ldiff * Ldiff;
+
+			if( Sdiff2 > max_outlier_angle ) max_outlier_angle = Sdiff2;
+			if( Ldiff2 > max_outlier_length ) max_outlier_length = Ldiff2;
+
+			if( Sdiff2 > OUTLIER_ANGLE || Ldiff2 > OUTLIER_LENGTH )
+			{
+				cd->all_lengths[sen][lh][axis][i] = -1;
+			}
+			else
+			{
+				count++;
+			}
+		}
+
+		if( count < DRPTS_NEEDED_FOR_AVG )
+		{
+			printf( "DPAVG %d\n", count );
+			//Not enough for this point to be considered.
+			continue;
+		}
+
+		sumsweepangle = 0;
+		sumlentime = 0;
+		//Redo, finding new average:
+		for( i = 0; i < dpmax; i++ )
+		{
+			FLT sweepangle = cd->all_angles[sen][lh][axis][i];
+			FLT datalen = cd->all_lengths[sen][lh][axis][i];
+			if( datalen < 0 ) continue;
+			sumsweepangle += sweepangle;
+			sumlentime += datalen;
+		}
+
+		avgsweep = sumsweepangle / count;
+		avglen = sumlentime / count;
+
+		FLT stddevang = 0;
+		FLT stddevlen = 0;
+
+		#define HISTOGRAMSIZE   31
+		#define HISTOGRAMBINANG 0.00001  //TODO: Tune
+
+		int histo[HISTOGRAMSIZE];
+		memset( histo, 0, sizeof( histo ) );
+
+		for( i = 0; i < dpmax; i++ )
+		{
+			FLT sweepangle = cd->all_angles[sen][lh][axis][i];
+			FLT datalen = cd->all_lengths[sen][lh][axis][i];
+			if( datalen < 0 ) continue;
+
+			FLT Sdiff = sweepangle - avgsweep;
+			FLT Ldiff = datalen - avglen;
+			FLT Sdiff2 = Sdiff * Sdiff;
+			FLT Ldiff2 = Ldiff * Ldiff;
+
+			stddevang += Sdiff2;
+			stddevlen += Ldiff2;
+
+			int llm = Sdiff / HISTOGRAMBINANG + (HISTOGRAMSIZE/2.0);
+			if( llm < 0 ) llm = 0;
+			if( llm >= HISTOGRAMSIZE ) llm = HISTOGRAMSIZE-1;
+
+			histo[llm]++;
+		}
+
+		stddevang /= count;
+		stddevlen /= count;
+
+		if( stddevang > ANGLE_STDEV_TOO_HIGH )
+		{
+			SV_INFO( "DROPPED: %02d:%d:%d dropped because stddev (%f) was too high.", sen, lh, axis, stddevang );
+			continue;
+		}
+
+		fprintf( hists, "%02d_%d_%d, ", sen, lh, axis );
 
-	reset_calibration( cd );
+		for( i = 0; i < HISTOGRAMSIZE; i++ )
+		{
+			fprintf( hists, "%d ", histo[i] );
+		}
+		fprintf( hists, "\n" );
+
+		fprintf( ptinfo, "%d %d %d %d %f %f %f %f %f %f\n", sen, lh, axis, count, avgsweep, avglen*1000000, stddevang*1000000000, stddevlen*1000000000, max_outlier_length*1000000000, max_outlier_angle*1000000000 );
+
+		int dataindex = sen*(2*NUM_LIGHTHOUSES)+lh*2+axis;
+		cd->avgsweeps[dataindex] = avgsweep;
+		cd->avglens[dataindex] = avglen;
+		cd->stdsweeps[dataindex] = stddevang;
+		cd->stdlens[dataindex] = stddevlen;
+		cd->ctsweeps[dataindex] = count;
+	}
+	fclose( hists );
+	fclose( ptinfo );
+
+	//Comb through data and make sure we still have a sensor on a WM that 
+	int bcp_senid = 0;
+	int bcp_count = 0;
+	for( sen = 0; sen < MAX_SENSORS_TO_CAL; sen++ )
+	{
+		int ct0 = cd->ctsweeps[sen*4+0];
+		int ct1 = cd->ctsweeps[sen*4+0];
+		int ct2 = cd->ctsweeps[sen*4+0];
+		int ct3 = cd->ctsweeps[sen*4+0];
+
+		if( ct0 > ct1 ) ct0 = ct1;
+		if( ct0 > ct2 ) ct0 = ct2;
+		if( ct0 > ct3 ) ct0 = ct3;
+
+		if( ct0 > bcp_count ) { bcp_count = ct0; bcp_senid = sen; }
+	}
+
+	if( bcp_count < DRPTS_NEEDED_FOR_AVG )
+	{
+		SV_INFO( "Stage 3 could not find a suitable common point on a watchman" );
+		reset_calibration( cd );
+		return;
+	}
+
+	cd->senid_of_checkpt = bcp_senid;
+
+	if( survive_cal_lhfind( cd ) == 0 )
+	{
+		SV_INFO( "Stage 4 succeeded." );
+		cd->stage = 5;
+	}
+	else
+	{
+		SV_INFO( "Stage 4 failed." );
+		reset_calibration( cd );
+	}
 }
+
+
+
+
diff --git a/src/survive_cal.h b/src/survive_cal.h
index 42ff1ee..bb4eb32 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -30,20 +30,34 @@ int survive_cal_get_status( struct SurviveContext * ctx, char * description, int
 void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  );
 void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle );
 
-#define MAX_TO_CAL 96
-#define DRPTS 512
-
+#define MAX_SENSORS_TO_CAL 96
+#define DRPTS 1024
+#define MAX_CAL_PT_DAT (MAX_SENSORS_TO_CAL*NUM_LIGHTHOUSES*2)
 struct SurviveCalData
 {
+	struct SurviveContext * ctx;
 	//OOTX Data is sync'd off of the sync pulses coming from the lighthouses.
 	ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES];
 
-	//For statistics-gathering phase.
-	FLT all_lengths[MAX_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
-	FLT all_angles[MAX_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
-	int16_t all_counts[MAX_TO_CAL][NUM_LIGHTHOUSES][2];
+	//For statistics-gathering phase. (Stage 2/3)
+	FLT all_lengths[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
+	FLT all_angles[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS];
+	int16_t all_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2];
 	int16_t peak_counts;
 	int8_t found_common;
+	int8_t times_found_common;
+
+	//For camfind (4+)
+	//Index is calculated with:      int dataindex = sen*(2*NUM_LIGHTHOUSES)+lh*2+axis;
+	FLT avgsweeps[MAX_CAL_PT_DAT];
+	FLT avglens[MAX_CAL_PT_DAT];
+	FLT stdsweeps[MAX_CAL_PT_DAT];
+	FLT stdlens[MAX_CAL_PT_DAT];
+	int ctsweeps[MAX_CAL_PT_DAT];
+
+	int senid_of_checkpt; //This is a point on a watchman that can be used to check the lh solution.
+
+	struct SurviveObject * hmd;
 
 	//Stage:
 	// 0: Idle
@@ -52,6 +66,10 @@ struct SurviveCalData
 };
 
 
+//The following function is not included in the core survive_cal and must be compiled from a camfind file.
+//It should use data for stage 4 and report if it found the 
+int survive_cal_lhfind( struct SurviveCalData * cd );
+
 
 #endif
 
diff --git a/src/survive_cal_lhfind.c b/src/survive_cal_lhfind.c
new file mode 100644
index 0000000..93d9dc0
--- /dev/null
+++ b/src/survive_cal_lhfind.c
@@ -0,0 +1,289 @@
+#include "survive_cal.h"
+#include <math.h>
+#include <string.h>
+#include "linmath.h"
+
+#define MAX_CHECKS 40000
+#define MIN_HITS_FOR_VALID 10
+
+
+FLT static RunOpti(  struct SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat );
+
+//Values used for RunTest()
+
+int survive_cal_lhfind( struct SurviveCalData * cd )
+{
+	struct SurviveContext * ctx = cd->ctx;
+	int cycle, i;
+	int lh = 0;
+	FLT dx, dy, dz;
+
+	//Use the following:
+	//	FLT avgsweeps[MAX_CAL_PT_DAT];
+	//	FLT avglens[MAX_CAL_PT_DAT];
+	//	FLT stdsweeps[MAX_CAL_PT_DAT];
+	//	FLT stdlens[MAX_CAL_PT_DAT];
+	//	int ctsweeps[MAX_CAL_PT_DAT];
+	//
+	// Check your solution against point: senid_of_checkpt's data.
+
+
+
+	for( lh = 0; lh < 2; lh++ )
+	{
+		FLT beste = 1e20;
+
+		FLT LighthousePos[3];
+		FLT LighthouseQuat[4];
+
+		LighthousePos[0] = 0;
+		LighthousePos[1] = 0;
+		LighthousePos[2] = 0;
+		LighthouseQuat[0] = 1;
+		LighthouseQuat[1] = 1;
+		LighthouseQuat[2] = 1;
+		LighthouseQuat[3] = 1;
+
+		FLT bestxyz[3];
+		memcpy( bestxyz, LighthousePos, sizeof( LighthousePos ) );
+
+		//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)
+	
+
+		//Sweep whole area 30 times
+		for( cycle = 0; cycle < 30; cycle ++ )
+		{
+
+			//Adjust position, one axis at a time, over and over until we zero in.
+			{
+				FLT bestxyzrunning[3];
+				beste = 1e20;
+
+				FILE * f;
+				if( cycle == 0 )
+				{
+					char filename[1024];
+					sprintf( filename, "calinfo/%d_lighthouse.dat", lh );
+					f = fopen( filename, "wb" );
+				}
+
+				//We split the space into this many groups (times 2) and
+				//if we're on the first cycle, we want to do a very linear
+				//search.  As we refine our search we can then use a more
+				//binary search technique.
+				FLT splits = 4;
+				if( cycle == 0 ) splits = 32;
+				if( cycle == 1 ) splits = 13;
+				if( cycle == 2 ) splits = 10;
+				if( cycle == 3 ) splits = 8;
+				if( cycle == 4 ) splits = 5;
+
+				//Wwe search throug the whole space.
+				for( dz = 0; dz < fullrange; dz += fullrange/splits )
+				for( dy = -fullrange; dy < fullrange; dy += fullrange/splits )
+				for( dx = -fullrange; dx < fullrange; dx += fullrange/splits )
+				{
+					//Specificially adjust one axis at a time, searching for the best.
+					memcpy( LighthousePos, bestxyz, sizeof( LighthousePos ) );
+					LighthousePos[0] += dx;  //These are adjustments to the "best" from last frame.
+					LighthousePos[1] += dy;
+					LighthousePos[2] += dz;
+
+					FLT ft;
+					//Try refining the search for the best orientation several times.
+					ft = RunOpti(cd, lh, 0, LighthousePos, LighthouseQuat);
+					if( cycle == 0 )
+					{
+						float sk = ft*10.;
+						if( sk > 1 ) sk = 1;
+						uint8_t cell = (1.0 - sk) * 255;
+						FLT epsilon = 0.1;
+
+						if( dz == 0 ) { /* Why is dz special? ? */
+						  if ( dx > -epsilon && dx < epsilon )
+							cell =  255;
+						  if ( dy > -epsilon && dy < epsilon )
+		                                        cell = 128;
+						}
+
+						fprintf( f, "%c", cell );
+					}
+
+					if( ft < beste ) { beste = ft; memcpy( bestxyzrunning, LighthousePos, sizeof( LighthousePos ) ); }
+				}
+
+				if( cycle == 0 )
+				{
+					fclose( f );
+				}
+				memcpy( bestxyz, bestxyzrunning, sizeof( bestxyz ) );
+
+				//Print out the quality of the lock this time.
+				FLT dist = sqrt(bestxyz[0]*bestxyz[0] + bestxyz[1]*bestxyz[1] + bestxyz[2]*bestxyz[2]);
+				printf( "%f %f %f (%f) = %f\n", bestxyz[0], bestxyz[1], bestxyz[2], dist, beste );
+			}
+
+			//Every cycle, tighten up the search area.
+			fullrange *= 0.25;
+		}
+
+		if( beste > 0.01 )
+		{
+			//Error too high
+			SV_ERROR( "LH: %d / Best E %f Error too high\n", lh, beste );
+			return -1;
+		}
+
+		cd->ctx->bsd[lh].PositionSet = 1;
+		copy3d( cd->ctx->bsd[lh].Position, LighthousePos );
+		quatcopy( cd->ctx->bsd[lh].Quaternion, LighthouseQuat );
+	}
+
+	return 0; //Return 0 if success.
+}
+
+
+
+
+
+
+static FLT RunOpti( struct SurviveCalData * cd, int lh, int print, FLT * LighthousePos, FLT * LighthouseQuat )
+{
+	int i, p;
+	FLT UsToTarget[3];
+	FLT LastUsToTarget[3];
+	FLT mux = .9;
+	quatsetnone( LighthouseQuat );
+	struct SurviveObject * hmd = cd->hmd;
+	FLT * hmd_points  = hmd->sensor_locations;
+	FLT * hmd_normals = hmd->sensor_normals;
+
+	int first = 1, second = 0;
+
+	//First check to see if this is a valid viewpoint.
+	//If a sensor is pointed away from where we are testing a possible lighthouse position.
+	//BUT We get data from that light house, then we KNOW this is not a possible
+	//lighthouse position.
+	for( p = 0; p < 32; p++ )
+	{
+		int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2;
+		if( cd->ctsweeps[dataindex+0] < MIN_HITS_FOR_VALID || cd->ctsweeps[dataindex+1] < MIN_HITS_FOR_VALID ) continue;
+		FLT me_to_dot[3];
+		sub3d( me_to_dot, LighthousePos, &hmd_points[p*3] );
+		float dot = dot3d( &hmd_normals[p*3], me_to_dot );
+		if( dot < -.01 ) { 	return 1000; }
+	}
+	int iters = 6;
+
+	//Iterate over a refinement of the quaternion that constitutes the
+	//lighthouse.
+	for( i = 0; i < iters; i++ )
+	{
+		first = 1;
+		for( p = 0; p < 32; p++ )
+		{
+			int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2;
+			if( cd->ctsweeps[dataindex+0] < MIN_HITS_FOR_VALID || cd->ctsweeps[dataindex+1] < MIN_HITS_FOR_VALID ) continue;
+
+			//Find out where our ray shoots forth from.
+			FLT ax = cd->avgsweeps[dataindex+0];
+			FLT ay = cd->avgsweeps[dataindex+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 );
+			}
+
+			quatrotateabout( LighthouseQuat, ConcatQuat, LighthouseQuat );  //Chekcked.  This appears to be 
+
+			mux = mux * 0.94;
+			if( second ) { second = 0; }
+			if( first ) { first = 0; second = 1; }
+			copy3d( LastUsToTarget, RayShootOutWorld );
+		}
+	}
+
+	//Step 2: Determine error.
+	float errorsq = 0.0;
+	int count = 0;
+	for( p = 0; p < 32; p++ )
+	{
+		int dataindex = p*(2*NUM_LIGHTHOUSES)+lh*2;
+		if( cd->ctsweeps[dataindex+0] < MIN_HITS_FOR_VALID || cd->ctsweeps[dataindex+1] < MIN_HITS_FOR_VALID ) continue;
+
+		//Find out where our ray shoots forth from.
+		FLT ax = cd->avgsweeps[dataindex+0];
+		FLT ay = cd->avgsweeps[dataindex+1];
+		FLT RayShootOut[3] = { sin(ax), sin(ay), 0 };
+		RayShootOut[2] = sqrt( 1 - (RayShootOut[0]*RayShootOut[0] + RayShootOut[1]*RayShootOut[1]) );
+
+		//Rotate that ray by the current rotation estimation.
+		quatrotatevector( RayShootOut, LighthouseQuat, RayShootOut );
+
+		//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, cd->ctsweeps[dataindex+0], cd->ctsweeps[dataindex+1] );
+	}
+	if( print ) printf( " = %f\n", sqrt( errorsq ) );
+	return sqrt(errorsq);
+}
+
diff --git a/src/survive_config.c b/src/survive_config.c
new file mode 100644
index 0000000..c46e300
--- /dev/null
+++ b/src/survive_config.c
@@ -0,0 +1,215 @@
+// (C) 2017 <>< Joshua Allen, Under MIT/x11 License.
+#include <string.h>
+#include <assert.h>
+#include "survive_config.h"
+#include <json_helpers.h>
+
+#define MAX_CONFIG_ENTRIES 100
+#define MAX_LIGHTHOUSES 2
+
+
+
+config_group global_config_values;
+config_group lh_config[MAX_LIGHTHOUSES]; //lighthouse configs
+
+//static uint16_t used_entries = 0;
+
+static FILE *config_file = NULL;
+const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* values, uint8_t count);
+
+void init_config_group(config_group *cg, uint16_t count) {
+	uint16_t i = 0;
+	cg->config_entries = malloc(count*sizeof(config_entry));
+	cg->used_entries = 0;
+	cg->max_entries = count;
+
+	for (i=0;i<count;++i) {
+		cg->config_entries[i].data = NULL;
+		cg->config_entries[i].tag = NULL;
+		cg->config_entries[i].type = CONFIG_UNKNOWN;
+		cg->config_entries[i].elements = 0;
+	}
+}
+
+void config_init() {
+	uint16_t i = 0;
+	init_config_group(&global_config_values, MAX_CONFIG_ENTRIES);
+	for(i=0;i<MAX_LIGHTHOUSES;i++) {
+		init_config_group(lh_config+i, 9);
+	}
+}
+
+void config_load(const char* path) {
+	config_file = fopen(path, "r");
+}
+
+void config_close() {
+	fclose(config_file);
+}
+
+void config_set_lighthouse(struct BaseStationData* bsd, uint8_t idx) {
+	config_group *cg = lh_config+idx;
+	config_set_uint32(cg,"index", idx);
+	config_set_uint32(cg,"id", bsd->BaseStationID);
+	config_set_float_a(cg,"position", bsd->Position, 3);
+	config_set_float_a(cg,"quaternion", bsd->Quaternion, 4);
+	config_set_float_a(cg,"fcalphase", bsd->fcalphase, 2);
+	config_set_float_a(cg,"fcaltilt", bsd->fcaltilt,2);
+	config_set_float_a(cg,"fcalcurve", bsd->fcalcurve,2);
+	config_set_float_a(cg,"fcalgibpha", bsd->fcalgibpha,2);
+	config_set_float_a(cg,"fcalgibmag", bsd->fcalgibmag,2);
+}
+
+void sstrcpy(char** dest, const char *src) {
+	uint32_t len = strlen(src)+1;
+	assert(dest!=NULL);
+
+	if (*dest == NULL) {
+		*dest = (char*)malloc(len);
+	} else {
+		*dest = (char*)realloc(*dest, len);
+	}
+	strcpy(*dest,src);
+}
+
+config_entry* find_config_entry(config_group *cg, const char *tag) {
+	uint16_t i = 0;
+	for (i=0;i < cg->used_entries;++i) {
+		if ( strcmp(cg->config_entries[i].tag, tag) == 0 ) {
+			return cg->config_entries+i;
+		}
+	}
+	return NULL;
+}
+
+const char* config_read_str(config_group *cg, const char *tag, const char *def) {
+	config_entry *cv = find_config_entry(cg, tag);
+
+	if (cv != NULL) return cv->data;
+
+	return config_set_str(cg,tag,def);
+}
+
+uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def) {
+	config_entry *cv = find_config_entry(cg, tag);
+
+	if (cv != NULL) return cv->numeric.i;
+
+	return config_set_uint32(cg, tag, def);
+}
+
+FLT config_read_float(config_group *cg, const char *tag, const FLT def) {
+	config_entry *cv = find_config_entry(cg, tag);
+
+	if (cv != NULL) return cv->numeric.f;
+
+	return config_set_float(cg, tag, def);
+}
+
+config_entry* next_unused_entry(config_group *cg) {
+	config_entry *cv = cg->config_entries + cg->used_entries;
+	assert(cg->used_entries < cg->max_entries);
+	cg->used_entries++;
+	return cv;
+}
+
+const char* config_set_str(config_group *cg, const char *tag, const char* value) {
+	config_entry *cv = find_config_entry(cg, tag);
+	if (cv == NULL) cv = next_unused_entry(cg);
+
+	sstrcpy(&(cv->tag), tag);
+	sstrcpy(&(cv->data), value);
+	cv->type = CONFIG_STRING;
+
+	return value;
+}
+
+const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value) {
+	config_entry *cv = find_config_entry(cg, tag);
+	if (cv == NULL) cv = next_unused_entry(cg);
+
+	sstrcpy(&(cv->tag), tag);
+	cv->numeric.i = value;
+	cv->type = CONFIG_UINT32;
+
+	return value;
+}
+
+const FLT config_set_float(config_group *cg, const char *tag, const FLT value) {
+	config_entry *cv = find_config_entry(cg, tag);
+	if (cv == NULL) cv = next_unused_entry(cg);
+
+	sstrcpy(&(cv->tag), tag);
+	cv->numeric.f = value;
+	cv->type = CONFIG_FLOAT;
+
+	return value;
+}
+
+const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* values, uint8_t count) {
+	config_entry *cv = find_config_entry(cg, tag);
+	if (cv == NULL) cv = next_unused_entry(cg);
+
+	sstrcpy(&(cv->tag), tag);
+
+	if (cv->data == NULL) {
+		cv->data = (char*)malloc(sizeof(FLT)*count);
+	}
+	else {
+		cv->data = (char*)realloc(cv->data, sizeof(FLT)*count);
+	}
+	printf("float array\n");
+
+	memcpy(cv->data,values,sizeof(FLT)*count);
+	cv->type = CONFIG_FLOAT_ARRAY;
+	cv->elements = count;
+
+	return values;
+}
+
+void _json_write_float_array(FILE* f, const char* tag, FLT* v, uint8_t count) {
+	#ifdef USE_DOUBLE
+		json_write_double_array(f,tag,v,count);
+	#else
+		json_write_float_array(f,tag,v,count);
+	#endif
+}
+
+void write_config_group(FILE* f, config_group *cg, char *tag) {
+	uint16_t i = 0;
+
+	if (tag != NULL) {
+		fprintf(f, "\"%s\":{\n", tag);
+	}
+
+	for (i=0;i < cg->used_entries;++i) {
+		if (cg->config_entries[i].type == CONFIG_FLOAT) {
+			json_write_float(f, cg->config_entries[i].tag, cg->config_entries[i].numeric.f);
+		} else if (cg->config_entries[i].type == CONFIG_UINT32) {
+			json_write_uint32(f, cg->config_entries[i].tag, cg->config_entries[i].numeric.i);
+		} else if (cg->config_entries[i].type == CONFIG_STRING) {
+			json_write_str(f, cg->config_entries[i].tag, cg->config_entries[i].data);
+		} else if (cg->config_entries[i].type == CONFIG_FLOAT_ARRAY) {
+			_json_write_float_array(f, cg->config_entries[i].tag, (FLT*)cg->config_entries[i].data, cg->config_entries[i].elements);
+		}
+		if ((i+1) < cg->used_entries) fprintf(f,",");
+		fprintf(f,"\n");
+	};
+
+	if (tag != NULL) {
+		fprintf(f,"}\n");
+	}
+}
+
+void config_save(const char* path) {
+	uint16_t i = 0;
+
+	FILE* f = fopen(path, "w");
+
+	write_config_group(f,&global_config_values, NULL);
+	write_config_group(f,lh_config, "lighthouse0");
+	write_config_group(f,lh_config+1, "lighthouse1");
+
+	fclose(f);
+}
+
diff --git a/src/survive_config.h b/src/survive_config.h
new file mode 100644
index 0000000..14e2fc6
--- /dev/null
+++ b/src/survive_config.h
@@ -0,0 +1,54 @@
+// (C) 2017 <>< Joshua Allen, Under MIT/x11 License.
+
+
+#ifndef _SURVIVE_CONFIG_H
+#define _SURVIVE_CONFIG_H
+
+#include "survive_internal.h"
+
+typedef enum {
+	CONFIG_UNKNOWN = 0,
+	CONFIG_FLOAT = 1,
+	CONFIG_UINT32 = 2,
+	CONFIG_STRING = 3,
+	CONFIG_FLOAT_ARRAY = 4,
+} cval_type;
+
+
+typedef struct {
+	char *tag;
+	cval_type type;
+	union {
+		uint32_t i;
+		FLT f;
+	} numeric;
+	char *data;
+	uint32_t elements;
+} config_entry;
+
+typedef struct {
+	config_entry *config_entries;
+	uint16_t	used_entries;
+	uint16_t	max_entries;
+} config_group;
+
+extern config_group global_config_values;
+extern config_group lh_config[2]; //lighthouse configs
+
+
+void config_init();
+void config_open(const char* path, const char* mode);
+void config_close();
+//void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length);
+void config_set_lighthouse(struct BaseStationData* bsd, uint8_t idx);
+
+void config_save(const char* path);
+const FLT config_set_float(config_group *cg, const char *tag, const FLT value);
+const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value);
+const char* config_set_str(config_group *cg, const char *tag, const char* value);
+
+FLT config_read_float(config_group *cg, const char *tag, const FLT def);
+uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def);
+const char* config_read_str(config_group *cg, const char *tag, const char *def);
+
+#endif
\ No newline at end of file
diff --git a/src/survive_data.c b/src/survive_data.c
index ad834cf..e55b640 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -1,35 +1,12 @@
 //<>< (C) 2016 C. N. Lohr, MOSTLY Under MIT/x11 License.
 //
-//Based off of https://github.com/collabora/OSVR-Vive-Libre
-// Originally Copyright 2016 Philipp Zabel
-// Originally Copyright 2016 Lubosz Sarnecki <lubosz.sarnecki@collabora.co.uk>
-// Originally Copyright (C) 2013 Fredrik Hultin
-// Originally Copyright (C) 2013 Jakob Bornecrantz
-//
-//But, re-written as best as I can to get it put under an open souce license instead of a forced-source license.
-//If there are portions of the code too similar to the original, I would like to know  so they can be re-written.
-//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
 
 #include "survive_internal.h"
 #include <stdint.h>
 #include <string.h>
 
-#define POP1  (*(readdata++))
-#define POP2  (*(((uint16_t*)((readdata+=2)-2))))
-#define POP4  (*(((uint32_t*)((readdata+=4)-4))))
-
-
-struct LightcapElement
-{
-	uint8_t sensor_id;
-	uint8_t type;
-	uint16_t length;
-	uint32_t timestamp;
-} __attribute__((packed));
-
-
 //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
-static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le )
+void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le )
 {
 	struct SurviveContext * ctx = so->ctx;
 	//int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_master_time;
@@ -177,7 +154,8 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 		if (acode > 3) {
 			if( ssn == 0 )
 			{
-				SV_INFO( "Warning: got a slave marker but only got a master sync." );
+				//SV_INFO( "Warning: got a slave marker but only got a master sync." );
+				//This happens too frequently.  Consider further examination.
 			}
 			dl = so->last_time[1];
 			tpco = so->last_length[1];
@@ -199,336 +177,3 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 }
 
 
-
-static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
-{
-	int i;
-
-	uint8_t startread[29];
-	memcpy( startread, readdata, 29 );
-
-#if 0
-	printf( "DAT:     " );
-		for( i = 0; i < 29; i++ )
-		{
-			printf( "%02x ", readdata[i] );
-		}
-		printf("\n");
-#endif
-
-	uint8_t time1 = POP1;
-	uint8_t qty = POP1;
-	uint8_t time2 = POP1;
-	uint8_t type = POP1;
-	qty-=2;
-	int propset = 0;
-	int doimu = 0;
-
-
-	if( (type & 0xf0) == 0xf0 )
-	{
-		propset |= 4;
-		//printf( "%02x %02x %02x %02x\n", qty, type, time1, time2 );
-		type &= ~0x10;
-
-		if( type & 0x01 )
-		{
-			qty-=1;
-			w->buttonmask = POP1;
-			type &= ~0x01;
-		}
-		if( type & 0x04 ) 
-		{
-			qty-=1;
-			w->axis1 = ( POP1 ) * 128; 
-			type &= ~0x04;
-		}
-		if( type & 0x02 )
-		{
-			qty-=4;
-			w->axis2 = POP2;
-			w->axis3 = POP2;
-			type &= ~0x02;
-		}
-
-		//XXX TODO: Is this correct?  It looks SO WACKY
-		type &= 0x7f;
-		if( type == 0x68 ) doimu = 1;
-		type &= 0x0f;
-		if( type == 0x00 && qty ) { type = POP1; qty--; }
-	}
-
-	if( type == 0xe1 )
-	{
-		propset |= 1;
-		w->charging = readdata[0]>>7;
-		w->charge = POP1&0x7f; qty--;
-		w->ison = 1; 
-		if( qty )
-		{
-			qty--;
-			type = POP1; //IMU usually follows.
-		}
-	}
-
-	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;
-		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;
-		type &= ~0xe8;
-		if( qty )
-		{
-			qty--;
-			type = POP1;
-		}
-	}
-
-
-	if( qty )
-	{
-		int j;
-		qty++;
-		readdata--;
-		*readdata = type; //Put 'type' back on stack.
-		uint8_t * mptr = readdata + qty-3-1; //-3 for timecode, -1 to 
-
-//#define DEBUG_WATCHMAN
-#ifdef DEBUG_WATCHMAN
-		printf( "_%s ", w->codename);
-		for( i = 0; i < qty; i++ )
-		{
-			printf( "%02x ", readdata[i] );
-		}
-		printf("\n");
-#endif
-
-
-		uint32_t mytime = (mptr[3] << 16)|(mptr[2] << 8)|(mptr[1] << 0);
-
-		uint32_t times[20];
-		const int nrtime = sizeof(times)/sizeof(uint32_t);
-		int timecount = 0;
-		int leds;
-		int parameters;
-		int fault = 0;
-
-		///Handle uint32_tifying (making sure we keep it incrementing)
-		uint32_t llt = w->last_lighttime;
-		uint32_t imumsb = time1<<24;
-		mytime |= imumsb;
-
-		//Compare mytime to llt
-
-		int diff = mytime - llt;
-		if( diff < -0x1000000 )
-			mytime += 0x1000000;
-		else if( diff > 0x100000 )
-			mytime -= 0x1000000;
-
-		w->last_lighttime = mytime;
-
-		times[timecount++] = mytime;
-#ifdef DEBUG_WATCHMAN
-		printf( "_%s Packet Start Time: %d\n", w->codename, mytime );
-#endif	
-
-		//First, pull off the times, starting with the current time, then all the delta times going backwards.
-		{
-			while( mptr - readdata > (timecount>>1) )
-			{
-				uint32_t arcane_value = 0;
-				//ArcanePop (Pop off values from the back, forward, checking if the MSB is set)
-				do
-				{
-					uint8_t ap = *(mptr--);
-					arcane_value |= (ap&0x7f);
-					if( ap & 0x80 )  break;
-					arcane_value <<= 7;
-				} while(1);
-				times[timecount++] = (mytime -= arcane_value);
-#ifdef DEBUG_WATCHMAN
-				printf( "_%s Time: %d  newtime: %d\n", w->codename, arcane_value, mytime );
-#endif
-			}
-
-			leds = timecount>>1;
-			//Check that the # of sensors at the beginning match the # of parameters we would expect.
-			if( timecount & 1 ) { fault = 1; goto end; }				//Inordinal LED count
-			if( leds != mptr - readdata + 1 ) { fault = 2; goto end; }	//LED Count does not line up with parameters
-		}
-
-
-		struct LightcapElement les[10];
-		int lese = 0; //les's end
-
-		//Second, go through all LEDs and extract the lightevent from them. 
-		{
-			uint8_t marked[nrtime];
-			memset( marked, 0, sizeof( marked ) );
-			int i, parpl = 0;
-			timecount--;
-			int timepl = 0;
-
-			//This works, but usually returns the values in reverse end-time order.
-			for( i = 0; i < leds; i++ )
-			{
-				int led = readdata[i];
-				int adv = led & 0x07;
-				led >>= 3;
-
-				while( marked[timepl] ) timepl++;
-				if( timepl > timecount ) { fault = 3; goto end; }         //Ran off max of list.
-				uint32_t endtime = times[timepl++];
-				int end = timepl + adv;
-				if( end > timecount ) { fault = 4; goto end; } //end referencing off list
-				if( marked[end] > 0 ) { fault = 5; goto end; } //Already marked trying to be used.
-				uint32_t starttime = times[end];
-				marked[end] = 1;
-
-				//Insert all lighting things into a sorted list.  This list will be
-				//reverse sorted, but that is to minimize operations.  To read it
-				//in sorted order simply read it back backwards.
-				//Use insertion sort, since we should most of the time, be in order.
-				struct LightcapElement * le = &les[lese++];
-				le->sensor_id = led;
-				le->type = 0xfe;
-
-				if( (uint32_t)(endtime - starttime) > 65535 ) { fault = 6; goto end; } //Length of pulse dumb.
-				le->length = endtime - starttime;
-				le->timestamp = starttime;
-
-#ifdef DEBUG_WATCHMAN
-				printf( "_%s Event: %d %d %d-%d\n", w->codename, led, le->length, endtime, starttime );
-#endif
-				int swap = lese-2;
-				while( swap >= 0 && les[swap].timestamp < les[swap+1].timestamp )
-				{
-					struct LightcapElement l;
-					memcpy( &l, &les[swap], sizeof( l ) );
-					memcpy( &les[swap], &les[swap+1], sizeof( l ) );
-					memcpy( &les[swap+1], &l, sizeof( l ) );
-					swap--;
-				}
-			}
-		}
-
-		int i;
-		for( i = lese-1; i >= 0; i-- )
-		{
-			//printf( "%d: %d [%d]\n", les[i].sensor_id, les[i].length, les[i].timestamp );
-			handle_lightcap( w, &les[i] );
-		}
-
-		return;
-	end:
-		{
-			struct SurviveContext * ctx = w->ctx;
-			SV_INFO( "Light decoding fault: %d\n", fault );
-		}
-	}
-}
-
-
-void survive_data_cb( struct SurviveUSBInterface * si )
-{
-	int size = si->actual_len;
-	struct SurviveContext * ctx = si->ctx;
-#if 0
-	int i;
-	printf( "%16s: %d: ", si->hname, len );
-	for( i = 0; i < size; i++ )
-	{
-		printf( "%02x ", si->buffer[i] );
-	}
-	printf( "\n" );
-	return;
-#endif 
-
-	int iface = si->which_interface_am_i;
-	uint8_t * readdata = si->buffer;
-
-	int id = POP1;
-//	printf( "%16s Size: %2d ID: %d / %d\n", si->hname, size, id, iface );
-
-
-	switch( si->which_interface_am_i )
-	{
-	case USB_IF_HMD:
-	{
-		struct SurviveObject * headset = &ctx->headset;
-		readdata+=2;
-		headset->buttonmask = POP1;		//Lens
-		headset->axis2 = POP2;			//Lens Separation
-		readdata+=2;
-		headset->buttonmask |= POP1;	//Button
-		readdata+=3;
-		readdata++;						//Proxchange, No change = 0, Decrease = 1, Increase = 2
-		readdata++;
-		headset->axis3 = POP2;			//Proximity  	<< how close to face are you?  Less than 80 = not on face.
-		headset->axis1 = POP2;			//IPD   		<< what is this?
-		headset->ison = 1;
-		break;
-	}
-	case USB_IF_LIGHTHOUSE:
-	{
-		int i;
-		//printf( "%d -> ", size );
-		for( i = 0; i < 3; i++ )
-		{
-			int16_t * acceldata = (int16_t*)readdata;
-			readdata += 12;
-			uint32_t timecode = POP4;
-			uint8_t code = POP1;
-			//printf( "%d ", code );
-			int8_t cd = code - ctx->headset.oldcode;
-
-			if( cd > 0 )
-			{
-				ctx->headset.oldcode = code;
-				ctx->imuproc( &ctx->headset, acceldata, timecode, code );
-			}
-		}
-
-		//DONE OK.
-		break;
-	}
-	case USB_IF_WATCHMAN1:
-	case USB_IF_WATCHMAN2:
-	{
-		struct SurviveObject * w = &ctx->watchman[si->which_interface_am_i-USB_IF_WATCHMAN1];
-		if( id == 35 )
-		{
-			handle_watchman( w, readdata);
-		}
-		else if( id == 36 )
-		{
-			handle_watchman( w, readdata);
-			handle_watchman( w, readdata+29 );
-		}
-		else if( id == 38 )
-		{
-			w->ison = 0;
-		}
-		else
-		{
-			SV_INFO( "Unknown watchman code %d\n", id );
-		}
-		break;
-	}
-	case USB_IF_LIGHTCAP:
-	{
-		//Done!
-		int i;
-		for( i = 0; i < 7; i++ )
-		{
-			handle_lightcap( &ctx->headset, (struct LightcapElement*)&readdata[i*8] );
-		}
-		break;
-	}
-	}
-}
-
-
diff --git a/src/survive_driverman.c b/src/survive_driverman.c
new file mode 100644
index 0000000..8cdfb71
--- /dev/null
+++ b/src/survive_driverman.c
@@ -0,0 +1,54 @@
+// (C) 2017 <>< C. N. Lohr, Under MIT/x11 License.
+//
+// See notice in survive_driverman.h
+//
+
+#include "survive_driverman.h"
+#include <string.h>
+#include <stdio.h>
+
+static void * Drivers[MAX_DRIVERS];
+static const char * DriverNames[MAX_DRIVERS];
+static int NrDrivers;
+
+void   RegisterDriver( const char * element, void * data )
+{
+	Drivers[NrDrivers] = data;
+	DriverNames[NrDrivers] = element;
+	NrDrivers++;
+}
+
+void * GetDriver( const char * element )
+{
+	int i;
+	for( i = 0; i < NrDrivers; i++ )
+	{
+		if( strcmp( element, DriverNames[i] ) == 0 ) return Drivers[i];
+	}
+	return 0;
+}
+
+const char * GetDriverNameMatching( const char * prefix, int place )
+{
+	int i;
+	int prefixlen = strlen( prefix );
+
+	for( i = 0; i < NrDrivers; i++ )
+	{
+		if( memcmp( prefix, DriverNames[i], prefixlen ) == 0 )
+			if( 0 == (place--) )
+				return DriverNames[i];
+	}
+	return 0;
+}
+
+void   ListDrivers()
+{
+	int i;
+	printf( "Drivers (%d/%d):\n", NrDrivers, MAX_DRIVERS );
+	for( i = 0; i < NrDrivers; i++ )
+	{
+		printf( " %s\n", DriverNames[i] );
+	}
+}
+
diff --git a/src/survive_driverman.h b/src/survive_driverman.h
new file mode 100644
index 0000000..5e13caf
--- /dev/null
+++ b/src/survive_driverman.h
@@ -0,0 +1,38 @@
+// (C) 2017 <>< C. N. Lohr, Under MIT/x11 License.
+//
+// This file is intended to be used for self-registering functions.  By using
+// this it means that you do not need to have complicated switch statements or
+// #defines for dfferent inclusion of drivers/other code.  You can simply
+// register your function and it will be put into a list.
+//
+//
+
+#ifndef SURVIVE_DRIVERMAN_H
+#define SURVIVE_DRIVERMAN_H
+
+//Driver registration
+#define MAX_DRIVERS 32
+
+void   RegisterDriver( const char * name, void * data );
+void * GetDriver( const char * name );
+const char * GetDriverNameMatching( const char * prefix, int place );
+void   ListDrivers();
+
+#define REGISTER_LINKTIME( func ) \
+	void __attribute__((constructor)) Register##func() { RegisterDriver( #func, &func ); }
+
+
+//
+// Specific types of drivers.
+//
+
+struct SurviveContext;
+
+//Device drivers (prefix your drivers with "DriverReg") i.e.
+//		REGISTER_LINKTIME( DriverRegHTCVive );
+typedef int (*DeviceDriver)( struct SurviveContext * ctx );
+
+//more driver types here? i.e. posefinders, etc.
+
+#endif
+
diff --git a/src/survive_internal.h b/src/survive_internal.h
index 0d0b8a4..5962623 100644
--- a/src/survive_internal.h
+++ b/src/survive_internal.h
@@ -17,7 +17,7 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <stdint.h>
-#include <libusb-1.0/libusb.h>
+#include "survive_driverman.h"
 #include <zlib.h>
 #include <survive.h>
 
@@ -27,38 +27,15 @@
 //XXX TODO This one needs to be rewritten.
 #define SV_KILL()		exit(0)
 
-#define USB_DEV_HMD			0
-#define USB_DEV_LIGHTHOUSE	1
-#define USB_DEV_WATCHMAN1	2
-#define USB_DEV_WATCHMAN2	3
-#define MAX_USB_DEVS				4
-
-
-#define USB_IF_HMD			0
-#define USB_IF_LIGHTHOUSE 	1
-#define USB_IF_WATCHMAN1	2
-#define USB_IF_WATCHMAN2	3
-#define USB_IF_LIGHTCAP		4
-#define MAX_INTERFACES				5
-
 #define INTBUFFSIZE			64
 #define SENSORS_PER_OBJECT	32
 
 struct SurviveContext;
 struct SurviveUSBInterface;
 
-typedef void (*usb_callback)( struct SurviveUSBInterface * ti );
+typedef int (*DeviceDriverCb)( struct SurviveContext * ctx, void * driver );
+typedef int (*DeviceDriverMagicCb)( struct SurviveContext * ctx, void * driver, int magic_code, void * data, int datalen );
 
-struct SurviveUSBInterface
-{
-	struct libusb_transfer * transfer;
-	struct SurviveContext * ctx;
-	int actual_len;
-	uint8_t buffer[INTBUFFSIZE];
-	usb_callback cb;
-	int which_interface_am_i;	//for indexing into uiface
-	const char * hname;			//human-readable names
-};
 
 //This is defined in survive.h
 struct SurviveObject;
@@ -67,49 +44,57 @@ struct SurviveCalData;
 struct BaseStationData
 {
 	uint8_t PositionSet:1;
-	float Position[3];
-	float Quaternion[4];
+	FLT Position[3];
+	FLT Quaternion[4];
 
 	uint8_t OOTXSet:1;
 	uint32_t BaseStationID;
-	float fcalphase[2];
-	float fcaltilt[2];
-	float fcalcurve[2];
-	float fcalgibpha[2];
-	float fcalgibmag[2];
+	FLT fcalphase[2];
+	FLT fcaltilt[2];
+	FLT fcalcurve[2];
+	FLT fcalgibpha[2];
+	FLT fcalgibmag[2];
 };
 
 struct SurviveContext
 {
-	//USB Subsystem
-    struct libusb_context* usbctx;
-	struct libusb_device_handle * udev[MAX_USB_DEVS];
-	struct SurviveUSBInterface uiface[MAX_INTERFACES];
-
 	text_feedback_func faultfunction;
 	text_feedback_func notefunction;
 	light_process_func lightproc;
 	imu_process_func imuproc;
 	angle_process_func angleproc;
 
-
 	//Calibration data:
 	struct BaseStationData bsd[NUM_LIGHTHOUSES];
 
 	struct SurviveCalData * calptr; //If and only if the calibration subsystem is attached.
 
-	//Data Subsystem.  These should be last, as there may be additional surviveobjects.
-	struct SurviveObject headset;
-	struct SurviveObject watchman[2]; //Currently only two supported watchmen.
+	struct SurviveObject ** objs;
+	int objs_ct;
 
+	void ** drivers;
+	DeviceDriverCb * driverpolls;
+	DeviceDriverCb * drivercloses;
+	DeviceDriverMagicCb * drivermagics;
+	int driver_ct;
 };
 
+int survive_add_object( struct SurviveContext * ctx, struct SurviveObject * obj );
+
+void survive_add_driver( struct SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic );
+
+//For lightcap, etc.  Don't change this structure at all.  Regular vive is dependent on it being exactly as-is.
+struct LightcapElement
+{
+	uint8_t sensor_id;
+	uint8_t type;
+	uint16_t length;
+	uint32_t timestamp;
+} __attribute__((packed));
+
+//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
+void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le );
 
-//USB Subsystem 
-void survive_usb_close( struct SurviveContext * t );
-int survive_usb_init( struct SurviveContext * t );
-int survive_usb_poll( struct SurviveContext * ctx );
-int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, int interface, int send_extra_magic );
 
 //Accept Data from backend.
 void survive_data_cb( struct SurviveUSBInterface * si );
diff --git a/src/survive_usb.c b/src/survive_usb.c
index aec76db..743d805 100644
--- a/src/survive_usb.c
+++ b/src/survive_usb.c
@@ -10,435 +10,4 @@
 //If there are portions of the code too similar to the original, I would like to know  so they can be re-written.
 //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
 
-
-#include "survive_internal.h"
-#include <libusb-1.0/libusb.h>
-#include <stdio.h>
-#include <unistd.h> //sleep if I ever use it.
-#include <errno.h>
-#include <string.h>
-
-const short vidpids[] = {
-	0x0bb4, 0x2c87, 0, //The main HTC HMD device
-	0x28de, 0x2000, 0, //Valve lighthouse
-	0x28de, 0x2101, 0, //Valve Watchman
-	0x28de, 0x2101, 1, //Valve Watchman
-}; //length MAX_USB_INTERFACES*2
-
-const char * devnames[] = {
-	"HMD",
-	"Lighthouse",
-	"Watchman 1",
-	"Watchman 2",
-}; //length MAX_USB_INTERFACES
-
-
-static void handle_transfer(struct libusb_transfer* transfer)
-{
-	struct SurviveUSBInterface * iface = transfer->user_data;
-	struct SurviveContext * ctx = iface->ctx;
-
-	if( transfer->status != LIBUSB_TRANSFER_COMPLETED )
-	{
-		SV_ERROR("Transfer problem %d with %s", transfer->status, iface->hname );
-		SV_KILL();
-		return;
-	}
-
-	iface->actual_len = transfer->actual_length;
-	iface->cb( iface );
-
-	if( libusb_submit_transfer(transfer) )
-	{
-		SV_ERROR( "Error resubmitting transfer for %s", iface->hname );
-		SV_KILL();
-	}
-}
-
-
-
-static int AttachInterface( struct SurviveContext * ctx, int which_interface_am_i, libusb_device_handle * devh, int endpoint, usb_callback cb, const char * hname )
-{
-	struct SurviveUSBInterface * iface = &ctx->uiface[which_interface_am_i];
-	iface->ctx = ctx;
-	iface->which_interface_am_i = which_interface_am_i;
-	iface->hname = hname;
-	struct libusb_transfer * tx = iface->transfer = libusb_alloc_transfer(0);
-	iface->cb = cb;
-	//printf( "%p %d %p %p\n", iface, which_interface_am_i, tx, devh );
-
-	if (!iface->transfer)
-	{
-		SV_ERROR( "Error: failed on libusb_alloc_transfer for %s", hname );
-		return 4;
-	}
-
-	libusb_fill_interrupt_transfer( tx, devh, endpoint, iface->buffer, INTBUFFSIZE, handle_transfer, iface, 0);
-
-	int rc = libusb_submit_transfer( tx );
-	if( rc )
-	{
-		SV_ERROR( "Error: Could not submit transfer for %s (Code %d)", hname, rc );
-		return 6;
-	}
-
-	return 0;
-}
-
-/*
-static void debug_cb( struct SurviveUSBInterface * si )
-{
-	int i;
-	int len = si->actual_len;
-	printf( "%16s: %d: ", si->hname, len );
-	for( i = 0; i < len; i++ )
-	{
-		printf( "%02x ", si->buffer[i] );
-	}
-	printf( "\n" );
-}*/
-
-//XXX TODO: Redo this subsystem for setting/updating feature reports.
-
-static inline int update_feature_report(libusb_device_handle* dev, uint16_t interface, uint8_t * data, int datalen ) {
-//	int xfer;
-//	int r = libusb_interrupt_transfer(dev, 0x01, data, datalen, &xfer, 1000);
-//	printf( "XFER: %d / R: %d\n", xfer, r );
-//	return xfer;
-	return libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT,
-		0x09, 0x300 | data[0], interface, data, datalen, 1000 );
-}
-
-
-static inline int getupdate_feature_report(libusb_device_handle* dev, uint16_t interface, uint8_t * data, int datalen ) {
-
-	int ret = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_IN,
-		0x01, 0x300 | data[0], interface, data, datalen, 1000 );
-	if( ret == -9 ) return -9;
-    if (ret < 0)
-        return -1;
-	return ret;
-}
-
-
-
-static inline int hid_get_feature_report_timeout(libusb_device_handle* device, uint16_t interface, unsigned char *buf, size_t len )
-{
-	int ret;
-	uint8_t i = 0;
-    for (i = 0; i < 100; i++)
-	{
-        ret = getupdate_feature_report(device, interface, buf, len);
-		if( ret != -9 && ( ret != -1 || errno != EPIPE ) ) return ret;
-		usleep( 1000 );
-	}
-
-	return -1;
-}
-
-
-int survive_usb_init( struct SurviveContext * ctx )
-{
-	int r = libusb_init( &ctx->usbctx );
-	if( r )
-	{
-		SV_ERROR( "libusb fault %d\n", r );
-		return r;
-	}
-
-	int i;
-	int16_t j;
-	libusb_device** devs;
-	int ret = libusb_get_device_list(ctx->usbctx, &devs);
-
-	if( ret < 0 )
-	{
-		SV_ERROR( "Couldn't get list of USB devices %d", ret );
-		return ret;
-	}
-
-
-	//Open all interfaces.
-	for( i = 0; i < MAX_USB_DEVS; i++ )
-	{
-		libusb_device * d;
-		int vid = vidpids[i*3+0];
-		int pid = vidpids[i*3+1];
-		int which = vidpids[i*3+2];
-
-		int did;
-		for( did = 0; d = devs[did]; did++ )
-		{
-			struct libusb_device_descriptor desc;
-
-			int ret = libusb_get_device_descriptor( d, &desc);
-			if (ret < 0) {
-				continue;
-			}
-
-			if( desc.idVendor == vid && desc.idProduct == pid)
-			{
-				if( which == 0 ) break;
-				which--;
-			}
-		}
-
-		if( d == 0 )
-		{
-			SV_ERROR( "Couldn't find device %s (%04x:%04x.%d)", devnames[i], vid, pid, which );
-			return -99;
-		}
-
-		struct libusb_config_descriptor *conf;
-		ret = libusb_get_config_descriptor(d, 0, &conf);
-		if( ret )
-			continue;
-
-		ret = libusb_open(d, &ctx->udev[i]);
-
-		if( !ctx->udev[i] || ret )
-		{
-			SV_ERROR( "Error: cannot open device \"%s\" with vid/pid %04x:%04x", devnames[i], vid, pid );
-			return -5;
-		}
-
-		libusb_set_auto_detach_kernel_driver( ctx->udev[i], 1 );
-		for (j = 0; j < conf->bNumInterfaces; j++ )
-		{
-#if 0
-		    if (libusb_kernel_driver_active(ctx->udev[i], j) == 1) {
-		        ret = libusb_detach_kernel_driver(ctx->udev[i], j);
-		        if (ret != LIBUSB_SUCCESS) {
-		            SV_ERROR("Failed to unclaim interface %d for device %s "
-		                    "from the kernel.", j, devnames[i] );
-		            libusb_free_config_descriptor(conf);
-		            libusb_close(ctx->udev[i]);
-		            continue;
-		        }
-		    }
-#endif
-
-			if( libusb_claim_interface(ctx->udev[i], j) )
-			{
-				SV_ERROR( "Could not claim interface %d of %s", j, devnames[i] );
-				return -9;
-			}
-		}
-
-		SV_INFO( "Successfully enumerated %s (%d, %d)", devnames[i], did, conf->bNumInterfaces );
-	}
-	libusb_free_device_list( devs, 1 );
-
-	if( AttachInterface( ctx, USB_IF_HMD,        ctx->udev[USB_DEV_HMD],        0x81, survive_data_cb, "Mainboard" ) ) { return -6; }
-	if( AttachInterface( ctx, USB_IF_LIGHTHOUSE, ctx->udev[USB_DEV_LIGHTHOUSE], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; }
-	if( AttachInterface( ctx, USB_IF_WATCHMAN1,  ctx->udev[USB_DEV_WATCHMAN1],  0x81, survive_data_cb, "Watchman 1" ) ) { return -8; }
-	if( AttachInterface( ctx, USB_IF_WATCHMAN2,  ctx->udev[USB_DEV_WATCHMAN2],  0x81, survive_data_cb, "Watchman 2" ) ) { return -9; }
-	if( AttachInterface( ctx, USB_IF_LIGHTCAP,   ctx->udev[USB_DEV_LIGHTHOUSE], 0x82, survive_data_cb, "Lightcap" ) ) { return -10; }
-
-	SV_INFO( "All devices attached." );
-
-
-	//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,
-			0x00, 0x6c, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0x0d, 0x76, 0x00, 0x68, 0xfc,
-			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,
-		};
-#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 ) );
-		if( r != sizeof( vive_magic_power_on ) ) return 5;
-
-		static uint8_t vive_magic_enable_lighthouse[64] = { 0x04 };  //[64] wat?  Why did that fix it?
-		r = update_feature_report( ctx->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
-		if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
-
-#if 0
-		for( i = 0; i < 256; i++ )
-		{
-			static uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0xff, 0, 0, 0, 0, 0, 0, 0 };
-			r = update_feature_report( ctx->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
-			SV_INFO( "UCR: %d", r );
-			if( r != sizeof( vive_controller_haptic_pulse ) ) return 5;
-			usleep( 1000 );
-		}
-#endif
-		SV_INFO( "Powered unit on." );
-	}
-	else
-	{
-
-		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;
-
-		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 )
-{
-	int i;
-	for( i = 0; i < MAX_USB_DEVS; i++ )
-	{
-	    libusb_close( t->udev[i] );
-	}
-    libusb_exit(t->usbctx);	
-}
-
-int survive_usb_poll( struct SurviveContext * ctx )
-{
-	int r = libusb_handle_events( ctx->usbctx );
-	if( r )
-	{
-		SV_ERROR( "Libusb poll failed." );
-	}
-	return r;
-}
-
-
-int survive_get_config( char ** config, struct SurviveContext * ctx, int devno, int iface, int send_extra_magic )
-{
-	int i, ret, count = 0, size = 0;
-	uint8_t cfgbuff[64];
-	uint8_t compressed_data[8192];
-	uint8_t uncompressed_data[65536];
-	struct libusb_device_handle * dev = ctx->udev[devno];
-
-	if( send_extra_magic )
-	{
-		uint8_t cfgbuffwide[65];
-
-		memset( cfgbuffwide, 0, sizeof( cfgbuff ) );
-		cfgbuffwide[0] = 0x01;
-		ret = hid_get_feature_report_timeout( dev, iface, cfgbuffwide, sizeof( cfgbuffwide ) );
-		usleep(1000);
-
-		int k;
-
-		//Switch mode to pull config?
-		for( k = 0; k < 10; k++ )
-		{
-			uint8_t cfgbuff_send[64] = { 
-				0xff, 0x83, 0x00, 0xb6, 0x5b, 0xb0, 0x78, 0x69,
-				0x0f, 0xf8, 0x78, 0x69, 0x0f, 0xa0, 0xf3, 0x18,
-				0x00, 0xe8,	0xf2, 0x18, 0x00, 0x27, 0x44, 0x5a,
-				0x0f, 0xf8, 0x78, 0x69, 0x0f, 0xf0, 0x77, 0x69,
-				0x0f, 0xf0, 0x77, 0x69, 0x0f, 0x50, 0xca, 0x45,
-				0x77, 0xa0, 0xf3, 0x18, 0x00, 0xf8, 0x78, 0x69,
-				0x0f, 0x00, 0x00, 0xa0, 0x0f, 0xa0, 0x9b, 0x0a,
-				0x01, 0x00, 0x00, 0x35, 0x00, 0x34, 0x02, 0x00
-			};
-
-			int rk = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT,
-			0x09, 0x300 | cfgbuff_send[0], iface, cfgbuff_send, 64, 1000 );
-			usleep(1000);
-		}
-
-		cfgbuffwide[0] = 0xff;
-		ret = hid_get_feature_report_timeout( dev, iface, cfgbuffwide, sizeof( cfgbuffwide ) );
-		usleep(1000);
-	}
-
-	memset( cfgbuff, 0, sizeof( cfgbuff ) );
-	cfgbuff[0] = 0x10;
-	if( ( ret = hid_get_feature_report_timeout( dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 )
-	{
-		SV_INFO( "Could not get survive config data for device %d:%d", devno, iface );
-		return -1;
-	}
-
-
-	cfgbuff[1] = 0xaa;
-	cfgbuff[0] = 0x11;
-	do
-	{
-		if( (ret = hid_get_feature_report_timeout(dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 )
-		{
-			SV_INFO( "Could not read config data (after first packet) on device %d:%d (count: %d)\n", devno, iface, count );
-			return -2;
-		}
-
-		size = cfgbuff[1];
-
-		if( !size ) break;
-
-		if( size > 62 )
-		{
-			SV_INFO( "Too much data (%d) on packet from config for device %d:%d (count: %d)", size, devno, iface, count );
-			return -3;
-		}
-
-		if( count + size >= sizeof( compressed_data ) )
-		{
-			SV_INFO( "Configuration length too long %d:%d (count: %d)", devno, iface, count );
-			return -4;
-		}
-
-        memcpy( &compressed_data[count], cfgbuff + 2, size );
-		count += size;
-	} while( 1 );
-
-	if( count == 0 )
-	{
-		SV_INFO( "Empty configuration for %d:%d", devno, iface );
-		return -5;
-	}
-
-	SV_INFO( "Got config data length %d", count );
-
-	int len = survive_simple_inflate( ctx, compressed_data, count, uncompressed_data, sizeof(uncompressed_data)-1 );
-	if( len <= 0 )
-	{
-		SV_INFO( "Error: data for config descriptor %d:%d is bad.", devno, iface );
-		return -5;
-	}
-
-	*config = malloc( len + 1 );
-	memcpy( *config, uncompressed_data, len );
-	return len;
-}
-
-
+//No code?
diff --git a/src/survive_vive.c b/src/survive_vive.c
new file mode 100644
index 0000000..47886c9
--- /dev/null
+++ b/src/survive_vive.c
@@ -0,0 +1,1097 @@
+//Unofficial driver for the official Valve/HTC Vive hardware.
+//
+//Based off of https://github.com/collabora/OSVR-Vive-Libre
+// Originally Copyright 2016 Philipp Zabel
+// Originally Copyright 2016 Lubosz Sarnecki <lubosz.sarnecki@collabora.co.uk>
+// Originally Copyright (C) 2013 Fredrik Hultin
+// Originally Copyright (C) 2013 Jakob Bornecrantz
+//
+//But, re-written as best as I can to get it put under an open souce license instead of a forced-source license.
+//If there are portions of the code too similar to the original, I would like to know  so they can be re-written.
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+
+#include "survive_internal.h"
+#include "survive_driverman.h"
+#include <jsmn.h>
+
+#include "survive_internal.h"
+#include <libusb-1.0/libusb.h>
+#include <stdio.h>
+#include <unistd.h> //sleep if I ever use it.
+#include <errno.h>
+#include <string.h>
+#include <sys/stat.h>
+
+struct SurviveViveData;
+
+//USB Subsystem 
+void survive_usb_close( struct SurviveContext * t );
+int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1 );
+int survive_usb_poll( struct SurviveContext * ctx );
+int survive_get_config( char ** config, struct SurviveViveData * ctx, int devno, int interface, int send_extra_magic );
+
+
+const short vidpids[] = {
+	0x0bb4, 0x2c87, 0, //The main HTC HMD device
+	0x28de, 0x2000, 0, //Valve lighthouse
+	0x28de, 0x2101, 0, //Valve Watchman
+	0x28de, 0x2101, 1, //Valve Watchman
+}; //length MAX_USB_INTERFACES*2
+
+const char * devnames[] = {
+	"HMD",
+	"Lighthouse",
+	"Watchman 1",
+	"Watchman 2",
+}; //length MAX_USB_INTERFACES
+
+
+#define USB_DEV_HMD			0
+#define USB_DEV_LIGHTHOUSE	1
+#define USB_DEV_WATCHMAN1	2
+#define USB_DEV_WATCHMAN2	3
+#define MAX_USB_DEVS				4
+
+
+#define USB_IF_HMD			0
+#define USB_IF_LIGHTHOUSE 	1
+#define USB_IF_WATCHMAN1	2
+#define USB_IF_WATCHMAN2	3
+#define USB_IF_LIGHTCAP		4
+#define MAX_INTERFACES				5
+
+typedef void (*usb_callback)( struct SurviveUSBInterface * ti );
+
+struct SurviveUSBInterface
+{
+	struct SurviveViveData * sv;
+	struct SurviveContext * ctx;
+
+	struct libusb_transfer * transfer;
+	struct SurviveObject * assoc_obj;
+	int actual_len;
+	uint8_t buffer[INTBUFFSIZE];
+	usb_callback cb;
+	int which_interface_am_i;	//for indexing into uiface
+	const char * hname;			//human-readable names
+};
+
+struct SurviveViveData
+{
+	struct SurviveContext * ctx;
+
+	//XXX TODO: UN-STATICIFY THIS.
+	struct SurviveUSBInterface uiface[MAX_INTERFACES];
+	//USB Subsystem
+	struct libusb_context* usbctx;
+	struct libusb_device_handle * udev[MAX_USB_DEVS];
+
+};
+
+
+
+
+static void handle_transfer(struct libusb_transfer* transfer)
+{
+	struct SurviveUSBInterface * iface = transfer->user_data;
+	struct SurviveContext * ctx = iface->ctx;
+
+	if( transfer->status != LIBUSB_TRANSFER_COMPLETED )
+	{
+		SV_ERROR("Transfer problem %d with %s", transfer->status, iface->hname );
+		SV_KILL();
+		return;
+	}
+
+	iface->actual_len = transfer->actual_length;
+	iface->cb( iface );
+
+	if( libusb_submit_transfer(transfer) )
+	{
+		SV_ERROR( "Error resubmitting transfer for %s", iface->hname );
+		SV_KILL();
+	}
+}
+
+
+
+static int AttachInterface( struct SurviveViveData * sv, struct SurviveObject * assocobj, int which_interface_am_i, libusb_device_handle * devh, int endpoint, usb_callback cb, const char * hname )
+{
+	struct SurviveContext * ctx = sv->ctx;
+	struct SurviveUSBInterface * iface = &sv->uiface[which_interface_am_i];
+	iface->ctx = ctx;
+	iface->sv = sv;
+	iface->which_interface_am_i = which_interface_am_i;
+	iface->assoc_obj = assocobj;
+	iface->hname = hname;
+	struct libusb_transfer * tx = iface->transfer = libusb_alloc_transfer(0);
+	iface->cb = cb;
+	//printf( "%p %d %p %p\n", iface, which_interface_am_i, tx, devh );
+
+	if (!iface->transfer)
+	{
+		SV_ERROR( "Error: failed on libusb_alloc_transfer for %s", hname );
+		return 4;
+	}
+
+	libusb_fill_interrupt_transfer( tx, devh, endpoint, iface->buffer, INTBUFFSIZE, handle_transfer, iface, 0);
+
+	int rc = libusb_submit_transfer( tx );
+	if( rc )
+	{
+		SV_ERROR( "Error: Could not submit transfer for %s (Code %d)", hname, rc );
+		return 6;
+	}
+
+	return 0;
+}
+
+/*
+static void debug_cb( struct SurviveUSBInterface * si )
+{
+	int i;
+	int len = si->actual_len;
+	printf( "%16s: %d: ", si->hname, len );
+	for( i = 0; i < len; i++ )
+	{
+		printf( "%02x ", si->buffer[i] );
+	}
+	printf( "\n" );
+}*/
+
+//XXX TODO: Redo this subsystem for setting/updating feature reports.
+
+static inline int update_feature_report(libusb_device_handle* dev, uint16_t interface, uint8_t * data, int datalen ) {
+//	int xfer;
+//	int r = libusb_interrupt_transfer(dev, 0x01, data, datalen, &xfer, 1000);
+//	printf( "XFER: %d / R: %d\n", xfer, r );
+//	return xfer;
+	return libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT,
+		0x09, 0x300 | data[0], interface, data, datalen, 1000 );
+}
+
+
+static inline int getupdate_feature_report(libusb_device_handle* dev, uint16_t interface, uint8_t * data, int datalen ) {
+
+	int ret = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_IN,
+		0x01, 0x300 | data[0], interface, data, datalen, 1000 );
+	if( ret == -9 ) return -9;
+    if (ret < 0)
+        return -1;
+	return ret;
+}
+
+
+
+static inline int hid_get_feature_report_timeout(libusb_device_handle* device, uint16_t interface, unsigned char *buf, size_t len )
+{
+	int ret;
+	uint8_t i = 0;
+    for (i = 0; i < 100; i++)
+	{
+        ret = getupdate_feature_report(device, interface, buf, len);
+		if( ret != -9 && ( ret != -1 || errno != EPIPE ) ) return ret;
+		usleep( 1000 );
+	}
+
+	return -1;
+}
+
+
+int survive_usb_init( struct SurviveViveData * sv, struct SurviveObject * hmd, struct SurviveObject *wm0, struct SurviveObject * wm1 )
+{
+	struct SurviveContext * ctx = sv->ctx;
+	int r = libusb_init( &sv->usbctx );
+	if( r )
+	{
+		SV_ERROR( "libusb fault %d\n", r );
+		return r;
+	}
+
+	int i;
+	int16_t j;
+	libusb_device** devs;
+	int ret = libusb_get_device_list(sv->usbctx, &devs);
+
+	if( ret < 0 )
+	{
+		SV_ERROR( "Couldn't get list of USB devices %d", ret );
+		return ret;
+	}
+
+
+	//Open all interfaces.
+	for( i = 0; i < MAX_USB_DEVS; i++ )
+	{
+		libusb_device * d;
+		int vid = vidpids[i*3+0];
+		int pid = vidpids[i*3+1];
+		int which = vidpids[i*3+2];
+
+		int did;
+		for( did = 0; d = devs[did]; did++ )
+		{
+			struct libusb_device_descriptor desc;
+
+			int ret = libusb_get_device_descriptor( d, &desc);
+			if (ret < 0) {
+				continue;
+			}
+
+			if( desc.idVendor == vid && desc.idProduct == pid)
+			{
+				if( which == 0 ) break;
+				which--;
+			}
+		}
+
+		if( d == 0 )
+		{
+			SV_ERROR( "Couldn't find device %s (%04x:%04x.%d)", devnames[i], vid, pid, which );
+			return -99;
+		}
+
+		struct libusb_config_descriptor *conf;
+		ret = libusb_get_config_descriptor(d, 0, &conf);
+		if( ret )
+			continue;
+
+		ret = libusb_open(d, &sv->udev[i]);
+
+		if( !sv->udev[i] || ret )
+		{
+			SV_ERROR( "Error: cannot open device \"%s\" with vid/pid %04x:%04x", devnames[i], vid, pid );
+			return -5;
+		}
+
+		libusb_set_auto_detach_kernel_driver( sv->udev[i], 1 );
+		for (j = 0; j < conf->bNumInterfaces; j++ )
+		{
+#if 0
+		    if (libusb_kernel_driver_active(sv->udev[i], j) == 1) {
+		        ret = libusb_detach_kernel_driver(sv->udev[i], j);
+		        if (ret != LIBUSB_SUCCESS) {
+		            SV_ERROR("Failed to unclaim interface %d for device %s "
+		                    "from the kernel.", j, devnames[i] );
+		            libusb_free_config_descriptor(conf);
+		            libusb_close(sv->udev[i]);
+		            continue;
+		        }
+		    }
+#endif
+
+			if( libusb_claim_interface(sv->udev[i], j) )
+			{
+				SV_ERROR( "Could not claim interface %d of %s", j, devnames[i] );
+				return -9;
+			}
+		}
+
+		SV_INFO( "Successfully enumerated %s (%d, %d)", devnames[i], did, conf->bNumInterfaces );
+	}
+	libusb_free_device_list( devs, 1 );
+
+	if( AttachInterface( sv, hmd, USB_IF_HMD,        sv->udev[USB_DEV_HMD],        0x81, survive_data_cb, "Mainboard" ) ) { return -6; }
+	if( AttachInterface( sv, hmd, USB_IF_LIGHTHOUSE, sv->udev[USB_DEV_LIGHTHOUSE], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; }
+	if( AttachInterface( sv, wm0, USB_IF_WATCHMAN1,  sv->udev[USB_DEV_WATCHMAN1],  0x81, survive_data_cb, "Watchman 1" ) ) { return -8; }
+	if( AttachInterface( sv, wm1, USB_IF_WATCHMAN2,  sv->udev[USB_DEV_WATCHMAN2],  0x81, survive_data_cb, "Watchman 2" ) ) { return -9; }
+	if( AttachInterface( sv, hmd, USB_IF_LIGHTCAP,   sv->udev[USB_DEV_LIGHTHOUSE], 0x82, survive_data_cb, "Lightcap" ) ) { return -10; }
+
+	SV_INFO( "All devices attached." );
+
+
+	//libUSB initialized.  Continue.
+	return 0;
+}
+
+int survive_vive_send_magic(struct SurviveContext * ctx, void * drv, int magic_code, void * data, int datalen )
+{
+	int r;
+	struct SurviveViveData * sv = drv;
+	printf( "*CALLING %p %p\n", ctx, sv );
+
+	//XXX TODO: Handle haptics, etc.
+	int turnon = magic_code;
+
+
+	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,
+			0x00, 0x6c, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0x0d, 0x76, 0x00, 0x68, 0xfc,
+			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,
+		};
+#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( sv->udev[USB_DEV_HMD], 0, vive_magic_power_on, sizeof( vive_magic_power_on ) );
+		if( r != sizeof( vive_magic_power_on ) ) return 5;
+
+		static uint8_t vive_magic_enable_lighthouse[64] = { 0x04 };  //[64] wat?  Why did that fix it?
+		r = update_feature_report( sv->udev[USB_DEV_LIGHTHOUSE], 0, vive_magic_enable_lighthouse, sizeof( vive_magic_enable_lighthouse ) );
+		if( r != sizeof( vive_magic_enable_lighthouse ) ) return 5;
+
+#if 0
+		for( i = 0; i < 256; i++ )
+		{
+			static uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0xff, 0, 0, 0, 0, 0, 0, 0 };
+			r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
+			SV_INFO( "UCR: %d", r );
+			if( r != sizeof( vive_controller_haptic_pulse ) ) return 5;
+			usleep( 1000 );
+		}
+#endif
+		SV_INFO( "Powered unit on." );
+	}
+	else
+	{
+
+		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( sv->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;
+
+		r = update_feature_report( sv->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_vive_usb_close( struct SurviveViveData * sv )
+{
+	int i;
+	for( i = 0; i < MAX_USB_DEVS; i++ )
+	{
+	    libusb_close( sv->udev[i] );
+	}
+    libusb_exit(sv->usbctx);	
+}
+
+int survive_vive_usb_poll( struct SurviveContext * ctx, void * v )
+{
+	struct SurviveViveData * sv = v;
+	int r = libusb_handle_events( sv->usbctx );
+	if( r )
+	{
+		struct SurviveContext * ctx = sv->ctx;
+		SV_ERROR( "Libusb poll failed." );
+	}
+	return r;
+}
+
+
+int survive_get_config( char ** config, struct SurviveViveData * sv, int devno, int iface, int send_extra_magic )
+{
+	struct SurviveContext * ctx = sv->ctx;
+	int i, ret, count = 0, size = 0;
+	uint8_t cfgbuff[64];
+	uint8_t compressed_data[8192];
+	uint8_t uncompressed_data[65536];
+	struct libusb_device_handle * dev = sv->udev[devno];
+
+	if( send_extra_magic )
+	{
+		uint8_t cfgbuffwide[65];
+
+		memset( cfgbuffwide, 0, sizeof( cfgbuff ) );
+		cfgbuffwide[0] = 0x01;
+		ret = hid_get_feature_report_timeout( dev, iface, cfgbuffwide, sizeof( cfgbuffwide ) );
+		usleep(1000);
+
+		int k;
+
+		//Switch mode to pull config?
+		for( k = 0; k < 10; k++ )
+		{
+			uint8_t cfgbuff_send[64] = { 
+				0xff, 0x83, 0x00, 0xb6, 0x5b, 0xb0, 0x78, 0x69,
+				0x0f, 0xf8, 0x78, 0x69, 0x0f, 0xa0, 0xf3, 0x18,
+				0x00, 0xe8,	0xf2, 0x18, 0x00, 0x27, 0x44, 0x5a,
+				0x0f, 0xf8, 0x78, 0x69, 0x0f, 0xf0, 0x77, 0x69,
+				0x0f, 0xf0, 0x77, 0x69, 0x0f, 0x50, 0xca, 0x45,
+				0x77, 0xa0, 0xf3, 0x18, 0x00, 0xf8, 0x78, 0x69,
+				0x0f, 0x00, 0x00, 0xa0, 0x0f, 0xa0, 0x9b, 0x0a,
+				0x01, 0x00, 0x00, 0x35, 0x00, 0x34, 0x02, 0x00
+			};
+
+			int rk = libusb_control_transfer(dev, LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE | LIBUSB_ENDPOINT_OUT,
+			0x09, 0x300 | cfgbuff_send[0], iface, cfgbuff_send, 64, 1000 );
+			usleep(1000);
+		}
+
+		cfgbuffwide[0] = 0xff;
+		ret = hid_get_feature_report_timeout( dev, iface, cfgbuffwide, sizeof( cfgbuffwide ) );
+		usleep(1000);
+	}
+
+	memset( cfgbuff, 0, sizeof( cfgbuff ) );
+	cfgbuff[0] = 0x10;
+	if( ( ret = hid_get_feature_report_timeout( dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 )
+	{
+		SV_INFO( "Could not get survive config data for device %d:%d", devno, iface );
+		return -1;
+	}
+
+
+	cfgbuff[1] = 0xaa;
+	cfgbuff[0] = 0x11;
+	do
+	{
+		if( (ret = hid_get_feature_report_timeout(dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 )
+		{
+			SV_INFO( "Could not read config data (after first packet) on device %d:%d (count: %d)\n", devno, iface, count );
+			return -2;
+		}
+
+		size = cfgbuff[1];
+
+		if( !size ) break;
+
+		if( size > 62 )
+		{
+			SV_INFO( "Too much data (%d) on packet from config for device %d:%d (count: %d)", size, devno, iface, count );
+			return -3;
+		}
+
+		if( count + size >= sizeof( compressed_data ) )
+		{
+			SV_INFO( "Configuration length too long %d:%d (count: %d)", devno, iface, count );
+			return -4;
+		}
+
+        memcpy( &compressed_data[count], cfgbuff + 2, size );
+		count += size;
+	} while( 1 );
+
+	if( count == 0 )
+	{
+		SV_INFO( "Empty configuration for %d:%d", devno, iface );
+		return -5;
+	}
+
+	SV_INFO( "Got config data length %d", count );
+
+	int len = survive_simple_inflate( ctx, compressed_data, count, uncompressed_data, sizeof(uncompressed_data)-1 );
+	if( len <= 0 )
+	{
+		SV_INFO( "Error: data for config descriptor %d:%d is bad.", devno, iface );
+		return -5;
+	}
+
+	*config = malloc( len + 1 );
+	memcpy( *config, uncompressed_data, len );
+	return len;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+
+#define POP1  (*(readdata++))
+#define POP2  (*(((uint16_t*)((readdata+=2)-2))))
+#define POP4  (*(((uint32_t*)((readdata+=4)-4))))
+
+static void handle_watchman( struct SurviveObject * w, uint8_t * readdata )
+{
+	int i;
+
+	uint8_t startread[29];
+	memcpy( startread, readdata, 29 );
+
+#if 0
+	printf( "DAT:     " );
+		for( i = 0; i < 29; i++ )
+		{
+			printf( "%02x ", readdata[i] );
+		}
+		printf("\n");
+#endif
+
+	uint8_t time1 = POP1;
+	uint8_t qty = POP1;
+	uint8_t time2 = POP1;
+	uint8_t type = POP1;
+	qty-=2;
+	int propset = 0;
+	int doimu = 0;
+
+
+	if( (type & 0xf0) == 0xf0 )
+	{
+		propset |= 4;
+		//printf( "%02x %02x %02x %02x\n", qty, type, time1, time2 );
+		type &= ~0x10;
+
+		if( type & 0x01 )
+		{
+			qty-=1;
+			w->buttonmask = POP1;
+			type &= ~0x01;
+		}
+		if( type & 0x04 ) 
+		{
+			qty-=1;
+			w->axis1 = ( POP1 ) * 128; 
+			type &= ~0x04;
+		}
+		if( type & 0x02 )
+		{
+			qty-=4;
+			w->axis2 = POP2;
+			w->axis3 = POP2;
+			type &= ~0x02;
+		}
+
+		//XXX TODO: Is this correct?  It looks SO WACKY
+		type &= 0x7f;
+		if( type == 0x68 ) doimu = 1;
+		type &= 0x0f;
+		if( type == 0x00 && qty ) { type = POP1; qty--; }
+	}
+
+	if( type == 0xe1 )
+	{
+		propset |= 1;
+		w->charging = readdata[0]>>7;
+		w->charge = POP1&0x7f; qty--;
+		w->ison = 1; 
+		if( qty )
+		{
+			qty--;
+			type = POP1; //IMU usually follows.
+		}
+	}
+
+	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;
+		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;
+		type &= ~0xe8;
+		if( qty )
+		{
+			qty--;
+			type = POP1;
+		}
+	}
+
+
+	if( qty )
+	{
+		int j;
+		qty++;
+		readdata--;
+		*readdata = type; //Put 'type' back on stack.
+		uint8_t * mptr = readdata + qty-3-1; //-3 for timecode, -1 to 
+
+//#define DEBUG_WATCHMAN
+#ifdef DEBUG_WATCHMAN
+		printf( "_%s ", w->codename);
+		for( i = 0; i < qty; i++ )
+		{
+			printf( "%02x ", readdata[i] );
+		}
+		printf("\n");
+#endif
+
+
+		uint32_t mytime = (mptr[3] << 16)|(mptr[2] << 8)|(mptr[1] << 0);
+
+		uint32_t times[20];
+		const int nrtime = sizeof(times)/sizeof(uint32_t);
+		int timecount = 0;
+		int leds;
+		int parameters;
+		int fault = 0;
+
+		///Handle uint32_tifying (making sure we keep it incrementing)
+		uint32_t llt = w->last_lighttime;
+		uint32_t imumsb = time1<<24;
+		mytime |= imumsb;
+
+		//Compare mytime to llt
+
+		int diff = mytime - llt;
+		if( diff < -0x1000000 )
+			mytime += 0x1000000;
+		else if( diff > 0x100000 )
+			mytime -= 0x1000000;
+
+		w->last_lighttime = mytime;
+
+		times[timecount++] = mytime;
+#ifdef DEBUG_WATCHMAN
+		printf( "_%s Packet Start Time: %d\n", w->codename, mytime );
+#endif	
+
+		//First, pull off the times, starting with the current time, then all the delta times going backwards.
+		{
+			while( mptr - readdata > (timecount>>1) )
+			{
+				uint32_t arcane_value = 0;
+				//ArcanePop (Pop off values from the back, forward, checking if the MSB is set)
+				do
+				{
+					uint8_t ap = *(mptr--);
+					arcane_value |= (ap&0x7f);
+					if( ap & 0x80 )  break;
+					arcane_value <<= 7;
+				} while(1);
+				times[timecount++] = (mytime -= arcane_value);
+#ifdef DEBUG_WATCHMAN
+				printf( "_%s Time: %d  newtime: %d\n", w->codename, arcane_value, mytime );
+#endif
+			}
+
+			leds = timecount>>1;
+			//Check that the # of sensors at the beginning match the # of parameters we would expect.
+			if( timecount & 1 ) { fault = 1; goto end; }				//Inordinal LED count
+			if( leds != mptr - readdata + 1 ) { fault = 2; goto end; }	//LED Count does not line up with parameters
+		}
+
+
+		struct LightcapElement les[10];
+		int lese = 0; //les's end
+
+		//Second, go through all LEDs and extract the lightevent from them. 
+		{
+			uint8_t marked[nrtime];
+			memset( marked, 0, sizeof( marked ) );
+			int i, parpl = 0;
+			timecount--;
+			int timepl = 0;
+
+			//This works, but usually returns the values in reverse end-time order.
+			for( i = 0; i < leds; i++ )
+			{
+				int led = readdata[i];
+				int adv = led & 0x07;
+				led >>= 3;
+
+				while( marked[timepl] ) timepl++;
+				if( timepl > timecount ) { fault = 3; goto end; }         //Ran off max of list.
+				uint32_t endtime = times[timepl++];
+				int end = timepl + adv;
+				if( end > timecount ) { fault = 4; goto end; } //end referencing off list
+				if( marked[end] > 0 ) { fault = 5; goto end; } //Already marked trying to be used.
+				uint32_t starttime = times[end];
+				marked[end] = 1;
+
+				//Insert all lighting things into a sorted list.  This list will be
+				//reverse sorted, but that is to minimize operations.  To read it
+				//in sorted order simply read it back backwards.
+				//Use insertion sort, since we should most of the time, be in order.
+				struct LightcapElement * le = &les[lese++];
+				le->sensor_id = led;
+				le->type = 0xfe;
+
+				if( (uint32_t)(endtime - starttime) > 65535 ) { fault = 6; goto end; } //Length of pulse dumb.
+				le->length = endtime - starttime;
+				le->timestamp = starttime;
+
+#ifdef DEBUG_WATCHMAN
+				printf( "_%s Event: %d %d %d-%d\n", w->codename, led, le->length, endtime, starttime );
+#endif
+				int swap = lese-2;
+				while( swap >= 0 && les[swap].timestamp < les[swap+1].timestamp )
+				{
+					struct LightcapElement l;
+					memcpy( &l, &les[swap], sizeof( l ) );
+					memcpy( &les[swap], &les[swap+1], sizeof( l ) );
+					memcpy( &les[swap+1], &l, sizeof( l ) );
+					swap--;
+				}
+			}
+		}
+
+		int i;
+		for( i = lese-1; i >= 0; i-- )
+		{
+			//printf( "%d: %d [%d]\n", les[i].sensor_id, les[i].length, les[i].timestamp );
+			handle_lightcap( w, &les[i] );
+		}
+
+		return;
+	end:
+		{
+			struct SurviveContext * ctx = w->ctx;
+			SV_INFO( "Light decoding fault: %d\n", fault );
+		}
+	}
+}
+
+
+void survive_data_cb( struct SurviveUSBInterface * si )
+{
+	int size = si->actual_len;
+	struct SurviveContext * ctx = si->ctx;
+#if 0
+	int i;
+	printf( "%16s: %d: ", si->hname, len );
+	for( i = 0; i < size; i++ )
+	{
+		printf( "%02x ", si->buffer[i] );
+	}
+	printf( "\n" );
+	return;
+#endif 
+
+	int iface = si->which_interface_am_i;
+	struct SurviveObject * obj = si->assoc_obj;
+	uint8_t * readdata = si->buffer;
+
+	int id = POP1;
+//	printf( "%16s Size: %2d ID: %d / %d\n", si->hname, size, id, iface );
+
+
+	switch( si->which_interface_am_i )
+	{
+	case USB_IF_HMD:
+	{
+		struct SurviveObject * headset = obj;
+		readdata+=2;
+		headset->buttonmask = POP1;		//Lens
+		headset->axis2 = POP2;			//Lens Separation
+		readdata+=2;
+		headset->buttonmask |= POP1;	//Button
+		readdata+=3;
+		readdata++;						//Proxchange, No change = 0, Decrease = 1, Increase = 2
+		readdata++;
+		headset->axis3 = POP2;			//Proximity  	<< how close to face are you?  Less than 80 = not on face.
+		headset->axis1 = POP2;			//IPD   		<< what is this?
+		headset->ison = 1;
+		break;
+	}
+	case USB_IF_LIGHTHOUSE:
+	{
+		int i;
+		//printf( "%d -> ", size );
+		for( i = 0; i < 3; i++ )
+		{
+			int16_t * acceldata = (int16_t*)readdata;
+			readdata += 12;
+			uint32_t timecode = POP4;
+			uint8_t code = POP1;
+			//printf( "%d ", code );
+			int8_t cd = code - obj->oldcode;
+
+			if( cd > 0 )
+			{
+				obj->oldcode = code;
+				ctx->imuproc( obj, acceldata, timecode, code );
+			}
+		}
+
+		//DONE OK.
+		break;
+	}
+	case USB_IF_WATCHMAN1:
+	case USB_IF_WATCHMAN2:
+	{
+		struct SurviveObject * w = obj;
+		if( id == 35 )
+		{
+			handle_watchman( w, readdata);
+		}
+		else if( id == 36 )
+		{
+			handle_watchman( w, readdata);
+			handle_watchman( w, readdata+29 );
+		}
+		else if( id == 38 )
+		{
+			w->ison = 0;
+		}
+		else
+		{
+			SV_INFO( "Unknown watchman code %d\n", id );
+		}
+		break;
+	}
+	case USB_IF_LIGHTCAP:
+	{
+		//Done!
+		int i;
+		for( i = 0; i < 7; i++ )
+		{
+			handle_lightcap( obj, (struct LightcapElement*)&readdata[i*8] );
+		}
+		break;
+	}
+	}
+}
+
+
+
+
+
+
+
+
+
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+
+
+
+static int jsoneq(const char *json, jsmntok_t *tok, const char *s) {
+ if (tok->type == JSMN_STRING && (int) strlen(s) == tok->end - tok->start &&
+    strncmp(json + tok->start, s, tok->end - tok->start) == 0) {
+		return 0;
+	}
+	return -1;
+}
+
+
+static int ParsePoints( struct SurviveContext * ctx, struct SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i )
+{
+	int k;
+	int pts = t[i+1].size;
+	jsmntok_t * tk;
+
+	so->nr_locations = 0;
+	*floats_out = malloc( sizeof( **floats_out ) * 32 * 3 );
+
+	for( k = 0; k < pts; k++ )
+	{
+		tk = &t[i+2+k*4];
+
+		FLT vals[3];
+		int m;
+		for( m = 0; m < 3; m++ )
+		{
+			char ctt[128];
+
+			tk++;
+			int elemlen = tk->end - tk->start;
+
+			if( tk->type != 4 || elemlen > sizeof( ctt )-1 )
+			{
+				SV_ERROR( "Parse error in JSON\n" );
+				return 1;
+			}
+
+			memcpy( ctt, ct0conf + tk->start, elemlen );
+			ctt[elemlen] = 0;
+			FLT f = atof( ctt );
+			int id = so->nr_locations*3+m;
+			(*floats_out)[id] = f;
+		}
+		so->nr_locations++;
+	}
+	return 0;
+}
+
+static int LoadConfig( struct SurviveViveData * sv, struct SurviveObject * so, int devno, int iface, int extra_magic )
+{
+	struct SurviveContext * ctx = sv->ctx;
+	char * ct0conf = 0;
+	int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic );
+
+#if 0
+	char fname[100];
+	sprintf( fname, "%s_config.json", so->codename );
+	FILE * f = fopen( fname, "w" );
+	fwrite( ct0conf, strlen(ct0conf), 1, f );
+	fclose( f );
+#endif
+
+	if( len > 0 )
+	{
+
+		//From JSMN example.
+		jsmn_parser p;
+		jsmntok_t t[4096];
+		jsmn_init(&p);
+		int i;
+		int r = jsmn_parse(&p, ct0conf, len, t, sizeof(t)/sizeof(t[0]));	
+		if (r < 0) {
+			SV_INFO("Failed to parse JSON in HMD configuration: %d\n", r);
+			return -1;
+		}
+		if (r < 1 || t[0].type != JSMN_OBJECT) {
+			SV_INFO("Object expected in HMD configuration\n");
+			return -2;
+		}
+
+		for (i = 1; i < r; i++) {
+			jsmntok_t * tk = &t[i];
+
+			char ctxo[100];
+			int ilen = tk->end - tk->start;
+			if( ilen > 99 ) ilen = 99;
+			memcpy(ctxo, ct0conf + tk->start, ilen);
+			ctxo[ilen] = 0;
+
+//				printf( "%d / %d / %d / %d %s %d\n", tk->type, tk->start, tk->end, tk->size, ctxo, jsoneq(ct0conf, &t[i], "modelPoints") );
+//				printf( "%.*s\n", ilen, ct0conf + tk->start );
+
+			if (jsoneq(ct0conf, tk, "modelPoints") == 0) {
+				if( ParsePoints( ctx, so, ct0conf, &so->sensor_locations, t, i  ) )
+				{
+					break;
+				}
+			}
+			if (jsoneq(ct0conf, tk, "modelNormals") == 0) {
+				if( ParsePoints( ctx, so, ct0conf, &so->sensor_normals, t, i  ) )
+				{
+					break;
+				}
+			}
+		}
+	}
+	else
+	{
+		//TODO: Cleanup any remaining USB stuff.
+		return 1;
+	}
+
+	char fname[20];
+	mkdir( "calinfo", 0755 );
+
+	sprintf( fname, "calinfo/%s_points.csv", so->codename );
+	FILE * f = fopen( fname, "w" );
+	int j;
+	for( j = 0; j < so->nr_locations; j++ )
+	{
+		fprintf( f, "%f %f %f\n", so->sensor_locations[j*3+0], so->sensor_locations[j*3+1], so->sensor_locations[j*3+2] );
+	}
+	fclose( f );
+
+	sprintf( fname, "calinfo/%s_normals.csv", so->codename );
+	f = fopen( fname, "w" );
+	for( j = 0; j < so->nr_locations; j++ )
+	{
+		fprintf( f, "%f %f %f\n", so->sensor_normals[j*3+0], so->sensor_normals[j*3+1], so->sensor_normals[j*3+2] );
+	}
+	fclose( f );
+
+	return 0;
+}
+
+
+int survive_vive_close( struct SurviveContext * ctx, void * driver )
+{
+	struct SurviveViveData * sv = driver;
+ 
+	survive_vive_usb_close( sv );
+}
+
+
+int DriverRegHTCVive( struct SurviveContext * ctx )
+{
+	int i, r;
+	struct SurviveObject * hmd = calloc( 1, sizeof( struct SurviveObject ) );
+	struct SurviveObject * wm0 = calloc( 1, sizeof( struct SurviveObject ) );
+	struct SurviveObject * wm1 = calloc( 1, sizeof( struct SurviveObject ) );
+	struct SurviveViveData * sv = calloc( 1, sizeof( struct SurviveViveData ) );
+
+	sv->ctx = ctx;
+
+	hmd->ctx = ctx;
+	memcpy( hmd->codename, "HMD", 4 );
+	memcpy( hmd->drivername, "HTC", 4 );
+	wm0->ctx = ctx;
+	memcpy( wm0->codename, "WM0", 4 );
+	memcpy( wm0->drivername, "HTC", 4 );
+	wm1->ctx = ctx;
+	memcpy( wm1->codename, "WM1", 4 );
+	memcpy( wm1->drivername, "HTC", 4 );
+
+	//USB must happen last.
+	if( r = survive_usb_init( sv, hmd, wm0, wm1 ) )
+	{
+		//TODO: Cleanup any libUSB stuff sitting around.
+		goto fail_gracefully;
+	}
+
+	//Next, pull out the config stuff.
+	if( LoadConfig( sv, hmd, 1, 0, 0 ) ) goto fail_gracefully;
+	if( LoadConfig( sv, wm0, 2, 0, 1 ) ) { SV_INFO( "Watchman 0 config issue." ); }
+	if( LoadConfig( sv, wm1, 3, 0, 1 ) ) { SV_INFO( "Watchman 1 config issue." ); }
+
+	hmd->timebase_hz = wm0->timebase_hz = wm1->timebase_hz = 48000000;
+	hmd->pulsedist_max_ticks = wm0->pulsedist_max_ticks = wm1->pulsedist_max_ticks = 500000;
+	hmd->pulselength_min_sync = wm0->pulselength_min_sync = wm1->pulselength_min_sync = 2200;
+	hmd->pulse_in_clear_time = wm0->pulse_in_clear_time = wm1->pulse_in_clear_time = 35000;
+	hmd->pulse_max_for_sweep = wm0->pulse_max_for_sweep = wm1->pulse_max_for_sweep = 1800;
+
+	hmd->pulse_synctime_offset = wm0->pulse_synctime_offset = wm1->pulse_synctime_offset = 20000;
+	hmd->pulse_synctime_slack = wm0->pulse_synctime_slack = wm1->pulse_synctime_slack = 5000;
+
+	hmd->timecenter_ticks = hmd->timebase_hz / 240;
+	wm0->timecenter_ticks = wm0->timebase_hz / 240;
+	wm1->timecenter_ticks = wm1->timebase_hz / 240;
+/*
+	int i;
+	int locs = hmd->nr_locations;
+	printf( "Locs: %d\n", locs );
+	if (hmd->sensor_locations )
+	{
+		printf( "POSITIONS:\n" );
+		for( i = 0; i < locs*3; i+=3 )
+		{
+			printf( "%f %f %f\n", hmd->sensor_locations[i+0], hmd->sensor_locations[i+1], hmd->sensor_locations[i+2] );
+		}
+	}
+	if( hmd->sensor_normals )
+	{
+		printf( "NORMALS:\n" );
+		for( i = 0; i < locs*3; i+=3 )
+		{
+			printf( "%f %f %f\n", hmd->sensor_normals[i+0], hmd->sensor_normals[i+1], hmd->sensor_normals[i+2] );
+		}
+	}
+*/
+
+	//Add the drivers.
+
+	survive_add_object( ctx, hmd );
+	survive_add_object( ctx, wm0 );
+	survive_add_object( ctx, wm1 );
+	survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic );
+
+	return 0;
+fail_gracefully:
+	free( hmd );
+	free( wm0 );
+	free( wm1 );
+	survive_vive_usb_close( sv );
+	free( sv );
+	return -1;
+}
+
+REGISTER_LINKTIME( DriverRegHTCVive );
+
diff --git a/test.c b/test.c
index 17fa7f3..9dc3631 100644
--- a/test.c
+++ b/test.c
@@ -42,7 +42,7 @@ int main()
 	int magicon = 0;
 	double Start = OGGetAbsoluteTime();
 
-	ctx = survive_init( );
+	ctx = survive_init( 0 );
 
 	if( !ctx )
 	{
@@ -59,7 +59,7 @@ int main()
 		double Now = OGGetAbsoluteTime();
 		if( Now > (Start+1) && !magicon )
 		{
-			survive_usb_send_magic(ctx,1);
+			survive_send_magic(ctx,1,0,0);
 			magicon = 1;
 		}
 		//Do stuff.
diff --git a/tools/data_server/data_server.c b/tools/data_server/data_server.c
index 29f5ead..5147d97 100644
--- a/tools/data_server/data_server.c
+++ b/tools/data_server/data_server.c
@@ -19,6 +19,7 @@
 #define MAX_CONNS 32
 
 int SocketList[MAX_CONNS];
+int droppedct[MAX_CONNS];
 
 void error(char *msg) {
 	perror(msg);
@@ -44,10 +45,17 @@ void * SendThread( void * v )
 			int ss = send( sockc, buff, rd, MSG_DONTWAIT | MSG_NOSIGNAL );
 			if( ss < rd )
 			{
-				fprintf( stderr, "Dropped %d\n", i );
-				close( SocketList[i] );
-				SocketList[i] = 0;
+                if( droppedct[i]++ > 20 )
+                {
+				    fprintf( stderr, "Dropped %d\n", i );
+				    close( SocketList[i] );
+				    SocketList[i] = 0;
+                }
 			}
+            else
+            {
+                droppedct[i] = 0;
+            }
 		}
 	}
 }
@@ -142,6 +150,7 @@ int main( int argc, char ** argv )
 			if( SocketList[il] == 0 )
 			{
 				SocketList[il] = childfd;
+                droppedct[il] = 0;
 				printf("Conn %s At %d\n", 
 					hostaddrp, il);
 				break;
diff --git a/tools/lighthousefind_tori/main.c b/tools/lighthousefind_tori/main.c
index ee56b37..d3a9f27 100644
--- a/tools/lighthousefind_tori/main.c
+++ b/tools/lighthousefind_tori/main.c
@@ -64,7 +64,19 @@ static void runTheNumbers()
 
 	printf("Using %d sensors to find lighthouse.\n", sensorCount);
 
-	Point lh = SolveForLighthouse(to, 1);
+	Point lh;
+	//for (int i = 0; i < 200; i++)
+	for (int i = 0; i < 1; i++)
+	{
+		lh = SolveForLighthouse(to, 0);
+		//(0.156754, -2.403268, 2.280167)
+		//assert(fabs((lh.x / 0.1419305302702402) - 1) < 0.00001);
+		//assert(fabs((lh.y / 2.5574949720325431) - 1) < 0.00001);
+		//assert(fabs((lh.z / 2.2451193935772080) - 1) < 0.00001);
+		//assert(lh.x > 0);
+		//assert(lh.y > 0);
+		//assert(lh.z > 0);
+	}
 
 	printf("(%f, %f, %f)\n", lh.x, lh.y, lh.z);
 
diff --git a/tools/lighthousefind_tori/torus_localizer.c b/tools/lighthousefind_tori/torus_localizer.c
index 837b745..fd74b22 100644
--- a/tools/lighthousefind_tori/torus_localizer.c
+++ b/tools/lighthousefind_tori/torus_localizer.c
@@ -26,21 +26,23 @@ Matrix3x3 GetRotationMatrixForTorus(Point a, Point b)
 	rotation_between_vecs_to_m3(&result, v1, v2);
 
 	// Useful for debugging...
-	FLT v2b[3];
-	rotate_vec(v2b, v1, result);
+	//FLT v2b[3];
+	//rotate_vec(v2b, v1, result);
 
 	return result;
 }
 
-//void TestGetRotationMatrixForTorus()
-//{
-//	// a={x=0.079967096447944641 y=0.045225199311971664 z=0.034787099808454514 }
-//	// b={x=0.085181400179862976 y=0.017062099650502205 z=0.046403601765632629 }
-//
-//	// a={x=0.050822000950574875 y=0.052537899464368820 z=0.033285100013017654 }
-//	// b={x=0.085181400179862976 y=0.017062099650502205 z=0.046403601765632629 }
-//
-//}
+typedef struct
+{
+	Point a;
+	Point b;
+	FLT angle;
+	FLT tanAngle; // tangent of angle
+	Matrix3x3 rotation;
+	Matrix3x3 invRotation; // inverse of rotation
+
+} PointsAndAngle;
+
 
 Point RotateAndTranslatePoint(Point p, Matrix3x3 rot, Point newOrigin)
 {
@@ -92,149 +94,29 @@ Point midpoint(Point a, Point b)
 	return m;
 }
 
-
-
-
-// This torus generator creates a point cloud of the given torus, and attempts to keep the 
-// density of the points fairly uniform across the surface of the torus.
-void partialTorusGenerator(
-	Point p1,
-	Point p2,
-	double toroidalStartAngle,
-	double toroidalEndAngle,
-	double poloidalStartAngle,
-	double poloidalEndAngle,
-	double lighthouseAngle,
-	double toroidalPrecision,
-	Point **pointCloud)
-{
-	double poloidalRadius = 0;
-	double toroidalRadius = 0;
-
-	Point m = midpoint(p1, p2);
-	double distanceBetweenPoints = distance(p1, p2);
-
-	// ideally should only need to be lighthouseAngle, but increasing it here keeps us from accidentally
-	// thinking the tori converge at the location of the tracked object instead of at the lighthouse.
-	double centralAngleToIgnore = lighthouseAngle * 3;
-
-	Matrix3x3 rot = GetRotationMatrixForTorus(p1, p2);
-
-	toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle));
-
-	poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2));
-
-	double poloidalPrecision = M_PI * 2 / toroidalPrecision;
-
-
-	unsigned int pointCount = 0;
-
-	// This loop tries to (cheaply) figure out an upper bound on the number of points we'll have in our point cloud
-	for (double poloidalStep = poloidalStartAngle; poloidalStep < poloidalEndAngle; poloidalStep += poloidalPrecision)
-	{
-		// here, we specify the number of steps that will occur on the toroidal circle for a given poloidal angle
-		// We do this so our point cloud will have a more even distribution of points across the surface of the torus.
-		double steps = (cos(poloidalStep) + 1) / 2 * toroidalPrecision;
-
-		double step_distance = 2 * M_PI / steps;
-
-		pointCount += (unsigned int)((toroidalEndAngle - toroidalStartAngle) / step_distance + 2);
-	}
-
-	*pointCloud = malloc(pointCount * sizeof(Point) );
-
-	assert(0 != *pointCloud);
-
-	(*pointCloud)[pointCount - 1].x = -1000;
-	(*pointCloud)[pointCount - 1].y = -1000;
-	(*pointCloud)[pointCount - 1].z = -1000; // need a better magic number or flag, but this'll do for now.
-
-	size_t currentPoint = 0;
-
-	for (double poloidalStep = poloidalStartAngle; poloidalStep < poloidalEndAngle; poloidalStep += poloidalPrecision)
-	{
-		// here, we specify the number of steps that will occur on the toroidal circle for a given poloidal angle
-		// We do this so our point cloud will have a more even distribution of points across the surface of the torus.
-		double steps = (cos(poloidalStep) + 1) / 2 * toroidalPrecision;
-
-		double step_distance = 2 * M_PI / steps;
-
-		//for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += M_PI / 40)
-		for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += step_distance)
-		{
-			if (currentPoint >= pointCount - 1)
-			{
-				int a = 0;
-			}
-			assert(currentPoint < pointCount - 1);
-			(*pointCloud)[currentPoint].x = (toroidalRadius + poloidalRadius*cos(poloidalStep))*cos(toroidalStep);
-			(*pointCloud)[currentPoint].y = (toroidalRadius + poloidalRadius*cos(poloidalStep))*sin(toroidalStep);
-			(*pointCloud)[currentPoint].z = poloidalRadius*sin(poloidalStep);
-			(*pointCloud)[currentPoint] = RotateAndTranslatePoint((*pointCloud)[currentPoint], rot, m);
-
-			// TODO: HACK!!! Instead of doing anything with normals, we're "assuming" that all sensors point directly up
-			// and hence we know that nothing with a negative z value is a possible lightouse location.
-			// Before this code can go live, we'll have to take the normals into account and remove this hack.
-			if ((*pointCloud)[currentPoint].z > 0)
-			{
-				currentPoint++;
-			}
-		}
-	}
-
-#ifdef TORI_DEBUG
-	printf("%d / %d\n", currentPoint, pointCount);
-#endif
-	(*pointCloud)[currentPoint].x = -1000;
-	(*pointCloud)[currentPoint].y = -1000;
-	(*pointCloud)[currentPoint].z = -1000;
-}
-
-void torusGenerator(Point p1, Point p2, double lighthouseAngle, Point **pointCloud)
-{
-
-	double centralAngleToIgnore = lighthouseAngle * 6;
-
-	centralAngleToIgnore = 20.0 / 180.0 * M_PI;
-
-	partialTorusGenerator(p1, p2, 0, M_PI * 2, centralAngleToIgnore + M_PI, M_PI * 3 - centralAngleToIgnore, lighthouseAngle, DefaultPointsPerOuterDiameter, pointCloud);
-
-	return;
-}
-
-
 // What we're doing here is:
 // * Given a point in space
 // * And points and a lighthouse angle that implicitly define a torus
 // * for that torus, what is the toroidal angle of the plane that will go through that point in space
 // * and given that toroidal angle, what is the poloidal angle that will be directed toward that point in space?
-//
-// Given the toroidal and poloidal angles of a "good estimate" of a solution position, a caller of this function
-// will be able to "draw" the point cloud of a torus in just the surface of the torus near the point in space.
-// That way, the caller doesn't have to draw the entire torus in high resolution, just the part of the torus
-// that is most likely to contain the best solution.
 void estimateToroidalAndPoloidalAngleOfPoint(
-	Point torusP1,
-	Point torusP2,
-	double lighthouseAngle,
+	PointsAndAngle *pna,
 	Point point,
-	double *toroidalAngle,
-	double *poloidalAngle)
+	double *toroidalSin,
+	double *toroidalCos,
+	double *poloidalAngle,
+	double *poloidalSin)
 {
-	// this is the rotation matrix that shows how to rotate the torus from being in a simple "default" orientation
-	// into the coordinate system of the tracked object
-	Matrix3x3 rot = GetRotationMatrixForTorus(torusP1, torusP2);
-
 	// We take the inverse of the rotation matrix, and this now defines a rotation matrix that will take us from
 	// the tracked object coordinate system into the "easy" or "default" coordinate system of the torus.
 	// Using this will allow us to derive angles much more simply by being in a "friendly" coordinate system.
-	rot = inverseM33(rot);
+	Matrix3x3 rot = pna->invRotation;
 	Point origin;
 	origin.x = 0;
 	origin.y = 0;
 	origin.z = 0;
 
-	Point m = midpoint(torusP1, torusP2);
+	Point m = midpoint(pna->a, pna->b);
 
 	// in this new coordinate system, we'll rename all of the points we care about to have an "F" after them
 	// This will be their representation in the "friendly" coordinate system
@@ -256,11 +138,23 @@ void estimateToroidalAndPoloidalAngleOfPoint(
 	// We will "flatten" the z dimension to only look at the x and y values.  Then, we just need to measure the 
 	// angle between a vector to pointF and a vector along the x axis.  
 
-	*toroidalAngle = atan(pointF.y / pointF.x);
-	if (pointF.x < 0)
-	{
-		*toroidalAngle += M_PI;
-	}
+	FLT toroidalHyp = FLT_SQRT(SQUARED(pointF.y) + SQUARED(pointF.x));
+
+	*toroidalSin = pointF.y / toroidalHyp;
+
+	*toroidalCos = pointF.x / toroidalHyp;
+
+	//*toroidalAngle = atan(pointF.y / pointF.x);
+	//if (pointF.x < 0)
+	//{
+	//	*toroidalAngle += M_PI;
+	//}
+
+	//assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001);
+	//assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001);
+
+	//assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 < 0.000001);
+	//assert(*toroidalCos / FLT_COS(*toroidalAngle) - 1 > -0.000001);
 
 	// SCORE!! We've got the toroidal angle.  We're half done!
 
@@ -289,9 +183,9 @@ void estimateToroidalAndPoloidalAngleOfPoint(
 	// this as a 2D problem.  I think we're getting close...
 
 	// I stole these lines from the torus generator.  Gonna need the poloidal radius.
-	double distanceBetweenPoints = distance(torusP1, torusP2); // we don't care about the coordinate system of these points because we're just getting distance.
-	double toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle));
-	double poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2));
+	double distanceBetweenPoints = distance(pna->a, pna->b); // we don't care about the coordinate system of these points because we're just getting distance.
+	double toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle);
+	double poloidalRadius = sqrt(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
 
 	// The center of the polidal circle already lies on the z axis at this point, so we won't shift z at all. 
 	// The shift along the X axis will be the toroidal radius.  
@@ -304,240 +198,66 @@ void estimateToroidalAndPoloidalAngleOfPoint(
 	// Okay, almost there.  If we treat pointH as a vector on the XZ plane, if we get its angle,
 	// that will be the poloidal angle we're looking for.  (crosses fingers)
 
+	FLT poloidalHyp = FLT_SQRT(SQUARED(pointH.z) + SQUARED(pointH.x));
+
+	*poloidalSin = pointH.z / poloidalHyp;
+
+
 	*poloidalAngle = atan(pointH.z / pointH.x);
 	if (pointH.x < 0)
 	{
 		*poloidalAngle += M_PI;
 	}
 
-	// Wow, that ended up being not so much code, but a lot of interesting trig.
-	// can't remember the last time I spent so much time working through each line of code.
-
-	return;
-}
+	//assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 < 0.000001);
+	//assert(*toroidalSin / FLT_SIN(*toroidalAngle) - 1 > -0.000001);
 
-double FindSmallestDistance(Point p, Point* cloud)
-{
-	Point *cp = cloud;
-	double smallestDistance = 10000000000000.0;
 
-	while (cp->x != -1000 || cp->y != -1000 || cp->z != -1000)
-	{
-		double distance = (SQUARED(cp->x - p.x) + SQUARED(cp->y - p.y) + SQUARED(cp->z - p.z));
-		if (distance < smallestDistance)
-		{
-			smallestDistance = distance;
-		}
-		cp++;
-	}
-	smallestDistance = sqrt(smallestDistance);
-	return smallestDistance;
-}
-
-// Given a cloud and a list of clouds, find the point on masterCloud that best matches clouds.
-Point findBestPointMatch(Point *masterCloud, Point** clouds, int numClouds)
-{
 
-	Point bestMatch = { 0 };
-	double bestDistance = 10000000000000.0;
-	Point *cp = masterCloud;
-	int point = 0;
-	while (cp->x != -1000 || cp->y != -1000 || cp->z != -1000)
-	{
-		point++;
-#ifdef TORI_DEBUG
-		if (point % 100 == 0)
-		{
-			printf(".");
-		}
-#endif
-		double currentDistance = 0;
-		for (int i = 0; i < numClouds; i++)
-		{
-			if (clouds[i] == masterCloud)
-			{
-				continue;
-			}
-			Point* cloud = clouds[i];
-			currentDistance += FindSmallestDistance(*cp, cloud);
-		}
-
-		if (currentDistance < bestDistance)
-		{
-			bestDistance = currentDistance;
-			bestMatch = *cp;
-		}
-		cp++;
-	}
+	// Wow, that ended up being not so much code, but a lot of interesting trig.
+	// can't remember the last time I spent so much time working through each line of code.
 
-	return bestMatch;
+	return;
 }
 
-
 #define MAX_POINT_PAIRS 100
 
-typedef struct
-{
-	Point a;
-	Point b;
-	double angle;
-} PointsAndAngle;
-
-double angleBetweenSensors(TrackedSensor *a, TrackedSensor *b)
+FLT angleBetweenSensors(TrackedSensor *a, TrackedSensor *b)
 {
-	double angle = acos(cos(a->phi - b->phi)*cos(a->theta - b->theta));
-	double angle2 = acos(cos(b->phi - a->phi)*cos(b->theta - a->theta));
+	FLT angle = FLT_ACOS(FLT_COS(a->phi - b->phi)*FLT_COS(a->theta - b->theta));
+	FLT angle2 = FLT_ACOS(FLT_COS(b->phi - a->phi)*FLT_COS(b->theta - a->theta));
 
 	return angle;
 }
-double pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b)
-{
-	double p = (a->phi - b->phi);
-	double d = (a->theta - b->theta);
 
-	double adjd = sin((a->phi + b->phi) / 2);
-	double adjP = sin((a->theta + b->theta) / 2);
-	double pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd));
-	return pythAngle;
-}
-
-Point GetInitialEstimate(TrackedObject *obj, PointsAndAngle *pna, size_t pnaCount, FILE *logFile)
+// This provides a pretty good estimate of the angle above, probably better
+// the further away the lighthouse is.  But, it's not crazy-precise.
+// It's main advantage is speed.
+FLT pythAngleBetweenSensors2(TrackedSensor *a, TrackedSensor *b)
 {
-	Point **pointCloud = malloc(sizeof(void*)* pnaCount);
-
-
-	for (unsigned int i = 0; i < pnaCount; i++)
-	{
-		torusGenerator(pna[i].a, pna[i].b, pna[i].angle, &(pointCloud[i]));
-		if (logFile)
-		{
-			writePointCloud(logFile, pointCloud[i], COLORS[i%MAX_COLORS]);
-		}
-
-	}
-
-	Point bestMatchA = findBestPointMatch(pointCloud[0], pointCloud, pnaCount);
-
-	for (unsigned int i = 0; i < pnaCount; i++)
-	{
-		free(pointCloud[i]);
-		pointCloud[i] = NULL;
-	}
-
-	if (logFile)
-	{
-		markPointWithStar(logFile, bestMatchA, 0xFF0000);
-	}
-#ifdef TORI_DEBUG
-	printf("(%f,%f,%f)\n", bestMatchA.x, bestMatchA.y, bestMatchA.z);
-#endif
-
-	return bestMatchA;
-}
-
-Point RefineEstimateUsingPointCloud(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, TrackedObject *obj, FILE *logFile)
-{
-	// Now, let's add an extra patch or torus near the point we just found.
-
-	double toroidalAngle = 0;
-	double poloidalAngle = 0;
-
-
-
-	Point **pointCloud2 = malloc(sizeof(void*)* pnaCount);
-
-	for (unsigned int i = 0; i < pnaCount; i++)
-	{
-		estimateToroidalAndPoloidalAngleOfPoint(
-			pna[i].a,
-			pna[i].b,
-			pna[i].angle,
-			initialEstimate,
-			&toroidalAngle,
-			&poloidalAngle);
-
-		partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.1, toroidalAngle + 0.1, poloidalAngle - 0.2, poloidalAngle + 0.2, pna[i].angle, 800, &(pointCloud2[i]));
-
-		if (logFile)
-		{
-			writePointCloud(logFile, pointCloud2[i], COLORS[i%MAX_COLORS]);
-		}
-
-	}
-
-	Point bestMatchB = findBestPointMatch(pointCloud2[0], pointCloud2, pnaCount);
-
-	for (unsigned int i = 0; i < pnaCount; i++)
-	{
-		free(pointCloud2[i]);
-		pointCloud2[i] = NULL;
-	}
-
-	if (logFile)
-	{
-		markPointWithStar(logFile, bestMatchB, 0x00FF00);
-	}
-#ifdef TORI_DEBUG
-	printf("(%f,%f,%f)\n", bestMatchB.x, bestMatchB.y, bestMatchB.z);
-#endif
-
-	Point **pointCloud3 = malloc(sizeof(void*)* pnaCount);
-
-	for (unsigned int i = 0; i < pnaCount; i++)
-	{
-		estimateToroidalAndPoloidalAngleOfPoint(
-			pna[i].a,
-			pna[i].b,
-			pna[i].angle,
-			bestMatchB,
-			&toroidalAngle,
-			&poloidalAngle);
-
-		partialTorusGenerator(pna[i].a, pna[i].b, toroidalAngle - 0.05, toroidalAngle + 0.05, poloidalAngle - 0.1, poloidalAngle + 0.1, pna[i].angle, 3000, &(pointCloud3[i]));
-
-		if (logFile)
-		{
-			writePointCloud(logFile, pointCloud3[i], COLORS[i%MAX_COLORS]);
-		}
-
-	}
-
-	Point bestMatchC = findBestPointMatch(pointCloud3[0], pointCloud3, pnaCount);
-
-	for (unsigned int i = 0; i < pnaCount; i++)
-	{
-		free(pointCloud3[i]);
-		pointCloud3[i] = NULL;
-	}
-
-	if (logFile)
-	{
-		markPointWithStar(logFile, bestMatchC, 0xFFFFFF);
-	}
-#ifdef TORI_DEBUG
-	printf("(%f,%f,%f)\n", bestMatchC.x, bestMatchC.y, bestMatchC.z);
-#endif
-
-
+	FLT p = (a->phi - b->phi);
+	FLT d = (a->theta - b->theta);
 
-
-	return bestMatchC;
+	FLT adjd = FLT_SIN((a->phi + b->phi) / 2);
+	FLT adjP = FLT_SIN((a->theta + b->theta) / 2);
+	FLT pythAngle = sqrt(SQUARED(p*adjP) + SQUARED(d*adjd));
+	return pythAngle;
 }
 
-Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, double poloidalAngle)
+Point calculateTorusPointFromAngles(PointsAndAngle *pna, FLT toroidalSin, FLT toroidalCos, FLT poloidalAngle, FLT poloidalSin)
 {
 	Point result;
 
-	double distanceBetweenPoints = distance(pna->a, pna->b);
+	FLT distanceBetweenPoints = distance(pna->a, pna->b);
 	Point m = midpoint(pna->a, pna->b);
-	Matrix3x3 rot = GetRotationMatrixForTorus(pna->a, pna->b);
+	Matrix3x3 rot = pna->rotation;
 
-	double toroidalRadius = distanceBetweenPoints / (2 * tan(pna->angle));
-	double poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2));
+	FLT toroidalRadius = distanceBetweenPoints / (2 * pna->tanAngle);
+	FLT poloidalRadius = FLT_SQRT(SQUARED(toroidalRadius) + SQUARED(distanceBetweenPoints / 2));
 
-	result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*cos(toroidalAngle);
-	result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*sin(toroidalAngle);
-	result.z = poloidalRadius*sin(poloidalAngle);
+	result.x = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalCos;
+	result.y = (toroidalRadius + poloidalRadius*cos(poloidalAngle))*toroidalSin;
+	result.z = poloidalRadius*poloidalSin;
 	result = RotateAndTranslatePoint(result, rot, m);
 
 	return result;
@@ -546,18 +266,20 @@ Point calculateTorusPointFromAngles(PointsAndAngle *pna, double toroidalAngle, d
 FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
 {
 
-	double toroidalAngle = 0;
+	double toroidalSin = 0;
+	double toroidalCos = 0;
 	double poloidalAngle = 0;
+	double poloidalSin = 0;
 
 	estimateToroidalAndPoloidalAngleOfPoint(
-		pna->a,
-		pna->b,
-		pna->angle,
+		pna,
 		pointIn,
-		&toroidalAngle,
-		&poloidalAngle);
+		&toroidalSin,
+		&toroidalCos,
+		&poloidalAngle,
+		&poloidalSin);
 
-	Point torusPoint = calculateTorusPointFromAngles(pna, toroidalAngle, poloidalAngle);
+	Point torusPoint = calculateTorusPointFromAngles(pna, toroidalSin, toroidalCos, poloidalAngle, poloidalSin);
 
 	FLT dist = distance(pointIn, torusPoint);
 
@@ -571,14 +293,11 @@ FLT getPointFitnessForPna(Point pointIn, PointsAndAngle *pna)
 	// it is not clear if this is actually removing existing skew (to get a more accurate value)
 	// or if it is introducing an undesirable skew.
 	double fudge = FLT_SIN((poloidalAngle - M_PI) / 2);
-	//fudge *= fudge;
 	dist = dist / fudge;
 
 	return dist;
 }
 
-//Point RefineEstimateUsingPointCloud(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, TrackedObject *obj, FILE *logFile)
-//
 FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount)
 {
 	FLT fitness;
@@ -588,7 +307,6 @@ FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount)
 	for (size_t i = 0; i < pnaCount; i++)
 	{
 		fitness = getPointFitnessForPna(pointIn, &(pna[i]));
-		//printf("Distance[%d]: %f\n", i, fitness);
 		resultSum += SQUARED(fitness);
 	}
 
@@ -644,55 +362,6 @@ Point getAvgPoints(Point a, Point b)
 	return result;
 }
 
-// 0.95 is good value for descent
-// 0.1 is a good value for starting precision.
-static Point RefineEstimateUsingGradientDescent(Point initialEstimate, PointsAndAngle *pna, size_t pnaCount, FILE *logFile, FLT descent, FLT startingPrecision)
-{
-	int i = 0;
-	FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount);
-	Point lastPoint = initialEstimate;
-	Point lastGradient = getGradient(lastPoint, pna, pnaCount, .00000001 /*somewhat arbitrary*/);
-
-	for (FLT f = startingPrecision; f > 0.0001; f *= descent)
-	{
-		Point gradient = getGradient(lastPoint, pna, pnaCount, f / 1000 /*somewhat arbitrary*/);
-		gradient = getNormalizedVector(gradient, f);
-
-		//printf("Gradient: (%f, %f, %f)\n", gradient.x, gradient.y, gradient.z);
-
-		// gradient = getAvgPoints(gradient, lastGradient); // doesn't seem to help much.  might hurt in some cases.
-
-		Point newPoint;
-		newPoint.x = lastPoint.x + gradient.x;
-		newPoint.y = lastPoint.y + gradient.y;
-		newPoint.z = lastPoint.z + gradient.z;
-
-		FLT newMatchFitness = getPointFitness(newPoint, pna, pnaCount);
-
-		if (newMatchFitness > lastMatchFitness)
-		{
-			lastMatchFitness = newMatchFitness;
-			lastPoint = newPoint;
-			//printf("%f\n", newMatchFitness);
-			lastGradient = gradient;
-
-			if (logFile)
-			{
-				writePoint(logFile, lastPoint.x, lastPoint.y, lastPoint.z, 0xFFFFFF);
-			}
-		}
-		else
-		{
-			//printf("-");
-		}
-
-		i++;
-	}
-
-	//printf("i = %d\n", i);
-
-	return lastPoint;
-}
 
 // This is modifies the basic gradient descent algorithm to better handle the shallow valley case,
 // which appears to be typical of this convergence.  
@@ -701,7 +370,6 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
 	int i = 0;
 	FLT lastMatchFitness = getPointFitness(initialEstimate, pna, pnaCount);
 	Point lastPoint = initialEstimate;
-	//Point lastGradient = getGradient(lastPoint, pna, pnaCount, .00000001 /*somewhat arbitrary*/);
 
 	// The values below are somewhat magic, and definitely tunable
 	// The initial vlue of g will represent the biggest step that the gradient descent can take at first.
@@ -768,11 +436,15 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
 
 			lastMatchFitness = newMatchFitness;
 			lastPoint = point4;
+#ifdef TORI_DEBUG
 			printf("+");
+#endif
 		}
 		else
 		{
+#ifdef TORI_DEBUG
 			printf("-");
+#endif
 			g *= 0.7;
 
 		}
@@ -784,102 +456,13 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
 	return lastPoint;
 }
 
-// This torus generator creates a point cloud of the given torus, and attempts to keep the 
-// density of the points fairly uniform across the surface of the torus.
-void AnalyzeToroidalImpact(
-	Point p1,
-	Point p2,
-	double lighthouseAngle,
-	double *vals,
-	PointsAndAngle *pna, 
-	size_t pnaCount)
-{
-	double poloidalRadius = 0;
-	double toroidalRadius = 0;
-
-	Point m = midpoint(p1, p2);
-	double distanceBetweenPoints = distance(p1, p2);
-
-	// ideally should only need to be lighthouseAngle, but increasing it here keeps us from accidentally
-	// thinking the tori converge at the location of the tracked object instead of at the lighthouse.
-	double centralAngleToIgnore = lighthouseAngle * 3;
-
-	Matrix3x3 rot = GetRotationMatrixForTorus(p1, p2);
-
-	toroidalRadius = distanceBetweenPoints / (2 * tan(lighthouseAngle));
-
-	poloidalRadius = sqrt(pow(toroidalRadius, 2) + pow(distanceBetweenPoints / 2, 2));
-
-	unsigned int pointCount = 0;
-
-	size_t currentPoint = 0;
-	
-	for (size_t ps = 0; ps < 180; ps++)
-	{
-
-		//for (double toroidalStep = toroidalStartAngle; toroidalStep < toroidalEndAngle; toroidalStep += M_PI / 40)
-		for (double toroidalStep = 0; toroidalStep < M_PI / 2; toroidalStep += M_PI / 180 * 2)
-		{
-			double poloidalStep = M_PI + M_PI / 180 * 2 * ps;
-			Point tmp;
-
-			tmp.x = (toroidalRadius + poloidalRadius*cos(poloidalStep))*cos(toroidalStep);
-			tmp.y = (toroidalRadius + poloidalRadius*cos(poloidalStep))*sin(toroidalStep);
-			tmp.z = poloidalRadius*sin(poloidalStep);
-			tmp = RotateAndTranslatePoint(tmp, rot, m);
-
-			vals[ps] += getPointFitness(tmp, pna, pnaCount);
-
-		}
-
-		vals[ps] = vals[ps] / 180; // average.
-	}
-
-}
-
-void AnalyzePoloidalImpact(TrackedObject *obj, PointsAndAngle *pna, size_t pnaCount, FILE *logFile)
-{
-	Point **pointCloud = malloc(sizeof(void*)* pnaCount);
-
-	double vals[200][180] = { 0 };
-
-
-	for (unsigned int i = 0; i < pnaCount; i++)
-	{
-		//double tmpVals[180] = { 0 };
-
-		AnalyzeToroidalImpact(
-			pna[i].a,
-			pna[i].b,
-			pna[i].angle,
-			vals[i],
-			pna,
-			pnaCount);
-
-
-		//for (int j = 0; j < 180; j++)
-		//{
-		//	vals[j] += tmpVals[j];
-		//}
-
-	}
-	
-	for (int i = 0; i < 180; i++)
-	{
-		printf("%d", i * 2);
-		for (unsigned int j = 0; j < pnaCount; j++)
-		{
-			printf(", %f", vals[j][i]);
-		}
-		printf("\n");
-	}
-}
-
 
 Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 {
 	PointsAndAngle pna[MAX_POINT_PAIRS];
 
+	volatile size_t sizeNeeded = sizeof(pna);
+
 	Point avgNorm = { 0 };
 
 	size_t pnaCount = 0;
@@ -891,11 +474,17 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 			{
 				pna[pnaCount].a = obj->sensor[i].point;
 				pna[pnaCount].b = obj->sensor[j].point;
-
-				pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]);
+				
+				pna[pnaCount].angle = angleBetweenSensors(&obj->sensor[i], &obj->sensor[j]);
+				//pna[pnaCount].angle = pythAngleBetweenSensors2(&obj->sensor[i], &obj->sensor[j]);
+				pna[pnaCount].tanAngle = FLT_TAN(pna[pnaCount].angle);
 
 				double pythAngle = sqrt(SQUARED(obj->sensor[i].phi - obj->sensor[j].phi) + SQUARED(obj->sensor[i].theta - obj->sensor[j].theta));
 
+				pna[pnaCount].rotation = GetRotationMatrixForTorus(pna[pnaCount].a, pna[pnaCount].b);
+				pna[pnaCount].invRotation = inverseM33(pna[pnaCount].rotation);
+
+
 				pnaCount++;
 			}
 		}
@@ -919,15 +508,9 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 		writeAxes(logFile);
 	}
 
-	//Point initialEstimate = GetInitialEstimate(obj, pna, pnaCount, logFile);
-
-	//Point refinedEstimatePc = RefineEstimateUsingPointCloud(initialEstimate, pna, pnaCount, obj, logFile);
-
-	//Point refinedEstimageGd = RefineEstimateUsingGradientDescent(initialEstimate, pna, pnaCount, logFile, 0.95, 0.1);
 
 	// Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(initialEstimate, pna, pnaCount, logFile);
 
-	// AnalyzePoloidalImpact(obj, pna, pnaCount, logFile);
 
 	// arbitrarily picking a value of 8 meters out to start from.
 	// intentionally picking the direction of the average normal vector of the sensors that see the lighthouse
@@ -935,16 +518,16 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 	// back into the search for the correct point (see "if (a1 > M_PI / 2)" below)
 	Point p1 = getNormalizedVector(avgNorm, 8); 
 
-	Point refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile);
+	Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile);
 
-	FLT pf1[3] = { refinedEstimageGd.x, refinedEstimageGd.y, refinedEstimageGd.z };
+	FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z };
 
 	FLT a1 = anglebetween3d(pf1, avgNormF);
 
 	if (a1 > M_PI / 2)
 	{
-		Point p2 = { .x = -refinedEstimageGd.x, .y = -refinedEstimageGd.y, .z = -refinedEstimageGd.z };
-		refinedEstimageGd = RefineEstimateUsingModifiedGradientDescent1(p2, pna, pnaCount, logFile);
+		Point p2 = { .x = -refinedEstimateGd.x, .y = -refinedEstimateGd.y, .z = -refinedEstimateGd.z };
+		refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p2, pna, pnaCount, logFile);
 
 		//FLT pf2[3] = { refinedEstimageGd2.x, refinedEstimageGd2.y, refinedEstimageGd2.z };
 
@@ -952,9 +535,7 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 
 	}
 
-	//FLT fitPc = getPointFitness(refinedEstimatePc, pna, pnaCount);
-	FLT fitGd = getPointFitness(refinedEstimageGd, pna, pnaCount);
-	//FLT fitGd2 = getPointFitness(refinedEstimageGd2, pna, pnaCount);
+	FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount);
 
 	printf("Fitness is %f\n", fitGd);
 
@@ -964,45 +545,6 @@ Point SolveForLighthouse(TrackedObject *obj, char doLogOutput)
 		fclose(logFile);
 	}
 	//fgetc(stdin);
-	return refinedEstimageGd;
+	return refinedEstimateGd;
 }
 
-static Point makeUnitPoint(Point *p)
-{
-	Point newP;
-	double r = sqrt(p->x*p->x + p->y*p->y + p->z*p->z);
-	newP.x = p->x / r;
-	newP.y = p->y / r;
-	newP.z = p->z / r;
-
-	return newP;
-}
-
-static double getPhi(Point p)
-{
-	//	double phi = acos(p.z / (sqrt(p.x*p.x + p.y*p.y + p.z*p.z)));
-	//	double phi = atan(sqrt(p.x*p.x + p.y*p.y)/p.z);
-	double phi = atan(p.x / p.z);
-	return phi;
-}
-
-static double getTheta(Point p)
-{
-	//double theta = atan(p.y / p.x);
-	double theta = atan(p.x / p.y);
-	return theta;
-}
-
-// subtraction
-static Point PointSub(Point a, Point b)
-{
-	Point newPoint;
-
-	newPoint.x = a.x - b.x;
-	newPoint.y = a.y - b.y;
-	newPoint.z = a.z - b.z;
-
-	return newPoint;
-}
-
-