Merge branch 'internalize_record'

8 files changed, 208 insertions, 165 deletions

diff --git a/README.md b/README.md
index 518a985..4b18c43 100644
--- a/README.md
+++ b/README.md
@@ -268,25 +268,25 @@ Compiling this minimal example only requires the include path for survive.h as w
 
  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.
 
-# Playback
+# Record / Playback
 
 libsurvive has an integrated tool that allows you to record and playback streams from all supported devices. To save off a stream, invoke it as follows:
 
 ```
 make
-./data_recorder -o my_playback_file
+./data_recorder --record my_playback_file
 ```
 
 This gives you a file -- my_playback_file -- with all the device configurations and events file you need to replay it.
 
 You can also just let it stream to standard output, but this tends to be a lot of information. 
 
-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.
+To actually replay it, put that directory path in the 'playback' 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.
 
 You can also replay it just with command line options:
 
 ```
-./calibrate --playbackfile my_playback_file
+./calibrate --playback my_playback_file
 ```
 
 ## Playback speed
diff --git a/data_recorder.c b/data_recorder.c
index 7e5384a..eef233d 100644
--- a/data_recorder.c
+++ b/data_recorder.c
@@ -1,169 +1,30 @@
-// Data recorder mod with GUI showing light positions.
-
-#ifdef __linux__
-#include <unistd.h>
-#endif
-
-#include <os_generic.h>
-#include <stdarg.h>
-#include <stdint.h>
+/**
+ * Most of the data recorder functionality lives in the library now; this just enables
+ * stdout by default if no record file is specified.
+ */
 #include <stdio.h>
-#include <stdlib.h>
 #include <string.h>
 #include <survive.h>
 
