Playback now goes to a single file. Note that the file format changed too so that it always starts 'time' 'device name'

10 files changed, 135 insertions, 66 deletions

diff --git a/README.md b/README.md
index 3e7ff49..a4fd656 100644
--- a/README.md
+++ b/README.md
@@ -269,14 +269,16 @@ libsurvive has an integrated tool that allows you to record and playback streams
 
 ```
 make
-mkdir my_playback
-./datarecorder my_playback/events
-cp *_config.json my_playback
+./data_recorder my_playback_file
 ```
 
-This gives you a directory -- my_playback -- with all the device configurations and events file you need to replay it.
+This gives you a file -- my_playback_file -- with all the device configurations and events file you need to replay it.
 
-To actually replay it, put that directory path in the 'PlaybackDir' configuration value in config.json and run libsurvive as usual. Note that this will purposefully stop the USB devices from loading as to not confuse the library with inconsistent data.
+To actually replay it, put that directory path in the 'PlaybackFile' configuration value in config.json and run libsurvive as usual. Note that this will purposefully stop the USB devices from loading as to not confuse the library with inconsistent data.
+
+## Playback speed
+
+There is also a config variable -- `PlaybackFactor` -- which adjusts the speed at which playback happens. A value of 1 emulates the same time the events file took to create, a value of 0 streams the data in as fast as possible. 
 
 ## Notes about coordinate frames.
 
@@ -317,11 +319,6 @@ General information for LH pose:
 
 NOTE: This is NOT currently correct.
 
-
-## Playback speed
-
-There is also a config variable -- `PlaybackFactor` -- which adjusts the speed at which playback happens. A value of 1 emulates the same time the events file took to create, a value of 0 streams the data in as fast as possible. 
-
 # FAQ
 
  * The tracking quality is bad/jitters/too slow!
diff --git a/data_recorder.c b/data_recorder.c
index e835c73..e406387 100644
--- a/data_recorder.c
+++ b/data_recorder.c
@@ -62,14 +62,26 @@ void write_to_output(const char *format, ...) {
 
 	va_end(args);
 }
+int my_config_process(SurviveObject *so, char *ct0conf, int len) {
+	char *buffer = malloc(len);
+	memcpy(buffer, ct0conf, len);
+	for (int i = 0; i < len; i++)
+		if (buffer[i] == '\n')
+			buffer[i] = ' ';
+
+	write_to_output("%s CONFIG ", so->codename);
+	fwrite(buffer, 1, len, output_file);
+	fwrite("\n", 1, 1, output_file);
+	return survive_default_htc_config_process(so, ct0conf, len);
+}
 void my_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose) {
 	survive_default_lighthouse_pose_process(ctx, lighthouse, pose);
-	write_to_output("LH_POSE %d %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", lighthouse, pose->Pos[0], pose->Pos[1],
+	write_to_output("%d LH_POSE %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", lighthouse, pose->Pos[0], pose->Pos[1],
 					pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]);
 }
 void my_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
 	survive_default_raw_pose_process(so, lighthouse, pose);
-	write_to_output("POSE %s %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", so->codename, pose->Pos[0], pose->Pos[1],
+	write_to_output("%s POSE %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", so->codename, pose->Pos[0], pose->Pos[1],
 					pose->Pos[2], pose->Rot[0], pose->Rot[1], pose->Rot[2], pose->Rot[3]);
 }
 
@@ -80,8 +92,7 @@ void my_light_process(struct SurviveObject *so, int sensor_id, int acode,
 								  length, lh);
 
 	if (acode == -1 || sensor_id < 0) {
-		write_to_output("A %s %d %d %d %u %u %u\n", so->codename, sensor_id,
-						acode, timeinsweep, timecode, length, lh);
+		write_to_output("%s A %d %d %d %u %u %u\n", so->codename, sensor_id, acode, timeinsweep, timecode, length, lh);
 		return;
 	}
 
@@ -122,18 +133,16 @@ void my_light_process(struct SurviveObject *so, int sensor_id, int acode,
 		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);
