Merge pull request #104 from jdavidberger/playback_driver

Playback driver

8 files changed, 644 insertions, 361 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/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_types.h b/include/libsurvive/survive_types.h
index 5384345..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];
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_vive.c b/src/survive_vive.c
index cb05efc..288f8cb 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)
@@ -1647,171 +1649,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;
-				}
-			}
-		}
-	}
-	else
-	{
-		//TODO: Cleanup any remaining USB stuff.
-		return 1;
+	  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 );
 	}
 
-	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);
 }
 
 
@@ -1835,19 +1688,20 @@ void init_SurviveObject(SurviveObject* so) {
 
 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;
 	
@@ -1859,28 +1713,6 @@ int DriverRegHTCVive( SurviveContext * ctx )
 		mkdir( "calinfo", 0755 );
 	#endif
 
-
-	hmd->ctx = ctx;
-	hmd->driver = sv;
-	memcpy( hmd->codename, "HMD", 4 );
-	memcpy( hmd->drivername, "HTC", 4 );
-	wm0->ctx = ctx;
-	wm0->driver = sv;
-	memcpy( wm0->codename, "WM0", 4 );
-	memcpy( wm0->drivername, "HTC", 4 );
-	wm1->ctx = ctx;
-	wm1->driver = sv;
-	memcpy( wm1->codename, "WM1", 4 );
-	memcpy( wm1->drivername, "HTC", 4 );
-	tr0->ctx = ctx;
-	tr0->driver = sv;
-	memcpy( tr0->codename, "TR0", 4 );
-	memcpy( tr0->drivername, "HTC", 4 );
-	ww0->ctx = ctx;
-	ww0->driver = sv;
-	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) )
 	{
@@ -1894,58 +1726,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;
-
-	wm0->haptic = survive_vive_send_haptic;
-	wm1->haptic = survive_vive_send_haptic;
-/*
-	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 ); }
@@ -1953,7 +1734,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: