Merge pull request #1 from cnlohr/master

Pulling new changes

28 files changed, 2660 insertions, 467 deletions

diff --git a/Makefile b/Makefile
index 69b8e37..e57dae5 100644
--- a/Makefile
+++ b/Makefile
@@ -37,7 +37,7 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic.
 ifeq ($(UNAME), Darwin)
 REDISTS:=$(REDISTS) redist/hid-osx.c
 endif
-LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_vive.o src/survive_config.o src/survive_cal.o
+LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o
 
 
 #If you want to use HIDAPI on Linux.
diff --git a/README.md b/README.md
index 51213dc..98f75c8 100644
--- a/README.md
+++ b/README.md
@@ -79,6 +79,8 @@ Will ~~I~~ we succeed?  Probably not.  ~~Definitely going to try!~~ Though it's
 * pthread
 * libX11 (Linux) or Native (win32) or OpenGL (OSX)
 * zlib (Linux) or puff.c (win32, included in redist)
+* Optionally OpenGL.
+On Debian, ```sudo apt-get install build-essential zlib1g-dev libx11-dev libusb-1.0-0-dev freeglut3-dev``` should be sufficient.
 
 ## Architecture
 
@@ -135,7 +137,140 @@ The limiting factor for Vive viability on a given computer is the maximum availa
 
 To support the Vive on HDMI, you either need a newer version of HDMI, or you need to define a custom resolution that respects pixel clock and video port limits, and is also accepted and displayed by the Vive. So far, we have not had success using custom resolutions on linux or on Windows. Windows imposes additional limitations in the form of restriction of WHQL certified drivers forbidden from using custom display resolutions (only allowing those defined by EDID in the monitor). Intel has released uncertified beta drivers for Haswell and newer processors, which should be able to support custom resolutions for the Vive (untested at this time).
 
-
+# Getting Started
+
+## General Information
+
+The default configuration of libsurvive requires both basestations and both controllers to be active, but currently libsurvive can not make proper use of both basestations at the same time - it outputs a different pose for each basestation and does not fuse the data for one more accurate pose. This will hopefully be implemented soon. For now it's a good idea to change the configuration to only require one basestation.
+
+Here is an example of a default configuration file that libsurvive will create as `config.json` in the current working directory when any libsurvive client is executed:
+
+```
+"LighthouseCount":"2",
+"DefaultPoser":"PoserTurveyTori",
+"RequiredTrackersForCal":"",
+"AllowAllTrackersForCal":"1",
+"ConfigPoser":"PoserTurveyTori",
+"TurveyToriDebug":"0"
+"lighthouse0":{
+"index":"0",
+"id":"138441170",
+"pose":["0.000000","0.000000","0.000000","0.000000","0.000000","0.000000","0.000000"],
+"fcalphase":["-0.011757","0.020172"],
+"fcaltilt":["-0.003302","-0.001370"],
+"fcalcurve":["0.000323","-0.002600"],
+"fcalgibpha":["-4.316406","0.740723"],
+"fcalgibmag":["0.001188","-0.009270"]
+}
+"lighthouse1":{
+"index":"-1"
+}
+```
+
+To make libsurvive calibrate and run with one basestations, `LighthouseCount` needs to be changed to `1`.
+
+It may be annoying to always require the controllers for calibration. To make libsurvive calibrate by using the HMD, `RequiredTrackersForCal` needs to be changed to the magic string `HMD`. The strings for the controllers are `WM0` and `WM1`, short for  "Watchman". Other possible values are `WW0` (Wired Watchman) for a controller directly connected with USB or `TR0` for a Vive tracker directly connected with USB (When connected wirelessly, the tracker uses the dongles, so uses `WM0` or `WM1`).
+
+Lastly, to ensure libsurvive calibrates using the HMD, `AllowAllTrackersForCal` can be changed to `0`.
+
+Here is an example for such an altered `config.json` file
+
+```
+"LighthouseCount":"1",
+"DefaultPoser":"PoserTurveyTori",
+"RequiredTrackersForCal":"HMD",
+"AllowAllTrackersForCal":"0",
+"ConfigPoser":"PoserTurveyTori",
+"TurveyToriDebug":"0"
+"lighthouse0":{
+"index":"0",
+"id":"138441170",
+"pose":["0.000000","0.000000","0.000000","0.000000","0.000000","0.000000","0.000000"],
+"fcalphase":["-0.011757","0.020172"],
+"fcaltilt":["-0.003302","-0.001370"],
+"fcalcurve":["0.000323","-0.002600"],
+"fcalgibpha":["-4.316406","0.740723"],
+"fcalgibmag":["0.001188","-0.009270"]
+}
+"lighthouse1":{
+"index":"-1"
+}
+```
+
+Running libsurvive's `./test` with this `config.json` in the same directory should now go through the calibration and start printing poses with only the HMD and only one lighthouse basestation active. Enabling and tracking controllers will still work with this configuration.
+
+**For best results the HMD should not be moved while calibrating!**
+
+The important calibration steps are denoted by libsurvive printing
+
+```
+Info: Stage 2 good - continuing. 32 1 0
+Info: Stage 2 good - continuing. 32 1 1
+Info: Stage 2 good - continuing. 32 1 2
+Info: Stage 2 good - continuing. 32 1 3
+Info: Stage 2 good - continuing. 32 1 4
+Info: Stage 2 moving to stage 3. 32 1 5
+Lighthouse Pose: [0][ 0.28407975, 0.93606335,-0.37406892] [ 0.05594964,-0.33792987, 0.93887696, 0.03439615]
+Info: Stage 4 succeeded.
+```
+
+If libsurvive does not print these steps, make sure that the lighthouse basestation is visible to enough sensors on the HMD.
+
+Sometimes libsurvive goes very quickly through these steps and fills in all pose values as `NaN` or `-NaN`. This appears to be a bug in libsurvive that has not be found yet. Reflective surfaces nearby may trigger this problem more often.
+
+[Here is a short demo video how successfuly running ./test should look like](https://haagch.frickel.club/Peek%202018-02-21%2023-23.webm).
+
+## Using libsurvive in your own application
+
+Example code for libsurvive can be found in [test.c](https://github.com/cnlohr/libsurvive/blob/master/test.c). [calibrate.c](https://github.com/cnlohr/libsurvive/blob/master/calibrate.c) may contain some interesting code too.
+
+Here is minimal example that demonstrates using libsurvive's callback functionality to fill in pose data into a user defined data structure that is stored in libsurvive's SurviveContext.
+
+```c
+#include <stdio.h>
+#include <string.h>
+#include <survive.h>
+
+typedef struct {
+	double rotation[4];
+	double pos[3];
+} libsurvive_hmd;
+
+void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, FLT *pos, FLT *quat) {
+	survive_default_raw_pose_process(so, lighthouse, pos, quat);
+	printf("(Callback) Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
+	if (strcmp(so->codename, "HMD") == 0 && lighthouse == 0) {
+		libsurvive_hmd *hmd = so->ctx->user_ptr;
+		hmd->pos[0] = pos[0]; hmd->pos[1] = pos[1]; hmd->pos[2] = pos[2];
+		hmd->rotation[0] = quat[0]; hmd->rotation[1] = quat[1]; hmd->rotation[2] = quat[2]; hmd->rotation[3] = quat[3];
+	}
+}
+
+int main(int argc, char** argv) {
+	struct SurviveContext *ctx = survive_init( 0 );
+	survive_install_raw_pose_fn(ctx, testprog_raw_pose_process);
+	survive_cal_install(ctx);
+	libsurvive_hmd *hmd = &(libsurvive_hmd) { 0 };
+	ctx->user_ptr = hmd;
+	while(survive_poll(ctx) == 0) {
+		//printf("(Main) HMD Pose: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", hmd->pos[0], hmd->pos[1], hmd->pos[2], hmd->rotation[0], hmd->rotation[1], hmd->rotation[2], hmd->rotation[3]);
+	}
+	return 0;
+}
+```
+
+Compiling this minimal example only requires the include path for survive.h as well as the libsurvive library: `gcc demo.c -Iinclude/libsurvive/ -Llib -lsurvive -Wl,-rpath=./lib -o libsurvive-demo`.
+
+ As mentioned, only the pose from lighthouse number `0` is used. Since the callback is called for all tracked devices, `so->codename` can be used to differentiate between devices like `HMD`, `WM0`, etc.
+
+# FAQ
+
+* The tracking quality is bad/jitters/too slow!
+ * libsurvive is still a work in progress. For example the Vive contains a calibration blob that still needs to be decoded. Hopefully it will enable better tracking.
+* What VR software can I use with libsurvive?
+ * There is an unofficial [OpenHMD/libsurvive fork](https://github.com/ChristophHaag/OpenHMD/tree/libsurvive) that replaces OpenHMD's Vive driver with libsurvive. OpenHMD will not merge this branch as it depends on libsurvive as an external dependency, but it may pave the way for more code sharing.
+   * This OpenHMD/libsurvive fork can be plugged into [SteamVR-OpenHMD](https://github.com/ChristophHaag/SteamVR-OpenHMD) which allows SteamVR to use OpenHMD drivers.
+   * Godot 3.x has a [native OpenHMD plugin](https://github.com/BastiaanOlij/godot_openhmd) though it needs work for building and running properly and it is still missing motion controller support.
 
 ## Addendum and notes
 
diff --git a/calibrate.c b/calibrate.c
index 15c362c..ff3b774 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -14,6 +14,7 @@
 
 struct SurviveContext * ctx;
 int  quit = 0;
+static int LighthouseCount = 0;
 
 void HandleKey( int keycode, int bDown )
 {
@@ -206,9 +207,9 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution)
 
 		CNFGTackSegment(
 			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]),
-			yResolution-(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
+			(short)yResolution-(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
 			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]),
-			yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
+			(short)yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
 	}
 
 	// line for the (0,-y) to (0,+y))
@@ -232,9 +233,9 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution)
 
 		CNFGTackSegment(
 			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]),
-			yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
+			(short)yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
 			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]),
-			yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
+			(short)yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
 	}
 
 	// Small line to indicate (0,+y) 
@@ -244,23 +245,73 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution)
 		FLT tmp2[3];
 		FLT tmp2out[3];
 
-		tmp1[1] = minRectSize + ((pose.Pos[2] * 40.0));
+		tmp1[1] = minRectSize + ((pose.Pos[2] * 40.0)*1.3);
 		tmp1[2] = 0;
-		tmp1[0] = tmp1[1] * 0.3;
+		tmp1[0] = ((pose.Pos[2] * 40.0)) * 0.3;
 
 		quatrotatevector(tmp1out, pose.Rot, tmp1);
 
-		tmp2[1] = minRectSize + ((pose.Pos[2] * 40.0));
+		tmp2[1] = minRectSize + ((pose.Pos[2] * 40.0)*0.7);
+		tmp2[2] = 0;
+		tmp2[0] = -(((pose.Pos[2] * 40.0)) * 0.3);
+
+		quatrotatevector(tmp2out, pose.Rot, tmp2);
+
+		CNFGTackSegment(
+			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]),
+			(short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
+			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]),
+			(short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
+	}
+	// Small line to indicate (+x,0)
+	{
+		FLT tmp1[3];
+		FLT tmp1out[3];
+		FLT tmp2[3];
+		FLT tmp2out[3];
+
+		tmp1[0] = minRectSize + ((pose.Pos[2] * 40.0)*1.3);
+		tmp1[2] = 0;
+		tmp1[1] = tmp1[0] * 0.3;
+
+		quatrotatevector(tmp1out, pose.Rot, tmp1);
+
+		tmp2[0] = minRectSize + ((pose.Pos[2] * 40.0)*.7);
 		tmp2[2] = 0;
-		tmp2[0] = -(tmp1[1] * 0.3);
+		tmp2[1] = -(tmp1[0] * 0.3);
 
 		quatrotatevector(tmp2out, pose.Rot, tmp2);
 
 		CNFGTackSegment(
 			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]),
-			yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
+			(short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
 			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]),
-			yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
+			(short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
+	}
+	// Small line to indicate (+x,0)
+	{
+		FLT tmp1[3];
+		FLT tmp1out[3];
+		FLT tmp2[3];
+		FLT tmp2out[3];
+
+		tmp1[0] = minRectSize + ((pose.Pos[2] * 40.0)*.7);
+		tmp1[2] = 0;
+		tmp1[1] = tmp1[0] * 0.3;
+
+		quatrotatevector(tmp1out, pose.Rot, tmp1);
+
+		tmp2[0] = minRectSize + ((pose.Pos[2] * 40.0)*1.3);
+		tmp2[2] = 0;
+		tmp2[1] = -(tmp1[0] * 0.3);
+
+		quatrotatevector(tmp2out, pose.Rot, tmp2);
+
+		CNFGTackSegment(
+			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]),
+			(short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
+			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]),
+			(short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
 	}
 
 