+	write_to_output("%s %s %s %u %d %d %d %u %u\n", so->codename, LH_ID, LH_Axis, 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);
-	write_to_output("I %s %d %u %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %d\n",
-					so->codename, mask, timecode, accelgyro[0], accelgyro[1],
-					accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id);
+	write_to_output("%s I %d %u %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %d\n", so->codename, mask, timecode, accelgyro[0],
+					accelgyro[1], accelgyro[2], accelgyro[3], accelgyro[4], accelgyro[5], id);
 }
 
 void *GuiThread(void *v) {
@@ -177,12 +186,13 @@ void *GuiThread(void *v) {
 int SurviveThreadLoaded = 0;
 
 void *SurviveThread(void *junk) {
-	ctx = survive_init(0);
+	ctx = survive_init_with_config_cb(0, my_config_process);
 
 	survive_install_light_fn(ctx, my_light_process);
 	survive_install_imu_fn(ctx, my_imu_process);
 	survive_install_lighthouse_pose_fn(ctx, my_lighthouse_process);
 	survive_install_raw_pose_fn(ctx, my_raw_pose_process);
+
 	survive_cal_install(ctx);
 	if (!ctx) {
 		fprintf(stderr, "Fatal. Could not start\n");
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 7a9ec64..ed2f8d1 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -160,6 +160,7 @@ struct SurviveContext
 	button_process_func buttonproc;
 	raw_pose_func rawposeproc;
 	lighthouse_pose_func lighthouseposeproc;
+	htc_config_func configfunction;
 
 	struct config_group* global_config_values;
 	struct config_group* lh_config; //lighthouse configs
@@ -187,14 +188,18 @@ struct SurviveContext
 
 };
 
-SurviveContext * survive_init_internal( int headless );
+SurviveContext *survive_init_internal(int headless, htc_config_func cfcb);
 
 // 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 );
+  return survive_init_internal(headless, 0);
+}
+static inline SurviveContext *survive_init_with_config_cb(int headless, htc_config_func cfcb) {
+	survive_verify_FLT_size(sizeof(FLT));
+	return survive_init_internal(headless, cfcb);
 }
 
 //For any of these, you may pass in 0 for the function pointer to use default behavior.
@@ -235,6 +240,7 @@ 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);
 void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose);
 void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *pose);
+int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len);
 
 ////////////////////// Survive Drivers ////////////////////////////
 
diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h
index 3c7dc7f..5f5dd1f 100644
--- a/include/libsurvive/survive_types.h
+++ b/include/libsurvive/survive_types.h
@@ -46,6 +46,7 @@ typedef struct SurviveContext SurviveContext;
 typedef struct BaseStationData BaseStationData;
 typedef struct SurviveCalData SurviveCalData;   //XXX Warning: This may be removed.  Check at a later time for its defunctness.
 
+typedef int (*htc_config_func)(SurviveObject *so, char *ct0conf, int len);
 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 );
diff --git a/src/survive.c b/src/survive.c
index a27ba3e..bafacad 100755
--- a/src/survive.c
+++ b/src/survive.c
@@ -7,8 +7,9 @@
 #include <stdlib.h>
 #include <string.h>
 
-#include "survive_config.h"
 #include "os_generic.h"
+#include "survive_config.h"
+#include "survive_default_devices.h"
 
 #ifdef __APPLE__
 #define z_const const
@@ -103,8 +104,7 @@ void survive_verify_FLT_size(uint32_t user_size) {
   }
 }
 
-SurviveContext * survive_init_internal( int headless )
-{
+SurviveContext *survive_init_internal(int headless, htc_config_func configFunc) {
 #ifdef RUNTIME_SYMNUM
 	if( !did_runtime_symnum )
 	{
@@ -149,6 +149,7 @@ SurviveContext * survive_init_internal( int headless )
 	ctx->imuproc = survive_default_imu_process;
 	ctx->angleproc = survive_default_angle_process;
 	ctx->lighthouseposeproc = survive_default_lighthouse_pose_process;
+	ctx->configfunction = configFunc ? configFunc : survive_default_htc_config_process;
 
 	// initialize the button queue
 	memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue));
@@ -212,6 +213,13 @@ void survive_install_info_fn( SurviveContext * ctx,  text_feedback_func fbp )
 		ctx->notefunction = survivenote;
 }
 