-#include <time.h>
-
-#ifndef _MSC_VER
-#include <sys/time.h>
-#endif
-
-#include "redist/os_generic.h"
-
-struct SurviveContext *ctx;
-
-bool alwaysWriteStdOut = false;
-FILE *output_file = 0;
-
-double timestamp_in_us() {
-	static double start_time_us = 0;
-	if (start_time_us == 0)
-		start_time_us = OGGetAbsoluteTime();
-	return OGGetAbsoluteTime() - start_time_us;
-}
-
-void write_to_output(const char *format, ...) {
-	double ts = timestamp_in_us();
-
-	va_list args;
-	va_start(args, format);
-	fprintf(output_file, "%0.6f ", ts);
-	vfprintf(output_file, format, args);
-	va_end(args);
-
-	if (alwaysWriteStdOut) {
-		va_list args;
-		va_start(args, format);
-		fprintf(stdout, "%0.6f ", ts);
-		vfprintf(stdout, format, args);
-		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 *lh_pose, SurvivePose *obj) {
-	survive_default_lighthouse_pose_process(ctx, lighthouse, lh_pose, obj);
-	write_to_output("%d LH_POSE %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", lighthouse, lh_pose->Pos[0],
-					lh_pose->Pos[1], lh_pose->Pos[2], lh_pose->Rot[0], lh_pose->Rot[1], lh_pose->Rot[2],
-					lh_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("%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]);
-}
-
-void my_info_process(SurviveContext *ctx, const char *fault) { write_to_output("INFO LOG %s\n", fault); }
-void my_angle_process(struct SurviveObject *so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle,
-					  uint32_t lh) {
-	survive_default_angle_process(so, sensor_id, acode, timecode, length, angle, lh);
-	write_to_output("%s A %d %d %u %0.6f %0.6f %u\n", so->codename, sensor_id, acode, timecode, length, angle, lh);
-}
-
-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 == -1) {
-		write_to_output("%s S %d %d %d %u %u %u\n", so->codename, sensor_id, acode, timeinsweep, timecode, length, lh);
-		return;
-	}
-
-	const char *LH_ID = 0;
-	const char *LH_Axis = 0;
-
-	switch (acode) {
-	case 0:
-	case 2:
-		LH_ID = "L";
-		LH_Axis = "X";
-		break;
-	case 1:
-	case 3:
-		LH_ID = "L";
-		LH_Axis = "Y";
-		break;
-	case 4:
-	case 6:
-		LH_ID = "R";
-		LH_Axis = "X";
-		break;
-	case 5:
-	case 7:
-		LH_ID = "R";
-		LH_Axis = "Y";
-		break;
-	}
-	write_to_output("%s %s %s %d %d %d %u %u %u\n", so->codename, LH_ID, LH_Axis, sensor_id, acode, timeinsweep,
-					timecode, length, lh);
-}
-
-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("%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);
-}
-
 int main(int argc, char **argv) {
-	ctx = survive_init(argc, argv);
+	SurviveContext *ctx = survive_init(argc, argv);
 	if (ctx == 0) // implies -help or similiar
 		return 0;
 
-	const char *outfile = survive_configs(ctx, "o", SC_GET, "");
-	if (strlen(outfile) > 0) {
-		output_file = fopen(outfile, "w");
-		if (output_file == 0) {
-			fprintf(stderr, "Could not open %s for writing\n", argv[1]);
-			return -1;
-		}
-		alwaysWriteStdOut = survive_configi(ctx, "stdout", SC_GET, 0);
-	} else {
-		output_file = stdout;
-		alwaysWriteStdOut = false;
+	const char *dataout_file = survive_configs(ctx, "record", SC_SETCONFIG, "");
+	if (strlen(dataout_file) == 0) {
+		survive_configi(ctx, "record-stdout", SC_SET | SC_OVERRIDE, 1);
 	}
 
-	survive_install_htc_config_fn(ctx, 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_install_angle_fn(ctx, my_angle_process);
-	survive_install_info_fn(ctx, my_info_process);
-
 	survive_startup(ctx);
 	if (survive_configi(ctx, "calibrate", SC_GET, 1)) {
-		fprintf(stderr, "Installing calibration\n");
+		SV_INFO("Installing calibration");
 		survive_cal_install(ctx);
 	}
 
-	if (!ctx) {
-		fprintf(stderr, "Fatal. Could not start\n");
-		exit(1);
-	}
-
 	while (survive_poll(ctx) == 0) {
 	}
 
+	survive_close(ctx);
 	return 0;
 }
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index cb144bd..0b85bf3 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -159,6 +159,8 @@ typedef struct
 
 typedef enum { SURVIVE_STOPPED = 0, SURVIVE_RUNNING, SURVIVE_CLOSING, SURVIVE_STATE_MAX } SurviveState;
 
+struct SurviveRecordingData;
+
 struct SurviveContext
 {
 	text_feedback_func faultfunction;
@@ -180,7 +182,7 @@ struct SurviveContext
 	int activeLighthouses;
 	BaseStationData bsd[NUM_LIGHTHOUSES];
 	SurviveCalData * calptr; //If and only if the calibration subsystem is attached.
-
+	struct SurviveRecordingData *recptr; // Iff recording is attached
 	SurviveObject ** objs;
 	int objs_ct;
 
diff --git a/src/survive.c b/src/survive.c
index 2f3034b..9209e7c 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -10,6 +10,7 @@
 #include "os_generic.h"
 #include "survive_config.h"
 #include "survive_default_devices.h"
+#include "survive_playback.h"
 
 #ifdef __APPLE__
 #define z_const const
@@ -239,6 +240,9 @@ int survive_startup(SurviveContext *ctx) {
 	const char *DriverName;
 
 	i = 0;
+
+	survive_install_recording(ctx);
+
 	while ((DriverName = GetDriverNameMatching("DriverReg", i++))) {
 		DeviceDriver dd = GetDriver(DriverName);
 		SV_INFO("Loading driver %s (%p) (%d)", DriverName, dd, i);
diff --git a/src/survive_playback.c b/src/survive_playback.c
index 54b97f6..9261bb5 100644
--- a/src/survive_playback.c
+++ b/src/survive_playback.c
@@ -12,6 +12,139 @@
 #include "survive_default_devices.h"
 
 #include "os_generic.h"
+#include "stdarg.h"
+
+typedef struct SurviveRecordingData {
+	bool alwaysWriteStdOut;
+	FILE *output_file;
+} SurviveRecordingData;
+
+static 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 void write_to_output(SurviveRecordingData *recordingData, const char *format, ...) {
+	double ts = timestamp_in_us();
+
+	if (recordingData->output_file) {
+		va_list args;
+		va_start(args, format);
+		fprintf(recordingData->output_file, "%0.6f ", ts);
+		vfprintf(recordingData->output_file, format, args);
+		va_end(args);
+	}
+
+	if (recordingData->alwaysWriteStdOut) {
+		va_list args;
+		va_start(args, format);
+		fprintf(stdout, "%0.6f ", ts);
+		vfprintf(stdout, format, args);
+		va_end(args);
+	}
+}
+void survive_recording_config_process(SurviveObject *so, char *ct0conf, int len) {
+	SurviveRecordingData *recordingData = so->ctx->recptr;
+	if (recordingData == 0)
+		return;
+
+	char *buffer = malloc(len);
+	memcpy(buffer, ct0conf, len);
+	for (int i = 0; i < len; i++)
+		if (buffer[i] == '\n')
+			buffer[i] = ' ';
+
+	write_to_output(recordingData, "%s CONFIG %.*s\n", so->codename, len, buffer);
+}
+
+void survive_recording_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lh_pose,
+										  SurvivePose *obj) {
+	SurviveRecordingData *recordingData = ctx->recptr;
+	if (recordingData == 0)
+		return;
+
+	write_to_output(recordingData, "%d LH_POSE %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f %0.6f\n", lighthouse,
+					lh_pose->Pos[0], lh_pose->Pos[1], lh_pose->Pos[2], lh_pose->Rot[0], lh_pose->Rot[1],
+					lh_pose->Rot[2], lh_pose->Rot[3]);
+}
+void survive_recording_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
+	SurviveRecordingData *recordingData = so->ctx->recptr;
+	if (recordingData == 0)
+		return;
+
+	write_to_output(recordingData, "%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]);
+}
+
+void survive_recording_info_process(SurviveContext *ctx, const char *fault) {
+	SurviveRecordingData *recordingData = ctx->recptr;
+	if (recordingData == 0)
+		return;
+
+	write_to_output(recordingData, "INFO LOG %s\n", fault);
+}
+
+void survive_recording_angle_process(struct SurviveObject *so, int sensor_id, int acode, uint32_t timecode, FLT length,
+									 FLT angle, uint32_t lh) {
+	SurviveRecordingData *recordingData = so->ctx->recptr;
+	if (recordingData == 0)
+		return;
+
+	write_to_output(recordingData, "%s A %d %d %u %0.6f %0.6f %u\n", so->codename, sensor_id, acode, timecode, length,
+					angle, lh);
+}
+
+void survive_recording_light_process(struct SurviveObject *so, int sensor_id, int acode, int timeinsweep,
+									 uint32_t timecode, uint32_t length, uint32_t lh) {
+	SurviveRecordingData *recordingData = so->ctx->recptr;
+	if (recordingData == 0)
+		return;
+
+	if (acode == -1) {
+		write_to_output(recordingData, "%s S %d %d %d %u %u %u\n", so->codename, sensor_id, acode, timeinsweep,
+						timecode, length, lh);
+		return;
+	}
+
+	const char *LH_ID = 0;
+	const char *LH_Axis = 0;
+
+	switch (acode) {
+	case 0:
+	case 2:
+		LH_ID = "L";
+		LH_Axis = "X";
+		break;
+	case 1:
+	case 3:
+		LH_ID = "L";
+		LH_Axis = "Y";
+		break;
+	case 4:
+	case 6:
+		LH_ID = "R";
+		LH_Axis = "X";
+		break;
+	case 5:
+	case 7:
+		LH_ID = "R";
+		LH_Axis = "Y";
+		break;
+	}
+	write_to_output(recordingData, "%s %s %s %d %d %d %u %u %u\n", so->codename, LH_ID, LH_Axis, sensor_id, acode,
+					timeinsweep, timecode, length, lh);
+}
+
+void survive_recording_imu_process(struct SurviveObject *so, int mask, FLT *accelgyro, uint32_t timecode, int id) {
+	SurviveRecordingData *recordingData = so->ctx->recptr;
+	if (recordingData == 0)
+		return;
+
+	write_to_output(recordingData, "%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);
+}
 
 struct SurvivePlaybackData {
 	SurviveContext *ctx;
@@ -24,13 +157,6 @@ struct SurvivePlaybackData {
 };
 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;
@@ -174,8 +300,30 @@ static int playback_close(struct SurviveContext *ctx, void *_driver) {
 	return 0;
 }
 
+void survive_install_recording(SurviveContext *ctx) {
+	const char *dataout_file = survive_configs(ctx, "record", SC_SETCONFIG, "");
+	int record_to_stdout = survive_configi(ctx, "record-stdout", SC_SETCONFIG, 0);
+
+	if (strlen(dataout_file) > 0 || record_to_stdout) {
+		ctx->recptr = calloc(1, sizeof(struct SurviveRecordingData));
+
+		ctx->recptr->output_file = fopen(dataout_file, "w");
+		if (ctx->recptr->output_file == 0 && !record_to_stdout) {
+			SV_INFO("Could not open %s for writing\n", dataout_file);
+			free(ctx->recptr);
+			ctx->recptr = 0;
+			return;
+		}
+		SV_INFO("Recording to '%s'", dataout_file);
+		ctx->recptr->alwaysWriteStdOut = record_to_stdout;
+		if (record_to_stdout) {
+			SV_INFO("Recording to stdout");
+		}
+	}
+}
+
 int DriverRegPlayback(SurviveContext *ctx) {
-	const char *playback_file = survive_configs(ctx, "playbackfile", SC_SETCONFIG, "");
+	const char *playback_file = survive_configs(ctx, "playback", SC_SETCONFIG, "");
 
 	if (strlen(playback_file) == 0) {
 		return 0;
@@ -184,7 +332,7 @@ int DriverRegPlayback(SurviveContext *ctx) {
 	SurvivePlaybackData *sp = calloc(1, sizeof(SurvivePlaybackData));
 	sp->ctx = ctx;
 	sp->playback_dir = playback_file;
-	sp->time_factor = survive_configf(ctx, "playbackfactor", SC_SETCONFIG, 1.f);
+	sp->time_factor = survive_configf(ctx, "playback-factor", SC_SETCONFIG, 1.f);
 
 	printf("%s\n", playback_file);
 
@@ -195,7 +343,7 @@ int DriverRegPlayback(SurviveContext *ctx) {
 		return -1;
 	}
 
-	SV_INFO("Using playback file '%s'", playback_file);
+	SV_INFO("Using playback file '%s' with timefactor of %f", playback_file, sp->time_factor);
 	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);
diff --git a/src/survive_playback.h b/src/survive_playback.h
new file mode 100644
index 0000000..52638a9
--- /dev/null
+++ b/src/survive_playback.h
@@ -0,0 +1,17 @@
+#include <survive.h>
+
+void survive_install_recording(SurviveContext *ctx);
+void survive_recording_config_process(SurviveObject *so, char *ct0conf, int len);
+
+void survive_recording_lighthouse_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lh_pose,
+										  SurvivePose *obj);
+
+void survive_recording_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose);
+void survive_recording_info_process(SurviveContext *ctx, const char *fault);
+void survive_recording_angle_process(struct SurviveObject *so, int sensor_id, int acode, uint32_t timecode, FLT length,
+									 FLT angle, uint32_t lh);
+
+void survive_recording_light_process(struct SurviveObject *so, int sensor_id, int acode, int timeinsweep,
+									 uint32_t timecode, uint32_t length, uint32_t lh);
+
+void survive_recording_imu_process(struct SurviveObject *so, int mask, FLT *accelgyro, uint32_t timecode, int id);
diff --git a/src/survive_process.c b/src/survive_process.c
index 1402dab..6136148 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -4,6 +4,7 @@
 #include "survive_cal.h"
 #include "survive_config.h"
 #include "survive_default_devices.h"
+#include "survive_playback.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. 
@@ -18,6 +19,8 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
 		survive_cal_light( so, sensor_id, acode, timeinsweep, timecode, length, lh);
 	}
 
+	survive_recording_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lh);
+
 	//We don't use sync times, yet.
 	if (sensor_id <= -1) {
 		if (so->PoserFn) {
@@ -79,6 +82,8 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode
 
 	SurviveSensorActivations_add(&so->activations, &l);
 
+	survive_recording_angle_process(so, sensor_id, acode, timecode, length, angle, lh);
+
 	if (ctx->calptr) {
 		survive_cal_angle(so, sensor_id, acode, timecode, length, angle, lh);
 	}
@@ -130,6 +135,7 @@ void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, Sur
 	//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]);
 	so->OutPose = *pose;
 	so->FromLHPose[lighthouse] = *pose;
+	survive_recording_raw_pose_process(so, lighthouse, pose);
 }
 
 void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose,
@@ -143,9 +149,12 @@ void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighth
 
 	config_set_lighthouse(ctx->lh_config, &ctx->bsd[lighthouse], lighthouse);
 	config_save(ctx, "config.json");
+
+	survive_recording_lighthouse_process(ctx, lighthouse, lighthouse_pose, object_pose);
 }
 
 int survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len) {
+	survive_recording_config_process(so, ct0conf, 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 )
@@ -167,5 +176,7 @@ void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyrom
 	if (so->PoserFn) {
 		so->PoserFn( so, (PoserData *)&imu );
 	}
+
+	survive_recording_imu_process(so, mask, accelgyromag, timecode, id);
 }
 
diff --git a/src/survive_vive.c b/src/survive_vive.c
index cf068a3..d431207 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -1720,7 +1720,7 @@ void init_SurviveObject(SurviveObject* so) {
 
 int DriverRegHTCVive( SurviveContext * ctx )
 {
-	const char *playback_dir = survive_configs(ctx, "playbackfile", SC_SETCONFIG, "");
+	const char *playback_dir = survive_configs(ctx, "playback", SC_SETCONFIG, "");
 	if(strlen(playback_dir) != 0) {
 	  SV_INFO("Playback is active; disabling USB driver");
 	  return 0;