@@ -343,6 +394,7 @@ void * SurviveThread(void *jnk)
 {
 	ctx = survive_init( 0 );
 
+
 	uint8_t i =0;
 	for (i=0;i<32;++i) {
 		sensor_name[i] = malloc(3);
diff --git a/data_recorder.c b/data_recorder.c
index 002357e..6123d57 100644
--- a/data_recorder.c
+++ b/data_recorder.c
@@ -1,177 +1,204 @@
-//Data recorder mod with GUI showing light positions.
+// Data recorder mod with GUI showing light positions.
 
 #ifdef __linux__
 #include <unistd.h>
 #endif
+
+#include <CNFGFunctions.h>
+#include <os_generic.h>
+#include <stdarg.h>
+#include <stdint.h>
 #include <stdio.h>
 #include <stdlib.h>
-#include <stdint.h>
-#include <survive.h>
 #include <string.h>
-#include <os_generic.h>
-#include <CNFGFunctions.h>
+#include <survive.h>
+#include <sys/time.h>
+#include <time.h>
 
-struct SurviveContext * ctx;
+#include "redist/os_generic.h"
 
-void HandleKey( int keycode, int bDown )
-{
-	if( !bDown ) return;
+struct SurviveContext *ctx;
 
-	if( keycode == 'O' || keycode == 'o' )
-	{
-		survive_send_magic(ctx,1,0,0);
+FILE *output_file = 0;
+
+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);
+	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 )
-{
-}
+void HandleButton(int x, int y, int button, int bDown) {}
 
-void HandleDestroy()
-{
-}
+void HandleMotion(int x, int y, int mask) {}
 
-int bufferpts[32*2*3];
-char buffermts[32*128*3];
-int buffertimeto[32*3];
+void HandleDestroy() {}
 
-void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lh)
-{
-	survive_default_light_process( so, sensor_id, acode, timeinsweep, timecode, length, lh);
+int bufferpts[32 * 2 * 3];
+char buffermts[32 * 128 * 3];
+int buffertimeto[32 * 3];
 
-	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;
+double timestamp_in_us() {
+	static double start_time_us = 0;
+	if (start_time_us == 0)
+		start_time_us = OGGetAbsoluteTime();
+	return OGGetAbsoluteTime() - start_time_us;
+}
 
+int write_to_output(const char *format, ...) {
+	va_list args;
+	va_start(args, format);
+	fprintf(output_file, "%.17g ", timestamp_in_us());
+	vfprintf(output_file, format, args);
 
-	if( acode == 0 || acode == 2 ) //data = 0
-	{
-		printf( "L X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length );
-		bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
-		buffertimeto[jumpoffset] = 0;
-	}
-	if( acode == 1 || acode == 3 ) //data = 1
-	{
-		printf( "L Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length );
-		bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
-		buffertimeto[jumpoffset] = 0;
-	}
+	va_end(args);
+}
 
+void my_light_process(struct SurviveObject *so, int sensor_id, int acode,
+					  int timeinsweep, uint32_t timecode, uint32_t length,
+					  uint32_t lh) {
+	survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode,
+								  length, lh);
 
-	if( acode == 4 || acode == 6 ) //data = 0
-	{
-		printf( "R X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length );
-		bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
-		buffertimeto[jumpoffset] = 0;
+	if (acode == -1) {
+		write_to_output("A %s %d %d %d %u %u %u\n", so->codename, sensor_id,
+						acode, timeinsweep, timecode, length, lh);
+		return;
 	}
-	if( acode == 5 || acode == 7 ) //data = 1
-	{
-		printf( "R Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length );
-		bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
-		buffertimeto[jumpoffset] = 0;
+
+	int jumpoffset = sensor_id;
+	if (strcmp(so->codename, "WM0") == 0)
+		jumpoffset += 32;
+	else if (strcmp(so->codename, "WM1") == 0)
+		jumpoffset += 64;
+
+	const char *LH_ID = 0;
+	const char *LH_Axis = 0;
+
+	switch (acode) {
+	case 0:
+	case 2:
+		bufferpts[jumpoffset * 2 + 0] = (timeinsweep - 100000) / 500;
+		LH_ID = "L";
+		LH_Axis = "X";
+		break;
+	case 1:
+	case 3:
+		bufferpts[jumpoffset * 2 + 1] = (timeinsweep - 100000) / 500;
+		LH_ID = "L";
+		LH_Axis = "Y";
+		break;
+	case 4:
+	case 6:
+		bufferpts[jumpoffset * 2 + 0] = (timeinsweep - 100000) / 500;
+		LH_ID = "R";
+		LH_Axis = "X";
+		break;
+	case 5:
+	case 7:
+		bufferpts[jumpoffset * 2 + 1] = (timeinsweep - 100000) / 500;
+		LH_ID = "R";
+		LH_Axis = "Y";
+		break;
 	}
 
+	write_to_output("%s %s %s %u %d %d %d %u %u\n", LH_ID, LH_Axis,
+					so->codename, timecode, sensor_id, acode, timeinsweep,
+					length, lh);
+	buffertimeto[jumpoffset] = 0;
 }
 
-void my_imu_process( struct SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id )
-{
-	survive_default_imu_process( so, mask, accelgyro, timecode, id );
-
-//return;
-	//if( so->codename[0] == 'H' )
-	if( 1 )
-	{
-		printf( "I %s %d %f %f %f %f %f %f %d\n", so->codename, timecode, accelgyro[0], accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id );
-	}
+void my_imu_process(struct SurviveObject *so, int mask, FLT *accelgyro,
+					uint32_t timecode, int id) {
+	survive_default_imu_process(so, mask, accelgyro, timecode, id);
+	write_to_output("I %s %d %u %.17g %.17g %.17g %.17g %.17g %.17g %d\n",
+					so->codename, mask, timecode, accelgyro[0], accelgyro[1],
+					accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id);
 }
 
-
-
-
-void * GuiThread( void * v )
-{
+void *GuiThread(void *v) {
 	CNFGBGColor = 0x000000;
 	CNFGDialogColor = 0x444444;
-	CNFGSetup( "Survive GUI Debug", 640, 480 );
+	CNFGSetup("Survive GUI Debug", 640, 480);
 
 	short screenx, screeny;
-	while(1)
-	{
+	while (1) {
 		CNFGHandleInput();
 		CNFGClearFrame();
-		CNFGColor( 0xFFFFFF );
-		CNFGGetDimensions( &screenx, &screeny );
+		CNFGColor(0xFFFFFF);
+		CNFGGetDimensions(&screenx, &screeny);
 
 		int i;
-		for( i = 0; i < 32*3; i++ )
-		{
-			if( buffertimeto[i] < 50 )
-			{
+		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 );			
+				uint8_t g = (color >> 8) & 0xff;
+				uint8_t b = (color >> 16) & 0xff;
+				r = (r * (5 - buffertimeto[i])) / 5;
+				g = (g * (5 - buffertimeto[i])) / 5;
+				b = (b * (5 - buffertimeto[i])) / 5;
+				CNFGColor((b << 16) | (g << 8) | r);
+				CNFGTackRectangle(bufferpts[i * 2 + 0], bufferpts[i * 2 + 1],
+								  bufferpts[i * 2 + 0] + 5,
+								  bufferpts[i * 2 + 1] + 5);
+				CNFGPenX = bufferpts[i * 2 + 0];
+				CNFGPenY = bufferpts[i * 2 + 1];
+				CNFGDrawText(buffermts, 2);
 				buffertimeto[i]++;
 			}
 		}
 
 		CNFGSwapBuffers();
-		OGUSleep( 10000 );
+		OGUSleep(10000);
 	}
 }
 
-int SurviveThreadLoaded=0;
+int SurviveThreadLoaded = 0;
 
-void *SurviveThread(void *junk)
-{
-	ctx = survive_init( 0 );
+void *SurviveThread(void *junk) {
+	ctx = survive_init(0);
 
-	survive_install_light_fn( ctx,  my_light_process );
-	survive_install_imu_fn( ctx,  my_imu_process );
+	survive_install_light_fn(ctx, my_light_process);
+	survive_install_imu_fn(ctx, my_imu_process);
 
-	if( !ctx )
-	{
-		fprintf( stderr, "Fatal. Could not start\n" );
+	if (!ctx) {
+		fprintf(stderr, "Fatal. Could not start\n");
 		exit(1);
 	}
 
 	SurviveThreadLoaded = 1;
 
-	while(survive_poll(ctx) == 0)
-	{
-        printf("Do stuff.\n");
-		//Do stuff.
+	while (survive_poll(ctx) == 0) {
 	}
 
-    return 0;
+	return 0;
 }
 
-int main()
-{
-    // Create the libsurvive thread
-    OGCreateThread(SurviveThread, 0);
-    
+int main(int argc, char **argv) {
+	if (argc > 1) {
+		output_file = fopen(argv[1], "w");
+		if (output_file == 0) {
+			fprintf(stderr, "Could not open %s for writing", argv[1]);
+			return -1;
+		}
+	} else {
+		output_file = stdout;
+	}
+
+	// Create the libsurvive thread
+	OGCreateThread(SurviveThread, 0);
+
 	// Wait for the survive thread to load
-	while (!SurviveThreadLoaded) { OGUSleep(100); }
-	
-    // Run the Gui in the main thread
-    GuiThread(0);
-}
+	while (!SurviveThreadLoaded) {
+		OGUSleep(100);
+	}
 
+	// Run the Gui in the main thread
+	GuiThread(0);
+}
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 1532d62..d9b5f08 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -18,7 +18,8 @@ struct SurviveObject
 
 	char    codename[4];    //3 letters, null-terminated.  Currently HMD, WM0, WM1.
 	char    drivername[4];  //3 letters for driver.  Currently "HTC"
-	int16_t buttonmask;
+	void    *driver;
+	int32_t buttonmask;
 	int16_t axis1;
 
 	int16_t axis2;
@@ -67,6 +68,7 @@ struct SurviveObject
 	FLT* gyro_bias; // size is FLT*3. contains x,y,z
 	FLT* gyro_scale; // size is FLT*3. contains x,y,z
 
+	haptic_func haptic;
 
 	//Debug
 	int tsl;
@@ -90,6 +92,34 @@ struct BaseStationData
 
 struct config_group;
 
+#define BUTTON_QUEUE_MAX_LEN 32
+
+
+
+// note: buttonId and axisId are 1-indexed values.
+// a value of 0 for an id means that no data is present in that value
+// additionally, when x and y values are both present in axis data,
+// axis1 will be x, axis2 will be y.
+typedef struct
+{
+	uint8_t isPopulated;  //probably can remove this given the semaphore in the parent struct.   helps with debugging
+	uint8_t eventType;
+	uint8_t buttonId;
+	uint8_t axis1Id;
+	uint16_t axis1Val;
+	uint8_t axis2Id;
+	uint16_t axis2Val;
+	SurviveObject *so;
+} ButtonQueueEntry;
+
+typedef struct
+{
+	uint8_t nextReadIndex; //init to 0
+	uint8_t nextWriteIndex; // init to 0
+	void* buttonservicesem;
+	ButtonQueueEntry entry[BUTTON_QUEUE_MAX_LEN];
+} ButtonQueue;
+
 struct SurviveContext
 {
 	text_feedback_func faultfunction;
@@ -97,11 +127,14 @@ struct SurviveContext
 	light_process_func lightproc;
 	imu_process_func imuproc;
 	angle_process_func angleproc;
+	button_process_func buttonproc;
+	raw_pose_func rawposeproc;
 
 	struct config_group* global_config_values;
 	struct config_group* lh_config; //lighthouse configs
 
 	//Calibration data:
+	int activeLighthouses;
 	BaseStationData bsd[NUM_LIGHTHOUSES];
 	SurviveCalData * calptr; //If and only if the calibration subsystem is attached.
 
@@ -113,9 +146,25 @@ struct SurviveContext
 	DeviceDriverCb * drivercloses;
 	DeviceDriverMagicCb * drivermagics;
 	int driver_ct;
+
+	uint8_t isClosing; // flag to indicate if threads should terminate themselves
+
+	void* buttonservicethread;
+	ButtonQueue buttonQueue;
+
+	void *user_ptr;
+
 };
 
-SurviveContext * survive_init( int headless );
+SurviveContext * survive_init_internal( int headless );
+
+// Baked in size of FLT to verify users of the library have the correct setting. 
+void survive_verify_FLT_size(uint32_t user_size);
+  
+static inline SurviveContext * survive_init( int headless ) {
+  survive_verify_FLT_size(sizeof(FLT));
+  return survive_init_internal( headless );
+}
 
 //For any of these, you may pass in 0 for the function pointer to use default behavior.
 //In general unless you are doing wacky things like recording or playing back data, you won't need to use this.
@@ -124,6 +173,8 @@ void survive_install_error_fn( SurviveContext * ctx,  text_feedback_func fbp );
 void survive_install_light_fn( SurviveContext * ctx,  light_process_func fbp );
 void survive_install_imu_fn( SurviveContext * ctx,  imu_process_func fbp );
 void survive_install_angle_fn( SurviveContext * ctx,  angle_process_func fbp );
+void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp);
+void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp);
 
 void survive_close( SurviveContext * ctx );
 int survive_poll( SurviveContext * ctx );
@@ -141,11 +192,16 @@ void survive_cal_install( SurviveContext * ctx );  //XXX This will be removed if
 // Read back a human-readable string description of the calibration status
 int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length );
 
+// Induce haptic feedback
+int survive_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount);
+
 //Call these from your callback if overridden.  
 //Accept higher-level data.
 void survive_default_light_process( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length , uint32_t lh);
 void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id );
 void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh );
+void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val);
+void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *position, FLT *quaternion);
 
 
 ////////////////////// Survive Drivers ////////////////////////////
diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h
index 224719e..be1115b 100644
--- a/include/libsurvive/survive_types.h
+++ b/include/libsurvive/survive_types.h
@@ -14,6 +14,12 @@ extern "C" {
 #endif
 #endif
 
+#define float_format "%f"
+#define double_format "%lf"
+#define _FLT_format2(f) f##_format
+#define _FLT_format(f) _FLT_format2(f)
+#define FLT_format _FLT_format(FLT)
+  
 typedef struct SurvivePose
 {
 	FLT  Pos[3];
@@ -27,6 +33,12 @@ typedef struct SurvivePose
 #define INTBUFFSIZE			64
 #define SENSORS_PER_OBJECT	32
 
+// These are used for the eventType of button_process_func
+#define BUTTON_EVENT_BUTTON_NONE   0
+#define BUTTON_EVENT_BUTTON_DOWN   1
+#define BUTTON_EVENT_BUTTON_UP     2
+#define BUTTON_EVENT_AXIS_CHANGED  3
+
 typedef struct SurviveObject SurviveObject;
 typedef struct SurviveContext SurviveContext;
 typedef struct BaseStationData BaseStationData;
@@ -36,7 +48,10 @@ typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault );
 typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse);
 typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id );
 typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh);
+typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val);
+typedef void(*raw_pose_func)(SurviveObject * so, uint8_t lighthouse, FLT *position, FLT *quaternion);
 
+typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount);
 
 //Device drivers (prefix your drivers with "DriverReg") i.e.
 //		REGISTER_LINKTIME( DriverRegHTCVive );
diff --git a/redist/CNFGWinDriver.c b/redist/CNFGWinDriver.c
index b1c1eb0..9d1be6a 100644
--- a/redist/CNFGWinDriver.c
+++ b/redist/CNFGWinDriver.c
@@ -54,8 +54,8 @@ void CNFGGetDimensions( short * x, short * y )
 		lasty = buffery;
 		InternalHandleResize();
 	}
-	*x = bufferx;
-	*y = buffery;
+	*x = (short)bufferx;
+	*y = (short)buffery;
 }
 
 