+void survive_install_htc_config_fn(SurviveContext *ctx, htc_config_func fbp) {
+	if (fbp)
+		ctx->configfunction = fbp;
+	else
+		ctx->configfunction = survive_default_htc_config_process;
+}
+
 void survive_install_error_fn( SurviveContext * ctx,  text_feedback_func fbp )
 {
 	if( fbp )
diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c
index b52a2f7..6b65752 100644
--- a/src/survive_default_devices.c
+++ b/src/survive_default_devices.c
@@ -94,7 +94,7 @@ static int ParsePoints(SurviveContext *ctx, SurviveObject *so, char *ct0conf,
 	return 0;
 }
 
-int survive_load_htc_config_format(char *ct0conf, int len, SurviveObject *so) {
+int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int len) {
 	if (len == 0)
 		return -1;
 
diff --git a/src/survive_default_devices.h b/src/survive_default_devices.h
index 1fcca72..de7f0d4 100644
--- a/src/survive_default_devices.h
+++ b/src/survive_default_devices.h
@@ -14,6 +14,5 @@ SurviveObject *survive_create_tr0(SurviveContext *ctx, const char *driver_name,
 SurviveObject *survive_create_ww0(SurviveContext *ctx, const char *driver_name,
 								  void *driver);
 
-int survive_load_htc_config_format(char *ct0conf, int length,
-								   SurviveObject *so);
+int survive_load_htc_config_format(SurviveObject *so, char *ct0conf, int length);
 #endif
diff --git a/src/survive_playback.c b/src/survive_playback.c
index c4564c4..cd7906e 100644
--- a/src/survive_playback.c
+++ b/src/survive_playback.c
@@ -38,11 +38,10 @@ static int parse_and_run_imu(const char *line, SurvivePlaybackData *driver) {
 	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);
+	int rr = sscanf(line, "%s I %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",
@@ -52,8 +51,12 @@ static int parse_and_run_imu(const char *line, SurvivePlaybackData *driver) {
 
 	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);
+		static bool display_once = false;
+		SurviveContext *ctx = driver->ctx;
+		if (display_once == false) {
+			SV_ERROR("Could not find device named %s from lineno %d\n", dev, driver->lineno);
+		}
+		display_once = true;
 		return -1;
 	}
 
@@ -73,9 +76,8 @@ static int parse_and_run_lightcode(const char *line,
 	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);
+	int rr = sscanf(line, "%8s %8s %8s %u %d %d %d %u %u\n", dev, lhn, axn, &timecode, &sensor, &acode, &timeinsweep,
+					&pulselength, &lh);
 
 	if (rr != 9) {
 		fprintf(stderr, "Warning:  On line %d, only %d values read: '%s'\n",
@@ -85,8 +87,13 @@ static int parse_and_run_lightcode(const char *line,
 
 	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);
+		static bool display_once = false;
+		SurviveContext *ctx = driver->ctx;
+		if (display_once == false) {
+			SV_ERROR("Could not find device named %s from lineno %d\n", dev, driver->lineno);
+		}
+		display_once = true;
+
 		return -1;
 	}
 
@@ -129,11 +136,15 @@ static int playback_poll(struct SurviveContext *ctx, void *_driver) {
 		if (r <= 0)
 			return 0;
 
-		if ((line[0] != 'R' && line[0] != 'L' && line[0] != 'I') ||
-			line[1] != ' ')
+		char dev[10];
+		char op[10];
+		if (sscanf(line, "%8s %8s", dev, op) < 2)
+			return 0;
+
+		if ((op[0] != 'R' && op[0] != 'L' && op[0] != 'I') || op[1] != 0)
 			return 0;
 
-		switch (line[0]) {
+		switch (op[0]) {
 		case 'L':
 		case 'R':
 			parse_and_run_lightcode(line, driver);
@@ -185,7 +196,7 @@ static int LoadConfig(SurvivePlaybackData *sv, SurviveObject *so) {
 	ct0conf[len] = 0;
 
 	printf("Loading config: %d\n", len);
-	int rtn = survive_load_htc_config_format(ct0conf, len, so);
+	int rtn = ctx->configfunction(so, ct0conf, len);
 
 	free(ct0conf);
 
@@ -193,29 +204,28 @@ static int LoadConfig(SurvivePlaybackData *sv, SurviveObject *so) {
 }
 
 int DriverRegPlayback(SurviveContext *ctx) {
-	const char *playback_dir =
-		config_read_str(ctx->global_config_values, "PlaybackDir", "");
+	const char *playback_file = config_read_str(ctx->global_config_values, "PlaybackFile", "");
 
-	if (strlen(playback_dir) == 0) {
+	if (strlen(playback_file) == 0) {
 		return 0;
 	}
 
 	SurvivePlaybackData *sp = calloc(1, sizeof(SurvivePlaybackData));
 	sp->ctx = ctx;
-	sp->playback_dir = playback_dir;
+	sp->playback_dir = playback_file;
 	sp->time_factor =
 		config_read_float(ctx->global_config_values, "PlaybackFactor", 1.);
 
-	printf("%s\n", playback_dir);
+	printf("%s\n", playback_file);
 
-	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;
 	}
+
+	SV_INFO("Using playback file '%s'", playback_file);
 	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);
@@ -224,13 +234,48 @@ int DriverRegPlayback(SurviveContext *ctx) {
 
 	SurviveObject *objs[] = {hmd, wm0, wm1, tr0, ww0, 0};
 
+	FLT time;
+	while (!feof(sp->playback_file) && !ferror(sp->playback_file)) {
+		char *line = 0;
+		size_t n;
+		ssize_t r = getline(&line, &n, sp->playback_file);
+
+		if (r <= 0)
+			continue;
+
+		char dev[10];
+		char command[10];
+
+		if (sscanf(line, "%lf %s %s", &time, dev, command) != 3) {
+			break;
+		}
+
+		if (strcmp(command, "CONFIG") == 0) {
+			char *configStart = line;
+
+			// Skip three spaces
+			for (int i = 0; i < 3; i++) {
+				while (*(++configStart) != ' ')
+					;
+			}
+			size_t len = strlen(configStart);
+
+			for (SurviveObject **obj = objs; *obj; obj++) {
+				if (*obj && strcmp(dev, (*obj)->codename) == 0 && ctx->configfunction(*obj, configStart, len) == 0) {
+					SV_INFO("Found %s in playback file...", dev);
+					survive_add_object(ctx, *obj);
+					*obj = 0;
+				}
+			}
+		}
+	}
+
 	for (SurviveObject **obj = objs; *obj; obj++) {
-		if (!LoadConfig(sp, *obj)) {
-			survive_add_object(ctx, *obj);
-		} else {
+		if (*obj) {
 			free(*obj);
 		}
 	}
+	fseek(sp->playback_file, 0, SEEK_SET); // same as rewind(f);
 
 	survive_add_driver(ctx, sp, playback_poll, playback_close, 0);
 	return 0;
diff --git a/src/survive_process.c b/src/survive_process.c
index c9225e4..b697f4a 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -3,6 +3,7 @@
 
 #include "survive_cal.h"
 #include "survive_config.h"
+#include "survive_default_devices.h"
 
 //XXX TODO: Once data is avialble in the context, use the stuff here to handle converting from time codes to
 //proper angles, then from there perform the rest of the solution. 
@@ -126,6 +127,9 @@ void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighth
 	config_save(ctx, "config.json");
 }
 
+int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len) {
+	return survive_load_htc_config_format(so, ct0conf, len);
+}
 void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id )
 {
 	if( so->PoserFn )
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 256b70b..4411efb 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -1688,7 +1688,7 @@ static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int
 	  fclose( f );
 	}
 
-	return survive_load_htc_config_format(ct0conf, len, so);
+	return so->ctx->configfunction(so, ct0conf, len);
 }
 
 
@@ -1712,14 +1712,12 @@ void init_SurviveObject(SurviveObject* so) {
 
 int DriverRegHTCVive( SurviveContext * ctx )
 {
-	const char* playback_dir = config_read_str(ctx->global_config_values,
-						   "PlaybackDir", "");
+	const char *playback_dir = config_read_str(ctx->global_config_values, "PlaybackFile", "");
 	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);
@@ -1738,19 +1736,20 @@ int DriverRegHTCVive( SurviveContext * ctx )
 	#endif
 
 	//USB must happen last.
-	if( r = survive_usb_init( sv, hmd, wm0, wm1, tr0, ww0) )
-	{
-		//TODO: Cleanup any libUSB stuff sitting around.
-		goto fail_gracefully;
+		if (survive_usb_init(sv, hmd, wm0, wm1, tr0, ww0)) {
+			// TODO: Cleanup any libUSB stuff sitting around.
+			goto fail_gracefully;
 	}
 
 	//Next, pull out the config stuff.
-	if( sv->udev[USB_DEV_HMD_IMU_LH]       && LoadConfig( sv, hmd, 1, 0, 0 )) { SV_INFO( "HMD config issue." ); }
+	if (sv->udev[USB_DEV_HMD_IMU_LH] && LoadConfig(sv, hmd, 1, 0, 0)) {
+		SV_INFO("HMD config issue.");
+	}
 	if( sv->udev[USB_DEV_WATCHMAN1] && LoadConfig( sv, wm0, 2, 0, 1 )) { SV_INFO( "Watchman 0 config issue." ); }
 	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." ); }
-	
+
 	//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 ); }