@@ -212,7 +212,6 @@ void CNFGSetup( const char * name_of_window, int width, int height )
 
 void CNFGHandleInput()
 {
-	int ldown = 0;
 
 	MSG msg;
 	while( PeekMessage( &msg, lsHWND, 0, 0xFFFF, 1 ) )
diff --git a/redist/hid-linux.c b/redist/hid-linux.c
new file mode 100644
index 0000000..5cc3bf2
--- /dev/null
+++ b/redist/hid-linux.c
@@ -0,0 +1,798 @@
+/*******************************************************
+ HIDAPI - Multi-Platform library for
+ communication with HID devices.
+
+ Alan Ott
+ Signal 11 Software
+
+ 8/22/2009
+ Linux Version - 6/2/2009
+
+ Copyright 2009, All Rights Reserved.
+
+ At the discretion of the user of this library,
+ this software may be licensed under the terms of the
+ GNU General Public License v3, a BSD-Style license, or the
+ original HIDAPI license as outlined in the LICENSE.txt,
+ LICENSE-gpl3.txt, LICENSE-bsd.txt, and LICENSE-orig.txt
+ files located at the root of the source distribution.
+ These files may also be found in the public source
+ code repository located at:
+        http://github.com/signal11/hidapi .
+********************************************************/
+
+/* C */
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <locale.h>
+#include <errno.h>
+
+/* Unix */
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <sys/ioctl.h>
+#include <sys/utsname.h>
+#include <fcntl.h>
+#include <poll.h>
+
+/* Linux */
+#include <linux/hidraw.h>
+#include <linux/version.h>
+#include <linux/input.h>
+#include <libudev.h>
+
+#include "hidapi.h"
+
+/* Definitions from linux/hidraw.h. Since these are new, some distros
+   may not have header files which contain them. */
+#ifndef HIDIOCSFEATURE
+#define HIDIOCSFEATURE(len)    _IOC(_IOC_WRITE|_IOC_READ, 'H', 0x06, len)
+#endif
+#ifndef HIDIOCGFEATURE
+#define HIDIOCGFEATURE(len)    _IOC(_IOC_WRITE|_IOC_READ, 'H', 0x07, len)
+#endif
+
+
+/* USB HID device property names */
+const char *device_string_names[] = {
+	"manufacturer",
+	"product",
+	"serial",
+};
+
+/* Symbolic names for the properties above */
+enum device_string_id {
+	DEVICE_STRING_MANUFACTURER,
+	DEVICE_STRING_PRODUCT,
+	DEVICE_STRING_SERIAL,
+
+	DEVICE_STRING_COUNT,
+};
+
+struct hid_device_ {
+	int device_handle;
+	int blocking;
+	int uses_numbered_reports;
+};
+
+
+static __u32 kernel_version = 0;
+
+static __u32 detect_kernel_version(void)
+{
+	struct utsname name;
+	int major, minor, release;
+	int ret;
+
+	uname(&name);
+	ret = sscanf(name.release, "%d.%d.%d", &major, &minor, &release);
+	if (ret == 3) {
+		return KERNEL_VERSION(major, minor, release);
+	}
+
+	ret = sscanf(name.release, "%d.%d", &major, &minor);
+	if (ret == 2) {
+		return KERNEL_VERSION(major, minor, 0);
+	}
+
+	printf("Couldn't determine kernel version from version string \"%s\"\n", name.release);
+	return 0;
+}
+
+static hid_device *new_hid_device(void)
+{
+	hid_device *dev = calloc(1, sizeof(hid_device));
+	dev->device_handle = -1;
+	dev->blocking = 1;
+	dev->uses_numbered_reports = 0;
+
+	return dev;
+}
+
+
+/* The caller must free the returned string with free(). */
+static wchar_t *utf8_to_wchar_t(const char *utf8)
+{
+	wchar_t *ret = NULL;
+
+	if (utf8) {
+		size_t wlen = mbstowcs(NULL, utf8, 0);
+		if ((size_t) -1 == wlen) {
+			return wcsdup(L"");
+		}
+		ret = calloc(wlen+1, sizeof(wchar_t));
+		mbstowcs(ret, utf8, wlen+1);
+		ret[wlen] = 0x0000;
+	}
+
+	return ret;
+}
+
+/* Get an attribute value from a udev_device and return it as a whar_t
+   string. The returned string must be freed with free() when done.*/
+static wchar_t *copy_udev_string(struct udev_device *dev, const char *udev_name)
+{
+	return utf8_to_wchar_t(udev_device_get_sysattr_value(dev, udev_name));
+}
+
+/* uses_numbered_reports() returns 1 if report_descriptor describes a device
+   which contains numbered reports. */
+static int uses_numbered_reports(__u8 *report_descriptor, __u32 size) {
+	unsigned int i = 0;
+	int size_code;
+	int data_len, key_size;
+
+	while (i < size) {
+		int key = report_descriptor[i];
+
+		/* Check for the Report ID key */
+		if (key == 0x85/*Report ID*/) {
+			/* This device has a Report ID, which means it uses
+			   numbered reports. */
+			return 1;
+		}
+
+		//printf("key: %02hhx\n", key);
+
+		if ((key & 0xf0) == 0xf0) {
+			/* This is a Long Item. The next byte contains the
+			   length of the data section (value) for this key.
+			   See the HID specification, version 1.11, section
+			   6.2.2.3, titled "Long Items." */
+			if (i+1 < size)
+				data_len = report_descriptor[i+1];
+			else
+				data_len = 0; /* malformed report */
+			key_size = 3;
+		}
+		else {
+			/* This is a Short Item. The bottom two bits of the
+			   key contain the size code for the data section
+			   (value) for this key.  Refer to the HID
+			   specification, version 1.11, section 6.2.2.2,
+			   titled "Short Items." */
+			size_code = key & 0x3;
+			switch (size_code) {
+			case 0:
+			case 1:
+			case 2:
+				data_len = size_code;
+				break;
+			case 3:
+				data_len = 4;
+				break;
+			default:
+				/* Can't ever happen since size_code is & 0x3 */
+				data_len = 0;
+				break;
+			};
+			key_size = 1;
+		}
+
+		/* Skip over this key and it's associated data */
+		i += data_len + key_size;
+	}
+
+	/* Didn't find a Report ID key. Device doesn't use numbered reports. */
+	return 0;
+}
+
+/*
+ * The caller is responsible for free()ing the (newly-allocated) character
+ * strings pointed to by serial_number_utf8 and product_name_utf8 after use.
+ */
+static int
+parse_uevent_info(const char *uevent, int *bus_type,
+	unsigned short *vendor_id, unsigned short *product_id,
+	char **serial_number_utf8, char **product_name_utf8)
+{
+	char *tmp = strdup(uevent);
+	char *saveptr = NULL;
+	char *line;
+	char *key;
+	char *value;
+
+	int found_id = 0;
+	int found_serial = 0;
+	int found_name = 0;
+
+	line = strtok_r(tmp, "\n", &saveptr);
+	while (line != NULL) {
+		/* line: "KEY=value" */
+		key = line;
+		value = strchr(line, '=');
+		if (!value) {
+			goto next_line;
+		}
+		*value = '\0';
+		value++;
+
+		if (strcmp(key, "HID_ID") == 0) {
+			/**
+			 *        type vendor   product
+			 * HID_ID=0003:000005AC:00008242
+			 **/
+			int ret = sscanf(value, "%x:%hx:%hx", bus_type, vendor_id, product_id);
+			if (ret == 3) {
+				found_id = 1;
+			}
+		} else if (strcmp(key, "HID_NAME") == 0) {
+			/* The caller has to free the product name */
+			*product_name_utf8 = strdup(value);
+			found_name = 1;
+		} else if (strcmp(key, "HID_UNIQ") == 0) {
+			/* The caller has to free the serial number */
+			*serial_number_utf8 = strdup(value);
+			found_serial = 1;
+		}
+
+next_line:
+		line = strtok_r(NULL, "\n", &saveptr);
+	}
+
+	free(tmp);
+	return (found_id && found_name && found_serial);
+}
+
+
+static int get_device_string(hid_device *dev, enum device_string_id key, wchar_t *string, size_t maxlen)
+{
+	struct udev *udev;
+	struct udev_device *udev_dev, *parent, *hid_dev;
+	struct stat s;
+	int ret = -1;
+        char *serial_number_utf8 = NULL;
+        char *product_name_utf8 = NULL;
+
+	/* Create the udev object */
+	udev = udev_new();
+	if (!udev) {
+		printf("Can't create udev\n");
+		return -1;
+	}
+
+	/* Get the dev_t (major/minor numbers) from the file handle. */
+	ret = fstat(dev->device_handle, &s);
+	if (-1 == ret)
+		return ret;
+	/* Open a udev device from the dev_t. 'c' means character device. */
+	udev_dev = udev_device_new_from_devnum(udev, 'c', s.st_rdev);
+	if (udev_dev) {
+		hid_dev = udev_device_get_parent_with_subsystem_devtype(
+			udev_dev,
+			"hid",
+			NULL);
+		if (hid_dev) {
+			unsigned short dev_vid;
+			unsigned short dev_pid;
+			int bus_type;
+			size_t retm;
+
+			ret = parse_uevent_info(
+			           udev_device_get_sysattr_value(hid_dev, "uevent"),
+			           &bus_type,
+			           &dev_vid,
+			           &dev_pid,
+			           &serial_number_utf8,
+			           &product_name_utf8);
+
+			if (bus_type == BUS_BLUETOOTH) {
+				switch (key) {
+					case DEVICE_STRING_MANUFACTURER:
+						wcsncpy(string, L"", maxlen);
+						ret = 0;
+						break;
+					case DEVICE_STRING_PRODUCT:
+						retm = mbstowcs(string, product_name_utf8, maxlen);
+						ret = (retm == (size_t)-1)? -1: 0;
+						break;
+					case DEVICE_STRING_SERIAL:
+						retm = mbstowcs(string, serial_number_utf8, maxlen);
+						ret = (retm == (size_t)-1)? -1: 0;
+						break;
+					case DEVICE_STRING_COUNT:
+					default:
+						ret = -1;
+						break;
+				}
+			}
+			else {
+				/* This is a USB device. Find its parent USB Device node. */
+				parent = udev_device_get_parent_with_subsystem_devtype(
+					   udev_dev,
+					   "usb",
+					   "usb_device");
+				if (parent) {
+					const char *str;
+					const char *key_str = NULL;
+
+					if (key >= 0 && key < DEVICE_STRING_COUNT) {
+						key_str = device_string_names[key];
+					} else {
+						ret = -1;
+						goto end;
+					}
+
+					str = udev_device_get_sysattr_value(parent, key_str);
+					if (str) {
+						/* Convert the string from UTF-8 to wchar_t */
+						retm = mbstowcs(string, str, maxlen);
+						ret = (retm == (size_t)-1)? -1: 0;
+						goto end;
+					}
+				}
+			}
+		}
+	}
+
+end:
+        free(serial_number_utf8);
+        free(product_name_utf8);
+
+	udev_device_unref(udev_dev);
+	/* parent and hid_dev don't need to be (and can't be) unref'd.
+	   I'm not sure why, but they'll throw double-free() errors. */
+	udev_unref(udev);
+
+	return ret;
+}
+
+int HID_API_EXPORT hid_init(void)
+{
+	const char *locale;
+
+	/* Set the locale if it's not set. */
+	locale = setlocale(LC_CTYPE, NULL);
+	if (!locale)
+		setlocale(LC_CTYPE, "");
+
+	kernel_version = detect_kernel_version();
+
+	return 0;
+}
+
+int HID_API_EXPORT hid_exit(void)
+{
+	/* Nothing to do for this in the Linux/hidraw implementation. */
+	return 0;
+}
+
+
+struct hid_device_info  HID_API_EXPORT *hid_enumerate(unsigned short vendor_id, unsigned short product_id)
+{
+	struct udev *udev;
+	struct udev_enumerate *enumerate;
+	struct udev_list_entry *devices, *dev_list_entry;
+
+	struct hid_device_info *root = NULL; /* return object */
+	struct hid_device_info *cur_dev = NULL;
+	struct hid_device_info *prev_dev = NULL; /* previous device */
+
+	hid_init();
+
+	/* Create the udev object */
+	udev = udev_new();
+	if (!udev) {
+		printf("Can't create udev\n");
+		return NULL;
+	}
+
+	/* Create a list of the devices in the 'hidraw' subsystem. */
+	enumerate = udev_enumerate_new(udev);
+	udev_enumerate_add_match_subsystem(enumerate, "hidraw");
+	udev_enumerate_scan_devices(enumerate);
+	devices = udev_enumerate_get_list_entry(enumerate);
+	/* For each item, see if it matches the vid/pid, and if so
+	   create a udev_device record for it */
+	udev_list_entry_foreach(dev_list_entry, devices) {
+		const char *sysfs_path;
+		const char *dev_path;
+		const char *str;
+		struct udev_device *raw_dev; /* The device's hidraw udev node. */
+		struct udev_device *hid_dev; /* The device's HID udev node. */
+		struct udev_device *usb_dev; /* The device's USB udev node. */
+		struct udev_device *intf_dev; /* The device's interface (in the USB sense). */
+		unsigned short dev_vid;
+		unsigned short dev_pid;
+		char *serial_number_utf8 = NULL;
+		char *product_name_utf8 = NULL;
+		int bus_type;
+		int result;
+
+		/* Get the filename of the /sys entry for the device
+		   and create a udev_device object (dev) representing it */
+		sysfs_path = udev_list_entry_get_name(dev_list_entry);
+		raw_dev = udev_device_new_from_syspath(udev, sysfs_path);
+		dev_path = udev_device_get_devnode(raw_dev);
+
+		hid_dev = udev_device_get_parent_with_subsystem_devtype(
+			raw_dev,
+			"hid",
+			NULL);
+
+		if (!hid_dev) {
+			/* Unable to find parent hid device. */
+			goto next;
+		}
+
+		result = parse_uevent_info(
+			udev_device_get_sysattr_value(hid_dev, "uevent"),
+			&bus_type,
+			&dev_vid,
+			&dev_pid,
+			&serial_number_utf8,
+			&product_name_utf8);
+
+		if (!result) {
+			/* parse_uevent_info() failed for at least one field. */
+			goto next;
+		}
+
+		if (bus_type != BUS_USB && bus_type != BUS_BLUETOOTH) {
+			/* We only know how to handle USB and BT devices. */
+			goto next;
+		}
+
+		/* Check the VID/PID against the arguments */
+		if ((vendor_id == 0x0 || vendor_id == dev_vid) &&
+		    (product_id == 0x0 || product_id == dev_pid)) {
+			struct hid_device_info *tmp;
+
+			/* VID/PID match. Create the record. */
+			tmp = malloc(sizeof(struct hid_device_info));
+			if (cur_dev) {
+				cur_dev->next = tmp;
+			}
+			else {
+				root = tmp;
+			}
+			prev_dev = cur_dev;
+			cur_dev = tmp;
+
+			/* Fill out the record */
+			cur_dev->next = NULL;
+			cur_dev->path = dev_path? strdup(dev_path): NULL;
+
+			/* VID/PID */
+			cur_dev->vendor_id = dev_vid;
+			cur_dev->product_id = dev_pid;
+
+			/* Serial Number */
+			cur_dev->serial_number = utf8_to_wchar_t(serial_number_utf8);
+
+			/* Release Number */
+			cur_dev->release_number = 0x0;
+
+			/* Interface Number */
+			cur_dev->interface_number = -1;
+
+			switch (bus_type) {
+				case BUS_USB:
+					/* The device pointed to by raw_dev contains information about
+					   the hidraw device. In order to get information about the
+					   USB device, get the parent device with the
+					   subsystem/devtype pair of "usb"/"usb_device". This will
+					   be several levels up the tree, but the function will find
+					   it. */
+					usb_dev = udev_device_get_parent_with_subsystem_devtype(
+							raw_dev,
+							"usb",
+							"usb_device");
+
+					if (!usb_dev) {
+						/* Free this device */
+						free(cur_dev->serial_number);
+						free(cur_dev->path);
+						free(cur_dev);
+
+						/* Take it off the device list. */
+						if (prev_dev) {
+							prev_dev->next = NULL;
+							cur_dev = prev_dev;
+						}
+						else {
+							cur_dev = root = NULL;
+						}
+
+						goto next;
+					}
+
+					/* Manufacturer and Product strings */
+					cur_dev->manufacturer_string = copy_udev_string(usb_dev, device_string_names[DEVICE_STRING_MANUFACTURER]);
+					cur_dev->product_string = copy_udev_string(usb_dev, device_string_names[DEVICE_STRING_PRODUCT]);
+
+					/* Release Number */
+					str = udev_device_get_sysattr_value(usb_dev, "bcdDevice");
+					cur_dev->release_number = (str)? strtol(str, NULL, 16): 0x0;
+
+					/* Get a handle to the interface's udev node. */
+					intf_dev = udev_device_get_parent_with_subsystem_devtype(
+							raw_dev,
+							"usb",
+							"usb_interface");
+					if (intf_dev) {
+						str = udev_device_get_sysattr_value(intf_dev, "bInterfaceNumber");
+						cur_dev->interface_number = (str)? strtol(str, NULL, 16): -1;
+					}
+
+					break;
+
+				case BUS_BLUETOOTH:
+					/* Manufacturer and Product strings */
+					cur_dev->manufacturer_string = wcsdup(L"");
+					cur_dev->product_string = utf8_to_wchar_t(product_name_utf8);
+
+					break;
+
+				default:
+					/* Unknown device type - this should never happen, as we
+					 * check for USB and Bluetooth devices above */
+					break;
+			}
+		}
+
+	next:
+		free(serial_number_utf8);
+		free(product_name_utf8);
+		udev_device_unref(raw_dev);
+		/* hid_dev, usb_dev and intf_dev don't need to be (and can't be)
+		   unref()d.  It will cause a double-free() error.  I'm not
+		   sure why.  */
+	}
+	/* Free the enumerator and udev objects. */
+	udev_enumerate_unref(enumerate);
+	udev_unref(udev);
+
+	return root;
+}
+
+void  HID_API_EXPORT hid_free_enumeration(struct hid_device_info *devs)
+{
+	struct hid_device_info *d = devs;
+	while (d) {
+		struct hid_device_info *next = d->next;
+		free(d->path);
+		free(d->serial_number);
+		free(d->manufacturer_string);
+		free(d->product_string);
+		free(d);
+		d = next;
+	}
+}
+
+hid_device * hid_open(unsigned short vendor_id, unsigned short product_id, const wchar_t *serial_number)
+{
+	struct hid_device_info *devs, *cur_dev;
+	const char *path_to_open = NULL;
+	hid_device *handle = NULL;
+
+	devs = hid_enumerate(vendor_id, product_id);
+	cur_dev = devs;
+	while (cur_dev) {
+		if (cur_dev->vendor_id == vendor_id &&
+		    cur_dev->product_id == product_id) {
+			if (serial_number) {
+				if (wcscmp(serial_number, cur_dev->serial_number) == 0) {
+					path_to_open = cur_dev->path;
+					break;
+				}
+			}
+			else {
+				path_to_open = cur_dev->path;
+				break;
+			}
+		}
+		cur_dev = cur_dev->next;
+	}
+
+	if (path_to_open) {
+		/* Open the device */
+		handle = hid_open_path(path_to_open);
+	}
+
+	hid_free_enumeration(devs);
+
+	return handle;
+}
+
+hid_device * HID_API_EXPORT hid_open_path(const char *path)
+{
+	hid_device *dev = NULL;
+
+	hid_init();
+
+	dev = new_hid_device();
+
+	/* OPEN HERE */
+	dev->device_handle = open(path, O_RDWR);
+
+	/* If we have a good handle, return it. */
+	if (dev->device_handle > 0) {
+
+		/* Get the report descriptor */
+		int res, desc_size = 0;
+		struct hidraw_report_descriptor rpt_desc;
+
+		memset(&rpt_desc, 0x0, sizeof(rpt_desc));
+
+		/* Get Report Descriptor Size */
+		res = ioctl(dev->device_handle, HIDIOCGRDESCSIZE, &desc_size);
+		if (res < 0)
+			perror("HIDIOCGRDESCSIZE");
+
+
+		/* Get Report Descriptor */
+		rpt_desc.size = desc_size;
+		res = ioctl(dev->device_handle, HIDIOCGRDESC, &rpt_desc);
+		if (res < 0) {
+			perror("HIDIOCGRDESC");
+		} else {
+			/* Determine if this device uses numbered reports. */
+			dev->uses_numbered_reports =
+				uses_numbered_reports(rpt_desc.value,
+				                      rpt_desc.size);
+		}
+
+		return dev;
+	}
+	else {
+		/* Unable to open any devices. */
+		free(dev);
+		return NULL;
+	}
+}
+
+
+int HID_API_EXPORT hid_write(hid_device *dev, const unsigned char *data, size_t length)
+{
+	int bytes_written;
+
+	bytes_written = write(dev->device_handle, data, length);
+
+	return bytes_written;
+}
+
+
+int HID_API_EXPORT hid_read_timeout(hid_device *dev, unsigned char *data, size_t length, int milliseconds)
+{
+	int bytes_read;
+
+	if (milliseconds >= 0) {
+		/* Milliseconds is either 0 (non-blocking) or > 0 (contains
+		   a valid timeout). In both cases we want to call poll()
+		   and wait for data to arrive.  Don't rely on non-blocking
+		   operation (O_NONBLOCK) since some kernels don't seem to
+		   properly report device disconnection through read() when
+		   in non-blocking mode.  */
+		int ret;
+		struct pollfd fds;
+
+		fds.fd = dev->device_handle;
+		fds.events = POLLIN;
+		fds.revents = 0;
+		ret = poll(&fds, 1, milliseconds);
+		if (ret == -1 || ret == 0) {
+			/* Error or timeout */
+			return ret;
+		}
+		else {
+			/* Check for errors on the file descriptor. This will
+			   indicate a device disconnection. */
+			if (fds.revents & (POLLERR | POLLHUP | POLLNVAL))
+				return -1;
+		}
+	}
+
+	bytes_read = read(dev->device_handle, data, length);
+	if (bytes_read < 0 && (errno == EAGAIN || errno == EINPROGRESS))
+		bytes_read = 0;
+
+	if (bytes_read >= 0 &&
+	    kernel_version != 0 &&
+	    kernel_version < KERNEL_VERSION(2,6,34) &&
+	    dev->uses_numbered_reports) {
+		/* Work around a kernel bug. Chop off the first byte. */
+		memmove(data, data+1, bytes_read);
+		bytes_read--;
+	}
+
+	return bytes_read;
+}
+
+int HID_API_EXPORT hid_read(hid_device *dev, unsigned char *data, size_t length)
+{
+	return hid_read_timeout(dev, data, length, (dev->blocking)? -1: 0);
+}
+
+int HID_API_EXPORT hid_set_nonblocking(hid_device *dev, int nonblock)
+{
+	/* Do all non-blocking in userspace using poll(), since it looks
+	   like there's a bug in the kernel in some versions where
+	   read() will not return -1 on disconnection of the USB device */
+
+	dev->blocking = !nonblock;
+	return 0; /* Success */
+}
+
+
+int HID_API_EXPORT hid_send_feature_report(hid_device *dev, const unsigned char *data, size_t length)
+{
+	int res;
+
+	res = ioctl(dev->device_handle, HIDIOCSFEATURE(length), data);
+	if (res < 0)
+		perror("ioctl (SFEATURE)");
+
+	return res;
+}
+
+int HID_API_EXPORT hid_get_feature_report(hid_device *dev, unsigned char *data, size_t length)
+{
+	int res;
+
+	res = ioctl(dev->device_handle, HIDIOCGFEATURE(length), data);
+	if (res < 0)
+		perror("ioctl (GFEATURE)");
+
+
+	return res;
+}
+
+
+void HID_API_EXPORT hid_close(hid_device *dev)
+{
+	if (!dev)
+		return;
+	close(dev->device_handle);
+	free(dev);
+}
+
+
+int HID_API_EXPORT_CALL hid_get_manufacturer_string(hid_device *dev, wchar_t *string, size_t maxlen)
+{
+	return get_device_string(dev, DEVICE_STRING_MANUFACTURER, string, maxlen);
+}
+
+int HID_API_EXPORT_CALL hid_get_product_string(hid_device *dev, wchar_t *string, size_t maxlen)
+{
+	return get_device_string(dev, DEVICE_STRING_PRODUCT, string, maxlen);
+}
+
+int HID_API_EXPORT_CALL hid_get_serial_number_string(hid_device *dev, wchar_t *string, size_t maxlen)
+{
+	return get_device_string(dev, DEVICE_STRING_SERIAL, string, maxlen);
+}
+
+int HID_API_EXPORT_CALL hid_get_indexed_string(hid_device *dev, int string_index, wchar_t *string, size_t maxlen)
+{
+	return -1;
+}
+
+
+HID_API_EXPORT const wchar_t * HID_API_CALL  hid_error(hid_device *dev)
+{
+	return NULL;
+}
+
diff --git a/redist/json_helpers.c b/redist/json_helpers.c
index af7ddda..109708a 100644
--- a/redist/json_helpers.c
+++ b/redist/json_helpers.c
@@ -121,7 +121,7 @@ char* load_file_to_mem(const char* path) {
 	fseek( f, 0, SEEK_SET );
 	char * JSON_STRING = malloc( len + 1);
 	memset(JSON_STRING,0,len+1); 
-	int i = fread( JSON_STRING, len, 1, f ); i = i; //Ignore return value.
+	size_t i = fread( JSON_STRING, len, 1, f ); i = i; //Ignore return value.
 	fclose( f );
 	return JSON_STRING;
 }
@@ -193,7 +193,7 @@ void json_load_file(const char* path) {
 		} else if (value_t->type == JSMN_OBJECT) {
 			printf("Begin Object\n");
 			if (json_begin_object != NULL) json_begin_object(tag);
-			children = value_t->size +1; //+1 to account for this loop where we are not yed parsing children
+			children = (int16_t)(value_t->size +1); //+1 to account for this loop where we are not yed parsing children
 //			i += decode_jsmn_object(JSON_STRING, tokens+i+2,value_t->size);
 		}
 		else {
diff --git a/redist/linmath.c b/redist/linmath.c
index 5d51708..1d70ee1 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -143,6 +143,42 @@ void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest)
 }
 
 
+void axisanglefromquat(FLT *angle, FLT *axis, FLT *q)
+{
+	// this way might be fine, too.
+	//FLT dist = FLT_SQRT((q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3]));
+	//
+	//*angle = 2 * FLT_ATAN2(dist, q[0]);
+
+	//axis[0] = q[1] / dist;
+	//axis[1] = q[2] / dist;
+	//axis[2] = q[3] / dist;
+	
+
+	// Good mathematical foundation for this algorithm found here:
+	// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm
+
+	FLT tmp[4] = { q[0], q[1], q[2], q[3] };
+
+	quatnormalize(tmp, q);
+
+	if (FLT_FABS(q[0] - 1) < FLT_EPSILON)
+	{
+		// we have a degenerate case where we're rotating approx. 0 degrees
+		*angle = 0;
+		axis[0] = 1;
+		axis[1] = 0;
+		axis[2] = 0;
+		return;
+	}
+
+	axis[0] = tmp[1] / sqrt(1 - (tmp[0] * tmp[0]));
+	axis[1] = tmp[2] / sqrt(1 - (tmp[0] * tmp[0]));
+	axis[2] = tmp[3] / sqrt(1 - (tmp[0] * tmp[0]));
+
+	*angle = 2 * FLT_ACOS(tmp[0]);
+}
+
 /////////////////////////////////////QUATERNIONS//////////////////////////////////////////
 //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman)
 //Under the mit/X11 license.
diff --git a/redist/linmath.h b/redist/linmath.h
index 8d6cf05..1876b34 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -72,6 +72,8 @@ FLT anglebetween3d( FLT * a, FLT * b );
 
 void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle);
 void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest);
+void axisanglefromquat(FLT *angle, FLT *axis, FLT *quat);
+
 
 //Quaternion things...
 
diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c
index f7a7938..b7327d5 100644
--- a/src/ootx_decoder.c
+++ b/src/ootx_decoder.c
@@ -44,7 +44,7 @@ void ootx_free_decoder_context(ootx_decoder_context *ctx) {
 }
 
 uint8_t ootx_decode_bit(uint32_t length) {
-	uint8_t t = (length - 2750) / 500; //why 2750?
+	uint8_t t = (uint8_t)((length - 2750) / 500); //why 2750?
 //	return ((t & 0x02)>0)?0xFF:0x00; //easier if we need to bitshift right
 	return ((t & 0x02)>>1);
 }
@@ -182,8 +182,13 @@ union iFloat {
 	float f;
 };
 
+
+struct __attribute__((__packed__)) unaligned_u16_t {
+	uint16_t v;
+};
+
 float _half_to_float(uint8_t* data) {
-	uint16_t x = *(uint16_t*)data;
+	uint16_t x = ((struct unaligned_u16_t*)data)->v;
 	union iFloat fnum;
 	fnum.f = 0;
 
diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c
index 0d8674c..5805e1a 100644
--- a/src/poser_octavioradii.c
+++ b/src/poser_octavioradii.c
@@ -252,7 +252,7 @@ static void normalizeAndMultiplyVector(FLT *vectorToNormalize, size_t count, FLT
 }
 
 
-static RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile)
+static void RefineEstimateUsingGradientDescentRadii(FLT *estimateOut, SensorAngles *angles, FLT *initialEstimate, size_t numRadii, PointPair *pairs, size_t numPairs, FILE *logFile)
 {
 	int i = 0;
 	FLT lastMatchFitness = calculateFitness(angles, initialEstimate, pairs, numPairs);
@@ -521,9 +521,9 @@ static void QuickPose(SurviveObject *so)
 
 		if (sensorCount > 4)
 		{
-			FLT pos[3];
+			Point pos;
 			FLT orient[4];
-			SolveForLighthouseRadii(pos, orient, to);
+			SolveForLighthouseRadii(&pos, orient, to);
 		}
 
 		
@@ -591,7 +591,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd )
 				}
 				else
 				{
-					dd->hitCount[i][l->lh][axis] *= 0.5;
+					dd->hitCount[i][l->lh][axis] = (int)((double)dd->hitCount[i][l->lh][axis] * 0.5);
 				}
 			}
 			//if (0 == l->lh)
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 80e8d89..94d572e 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1,4 +1,5 @@
 #include <survive.h>
+#include "survive_config.h"
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -15,6 +16,8 @@
 #endif
 
 
+static int ttDebug = 0;
+
 #define PointToFlts(x) ((FLT*)(x))
 
 typedef struct
@@ -85,7 +88,8 @@ typedef struct
 	int lastAxis[NUM_LIGHTHOUSES];
 
 	Point lastLhPos[NUM_LIGHTHOUSES];
-	FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4];
+//	FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4];
+	FLT lastLhRotQuat[NUM_LIGHTHOUSES][4];
 } ToriData;
 
 
@@ -412,7 +416,7 @@ FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deu
 		}
 
 		fitnesses[i] = FLT_FABS(fitness);
-		if (deubgPrint)
+		if (0)
 		{
 			printf("  [%d, %d](%f)\n", pna[i].ai, pna[i].bi, fitness);
 		}
@@ -585,7 +589,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
 			break;
 		}
 	}
-	printf(" i=%3d ", i);
+	if (ttDebug) printf(" i=%3d ", i);
 
 	return lastPoint;
 }
@@ -772,6 +776,20 @@ FLT RotationEstimateFitnessAxisAngleOriginal(Point lhPoint, FLT *quaternion, Tra
 // just an x or y axis to make our estimate better.  TODO: bring that data to this fn.
 FLT RotationEstimateFitnessQuaternion(Point lhPoint, FLT *quaternion, TrackedObject *obj)
 {
+
+// TODO: ************************************************************************************************** THIS LIES!!!! NEED TO DO THIS IN QUATERNIONS!!!!!!!!!!!!!!!!!
+	{
+		FLT axisAngle[4];
+
+		axisanglefromquat(&(axisAngle[3]), axisAngle, quaternion);
+
+		FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj);
+
+		return throwaway;
+	}
+
+
+
 	FLT fitness = 0;
 	for (size_t i = 0; i < obj->numSensors; i++)
 	{
@@ -910,7 +928,7 @@ static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point l
 	
 	rotatearoundaxis(posOut, posOut, rotation, rotation[3]);
 
-	printf("{% 04.4f, % 04.4f, % 04.4f}  ", posOut[0], posOut[1], posOut[2]);
+	if (ttDebug) printf("{% 04.4f, % 04.4f, % 04.4f}  ", posOut[0], posOut[1], posOut[2]);
 }
 
 static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj)
@@ -1011,25 +1029,63 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini
 			break;
 		}
 	}
-	printf(" Ri=%d ", i);
+	if (ttDebug) printf(" Ri=%d ", i);
 }
-static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint)
+//static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint)
+//{
+//	FLT reverseRotation[4] = { rotation[0], rotation[1], rotation[2], -rotation[3] };
+//	FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z };
+//
+//	//rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]);
+//	quatrotatevector(objPoint, rotation, objPoint);
+//	if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]);
+//}
+static void WhereIsTheTrackedObjectQuaternion(FLT *posOut, FLT *rotation, Point lhPoint)
 {
-	FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]};
-	FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z};
-	
+	posOut[0] = -lhPoint.x;
+	posOut[1] = -lhPoint.y;
+	posOut[2] = -lhPoint.z;
+
+	FLT inverseRotation[4];
+
+	quatgetreciprocal(inverseRotation, rotation);
+
+	//FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z };
+
 	//rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]);
-	quatrotatevector(objPoint, rotation, objPoint);
-	printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]);
+	quatrotatevector(posOut, inverseRotation, posOut);
+//	if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]);
 }
 
+//static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point lhPoint)
+//{
+//	posOut[0] = -lhPoint.x;
+//	posOut[1] = -lhPoint.y;
+//	posOut[2] = -lhPoint.z;
+//
+//	rotatearoundaxis(posOut, posOut, rotation, rotation[3]);
+//
+//	if (ttDebug) printf("{% 04.4f, % 04.4f, % 04.4f}  ", posOut[0], posOut[1], posOut[2]);
+//}
 
 
 static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj)
 {
 	int i = 0;
+
 	FLT lastMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, initialEstimate, obj);
 
+	//{
+	//	FLT axisAngle[4];
+
+	//	axisanglefromquat(&(axisAngle[3]), axisAngle, initialEstimate);
+
+	//	FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj);
+
+	//	int a = throwaway;
+	//}
+
+
 	quatcopy(rotOut, initialEstimate);
 
 	// The values below are somewhat magic, and definitely tunable
@@ -1105,9 +1161,9 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in
 //#ifdef TORI_DEBUG
 			//printf("+  %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]);
 //#endif
-			g *= 1.02;
-			printf("+");
-			WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint);
+			g *= 1.04;
+			 //printf("+");
+			//WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint);
 		}
 		else
 		{
@@ -1115,12 +1171,13 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in
 			//printf("-         , %f\n", point4[3]);
 //#endif
 			g *= 0.7;
-			printf("-");
+			//printf("-");
+			//printf("%3f", lastMatchFitness);
 		}
 
 
 	}
-	printf("Ri=%3d  Fitness=%3f ", i, lastMatchFitness);
+	if (ttDebug) printf("Ri=%3d  Fitness=%3f ", i, lastMatchFitness);
 }
 
 
@@ -1129,9 +1186,9 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh)
 
 	// Step 1, create initial quaternion for guess.  
 	// This should have the lighthouse directly facing the tracked object.
-	Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z };
+	//Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z };
 	FLT theta = atan2(-lh.x, -lh.y);
-	FLT zAxis[4] = { 0, 0, 1 , theta-LINMATHPI/2};
+	FLT zAxis[4] = { 0, 0, 1 , theta - LINMATHPI / 2 };
 	FLT quat1[4];
 	quatfromaxisangle(quat1, zAxis, theta);
 
@@ -1143,6 +1200,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh)
 	RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj);
 
 
+	// TODO:  Need to use the quaternion version here!!!
 	//// Step 2, optimize the quaternion to match the data.
 	//RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj);
 
@@ -1150,6 +1208,32 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh)
 
 }
 
+void SolveForRotationQuat(FLT rotOut[4], TrackedObject *obj, Point lh)
+{
+
+	// Step 1, create initial quaternion for guess.  
+	// This should have the lighthouse directly facing the tracked object.
+	Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z };
+	FLT theta = atan2(-lh.x, -lh.y);
+	FLT zAxis[4] = { 0, 0, 1 , theta - LINMATHPI / 2 };
+	FLT quat1[4];
+	quatfromaxisangle(quat1, zAxis, theta);
+
+	//quatfrom2vectors(0,0)
+	// not correcting for phi, but that's less important.
+
+
+	// Step 2, optimize the axis/ angle to match the data.
+	//RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj);
+
+
+	//// Step 2, optimize the quaternion to match the data.
+	RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj);
+
+	//WhereIsTheTrackedObjectQuaternion(rotOut, lh);
+
+}
+
 
 static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput,const int lh,const int setLhCalibration)
 {
@@ -1280,21 +1364,29 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount, 0);
 
 	FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z));
-	printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance);
+	if (ttDebug) printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance);
 	//printf("Distance is %f,   Fitness is %f\n", distance, fitGd);
 
 	FLT rot[4]; // this is axis/ angle rotation, not a quaternion!
+	FLT rotQuat[4]; // this is a quaternion!
 
 	// if we've already guessed at the rotation of the lighthouse,
 	// then let's use that as a starting guess, because it's probably
 	// going to make convergence happen much faster.
-	if (toriData->lastLhRotAxisAngle[lh][0] != 0)
-	{
-		rot[0] = toriData->lastLhRotAxisAngle[lh][0];
-		rot[1] = toriData->lastLhRotAxisAngle[lh][1];
-		rot[2] = toriData->lastLhRotAxisAngle[lh][2];
-		rot[3] = toriData->lastLhRotAxisAngle[lh][3];
-	}
+	//if (toriData->lastLhRotAxisAngle[lh][0] != 0)
+	//{
+	//	rot[0] = toriData->lastLhRotAxisAngle[lh][0];
+	//	rot[1] = toriData->lastLhRotAxisAngle[lh][1];
+	//	rot[2] = toriData->lastLhRotAxisAngle[lh][2];
+	//	rot[3] = toriData->lastLhRotAxisAngle[lh][3];
+	//}
+	//if (toriData->lastLhRotQuat[lh][0] != 0)
+	//{
+	//	rotQuat[0] = toriData->lastLhRotQuat[lh][0];
+	//	rotQuat[1] = toriData->lastLhRotQuat[lh][1];
+	//	rotQuat[2] = toriData->lastLhRotQuat[lh][2];
+	//	rotQuat[3] = toriData->lastLhRotQuat[lh][3];
+	//}
 
 	// Given the relative position of the lighthouse
 	// to the tracked object, in the tracked object's coordinate
@@ -1302,22 +1394,30 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	// tracked object's coordinate system.
 	// TODO: I believe this could be radically improved
 	// using an SVD.  
+	SolveForRotationQuat(rotQuat, obj, refinedEstimateGd);
 	SolveForRotation(rot, obj, refinedEstimateGd);
 	FLT objPos[3];
+	//FLT objPos2[3];
 
-	{
-		toriData->lastLhRotAxisAngle[lh][0] = rot[0];
-		toriData->lastLhRotAxisAngle[lh][1] = rot[1];
-		toriData->lastLhRotAxisAngle[lh][2] = rot[2];
-		toriData->lastLhRotAxisAngle[lh][3] = rot[3];
-	}
+	//{
+	//	toriData->lastLhRotQuat[lh][0] = rotQuat[0];
+	//	toriData->lastLhRotQuat[lh][1] = rotQuat[1];
+	//	toriData->lastLhRotQuat[lh][2] = rotQuat[2];
+	//	toriData->lastLhRotQuat[lh][3] = rotQuat[3];
+	//}
 
-	WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd);
 
 
-	FLT rotQuat[4];
+	FLT rotQuat2[4];
+	FLT rot2[4];
 
-	quatfromaxisangle(rotQuat, rot, rot[3]);
+	quatfromaxisangle(rotQuat2, rot, rot[3]);
+	axisanglefromquat(&(rot2[3]), rot2, rotQuat);
+
+
+//	WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd); // this is the original axis angle one
+	WhereIsTheTrackedObjectAxisAngle(objPos, rot2, refinedEstimateGd); // this one is axis angle, but using data derived by quaternions.
+	//	WhereIsTheTrackedObjectQuaternion(objPos, rotQuat, refinedEstimateGd);  <--------------This is hte one we need to use, might need to be fixed.
 
 	//{
 		//FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z};
@@ -1405,7 +1505,16 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	so->FromLHPose[lh].Rot[2] = so->OutPose.Rot[2];
 	so->FromLHPose[lh].Rot[3] = so->OutPose.Rot[3];
 
-	printf(" <% 04.4f, % 04.4f, % 04.4f >  ", wcPos[0], wcPos[1], wcPos[2]);
+	if (ttDebug) printf(" <% 04.4f, % 04.4f, % 04.4f >  ", wcPos[0], wcPos[1], wcPos[2]);
+
+	posOut[0] = wcPos[0];
+	posOut[1] = wcPos[1];
+	posOut[2] = wcPos[2];
+
+	quatOut[0] = so->OutPose.Rot[0];
+	quatOut[1] = so->OutPose.Rot[1];
+	quatOut[2] = so->OutPose.Rot[2];
+	quatOut[3] = so->OutPose.Rot[3];
 
 	if (logFile)
 	{
@@ -1418,6 +1527,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	toriData->lastLhPos[lh].y = refinedEstimateGd.y;
 	toriData->lastLhPos[lh].z = refinedEstimateGd.z;
 
+
 	return refinedEstimateGd;
 }
 
@@ -1528,7 +1638,14 @@ static void QuickPose(SurviveObject *so, int lh)
 
 
 			SolveForLighthouse(pos, quat, to, so, 0, lh, 0);
-			printf("!\n");
+
+			//printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
+			if (so->ctx->rawposeproc)
+			{
+				so->ctx->rawposeproc(so, lh, pos, quat);
+			}
+
+			if (ttDebug) printf("!\n");
 		}
 
 		
@@ -1547,6 +1664,14 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 	SurviveContext * ctx = so->ctx;
 	ToriData * td = so->PoserData;
 
+	static int firstRun = 1;
+
+	if (firstRun)
+	{
+		ttDebug = config_read_uint32(ctx->global_config_values, "TurveyToriDebug", 0);
+
+		firstRun = 0;
+	}
 
 	if (!td)
 	{
@@ -1657,8 +1782,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 					FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] };
 					FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] };
 
-					//quatrotatevector(norm, downQuat, norm);
-					//quatrotatevector(point, downQuat, point);
+					quatrotatevector(norm, downQuat, norm);
+					quatrotatevector(point, downQuat, point);
 
 					//rotatearoundaxis(norm, norm, axis, angle);
 					//rotatearoundaxis(point, point, axis, angle);
@@ -1693,8 +1818,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 					FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] };
 					FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] };
 
-					//quatrotatevector(norm, downQuat, norm);
-					//quatrotatevector(point, downQuat, point);
+					quatrotatevector(norm, downQuat, norm);
+					quatrotatevector(point, downQuat, point);
 
 					//rotatearoundaxis(norm, norm, axis, angle);
 					//rotatearoundaxis(point, point, axis, angle);
@@ -1720,9 +1845,9 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 		}
 
 
-		// This code block rotates the lighthouse fixes to accound for any time the tracked object
+		// This code block rotates the lighthouse fixes to account for any time the tracked object
 		// is oriented other than +z = up
-		// This REALLY DOESN'T WORK!!!
+		 //This REALLY DOESN'T WORK!!!
 		//{
 		//	for (int lh = 0; lh < 2; lh++)
 		//	{
@@ -1732,6 +1857,22 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 		//	}
 		//}
 
+		for (int i=0; i < ctx->activeLighthouses; i++)
+		{ 
+			printf("Lighthouse Pose: [%1.1x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n",
+				i,
+				ctx->bsd[i].Pose.Pos[0],
+				ctx->bsd[i].Pose.Pos[1],
+				ctx->bsd[i].Pose.Pos[2],
+				ctx->bsd[i].Pose.Rot[0],
+				ctx->bsd[i].Pose.Rot[1],
+				ctx->bsd[i].Pose.Rot[2],
+				ctx->bsd[i].Pose.Rot[3]);
+		}
+		config_set_lighthouse(ctx->lh_config, &ctx->bsd[0], 0);
+		config_set_lighthouse(ctx->lh_config, &ctx->bsd[1], 1);
+
+		config_save(ctx, "config.json");
 
 		free(to);
 		//printf( "Full scene data.\n" );
diff --git a/src/survive.c b/src/survive.c
index f637bd8..76bf8e4 100755
--- a/src/survive.c
+++ b/src/survive.c
@@ -8,6 +8,7 @@
 #include <string.h>
 
 #include "survive_config.h"
+#include "os_generic.h"
 
 #ifdef __APPLE__
 #define z_const const
@@ -40,8 +41,69 @@ static void survivenote( struct SurviveContext * ctx, const char * fault )
 	fprintf( stderr, "Info: %s\n", fault );
 }
 
+static void *button_servicer(void * context)
+{
+	SurviveContext *ctx = (SurviveContext*)context;
+
+	while (1)
+	{
+		OGLockSema(ctx->buttonQueue.buttonservicesem);
+
+		if (ctx->isClosing)
+		{
+			// we're shutting down.  Close.
+			return NULL;
+		}
+
+		ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextReadIndex]);
+		if (entry->isPopulated == 0)
+		{
+			// should never happen.  indicates failure of code pushing stuff onto
+			// the buttonQueue
+			// if it does happen, it will kill all future button input
+			printf("ERROR: Unpopulated ButtonQueueEntry! NextReadIndex=%d\n", ctx->buttonQueue.nextReadIndex);
+			return NULL;
+		}
+
+		//printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n",
+		//	entry->eventType,
+		//	entry->buttonId,
+		//	entry->axis1Id,
+		//	entry->axis1Val,
+		//	entry->axis2Id,
+		//	entry->axis2Val);
+
+		button_process_func butt_func = ctx->buttonproc;
+		if (butt_func)
+		{
+			butt_func(entry->so,
+				entry->eventType,
+				entry->buttonId,
+				entry->axis1Id,
+				entry->axis1Val,
+				entry->axis2Id,
+				entry->axis2Val);
+		}
+
+		ctx->buttonQueue.nextReadIndex++;
+		if (ctx->buttonQueue.nextReadIndex >= BUTTON_QUEUE_MAX_LEN)
+		{
+			ctx->buttonQueue.nextReadIndex = 0;
+		}
+	};
+	return NULL;
+}
 
-SurviveContext * survive_init( int headless )
+void survive_verify_FLT_size(uint32_t user_size) {
+  if(sizeof(FLT) != user_size) {
+    fprintf(stderr, "FLT type incompatible; the shared library libsurvive has FLT size %lu vs user program %u\n", sizeof(FLT), user_size);
+    fprintf(stderr, "Add '#define FLT %s' before including survive.h or recompile the shared library with the appropriate flag. \n",
+	    sizeof(FLT) == sizeof(double) ? "double" : "float");
+    exit(-1);
+  }
+}
+
+SurviveContext * survive_init_internal( int headless )
 {
 #ifdef RUNTIME_SYMNUM
 	if( !did_runtime_symnum )
@@ -66,6 +128,8 @@ SurviveContext * survive_init( int headless )
 	int i = 0;
 	SurviveContext * ctx = calloc( 1, sizeof( SurviveContext ) );
 
+	ctx->isClosing = 0;
+
 	ctx->global_config_values = malloc( sizeof(config_group) );
 	ctx->lh_config = malloc( sizeof(config_group) * NUM_LIGHTHOUSES);
 
@@ -75,6 +139,10 @@ SurviveContext * survive_init( int headless )
 
 	config_read(ctx, "config.json");
 
+	ctx->activeLighthouses = config_read_uint32(ctx->global_config_values, "LighthouseCount", 2);
+	config_read_lighthouse(ctx->lh_config, &(ctx->bsd[0]), 0);
+	config_read_lighthouse(ctx->lh_config, &(ctx->bsd[1]), 1);
+
 	ctx->faultfunction = survivefault;
 	ctx->notefunction = survivenote;
 
@@ -92,7 +160,8 @@ SurviveContext * survive_init( int headless )
 	}
 
 	i = 0;
-	const char * PreferredPoser = config_read_str( ctx->global_config_values, "DefaultPoser", "PoserDummy" );
+	//const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserDummy");
+	const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserTurveyTori");
 	PoserCB PreferredPoserCB = 0;
 	const char * FirstPoser = 0;
 	printf( "Available posers:\n" );
@@ -115,6 +184,19 @@ SurviveContext * survive_init( int headless )
 		ctx->objs[i]->PoserFn = PreferredPoserCB;
 	}
 
+	// saving the config extra to make sure that the user has a config file they can change.
+	config_save(ctx, "config.json");
+
+	// initialize the button queue
+	memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue));
+
+	ctx->buttonQueue.buttonservicesem = OGCreateSema();	
+
+	// start the thread to process button data
+	ctx->buttonservicethread = OGCreateThread(button_servicer, ctx);
+	survive_install_button_fn(ctx, NULL);
+	survive_install_raw_pose_fn(ctx, NULL);
+
 	return ctx;
 }
 
@@ -159,6 +241,21 @@ void survive_install_angle_fn( SurviveContext * ctx,  angle_process_func fbp )
 		ctx->angleproc = survive_default_angle_process;
 }
 
+void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp)
+{
+	if (fbp)
+		ctx->buttonproc = fbp;
+	else
+		ctx->buttonproc = survive_default_button_process;
+}
+
+void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp)
+{
+	if (fbp)
+		ctx->rawposeproc = fbp;
+	else
+		ctx->rawposeproc = survive_default_raw_pose_process;
+}
 int survive_add_object( SurviveContext * ctx, SurviveObject * obj )
 {
 	int oldct = ctx->objs_ct;
@@ -193,10 +290,27 @@ int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int d
 	return 0;
 }
 
+int survive_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount)
+{
+	if (NULL == so || NULL == so->haptic)
+	{
+		return -404;
+	}
+
+	return so->haptic(so, reserved, pulseHigh, pulseLow, repeatCount);
+}
+
+
 void survive_close( SurviveContext * ctx )
 {
 	const char * DriverName;
 	int r = 0;
+
+	ctx->isClosing = 1;
+
+	// unlock/ post to button service semaphore so the thread can kill itself
+	OGUnlockSema(ctx->buttonQueue.buttonservicesem);
+
 	while( ( DriverName = GetDriverNameMatching( "DriverUnreg", r++ ) ) )
 	{
 		DeviceDriver dd = GetDriver( DriverName );
diff --git a/src/survive_cal.c b/src/survive_cal.c
index ae92bad..f3a682a 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -15,6 +15,7 @@
 #include <sys/stat.h>
 #include <sys/types.h>
 #include <linmath.h>
+#include <assert.h>
 
 #include "survive_config.h"
 
@@ -124,8 +125,11 @@ void survive_cal_install( struct SurviveContext * ctx )
 
 	cd->numPoseObjects = 0;
 
-	const char * RequiredTrackersForCal = config_read_str( ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1" );
-	const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 0 );
+	// setting the required trackers for calibration to be permissive to make it easier for a newbie to start-- 
+	// basically, libsurvive will detect whatever they have plugged in and start using that.  
+//	const char * RequiredTrackersForCal = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1");	
+	const char * RequiredTrackersForCal = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", ""); 
+	const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 1 );
 	size_t requiredTrackersFound = 0;
 
 	for (int j=0; j < ctx->objs_ct; j++)
@@ -169,7 +173,8 @@ void survive_cal_install( struct SurviveContext * ctx )
 	}
 
 	const char * DriverName;
-	const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow" );
+//	const char * PreferredPoser = config_read_str(ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow");
+	const char * PreferredPoser = config_read_str(ctx->global_config_values, "ConfigPoser", "PoserTurveyTori");
 	PoserCB PreferredPoserCB = 0;
 	const char * FirstPoser = 0;
 	printf( "Available posers:\n" );
@@ -218,7 +223,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 			int i;
 			for( i = 0; i < NUM_LIGHTHOUSES; i++ )
 				if( ctx->bsd[i].OOTXSet == 0 ) break;
-			if( i == NUM_LIGHTHOUSES ) cd->stage = 2;  //TODO: Make this configuratble to allow single lighthouse.
+			if( i == ctx->activeLighthouses ) cd->stage = 2;  //TODO: Make this configuratble to allow single lighthouse.
 		}
 		break;
 	case 3: //Look for light sync lengths.
@@ -309,7 +314,7 @@ void survive_cal_angle( struct SurviveObject * so, int sensor_id, int acode, uin
 			cd->found_common = 1;
 			for( i = 0; i < cd->numPoseObjects; i++ )
 			//for( i = 0; i < MAX_SENSORS_TO_CAL/SENSORS_PER_OBJECT; i++ )
-			for( j = 0; j < NUM_LIGHTHOUSES; j++ )
+			for( j = 0; j < ctx->activeLighthouses; j++ )
 			{
 				int sensors_visible = 0;
 				for( k = 0; k < SENSORS_PER_OBJECT; k++ )
diff --git a/src/survive_cal.h b/src/survive_cal.h
index ae644d1..f5ef6dc 100644
--- a/src/survive_cal.h
+++ b/src/survive_cal.h
@@ -51,14 +51,14 @@ struct SurviveCalData
 	ootx_decoder_context ootx_decoders[NUM_LIGHTHOUSES];
 
 	//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];
+	FLT all_lengths[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS + 1];
+	FLT all_angles[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2][DRPTS + 1];
 	int16_t all_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][2];
 	int16_t peak_counts;
 	int8_t found_common;
 	int8_t times_found_common;
 
-	FLT all_sync_times[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][DRPTS];
+	FLT all_sync_times[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES][DRPTS + 1];
 	int16_t all_sync_counts[MAX_SENSORS_TO_CAL][NUM_LIGHTHOUSES];
 
 	//For camfind (4+)
diff --git a/src/survive_config.c b/src/survive_config.c
index 3a83902..46c6658 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -86,6 +86,42 @@ void config_init() {
 }
 */
 
+void config_read_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8_t idx) {
+	config_group *cg = lh_config + idx;
+	uint8_t found = 0;
+	for (int i = 0; i < NUM_LIGHTHOUSES; i++)
+	{
+		uint32_t tmpIdx = 0xffffffff;
+		cg = lh_config + idx;
+
+		tmpIdx = config_read_uint32(cg, "index", 0xffffffff);
+
+		if (tmpIdx == idx && i == idx) // assumes that lighthouses are stored in the config in order.
+		{
+			found = 1;
+			break;
+		}
+	}
+
+//	assert(found); // throw an assertion if we didn't find it...  Is this good?  not necessarily?
+	if (!found)
+	{
+		return;
+	}
+
+
+	FLT defaults[7] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
+
+	bsd->BaseStationID = config_read_uint32(cg, "id", 0);
+	config_read_float_array(cg, "pose", &bsd->Pose.Pos[0], defaults, 7);
+	config_read_float_array(cg, "fcalphase", bsd->fcalphase, defaults, 2);
+	config_read_float_array(cg, "fcaltilt", bsd->fcaltilt, defaults, 2);
+	config_read_float_array(cg, "fcalcurve", bsd->fcalcurve, defaults, 2);
+	config_read_float_array(cg, "fcalgibpha", bsd->fcalgibpha, defaults, 2);
+	config_read_float_array(cg, "fcalgibmag", bsd->fcalgibmag, defaults, 2);
+}
+
+
 void config_set_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8_t idx) {
 	config_group *cg = lh_config+idx;
 	config_set_uint32(cg,"index", idx);
@@ -143,18 +179,28 @@ FLT config_read_float(config_group *cg, const char *tag, const FLT def) {
 	return config_set_float(cg, tag, def);
 }
 
-uint16_t config_read_float_array(config_group *cg, const char *tag, const FLT** values, const FLT* def, uint8_t count) {
+// TODO: Do something better than this:
+#define CFG_MIN(x,y) ((x) < (y)? (x): (y))
+
+
+uint16_t config_read_float_array(config_group *cg, const char *tag, FLT* values, const FLT* def, uint8_t count) {
 	config_entry *cv = find_config_entry(cg, tag);
 
 	if (cv != NULL) {
-		*values = (FLT*)cv->data;
+		for (unsigned int i=0; i < CFG_MIN(count, cv->elements); i++)
+		{
+			values[i] = ((double*)cv->data)[i];
+		}
 		return cv->elements;
 	}
 
 	if (def == NULL) return 0;
 
 	config_set_float_a(cg, tag, def, count);
-	*values = def;
+	for (int i = 0; i < count; i++)
+	{
+		values[i] = def[i];
+	}
 	return count;
 }
 
diff --git a/src/survive_config.h b/src/survive_config.h
index 83db624..e2686e9 100644
--- a/src/survive_config.h
+++ b/src/survive_config.h
@@ -43,6 +43,7 @@ void destroy_config_group(config_group* cg);
 
 //void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length);
 void config_set_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8_t idx);
+void config_read_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8_t idx);
 
 void config_read(SurviveContext* sctx, const char* path);
 void config_save(SurviveContext* sctx, const char* path);
@@ -52,7 +53,7 @@ const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32
 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);
-uint16_t config_read_float_array(config_group *cg, const char *tag, const FLT** values, const FLT* def, uint8_t count);
+uint16_t config_read_float_array(config_group *cg, const char *tag, FLT* values, const FLT* def, uint8_t count);
 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);
 
diff --git a/src/survive_data.c b/src/survive_data.c
index 1b80269..0427659 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -419,8 +419,8 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le )
 	}
 
 	if (lcd->per_sweep.activeLighthouse < 0) {
-		fprintf(stderr, "WARNING: No active lighthouse!\n");
-		fprintf(stderr, "            %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]);
+		//fprintf(stderr, "WARNING: No active lighthouse!\n");
+		//fprintf(stderr, "            %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]);
 		return;
 	}
 
diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c
new file mode 100644
index 0000000..6615f1e
--- /dev/null
+++ b/src/survive_default_devices.c
@@ -0,0 +1,199 @@
+#include "survive_default_devices.h"
+#include <jsmn.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "json_helpers.h"
+
+static SurviveObject *
+survive_create_device(SurviveContext *ctx, const char *driver_name,
+					  void *driver, const char *device_name, haptic_func fn) {
+	SurviveObject *device = calloc(1, sizeof(SurviveObject));
+
+	device->ctx = ctx;
+	device->driver = driver;
+	memcpy(device->codename, device_name, strlen(device_name));
+	memcpy(device->drivername, driver_name, strlen(driver_name));
+
+	device->timebase_hz = 48000000;
+	device->pulsedist_max_ticks = 500000;
+	device->pulselength_min_sync = 2200;
+	device->pulse_in_clear_time = 35000;
+	device->pulse_max_for_sweep = 1800;
+	device->pulse_synctime_offset = 20000;
+	device->pulse_synctime_slack = 5000;
+	device->timecenter_ticks = device->timebase_hz / 240;
+
+	device->haptic = fn;
+
+	return device;
+}
+
+SurviveObject *survive_create_hmd(SurviveContext *ctx, const char *driver_name,
+								  void *driver) {
+	return survive_create_device(ctx, driver_name, driver, "HMD", 0);
+}
+
+SurviveObject *survive_create_wm0(SurviveContext *ctx, const char *driver_name,
+								  void *driver, haptic_func fn) {
+	return survive_create_device(ctx, driver_name, driver, "WM0", fn);
+}
+SurviveObject *survive_create_wm1(SurviveContext *ctx, const char *driver_name,
+								  void *driver, haptic_func fn) {
+	return survive_create_device(ctx, driver_name, driver, "WM1", fn);
+}
+SurviveObject *survive_create_tr0(SurviveContext *ctx, const char *driver_name,
+								  void *driver) {
+	return survive_create_device(ctx, driver_name, driver, "TR0", 0);
+}
+SurviveObject *survive_create_ww0(SurviveContext *ctx, const char *driver_name,
+								  void *driver) {
+	return survive_create_device(ctx, driver_name, driver, "WW0", 0);
+}
+
+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(SurviveContext *ctx, 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];
+
+		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;
+}
+
+int survive_load_htc_config_format(char *ct0conf, int len, SurviveObject *so) {
+	if (len == 0)
+		return -1;
+
+	SurviveContext *ctx = so->ctx;
+	// 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;
+			}
+		}
+
+		if (jsoneq(ct0conf, tk, "acc_bias") == 0) {
+			int32_t count = (tk + 1)->size;
+			FLT *values = NULL;
+			if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) {
+				so->acc_bias = values;
+				so->acc_bias[0] *= .125; // XXX Wat?  Observed by CNL.  Biasing
+										 // by more than this seems to hose
+										 // things.
+				so->acc_bias[1] *= .125;
+				so->acc_bias[2] *= .125;
+			}
+		}
+		if (jsoneq(ct0conf, tk, "acc_scale") == 0) {
+			int32_t count = (tk + 1)->size;
+			FLT *values = NULL;
+			if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) {
+				so->acc_scale = values;
+			}
+		}
+
+		if (jsoneq(ct0conf, tk, "gyro_bias") == 0) {
+			int32_t count = (tk + 1)->size;
+			FLT *values = NULL;
+			if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) {
+				so->gyro_bias = values;
+			}
+		}
+		if (jsoneq(ct0conf, tk, "gyro_scale") == 0) {
+			int32_t count = (tk + 1)->size;
+			FLT *values = NULL;
+			if (parse_float_array(ct0conf, tk + 2, &values, count) > 0) {
+				so->gyro_scale = values;
+			}
+		}
+	}
+
+	char fname[64];
+
+	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;
+}
diff --git a/src/survive_default_devices.h b/src/survive_default_devices.h
new file mode 100644
index 0000000..1fcca72
--- /dev/null
+++ b/src/survive_default_devices.h
@@ -0,0 +1,19 @@
+#ifndef _SURVIVE_DEFAULT_DEVICES_H
+#define _SURVIVE_DEFAULT_DEVICES_H
+
+#include <survive.h>
+
+SurviveObject *survive_create_hmd(SurviveContext *ctx, const char *driver_name,
+								  void *driver);
+SurviveObject *survive_create_wm0(SurviveContext *ctx, const char *driver_name,
+								  void *driver, haptic_func cb);
+SurviveObject *survive_create_wm1(SurviveContext *ctx, const char *driver_name,
+								  void *driver, haptic_func cb);
+SurviveObject *survive_create_tr0(SurviveContext *ctx, const char *driver_name,
+								  void *driver);
+SurviveObject *survive_create_ww0(SurviveContext *ctx, const char *driver_name,
+								  void *driver);
+
+int survive_load_htc_config_format(char *ct0conf, int length,
+								   SurviveObject *so);
+#endif
diff --git a/src/survive_internal.h b/src/survive_internal.h
index e1a733d..86b119f 100644
--- a/src/survive_internal.h
+++ b/src/survive_internal.h
@@ -17,6 +17,7 @@ void * GetDriver( const char * name );
 const char * GetDriverNameMatching( const char * prefix, int place );
 void   ListDrivers();
 
+
 #endif
 
 
diff --git a/src/survive_playback.c b/src/survive_playback.c
new file mode 100644
index 0000000..4b25b4c
--- /dev/null
+++ b/src/survive_playback.c
@@ -0,0 +1,242 @@
+// All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL
+// or LGPL licenses.
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <survive.h>
+
+#include <string.h>
+#include <sys/time.h>
+
+#include "survive_config.h"
+#include "survive_default_devices.h"
+
+#include "redist/os_generic.h"
+
+struct SurvivePlaybackData {
+	SurviveContext *ctx;
+	const char *playback_dir;
+	FILE *playback_file;
+	int lineno;
+
+	FLT time_factor;
+	double next_time_us;
+};
+typedef struct SurvivePlaybackData SurvivePlaybackData;
+
+double timestamp_in_us() {
+	static double start_time_us = 0;
+	if (start_time_us == 0.)
+		start_time_us = OGGetAbsoluteTime();
+	return OGGetAbsoluteTime() - start_time_us;
+}
+
+static int parse_and_run_imu(const char *line, SurvivePlaybackData *driver) {
+	char dev[10];
+	int timecode = 0;
+	FLT accelgyro[6];
+	int mask;
+	int id;
+
+	int rr =
+		sscanf(line, "I %s %d %d " FLT_format " " FLT_format " " FLT_format
+					 " " FLT_format " " FLT_format " " FLT_format "%d",
+			   dev, &mask, &timecode, &accelgyro[0], &accelgyro[1],
+			   &accelgyro[2], &accelgyro[3], &accelgyro[4], &accelgyro[5], &id);
+
+	if (rr != 10) {
+		fprintf(stderr, "Warning:  On line %d, only %d values read: '%s'\n",
+				driver->lineno, rr, line);
+		return -1;
+	}
+
+	SurviveObject *so = survive_get_so_by_name(driver->ctx, dev);
+	if (!so) {
+		fprintf(stderr, "Could not find device named %s from lineno %d\n", dev,
+				driver->lineno);
+		return -1;
+	}
+
+	driver->ctx->imuproc(so, mask, accelgyro, timecode, id);
+	return 0;
+}
+
+static int parse_and_run_lightcode(const char *line,
+								   SurvivePlaybackData *driver) {
+	char lhn[10];
+	char axn[10];
+	char dev[10];
+	uint32_t timecode = 0;
+	int sensor = 0;
+	int acode = 0;
+	int timeinsweep = 0;
+	uint32_t pulselength = 0;
+	uint32_t lh = 0;
+
+	int rr =
+		sscanf(line, "%8s %8s %8s %u %d %d %d %u %u\n", lhn, axn, dev,
+			   &timecode, &sensor, &acode, &timeinsweep, &pulselength, &lh);
+
+	if (rr != 9) {
+		fprintf(stderr, "Warning:  On line %d, only %d values read: '%s'\n",
+				driver->lineno, rr, line);
+		return -1;
+	}
+
+	SurviveObject *so = survive_get_so_by_name(driver->ctx, dev);
+	if (!so) {
+		fprintf(stderr, "Could not find device named %s from lineno %d\n", dev,
+				driver->lineno);
+		return -1;
+	}
+
+	driver->ctx->lightproc(so, sensor, acode, timeinsweep, timecode,
+						   pulselength, lh);
+	return 0;
+}
+
+static int playback_poll(struct SurviveContext *ctx, void *_driver) {
+	SurvivePlaybackData *driver = _driver;
+	FILE *f = driver->playback_file;
+
+	if (f && !feof(f) && !ferror(f)) {
+		int i;
+		driver->lineno++;
+		char *line;
+
+		if (driver->next_time_us == 0) {
+			char *buffer;
+			size_t n = 0;
+			ssize_t r = getdelim(&line, &n, ' ', f);
+			if (r <= 0)
+				return 0;
+
+			if (sscanf(line, "%lf", &driver->next_time_us) != 1) {
+				free(line);
+				return 0;
+			}
+			free(line);
+			line = 0;
+		}
+
+		if (driver->next_time_us * driver->time_factor > timestamp_in_us())
+			return 0;
+		driver->next_time_us = 0;
+
+		char *buffer;
+		size_t n = 0;
+		ssize_t r = getline(&line, &n, f);
+		if (r <= 0)
+			return 0;
+
+		if ((line[0] != 'R' && line[0] != 'L' && line[0] != 'I') ||
+			line[1] != ' ')
+			return 0;
+
+		switch (line[0]) {
+		case 'L':
+		case 'R':
+			parse_and_run_lightcode(line, driver);
+			break;
+		case 'I':
+			parse_and_run_imu(line, driver);
+			break;
+		}
+
+		free(line);
+	} else {
+		if (f) {
+			fclose(driver->playback_file);
+		}
+		driver->playback_file = 0;
+		return -1;
+	}
+
+	return 0;
+}
+
+static int playback_close(struct SurviveContext *ctx, void *_driver) {
+	SurvivePlaybackData *driver = _driver;
+	if (driver->playback_file)
+		fclose(driver->playback_file);
+	driver->playback_file = 0;
+	return 0;
+}
+
+static int LoadConfig(SurvivePlaybackData *sv, SurviveObject *so) {
+	SurviveContext *ctx = sv->ctx;
+	char *ct0conf = 0;
+
+	char fname[100];
+	sprintf(fname, "%s/%s_config.json", sv->playback_dir, so->codename);
+	FILE *f = fopen(fname, "r");
+
+	if (f == 0 || feof(f) || ferror(f))
+		return 1;
+
+	fseek(f, 0, SEEK_END);
+	int len = ftell(f);
+	fseek(f, 0, SEEK_SET); // same as rewind(f);
+
+	ct0conf = malloc(len + 1);
+	int read = fread(ct0conf, len, 1, f);
+	fclose(f);
+	ct0conf[len] = 0;
+
+	printf("Loading config: %d\n", len);
+	return survive_load_htc_config_format(ct0conf, len, so);
+}
+
+int DriverRegPlayback(SurviveContext *ctx) {
+	const char *playback_dir =
+		config_read_str(ctx->global_config_values, "PlaybackDir", "");
+
+	if (strlen(playback_dir) == 0) {
+		return 0;
+	}
+
+	SurvivePlaybackData *sp = calloc(1, sizeof(SurvivePlaybackData));
+	sp->ctx = ctx;
+	sp->playback_dir = playback_dir;
+	sp->time_factor =
+		config_read_float(ctx->global_config_values, "PlaybackFactor", 1.);
+
+	printf("%s\n", playback_dir);
+
+	char playback_file[100];
+	sprintf(playback_file, "%s/events", playback_dir);
+	sp->playback_file = fopen(playback_file, "r");
+	if (sp->playback_file == 0) {
+		fprintf(stderr, "Could not open playback events file %s",
+				playback_file);
+		return -1;
+	}
+	SurviveObject *hmd = survive_create_hmd(ctx, "Playback", sp);
+	SurviveObject *wm0 = survive_create_wm0(ctx, "Playback", sp, 0);
+	SurviveObject *wm1 = survive_create_wm1(ctx, "Playback", sp, 0);
+	SurviveObject *tr0 = survive_create_tr0(ctx, "Playback", sp);
+	SurviveObject *ww0 = survive_create_ww0(ctx, "Playback", sp);
+
+	if (!LoadConfig(sp, hmd)) {
+		survive_add_object(ctx, hmd);
+	}
+	if (!LoadConfig(sp, wm0)) {
+		survive_add_object(ctx, wm0);
+	}
+	if (!LoadConfig(sp, wm1)) {
+		survive_add_object(ctx, wm1);
+	}
+	if (!LoadConfig(sp, tr0)) {
+		survive_add_object(ctx, tr0);
+	}
+	if (!LoadConfig(sp, ww0)) {
+		survive_add_object(ctx, ww0);
+	}
+
+	survive_add_driver(ctx, sp, playback_poll, playback_close, 0);
+	return 0;
+fail_gracefully:
+	return -1;
+}
+
+REGISTER_LINKTIME(DriverRegPlayback);
diff --git a/src/survive_process.c b/src/survive_process.c
index 3af2da9..0f19007 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -63,6 +63,50 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode
 	}
 }	
 
+void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val)
+{
+	// do nothing.
+	//printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n",
+	//	eventType,
+	//	buttonId,
+	//	axis1Id,
+	//	axis1Val,
+	//	axis2Id,
+	//	axis2Val);
+	//if (buttonId == 24 && eventType == 1) // trigger engage
+	//{
+	//	for (int j = 0; j < 6; j++)
+	//	{
+	//		for (int i = 0; i < 0x5; i++)
+	//		{
+	//			survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
+	//			//survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
+	//			OGUSleep(1000);
+	//		}
+	//		OGUSleep(20000);
+	//	}
+	//}
+	//if (buttonId == 2 && eventType == 1) // trigger engage
+	//{
+	//	for (int j = 0; j < 6; j++)
+	//	{
+	//		for (int i = 0; i < 0x1; i++)
+	//		{
+	//			survive_haptic(so, 0, 0xf401, 0x05a2, 0xf100);
+	//			//survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
+	//			OGUSleep(5000);
+	//		}
+	//		OGUSleep(20000);
+	//	}
+	//}
+}
+
+void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat)
+{
+	// print the pose;
+	//printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
+
+}
 
 void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id )
 {
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 9a3cb03..b3d990a 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -23,6 +23,8 @@
 #endif
 
 #include "json_helpers.h"
+#include "survive_default_devices.h"
+#include "survive_config.h"
 
 #ifdef HIDAPI
 #if defined(WINDOWS) || defined(WIN32) || defined (_WIN32)
@@ -51,6 +53,11 @@ const short vidpids[] = {
 	0x28de, 0x2000, 1, //Valve HMD lighthouse(B) (only used on HIDAPI, for lightcap)
 	0x28de, 0x2022, 1, //HTC Tracker (only used on HIDAPI, for lightcap)
 	0x28de, 0x2012, 1, //Valve Watchman, USB connected (only used on HIDAPI, for lightcap)
+
+	0x28de, 0x2000, 2, //Valve HMD lighthouse(B) (only used on HIDAPI, for lightcap)
+	0x28de, 0x2022, 2, //HTC Tracker (only used on HIDAPI, for lightcap)
+	0x28de, 0x2012, 2, //Valve Watchman, USB connected (only used on HIDAPI, for lightcap)
+
 #endif
 }; //length MAX_USB_INTERFACES*2
 
@@ -65,6 +72,10 @@ const char * devnames[] = {
 	"HMD Lightcap",
 	"Tracker 0 Lightcap",
 	"Wired Watchman 1 Lightcap",
+
+	"HMD Buttons",
+	"Tracker 0 Buttons",
+	"Wired Watchman 1 Buttons",
 #endif
 }; //length MAX_USB_INTERFACES
 
@@ -80,7 +91,12 @@ const char * devnames[] = {
 #define USB_DEV_HMD_IMU_LHB 6
 #define USB_DEV_TRACKER0_LIGHTCAP 7
 #define USB_DEV_W_WATCHMAN1_LIGHTCAP 8
-#define MAX_USB_DEVS		9
+
+#define USB_DEV_HMD_BUTTONS 9
+#define USB_DEV_TRACKER0_BUTTONS 10
+#define USB_DEV_W_WATCHMAN1_BUTTONS 11
+
+#define MAX_USB_DEVS		12
 #else
 #define MAX_USB_DEVS		6
 #endif
@@ -94,7 +110,10 @@ const char * devnames[] = {
 #define USB_IF_LIGHTCAP		6
 #define USB_IF_TRACKER0_LIGHTCAP		7
 #define USB_IF_W_WATCHMAN1_LIGHTCAP		8
-#define MAX_INTERFACES		9
+#define USB_IF_HMD_BUTTONS		9
+#define USB_IF_TRACKER0_BUTTONS		10
+#define USB_IF_W_WATCHMAN1_BUTTONS		11
+#define MAX_INTERFACES		12
 
 typedef struct SurviveUSBInterface SurviveUSBInterface;
 typedef struct SurviveViveData SurviveViveData;
@@ -292,8 +311,8 @@ static inline int getupdate_feature_report(libusb_device_handle* dev, uint16_t i
 	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;
+	if (ret < 0)
+		return -1;
 	return ret;
 }
 
@@ -427,7 +446,6 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
 
 		if( d == 0 )
 		{
-			printf( "!!%p  %d %04x %04x %d\n", devnames[i], i, vid, pid, which );
 			SV_INFO( "Did not find device %s (%04x:%04x.%d)", devnames[i], vid, pid, which );
 			sv->udev[i] = 0;
 			continue;
@@ -487,10 +505,16 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
 	// This is a HACK!  But it works.  Need to investigate further
 	sv->uiface[USB_DEV_TRACKER0_LIGHTCAP].actual_len = 64;
 	if( sv->udev[USB_DEV_TRACKER0_LIGHTCAP] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0_LIGHTCAP], 0x82, survive_data_cb, "Tracker 1 Lightcap")) { return -13; }
+
 	if( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; }
+
+
+	if (sv->udev[USB_DEV_TRACKER0_BUTTONS] && AttachInterface(sv, tr0, USB_IF_TRACKER0_BUTTONS, sv->udev[USB_DEV_TRACKER0_BUTTONS], 0x83, survive_data_cb, "Tracker 1 Buttons")) { return -13; }
+	if (sv->udev[USB_DEV_W_WATCHMAN1_BUTTONS] && AttachInterface(sv, ww0, USB_IF_W_WATCHMAN1_BUTTONS, sv->udev[USB_DEV_W_WATCHMAN1_BUTTONS], 0x83, survive_data_cb, "Wired Watchman 1 BUTTONS")) { return -13; }
+
 #else
 	if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_LIGHTCAP, sv->udev[USB_DEV_HMD_IMU_LH], 0x82, survive_data_cb, "Lightcap")) { return -12; }
-	if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, ww0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0], 0x82, survive_data_cb, "Tracker 0 Lightcap")) { return -13; }
+	if( sv->udev[USB_DEV_TRACKER0] && AttachInterface( sv, tr0, USB_IF_TRACKER0_LIGHTCAP, sv->udev[USB_DEV_TRACKER0], 0x82, survive_data_cb, "Tracker 0 Lightcap")) { return -13; }
 	if( sv->udev[USB_DEV_W_WATCHMAN1] && AttachInterface( sv, ww0, USB_IF_W_WATCHMAN1_LIGHTCAP, sv->udev[USB_DEV_W_WATCHMAN1], 0x82, survive_data_cb, "Wired Watchman 1 Lightcap")) { return -13; }
 #endif
 	SV_INFO( "All enumerated devices attached." );
@@ -596,18 +620,38 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
 
 //#endif
 
-#if 0
-		for( int i = 0; i < 256; i++ )
+#if 0		
+		for (int j=0; j < 40; j++)
 		{
-			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 ) );
-			r = update_feature_report( sv->udev[USB_DEV_W_WATCHMAN1_LIGHTCAP], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
-			SV_INFO( "UCR: %d", r );
-			if( r != sizeof( vive_controller_haptic_pulse ) ) return 5;
-			OGUSleep( 1000 );
+			for (int i = 0; i < 0x1; i++)
+			{
+				//uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x7, 0 /*right*/, 0xFF /*period on*/, 0xFF/*period on*/, 0xFF/*period off*/, 0xFF/*period off*/, 0xFF /* repeat Count */, 0xFF /* repeat count */ };
+				//uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; // data taken from Nairol's captures
+				uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01* j, 0x00 }; 
+				r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
+				r = getupdate_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)) printf("HAPTIC FAILED **************************\n"); // return 5;
+				OGUSleep(5000);
+			}
+
+			OGUSleep(20000);
 		}
+
+
 #endif
 
+#if 0
+		//// working code to turn off a wireless controller:
+		//{
+		//	uint8_t vive_controller_off[64] = { 0xff, 0x9f, 0x04, 'o', 'f', 'f', '!' };
+		//	//r = update_feature_report( sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof( vive_controller_haptic_pulse ) );
+		//	r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_off, sizeof(vive_controller_off));
+		//	SV_INFO("UCR: %d", r);
+		//	if (r != sizeof(vive_controller_off)) printf("OFF FAILED **************************\n"); // return 5;
+		//	OGUSleep(1000);
+		//}
+#endif
 		//if (sv->udev[USB_DEV_TRACKER0])
 		//{
 		//	static uint8_t vive_magic_power_on[64] = {  0x04, 0x78, 0x29, 0x38 };
@@ -650,6 +694,54 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
 	return 0;
 }
 
+int survive_vive_send_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount)
+{
+	SurviveViveData *sv = (SurviveViveData*)so->driver;
+
+	if (NULL == sv)
+	{
+		return -500;
+	}
+
+	int r;
+	uint8_t vive_controller_haptic_pulse[64] = { 
+		0xff, 0x8f, 0x07, 0x00, 
+		pulseHigh & 0xff00 >> 8,  pulseHigh & 0xff,
+		pulseLow & 0xff00 >> 8,  pulseLow & 0xff,
+		repeatCount & 0xff00 >> 8,  repeatCount & 0xff,
+	};
+
+	r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse));
+	r = getupdate_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)) 
+	{
+		printf("HAPTIC FAILED **************************\n"); 
+		return -1;
+	}
+
+	return 0;
+
+	//for (int j = 0; j < 40; j++)
+	//{
+	//	for (int i = 0; i < 0x1; i++)
+	//	{
+	//		//uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x7, 0 /*right*/, 0xFF /*period on*/, 0xFF/*period on*/, 0xFF/*period off*/, 0xFF/*period off*/, 0xFF /* repeat Count */, 0xFF /* repeat count */ };
+	//		//uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01, 0x00 }; // data taken from Nairol's captures
+	//		uint8_t vive_controller_haptic_pulse[64] = { 0xff, 0x8f, 0x07, 0x00, 0xf4, 0x01, 0xb5, 0xa2, 0x01 * j, 0x00 };
+	//		r = update_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse));
+	//		r = getupdate_feature_report(sv->udev[USB_DEV_WATCHMAN1], 0, vive_controller_haptic_pulse, sizeof(vive_controller_haptic_pulse));
+	//		if (r != sizeof(vive_controller_haptic_pulse)) printf("HAPTIC FAILED **************************\n"); // return 5;
+	//		OGUSleep(5000);
+	//	}
+
+	//	OGUSleep(20000);
+	//}
+
+	////OGUSleep(5000);
+	//return 0;
+}
+
 void survive_vive_usb_close( SurviveViveData * sv )
 {
 	int i;
@@ -811,8 +903,21 @@ int survive_get_config( char ** config, SurviveViveData * sv, int devno, int ifa
 ///////////////////////////////////////////////////////////////////////////////
 
 #define POP1  (*(readdata++))
-#define POP2  (*(((uint16_t*)((readdata+=2)-2))))
-#define POP4  (*(((uint32_t*)((readdata+=4)-4))))
+
+struct __attribute__((__packed__)) unaligned_16_t {
+    int16_t v;
+};
+struct __attribute__((__packed__)) unaligned_32_t {
+    int32_t v;
+};
+struct __attribute__((__packed__)) unaligned_u16_t {
+    uint16_t v;
+};
+struct __attribute__((__packed__)) unaligned_u32_t {
+    uint32_t v;
+};
+#define POP2  ((((( struct unaligned_u16_t*)((readdata+=2)-2))))->v)
+#define POP4  ((((( struct unaligned_u32_t*)((readdata+=4)-4))))->v)
 
 void calibrate_acc(SurviveObject* so, FLT* agm) {
 	if (so->acc_bias != NULL) {
@@ -842,6 +947,171 @@ void calibrate_gyro(SurviveObject* so, FLT* agm) {
 	}
 }
 
+typedef struct 
+{
+	// could use a bitfield here, but since this data is short-lived,
+	// the space savings probably isn't worth the processing overhead.
+	uint8_t pressedButtonsValid;
+	uint8_t triggerOfBatteryValid;
+	uint8_t batteryChargeValid;
+	uint8_t hardwareIdValid;
+	uint8_t touchpadHorizontalValid;
+	uint8_t touchpadVerticalValid;
+	uint8_t triggerHighResValid;
+
+	uint32_t pressedButtons;
+	uint16_t triggerOrBattery;
+	uint8_t  batteryCharge;
+	uint32_t hardwareId;
+	uint16_t touchpadHorizontal;
+	uint16_t touchpadVertical;
+	uint16_t triggerHighRes;
+} buttonEvent;
+
+void incrementAndPostButtonQueue(SurviveContext *ctx)
+{
+	ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextWriteIndex]);
+
+	if ((ctx->buttonQueue.nextWriteIndex+1)% BUTTON_QUEUE_MAX_LEN == ctx->buttonQueue.nextReadIndex)
+	{
+		// There's not enough space to write this entry.  Clear it out and move along
+		//printf("Button Buffer Full\n");
+		memset(entry, 0, sizeof(ButtonQueueEntry));
+		return;
+	}
+	entry->isPopulated = 1;
+	ctx->buttonQueue.nextWriteIndex++;
+	// if we've exceeded the size of the buffer, loop around to the beginning.
+	if (ctx->buttonQueue.nextWriteIndex >= BUTTON_QUEUE_MAX_LEN)
+	{
+		ctx->buttonQueue.nextWriteIndex = 0;
+	}
+	OGUnlockSema(ctx->buttonQueue.buttonservicesem);
+
+	// clear out any old data in the entry so we always start with a clean slate.
+	entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextWriteIndex]);
+	memset(entry, 0, sizeof(ButtonQueueEntry));
+}
+
+// important!  This must be the only place that we're posting to the buttonEntryQueue
+// if that ever needs to be changed, you will have to add locking so that only one
+// thread is posting at a time.
+void registerButtonEvent(SurviveObject *so, buttonEvent *event)
+{
+	ButtonQueueEntry *entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]);
+
+	memset(entry, 0, sizeof(ButtonQueueEntry));
+
+	entry->so = so;
+	if (event->pressedButtonsValid)
+	{
+		//printf("trigger %8.8x\n", event->triggerHighRes);
+		for (int a=0; a < 16; a++)
+		{
+			if (((event->pressedButtons) & (1<<a)) != ((so->buttonmask) & (1<<a)))
+			{
+				// Hey, the button did something
+				if (event->pressedButtons & (1 << a))
+				{
+					// it went down
+					entry->eventType = BUTTON_EVENT_BUTTON_DOWN;
+				}
+				else
+				{
+					// it went up
+					entry->eventType = BUTTON_EVENT_BUTTON_UP;
+				}
+				entry->buttonId = a;
+				if (entry->buttonId == 0)
+				{
+					// this fixes 2 issues.  First, is the a button id of 0 indicates no button pressed.
+					// second is that the trigger shows up as button 0 coming from the wireless controller,
+					// but we infer it from the position on the wired controller.  On the wired, we treat it
+					// as buttonId 24 (look further down in this function)
+					entry->buttonId = 24;
+				}
+				incrementAndPostButtonQueue(so->ctx);
+				entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]);
+			}
+		}
+		// if the trigger button is depressed & it wasn't before
+		if ((((event->pressedButtons) & (0xff000000)) == 0xff000000) &&
+			((so->buttonmask) & (0xff000000)) != 0xff000000)
+		{
+			entry->eventType = BUTTON_EVENT_BUTTON_DOWN;
+			entry->buttonId = 24;
+			incrementAndPostButtonQueue(so->ctx);
+			entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]);
+		}
+		// if the trigger button isn't depressed but it was before
+		else if ((((event->pressedButtons) & (0xff000000)) != 0xff000000) &&
+			((so->buttonmask) & (0xff000000)) == 0xff000000)
+		{
+			entry->eventType = BUTTON_EVENT_BUTTON_UP;
+			entry->buttonId = 24;
+			incrementAndPostButtonQueue(so->ctx);
+			entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]);
+		}
+	}
+	if (event->triggerHighResValid)
+	{
+		if (so->axis1 != event->triggerHighRes)
+		{
+			entry->eventType = BUTTON_EVENT_AXIS_CHANGED;
+			entry->axis1Id = 1;
+			entry->axis1Val = event->triggerHighRes;
+			incrementAndPostButtonQueue(so->ctx);
+			entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]);
+
+		}
+	}
+	if ((event->touchpadHorizontalValid) && (event->touchpadVerticalValid))
+	{
+		if ((so->axis2 != event->touchpadHorizontal) ||
+			(so->axis3 != event->touchpadVertical))
+		{
+			entry->eventType = BUTTON_EVENT_AXIS_CHANGED;
+			entry->axis1Id = 2;
+			entry->axis1Val = event->touchpadHorizontal;
+			entry->axis2Id = 3;
+			entry->axis2Val = event->touchpadVertical;
+			incrementAndPostButtonQueue(so->ctx);
+			entry = &(so->ctx->buttonQueue.entry[so->ctx->buttonQueue.nextWriteIndex]);
+
+		}
+	}
+
+	if (event->pressedButtonsValid)
+	{
+		so->buttonmask = event->pressedButtons;
+	}
+	if (event->batteryChargeValid)
+	{
+		so->charge = event->batteryCharge;
+	}
+	if (event->touchpadHorizontalValid)
+	{
+		so->axis2 = event->touchpadHorizontal;
+	}
+	if (event->touchpadVerticalValid)
+	{
+		so->axis3 = event->touchpadVertical;
+	}
+	if (event->triggerHighResValid)
+	{
+		so->axis1 = event->triggerHighRes;
+	}
+}
+
+uint8_t isPopulated;  //probably can remove this given the semaphore in the parent struct.   helps with debugging
+uint8_t eventType;
+uint8_t buttonId;
+uint8_t axis1Id;
+uint16_t axis1Val;
+uint8_t axis2Id;
+uint16_t axis2Val;
+SurviveObject *so;
+
 
 static void handle_watchman( SurviveObject * w, uint8_t * readdata )
 {
@@ -862,36 +1132,55 @@ static void handle_watchman( SurviveObject * w, uint8_t * readdata )
 	uint8_t qty = POP1;
 	uint8_t time2 = POP1;
 	uint8_t type = POP1;
+
+
 	qty-=2;
 	int propset = 0;
 	int doimu = 0;
 
 	if( (type & 0xf0) == 0xf0 )
 	{
+		buttonEvent bEvent;
+		memset(&bEvent, 0, sizeof(bEvent));
+
 		propset |= 4;
 		//printf( "%02x %02x %02x %02x\n", qty, type, time1, time2 );
 		type &= ~0x10;
 
 		if( type & 0x01 )
 		{
+			
 			qty-=1;
-			w->buttonmask = POP1;
+			bEvent.pressedButtonsValid = 1;
+			bEvent.pressedButtons = POP1;
+
+			//printf("buttonmask is %d\n", w->buttonmask);
 			type &= ~0x01;
 		}
 		if( type & 0x04 ) 
 		{
 			qty-=1;
-			w->axis1 = ( POP1 ) * 128; 
+			bEvent.triggerHighResValid = 1;
+			bEvent.triggerHighRes = ( POP1 ) * 128;
 			type &= ~0x04;
 		}
 		if( type & 0x02 )
 		{
 			qty-=4;
-			w->axis2 = POP2;
-			w->axis3 = POP2;
+			bEvent.touchpadHorizontalValid = 1;
+			bEvent.touchpadVerticalValid = 1;
+
+			bEvent.touchpadHorizontal = POP2;
+			bEvent.touchpadVertical = POP2;
 			type &= ~0x02;
 		}
 
+		if (bEvent.pressedButtonsValid || bEvent.triggerHighResValid || bEvent.touchpadHorizontalValid)
+		{
+			registerButtonEvent(w, &bEvent);
+		}
+
+
 		//XXX TODO: Is this correct?  It looks SO WACKY
 		type &= 0x7f;
 		if( type == 0x68 ) doimu = 1;
@@ -1156,7 +1445,7 @@ void survive_data_cb( SurviveUSBInterface * si )
 		//printf( "%d -> ", size );
 		for( i = 0; i < 3; i++ )
 		{
-			int16_t * acceldata = (int16_t*)readdata;
+            struct unaligned_16_t * acceldata = (struct unaligned_16_t *)readdata;
 			readdata += 12;
 			uint32_t timecode = POP4;
 			uint8_t code = POP1;
@@ -1168,8 +1457,8 @@ void survive_data_cb( SurviveUSBInterface * si )
 				obj->oldcode = code;
 
 				//XXX XXX BIG TODO!!! Actually recal gyro data.
-				FLT agm[9] = { acceldata[0], acceldata[1], acceldata[2],
-								acceldata[3], acceldata[4], acceldata[5],
+				FLT agm[9] = { acceldata[0].v, acceldata[1].v, acceldata[2].v,
+								acceldata[3].v, acceldata[4].v, acceldata[5].v,
 								0,0,0 };
 
 				agm[0]*=(float)(1./8192.0);
@@ -1192,7 +1481,10 @@ void survive_data_cb( SurviveUSBInterface * si )
 				ctx->imuproc( obj, 3, agm, timecode, code );
 			}
 		}
-
+		if (id != 32)
+		{
+			int a=0; // set breakpoint here
+		}
 		//DONE OK.
 		break;
 	}
@@ -1211,7 +1503,7 @@ void survive_data_cb( SurviveUSBInterface * si )
 		}
 		else if( id == 38 )
 		{
-			w->ison = 0;
+			w->ison = 0; // turning off
 		}
 		else
 		{
@@ -1250,7 +1542,94 @@ void survive_data_cb( SurviveUSBInterface * si )
 			handle_lightcap( obj, &le );
 		}		
 		break;
+
+		if (id != 33)
+		{
+			int a = 0; // breakpoint here
+		}
+	}
+	case USB_IF_TRACKER0_BUTTONS:
+	case USB_IF_W_WATCHMAN1_BUTTONS:
+	{
+		if (1 == id)
+		{
+			//0x00	uint8	1	reportID	HID report identifier(= 1)
+			//0x02	uint16	2	reportType(? )	0x0B04: Ping(every second) / 0x3C01 : User input
+			//0x04	uint32	4	reportCount	Counter that increases with every report
+			//0x08	uint32	4	pressedButtons	Bit field, see below for individual buttons
+			//0x0C	uint16	2	triggerOrBattery	Analog trigger value(user input) / Battery voltage ? (ping)
+			//0x0E	uint8	1	batteryCharge	Bit 7 : Charging / Bit 6..0 : Battery charge in percent
+			//0x10	uint32	4	hardwareID	Hardware ID(user input) / 0x00000000 (ping)
+			//0x14	int16	2	touchpadHorizontal	Horizontal thumb position(Left : -32768 / Right : 32767)
+			//0x16	int16	2	touchpadVertical	Vertical thumb position(Bottom : -32768 / Top : 32767)
+			//0x18 ? 2 ? unknown
+			//0x1A	uint16	2	triggerHighRes	Analog trigger value with higher resolution
+			//0x1C ? 24 ? unknown
+			//0x34	uint16	2	triggerRawMaybe	Analog trigger value, maybe raw sensor data
+			//0x36 ? 8 ? unknown
+			//0x3E	uint8	1	someBitFieldMaybe	0x00 : ping / 0x64 : user input
+			//0x3F ? 1 ? unknown
+
+			//typedef struct
+			//{
+			//	//uint8_t reportId;
+			//	uint16_t reportType;
+			//	uint32_t reportCount;
+			//	uint32_t pressedButtons;
+			//	uint16_t  triggerOrBattery;
+			//	uint8_t batteryCharge;
+			//	uint32_t hardwareId;
+			//	int16_t  touchpadHorizontal;
+			//	int16_t  touchpadVertical;
+			//	uint16_t unknown1;
+			//	uint16_t triggerHighRes;
+			//	uint8_t  unknown2;
+			//	uint8_t  unknown3;
+			//	uint8_t  unknown4;
+			//	uint16_t triggerRaw;
+			//	uint8_t  unknown5;
+			//	uint8_t  unknown6; // maybe some bitfield?
+			//	uint8_t  unknown7;
+			//} usb_buttons_raw;
+
+			//usb_buttons_raw *raw = (usb_buttons_raw*) readdata;
+			if (*((uint16_t*)(&(readdata[0x0]))) == 0x100)
+			{
+				buttonEvent bEvent;
+				memset(&bEvent, 0, sizeof(bEvent));
+
+				bEvent.pressedButtonsValid = 1;
+				bEvent.pressedButtons = *((uint32_t*)(&(readdata[0x07])));
+				bEvent.triggerHighResValid = 1;
+				//bEvent.triggerHighRes = raw->triggerHighRes; 
+				//bEvent.triggerHighRes = (raw->pressedButtons & 0xff000000) >> 24; // this seems to provide the same data at 2x the resolution as above
+				//bEvent.triggerHighRes = raw->triggerRaw;
+				
+				bEvent.triggerHighRes = *((uint16_t*)(&(readdata[0x19])));
+				bEvent.touchpadHorizontalValid = 1;
+				//bEvent.touchpadHorizontal = raw->touchpadHorizontal;
+				bEvent.touchpadHorizontal = *((int16_t*)(&(readdata[0x13])));
+				bEvent.touchpadVerticalValid = 1;
+				//bEvent.touchpadVertical = raw->touchpadVertical;
+				bEvent.touchpadVertical = *((int16_t*)(&(readdata[0x15])));
+
+				//printf("%4.4x\n", bEvent.triggerHighRes);
+				registerButtonEvent(obj, &bEvent);
+
+				//printf("Buttons: %8.8x\n", raw->pressedButtons);
+			}
+			int a = 0;
+		}
+		else
+		{
+			int a = 0;// breakpoint here
+		}
 	}
+	default:
+	{
+		int a = 0; // breakpoint here
+	}
+
 	}
 }
 
@@ -1269,171 +1648,22 @@ void survive_data_cb( SurviveUSBInterface * si )
 
 
 
-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( SurviveContext * ctx, 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];
-
-		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( SurviveViveData * sv, SurviveObject * so, int devno, int iface, int extra_magic )
 {
 	SurviveContext * ctx = sv->ctx;
 	char * ct0conf = 0;
 	int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic );
-printf( "Loading config: %d\n", len );
-#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
+	printf( "Loading config: %d\n", len );
 
-	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;
-				}
-			}
-
-
-			if (jsoneq(ct0conf, tk, "acc_bias") == 0) {
-				int32_t count = (tk+1)->size;
-				FLT* values = NULL;
-				if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
-					so->acc_bias = values;
-					so->acc_bias[0] *= .125; //XXX Wat?  Observed by CNL.  Biasing by more than this seems to hose things.
-					so->acc_bias[1] *= .125;
-					so->acc_bias[2] *= .125;
-				}
-			}
-			if (jsoneq(ct0conf, tk, "acc_scale") == 0) {
-				int32_t count = (tk+1)->size;
-				FLT* values = NULL;
-				if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
-					so->acc_scale = values;
-				}
-			}
-
-			if (jsoneq(ct0conf, tk, "gyro_bias") == 0) {
-				int32_t count = (tk+1)->size;
-				FLT* values = NULL;
-				if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
-					so->gyro_bias = values;
-				}
-			}
-			if (jsoneq(ct0conf, tk, "gyro_scale") == 0) {
-				int32_t count = (tk+1)->size;
-				FLT* values = NULL;
-				if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
-					so->gyro_scale = values;
-				}
-			}
-		}
+	  char raw_fname[100];
+	  sprintf( raw_fname, "%s_config.json", so->codename );
+	  FILE * f = fopen( raw_fname, "w" );
+	  fwrite( ct0conf, strlen(ct0conf), 1, f );
+	  fclose( f );
 	}
-	else
-	{
-		//TODO: Cleanup any remaining USB stuff.
-		return 1;
-	}
-
-	char fname[64];
 
-	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;
+	return survive_load_htc_config_format(ct0conf, len, so);
 }
 
 
@@ -1450,23 +1680,27 @@ void init_SurviveObject(SurviveObject* so) {
 	so->acc_bias = NULL;
 	so->gyro_scale = NULL;
 	so->gyro_bias = NULL;
+	so->haptic = NULL;
+	so->PoserData = NULL;
+	so->disambiguator_data = NULL;
 }
 
 int DriverRegHTCVive( SurviveContext * ctx )
 {
-	int r;
-	SurviveObject * hmd = calloc( 1, sizeof( SurviveObject ) );
-	SurviveObject * wm0 = calloc( 1, sizeof( SurviveObject ) );
-	SurviveObject * wm1 = calloc( 1, sizeof( SurviveObject ) );
-	SurviveObject * tr0 = calloc( 1, sizeof( SurviveObject ) );
-	SurviveObject * ww0 = calloc( 1, sizeof( SurviveObject ) );
-	SurviveViveData * sv = calloc( 1, sizeof( SurviveViveData ) );
-
-	init_SurviveObject(hmd);
-	init_SurviveObject(wm0);
-	init_SurviveObject(wm1);
-	init_SurviveObject(tr0);
-	init_SurviveObject(ww0);
+	const char* playback_dir = config_read_str(ctx->global_config_values,
+						   "PlaybackDir", "");
+	if(strlen(playback_dir) != 0) {
+	  SV_INFO("Playback is active; disabling USB driver");
+	  return 0;
+	}
+	
+	int r;	
+	SurviveViveData * sv = calloc(1, sizeof(SurviveViveData) );
+	SurviveObject * hmd = survive_create_hmd(ctx, "HTC", sv);
+	SurviveObject * wm0 = survive_create_wm0(ctx, "HTC", sv, 0);
+	SurviveObject * wm1 = survive_create_wm1(ctx, "HTC", sv, 0);
+	SurviveObject * tr0 = survive_create_tr0(ctx, "HTC", sv);
+	SurviveObject * ww0 = survive_create_ww0(ctx, "HTC", sv);
 
 	sv->ctx = ctx;
 	
@@ -1478,23 +1712,6 @@ int DriverRegHTCVive( SurviveContext * ctx )
 		mkdir( "calinfo", 0755 );
 	#endif
 
-
-	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 );
-	tr0->ctx = ctx;
-	memcpy( tr0->codename, "TR0", 4 );
-	memcpy( tr0->drivername, "HTC", 4 );
-	ww0->ctx = ctx;
-	memcpy( ww0->codename, "WW0", 4 );
-	memcpy( ww0->drivername, "HTC", 4 );
-
 	//USB must happen last.
 	if( r = survive_usb_init( sv, hmd, wm0, wm1, tr0, ww0) )
 	{
@@ -1508,55 +1725,7 @@ int DriverRegHTCVive( SurviveContext * ctx )
 	if( sv->udev[USB_DEV_WATCHMAN2] && LoadConfig( sv, wm1, 3, 0, 1 )) { SV_INFO( "Watchman 1 config issue." ); }
 	if( sv->udev[USB_DEV_TRACKER0]  && LoadConfig( sv, tr0, 4, 0, 0 )) { SV_INFO( "Tracker 0 config issue." ); }
 	if( sv->udev[USB_DEV_W_WATCHMAN1]  && LoadConfig( sv, ww0, 5, 0, 0 )) { SV_INFO( "Wired Watchman 0 config issue." ); }
-
-	hmd->timebase_hz = wm0->timebase_hz = wm1->timebase_hz = 48000000;
-	tr0->timebase_hz = ww0->timebase_hz = hmd->timebase_hz;
-
-	hmd->pulsedist_max_ticks = wm0->pulsedist_max_ticks = wm1->pulsedist_max_ticks = 500000;
-	tr0->pulsedist_max_ticks = ww0->pulsedist_max_ticks = hmd->pulsedist_max_ticks;
-
-	hmd->pulselength_min_sync = wm0->pulselength_min_sync = wm1->pulselength_min_sync = 2200;
-	tr0->pulselength_min_sync = ww0->pulselength_min_sync = hmd->pulselength_min_sync;
-
-	hmd->pulse_in_clear_time = wm0->pulse_in_clear_time = wm1->pulse_in_clear_time = 35000;
-	tr0->pulse_in_clear_time = ww0->pulse_in_clear_time = hmd->pulse_in_clear_time;
-
-	hmd->pulse_max_for_sweep = wm0->pulse_max_for_sweep = wm1->pulse_max_for_sweep = 1800;
-	tr0->pulse_max_for_sweep = ww0->pulse_max_for_sweep = hmd->pulse_max_for_sweep;
-
-	hmd->pulse_synctime_offset = wm0->pulse_synctime_offset = wm1->pulse_synctime_offset = 20000;
-	tr0->pulse_synctime_offset = ww0->pulse_synctime_offset = hmd->pulse_synctime_offset;
-
-	hmd->pulse_synctime_slack = wm0->pulse_synctime_slack = wm1->pulse_synctime_slack = 5000;
-	tr0->pulse_synctime_slack = ww0->pulse_synctime_slack = hmd->pulse_synctime_slack;
-
-	hmd->timecenter_ticks = hmd->timebase_hz / 240;
-	wm0->timecenter_ticks = wm0->timebase_hz / 240;
-	wm1->timecenter_ticks = wm1->timebase_hz / 240;
-	tr0->timecenter_ticks = tr0->timebase_hz / 240;
-	ww0->timecenter_ticks = ww0->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.
 	if( sv->udev[USB_DEV_HMD_IMU_LH]       ) { survive_add_object( ctx, hmd ); }
 	if( sv->udev[USB_DEV_WATCHMAN1] ) { survive_add_object( ctx, wm0 ); }
@@ -1564,7 +1733,15 @@ int DriverRegHTCVive( SurviveContext * ctx )
 	if( sv->udev[USB_DEV_TRACKER0]  ) { survive_add_object( ctx, tr0 ); }
 	if( sv->udev[USB_DEV_W_WATCHMAN1]  ) { survive_add_object( ctx, ww0 ); }
 
-	survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic );
+	if( sv->udev[USB_DEV_HMD_IMU_LH] ||
+	    sv->udev[USB_DEV_WATCHMAN1]  ||
+	    sv->udev[USB_DEV_WATCHMAN2]  ||
+	    sv->udev[USB_DEV_TRACKER0]   ||
+	    sv->udev[USB_DEV_W_WATCHMAN1] ) {
+	  survive_add_driver( ctx, sv, survive_vive_usb_poll, survive_vive_close, survive_vive_send_magic );
+	} else {
+	  fprintf(stderr, "No USB devices detected\n");
+	}
 
 	return 0;
 fail_gracefully:
diff --git a/test.c b/test.c
index 4909d50..5fc62af 100644
--- a/test.c
+++ b/test.c
@@ -37,6 +37,69 @@ void HandleDestroy()
 {
 }
 
+void testprog_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val)
+{
+	survive_default_button_process(so, eventType, buttonId, axis1Id, axis1Val, axis2Id, axis2Val);
+	
+	// do nothing.
+	printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n",
+		eventType,
+		buttonId,
+		axis1Id,
+		axis1Val,
+		axis2Id,
+		axis2Val);
+
+	// Note: the code for haptic feedback is not yet written to support wired USB connections to the controller.  
+	// Work is still needed to reverse engineer that USB protocol.
+
+	// let's do some haptic feedback if the trigger is pushed
+	if (buttonId == 24 && eventType == 1) // trigger engage
+	{
+		for (int j = 0; j < 6; j++)
+		{
+			for (int i = 0; i < 0x5; i++)
+			{
+				survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
+				//survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
+				OGUSleep(1000);
+			}
+			OGUSleep(20000);
+		}
+	}
+
+	// let's do some haptic feedback if the touchpad is pressed.
+	if (buttonId == 2 && eventType == 1) // trigger engage
+	{
+		for (int j = 0; j < 6; j++)
+		{
+			for (int i = 0; i < 0x1; i++)
+			{
+				survive_haptic(so, 0, 0xf401, 0x05a2, 0xf100);
+				//survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
+				OGUSleep(5000);
+			}
+			OGUSleep(20000);
+		}
+	}
+}
+
+void testprog_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat)
+{
+	survive_default_raw_pose_process(so, lighthouse, pos, quat);
+
+	// print the pose;
+	printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
+}
+
+void testprog_imu_process(SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id)
+{
+	survive_default_imu_process(so, mask, accelgyromag, timecode, id);
+
+	// so something here, like printing out the data...
+
+}
+
 
 static void dump_iface( struct SurviveObject * so, const char * prefix )
 {
@@ -73,6 +136,10 @@ int main()
 
 	ctx = survive_init( 0 );
 
+	survive_install_button_fn(ctx, testprog_button_process);
+	survive_install_raw_pose_fn(ctx, testprog_raw_pose_process);
+	survive_install_imu_fn(ctx, testprog_imu_process);
+
 	if( !ctx )
 	{
 		fprintf( stderr, "Fatal. Could not start\n" );
@@ -85,14 +152,16 @@ int main()
 	dump_iface( survive_get_so_by_name( ctx, "TR0" ), "TR0" );
 	dump_iface( survive_get_so_by_name( ctx, "WW0" ), "WW0" );
 
+	survive_cal_install(ctx);
+
 	while(survive_poll(ctx) == 0)
 	{
-		double Now = OGGetAbsoluteTime();
-		if( Now > (Start+1) && !magicon )
-		{
-			survive_send_magic(ctx,1,0,0);
-			magicon = 1;
-		}
+		//double Now = OGGetAbsoluteTime();
+		//if( Now > (Start+1) && !magicon )
+		//{
+		//	survive_send_magic(ctx,1,0,0);
+		//	magicon = 1;
+		//}
 		//Do stuff.
 	}
 }
diff --git a/useful_files/81-vive.rules b/useful_files/81-vive.rules
index ab38087..652b599 100644
--- a/useful_files/81-vive.rules
+++ b/useful_files/81-vive.rules
@@ -12,3 +12,13 @@ SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8328", TAG+="uacce
 # HTC Mass Storage Node
 SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8200", TAG+="uaccess"
 SUBSYSTEM=="usb", ATTRS{idVendor}=="114d", ATTRS{idProduct}=="8a12", TAG+="uaccess"
+
+
+#libsurvive
+SUBSYSTEM=="usb", ATTR{idVendor}=="0bb4", ATTR{idProduct}=="2c87", MODE="0666" # HTC HMD
+SUBSYSTEM=="usb", ATTR{idVendor}=="28de", ATTR{idProduct}=="2000", MODE="0666" # Light input
+SUBSYSTEM=="usb", ATTR{idVendor}=="28de", ATTR{idProduct}=="2101", MODE="0666" # Watchman
+
+#KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="0bb4", ATTR{idProduct}=="2c87", MODE="0666" # HTC
+#KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTR{idProduct}=="2000", MODE="0666" # Valve
+#KERNEL=="hidraw*", SUBSYSTEM=="hidraw", ATTRS{idVendor}=="28de", ATTR{idProduct}=="2101", MODE="0666" # Valve