Merge pull request #115 from cnlohr/charles_statbiguator

Charles statbiguator

17 files changed, 379 insertions, 98 deletions

diff --git a/README.md b/README.md
index f404f39..6639946 100644
--- a/README.md
+++ b/README.md
@@ -83,7 +83,7 @@ Will ~~I~~ we succeed?  Probably not.  ~~Definitely going to try!~~ Though it's
 On Debian, ```sudo apt-get install build-essential zlib1g-dev libx11-dev libusb-1.0-0-dev freeglut3-dev``` should be sufficient.
 
 Temporarily needed packages: liblapack and libopenblas
-On Debian, ```sudo apt-get install liblapacke-dev libopenblas-dev```
+On Debian, ```sudo apt-get install liblapacke-dev libopenblas-dev libatlas-base-dev```
 
 ## Architecture
 
@@ -149,8 +149,8 @@ The default configuration of libsurvive requires both basestations and both cont
 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",
+"lighthousecount":"2",
+"defaultposer":"PoserTurveyTori",
 "RequiredTrackersForCal":"",
 "AllowAllTrackersForCal":"1",
 "ConfigPoser":"PoserTurveyTori",
@@ -170,7 +170,7 @@ Here is an example of a default configuration file that libsurvive will create a
 }
 ```
 
-To make libsurvive calibrate and run with one basestations, `LighthouseCount` needs to be changed to `1`.
+To make libsurvive calibrate and run with one basestation, `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`).
 
@@ -179,8 +179,8 @@ Lastly, to ensure libsurvive calibrates using the HMD, `AllowAllTrackersForCal`
 Here is an example for such an altered `config.json` file
 
 ```
-"LighthouseCount":"1",
-"DefaultPoser":"PoserTurveyTori",
+"lighthousecount":"1",
+"defaultposer":"PoserTurveyTori",
 "RequiredTrackersForCal":"HMD",
 "AllowAllTrackersForCal":"0",
 "ConfigPoser":"PoserTurveyTori",
diff --git a/calibrate.c b/calibrate.c
index b4325b0..e0426f7 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -14,7 +14,6 @@
 
 struct SurviveContext * ctx;
 int  quit = 0;
-static int LighthouseCount = 0;
 
 void HandleKey( int keycode, int bDown )
 {
@@ -389,10 +388,12 @@ void * GuiThread( void * jnk )
 }
 
 int SurviveThreadLoaded=0;
+int gargc;
+char * const * gargv;
 
 void * SurviveThread(void *jnk)
 {
-	ctx = survive_init( 0 );
+	ctx = survive_init( gargc, gargv );
 
 
 	uint8_t i =0;
@@ -405,6 +406,9 @@ void * SurviveThread(void *jnk)
 	survive_install_imu_fn( ctx,  my_imu_process );
 	survive_install_angle_fn( ctx, my_angle_process );
 
+	survive_startup( ctx );
+
+
 	survive_cal_install( ctx );
 
 	if( !ctx )
@@ -425,8 +429,10 @@ void * SurviveThread(void *jnk)
 }
 
 
-int main()
+int main( int argc, char ** argv )
 {
+	gargc = argc;
+	gargv = argv;
 	// Create the survive thread
     OGCreateThread( SurviveThread, 0 );
 
diff --git a/calibrate_client.c b/calibrate_client.c
index abfbabc..43ab821 100644
--- a/calibrate_client.c
+++ b/calibrate_client.c
@@ -142,7 +142,7 @@ void * GuiThread( void * v )
 
 
 
-int main()
+int main( int argc, char ** argv )
 {
 /*
 	config_init();
@@ -161,7 +161,7 @@ int main()
 //	config_save("config.json");
 */
 
-	ctx = survive_init( 1 );
+	ctx = survive_init( argc, argv );
 
 	survive_install_light_fn( ctx,  my_light_process );
 	survive_install_imu_fn( ctx,  my_imu_process );
diff --git a/data_recorder.c b/data_recorder.c
index 271b996..47c4010 100644
--- a/data_recorder.c
+++ b/data_recorder.c
@@ -128,8 +128,9 @@ int main(int argc, char **argv) {
 		output_file = stdout;
 	}
 
-	ctx = survive_init_with_config_cb(0, my_config_process);
+	ctx = survive_init( argc, argv );
 
+	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);
@@ -139,6 +140,8 @@ int main(int argc, char **argv) {
 
 	survive_cal_install(ctx);
 
+	survive_startup(ctx);
+
 	if (!ctx) {
 		fprintf(stderr, "Fatal. Could not start\n");
 		exit(1);
diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index ed2f8d1..180d83c 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -150,6 +150,8 @@ typedef struct
 	ButtonQueueEntry entry[BUTTON_QUEUE_MAX_LEN];
 } ButtonQueue;
 
+typedef enum { SURVIVE_STOPPED = 0, SURVIVE_RUNNING, SURVIVE_CLOSING, SURVIVE_STATE_MAX } SurviveState;
+
 struct SurviveContext
 {
 	text_feedback_func faultfunction;
@@ -164,6 +166,7 @@ struct SurviveContext
 
 	struct config_group* global_config_values;
 	struct config_group* lh_config; //lighthouse configs
+	struct config_group* temporary_config_values; //Set per-session, from command-line. Not saved but override global_config_values
 
 	//Calibration data:
 	int activeLighthouses;
@@ -179,31 +182,31 @@ struct SurviveContext
 	DeviceDriverMagicCb * drivermagics;
 	int driver_ct;
 
-	uint8_t isClosing; // flag to indicate if threads should terminate themselves
+	SurviveState state;
 
 	void* buttonservicethread;
 	ButtonQueue buttonQueue;
 
 	void *user_ptr;
-
 };
 
-SurviveContext *survive_init_internal(int headless, htc_config_func cfcb);
+void survive_verify_FLT_size(uint32_t user_size); // Baked in size of FLT to verify users of the library have the correct setting. 
 
-// 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, 0);
-}
-static inline SurviveContext *survive_init_with_config_cb(int headless, htc_config_func cfcb) {
+
+
+
+
+SurviveContext * survive_init_internal( int argc, char * const * argv );
+static inline SurviveContext * survive_init( int argc, char * const * argv )
+{
 	survive_verify_FLT_size(sizeof(FLT));
-	return survive_init_internal(headless, cfcb);
+	return survive_init_internal(argc, argv);
 }
 
+
 //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.
+void survive_install_htc_config_fn( SurviveContext *ctx, htc_config_func fbp );
 void survive_install_info_fn( SurviveContext * ctx,  text_feedback_func fbp );
 void survive_install_error_fn( SurviveContext * ctx,  text_feedback_func fbp );
 void survive_install_light_fn( SurviveContext * ctx,  light_process_func fbp );
@@ -212,22 +215,32 @@ 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_install_lighthouse_pose_fn(SurviveContext *ctx, lighthouse_pose_func fbp);
-
-void survive_close( SurviveContext * ctx );
+int survive_startup( SurviveContext * ctx );
 int survive_poll( SurviveContext * ctx );
+void survive_close( SurviveContext * ctx );
 
 SurviveObject * survive_get_so_by_name( SurviveContext * ctx, const char * name );
 
 //Utilitiy functions.
 int survive_simple_inflate( SurviveContext * ctx, const char * input, int inlen, char * output, int outlen );
-
 int survive_send_magic( SurviveContext * ctx, int magic_code, void * data, int datalen );
 
+//These functions search both the stored-general and temporary sections for a parameter and return it.
+#define SC_GET 0		//Get, only.
+#define SC_SET 1		//Set, if not present
+#define SC_OVERRIDE 2	//Set, to new default value.
+#define SC_SETCONFIG 4	//Set, both in-memory and config file.  Use in conjunction with SC_OVERRIDE.
+
+FLT survive_configf( SurviveContext * ctx, const char *tag, char flags, FLT def );
+uint32_t survive_configi( SurviveContext * ctx, const char *tag, char flags, uint32_t def );
+const char * survive_configs( SurviveContext * ctx, const char *tag, char flags, const char *def );
+
+
 //Install the calibrator.
 void survive_cal_install( SurviveContext * ctx );  //XXX This will be removed if not already done so.
 
 // Read back a human-readable string description of the calibration status
-int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length );
+int survive_cal_get_status( 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);
@@ -240,7 +253,9 @@ 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);
+int  survive_default_htc_config_process(SurviveObject *so, char *ct0conf, int len);
+
+
 
 ////////////////////// Survive Drivers ////////////////////////////
 
diff --git a/redist/Makefile b/redist/Makefile
index 0d74106..1437758 100644
--- a/redist/Makefile
+++ b/redist/Makefile
@@ -7,7 +7,7 @@ lintest : lintest.c linmath.c linmath.h
 	gcc -g -O0 -o $@ $^ -lm
 
 test_dcl : test_dcl.c dclhelpers.c minimal_opencv.c ../src/epnp/epnp.c
-	gcc -o $@ $^  os_generic.c -DFLT=double -lpthread -lcblas -lm -llapacke
+	gcc -o $@ $^  os_generic.c -DFLT=double -lpthread -lcblas -lm -llapacke -O3 -msse2 -ftree-vectorize 
 
 clean :
 	rm -rf *.o *~ jsmntest lintest
diff --git a/redist/json_helpers.c b/redist/json_helpers.c
index 8704a93..ebacd2c 100644
--- a/redist/json_helpers.c
+++ b/redist/json_helpers.c
@@ -188,7 +188,6 @@ void json_load_file(const char* path) {
 		if (value_t->type == JSMN_ARRAY) {
 			i += json_load_array(JSON_STRING, tokens+i+2,value_t->size, tag); //look at array children
 		} else if (value_t->type == JSMN_OBJECT) {
-			printf("Begin Object\n");
 			if (json_begin_object != NULL) json_begin_object(tag);
 			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);
@@ -200,7 +199,6 @@ void json_load_file(const char* path) {
 		if (children>=0) children--;
 		if (children == 0) {
 			children = -1;
-			printf("End Object\n");
 			if (json_end_object!=NULL) json_end_object();
 		}
 
diff --git a/redist/test_dcl.c b/redist/test_dcl.c
index dc6a93e..b2481a6 100644
--- a/redist/test_dcl.c
+++ b/redist/test_dcl.c
@@ -90,7 +90,7 @@ void compareToCblasTrans() {
 	cvMulTransposed(&Em1, &Em1tEm1, 1, 0, 1);
 	print_mat(&Em1tEm1);
 
-	test_dcldgemm_speed("Trans", 1, 0,
+	test_dcldgemm_speed("Trans", 0, 0,
 						n, // # of rows in OP(A) == em1' -- 20
 						n, // # of cols in OP(B) == em1 -- 20
 						m, // # of cols in OP(A) == em1' -- 12
diff --git a/simple_pose_test.c b/simple_pose_test.c
index b39c932..91d80d7 100644
--- a/simple_pose_test.c
+++ b/simple_pose_test.c
@@ -152,12 +152,12 @@ void *GUIThread(void*v)
 }
 
 
-int main()
+int main( int argc, char ** argv )
 {
 	int magicon = 0;
 	double Start = OGGetAbsoluteTime();
 
-	ctx = survive_init( 0 );
+	ctx = survive_init( argc, argv );
 
 	//survive_install_button_fn(ctx, testprog_button_process);
 	survive_install_raw_pose_fn(ctx, testprog_raw_pose_process);
diff --git a/src/survive.c b/src/survive.c
index bafacad..05c66d7 100755
--- a/src/survive.c
+++ b/src/survive.c
@@ -50,7 +50,7 @@ static void *button_servicer(void * context)
 	{
 		OGLockSema(ctx->buttonQueue.buttonservicesem);
 
-		if (ctx->isClosing)
+		if (ctx->state != SURVIVE_RUNNING)
 		{
 			// we're shutting down.  Close.
 			return NULL;
@@ -104,7 +104,8 @@ void survive_verify_FLT_size(uint32_t user_size) {
   }
 }
 
-SurviveContext *survive_init_internal(int headless, htc_config_func configFunc) {
+SurviveContext * survive_init_internal( int argc, char * const * argv )
+{
 #ifdef RUNTIME_SYMNUM
 	if( !did_runtime_symnum )
 	{
@@ -121,35 +122,94 @@ SurviveContext *survive_init_internal(int headless, htc_config_func configFunc)
 	MANUAL_DRIVER_REGISTRATION(PoserDaveOrtho)
 	MANUAL_DRIVER_REGISTRATION(PoserDummy)
 	MANUAL_DRIVER_REGISTRATION(DriverRegHTCVive)
-
 #endif
 
-	int r = 0;
-	int i = 0;
 	SurviveContext * ctx = calloc( 1, sizeof( SurviveContext ) );
 
-	ctx->isClosing = 0;
+	ctx->state = SURVIVE_STOPPED;
 
 	ctx->global_config_values = malloc( sizeof(config_group) );
+	ctx->temporary_config_values = malloc( sizeof(config_group) );
 	ctx->lh_config = malloc( sizeof(config_group) * NUM_LIGHTHOUSES);
 
+	//initdata
 	init_config_group(ctx->global_config_values,10);
+	init_config_group(ctx->temporary_config_values,20);
 	init_config_group(&ctx->lh_config[0],10);
 	init_config_group(&ctx->lh_config[1],10);
 
-	config_read(ctx, "config.json");
-	ctx->activeLighthouses = config_read_uint32(ctx->global_config_values, "LighthouseCount", 2);
+	//Process command-line parameters.
+	char * const * argvend = &argv[argc];
+	char * const * av = argv+1;
+	int showhelp = 0;
+	for( ; av != argvend; av++ )
+	{
+		if( (*av)[0] != '-' )
+			showhelp = 1;
+		else
+		{
+			const char * vartoupdate = 0;
+
+			switch( (*av)[1] )
+			{
+			case '-': vartoupdate = &(*av)[2];    break;
+			case 'h': showhelp = 1; break;
+			case 'p': vartoupdate = "defaultposer";	break;
+			case 'l': vartoupdate = "lighthousecount";  break;
+			case 'c': vartoupdate = "configfile";   break;
+			default:
+				fprintf( stderr, "Error: unknown parameter %s\n", *av );
+				showhelp = 1;
+			}
+
+			if( vartoupdate )
+			{
+				if( av+1 == argvend )
+				{
+					fprintf( stderr, "Error: expected parameter after %s\n", *av );
+					showhelp = 1;
+				}
+				else
+				{
+					survive_configs( ctx, *av+2, SC_OVERRIDE|SC_SET, *(av+1) );
+					av++;
+				}
+			}
+		}
+	}
+	if( showhelp )
+	{
+		//Can't use SV_ERROR here since we don't have a context to send to yet.
+		fprintf( stderr, "libsurvive - usage:\n" );
+		fprintf( stderr, " --[parameter] [value]   - sets parameter\n" );
+		fprintf( stderr, " -h                      - shows help.\n" );
+		fprintf( stderr, " -p [poser]              - use a specific defaultposer.\n" );
+		fprintf( stderr, " -l [lighthouse count]   - use a specific number of lighthoses.\n" );
+		fprintf( stderr, " -c [config file]        - set config file\n" );
+		fprintf( stderr, " -p [lighthouse count]   - use a specific number of lighthoses.\n" );
+		return 0;
+	}
+
+	config_read(ctx, survive_configs( ctx, "configfile", SC_GET, "config.json" ) );
+	ctx->activeLighthouses = survive_configi( ctx, "lighthousecount", SC_SETCONFIG, 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;
-
 	ctx->lightproc = survive_default_light_process;
 	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;
+	ctx->configfunction = survive_default_htc_config_process;
+	return ctx;
+}
+
+int survive_startup( SurviveContext * ctx )
+{
+	int r = 0;
+	int i = 0;
 
 	// initialize the button queue
 	memset(&(ctx->buttonQueue), 0, sizeof(ctx->buttonQueue));
@@ -160,11 +220,10 @@ SurviveContext *survive_init_internal(int headless, htc_config_func configFunc)
 	survive_install_button_fn(ctx, NULL);
 	survive_install_raw_pose_fn(ctx, NULL);
 
-	i = 0;
 	const char * DriverName;
 
-	//const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserDummy");
-	const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserTurveyTori");
+	//const char * PreferredPoser = survive_config_reads(ctx->global_config_values, "defaultposer", "PoserDummy");
+	const char * PreferredPoser = survive_configs( ctx, "defaultposer", SC_SETCONFIG, "PoserTurveyTori");
 	PoserCB PreferredPoserCB = 0;
 	const char * FirstPoser = 0;
 	printf( "Available posers:\n" );
@@ -190,8 +249,6 @@ SurviveContext *survive_init_internal(int headless, htc_config_func configFunc)
 		r = dd( ctx );
 		printf( "Driver %s reports status %d\n", DriverName, r );
 	}
-printf( "REGISTERING DRIVERS\n" );
-
 	//Apply poser to objects.
 	for( i = 0; i < ctx->objs_ct; i++ )
 	{
@@ -199,12 +256,14 @@ printf( "REGISTERING DRIVERS\n" );
 	}
 
 	// saving the config extra to make sure that the user has a config file they can change.
-	config_save(ctx, "config.json");
+	config_save(ctx, survive_configs( ctx, "configfile", SC_GET, "config.json" ) );
 
+	ctx->state = SURVIVE_RUNNING;
 
-	return ctx;
+	return 0;
 }
 
+
 void survive_install_info_fn( SurviveContext * ctx,  text_feedback_func fbp )
 {
 	if( fbp )
@@ -328,7 +387,7 @@ void survive_close( SurviveContext * ctx )
 	const char * DriverName;
 	int r = 0;
 
-	ctx->isClosing = 1;
+	ctx->state = SURVIVE_CLOSING;
 
 	// unlock/ post to button service semaphore so the thread can kill itself
 	OGUnlockSema(ctx->buttonQueue.buttonservicesem);
@@ -358,9 +417,10 @@ void survive_close( SurviveContext * ctx )
 	}
 
 
-	config_save(ctx, "config.json");
+	config_save(ctx, survive_configs( ctx, "configfile", SC_GET, "config.json" ) );
 
 	destroy_config_group(ctx->global_config_values);
+	destroy_config_group(ctx->temporary_config_values);
 	destroy_config_group(ctx->lh_config);
 
 	for (i = 0; i < ctx->objs_ct; i++) {
@@ -373,6 +433,7 @@ void survive_close( SurviveContext * ctx )
 	free( ctx->drivermagics );
 	free( ctx->drivercloses );
 	free( ctx->global_config_values );
+	free( ctx->temporary_config_values );
 	free( ctx->lh_config );
 
 	free( ctx );
@@ -380,8 +441,15 @@ void survive_close( SurviveContext * ctx )
 
 int survive_poll( struct SurviveContext * ctx )
 {
-	int oldct = ctx->driver_ct;
 	int i, r;
+	if( ctx->state == SURVIVE_STOPPED )
+	{
+		r = survive_startup( ctx );
+		if( r )
+			return r;
+	}
+
+	int oldct = ctx->driver_ct;
 
 	for( i = 0; i < oldct; i++ )
 	{
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 2fd96ef..cb242ae 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -8,6 +8,7 @@
 // to not include it at all on any stripped-down versions of libsurvive.
 //
 
+
 #include "survive_cal.h"
 #include "survive_internal.h"
 #include "survive_reproject.h"
@@ -118,6 +119,11 @@ void survive_cal_install( struct SurviveContext * ctx )
 	int i;
 	struct SurviveCalData * cd = ctx->calptr = calloc( 1, sizeof( struct SurviveCalData ) );
 
+	if( ctx->state != SURVIVE_RUNNING )
+	{
+		SV_ERROR( "Error: You cannot install a calibrator until the system is running." );
+	}
+
 	for( i = 0; i < NUM_LIGHTHOUSES; i++ )
 	{
 		ootx_init_decoder_context(&cd->ootx_decoders[i]);
@@ -178,21 +184,30 @@ 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", "PoserTurveyTori");
-	PoserCB PreferredPoserCB = 0;
-	const char * FirstPoser = 0;
-	printf( "Available posers:\n" );
+//	const char * PreferredPoser = survive_configs(ctx, "configposer", "PoserCharlesSlow");
+	const char * PreferredPoser = survive_configs(ctx, "configposer", SC_SETCONFIG, "PoserTurveyTori");
+
+	SV_INFO( "Trying to load poser %s for cal.", PreferredPoser );
+	PoserCB SelectedPoserCB = 0;
+	const char * SelectedPoserName = 0;
 	i = 0;	
 	while( ( DriverName = GetDriverNameMatching( "Poser", i++ ) ) )
 	{
 		PoserCB p = GetDriver( DriverName );
-		if( !PreferredPoserCB ) PreferredPoserCB = p;
+		if( !SelectedPoserCB )
+		{
+			SelectedPoserCB = p;
+			SelectedPoserName = DriverName;
+		}
 		int ThisPoser = strcmp( DriverName, PreferredPoser ) == 0;
-		if( ThisPoser ) PreferredPoserCB = p;
+		if( ThisPoser )
+		{
+			SelectedPoserCB = p;
+			SelectedPoserName = DriverName;
+		}
 	}
-	cd->ConfigPoserFn = PreferredPoserCB;
-	printf( "Got config poser: %p\n", cd->ConfigPoserFn );
+	cd->ConfigPoserFn = SelectedPoserCB;
+	SV_INFO( "Got config poser: %s (%p)", SelectedPoserName, cd->ConfigPoserFn );
 	ootx_packet_clbk = ootx_packet_clbk_d;
 
 	ctx->calptr = cd;
@@ -205,7 +220,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 	struct SurviveCalData * cd = ctx->calptr;
 
 	if( !cd ) return;
-	
+
 	switch( cd->stage )
 	{
 	default:
@@ -220,6 +235,7 @@ void survive_cal_light( struct SurviveObject * so, int sensor_id, int acode, int
 				//fprintf(stderr, "%s\n", so->codename);
 			int lhid = -sensor_id-1;
 			// Take the OOTX data from the first device.  (if using HMD, WM0, WM1 only, this will be HMD)
+
 			if( lhid < NUM_LIGHTHOUSES && so == cd->poseobjects[0]  ) 
 			{
 				uint8_t dbit = (acode & 2)>>1;
diff --git a/src/survive_config.c b/src/survive_config.c
index d923ba4..44933fa 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -181,6 +181,8 @@ FLT config_read_float(config_group *cg, const char *tag, const FLT def) {
 	return config_set_float(cg, tag, def);
 }
 
+
+
 // TODO: Do something better than this:
 #define CFG_MIN(x,y) ((x) < (y)? (x): (y))
 
@@ -441,3 +443,87 @@ void config_read(SurviveContext* sctx, const char* path) {
 	json_tag_value = NULL;
 }
 
+
+
+
+static config_entry * sc_search(SurviveContext * ctx, const char *tag )
+{
+	config_entry *cv = find_config_entry(ctx->temporary_config_values, tag);
+	if( !cv )
+	{
+		cv = find_config_entry(ctx->global_config_values, tag);
+	}
+	return cv;
+}
+
+
+
+FLT survive_configf( SurviveContext * ctx, const char *tag, char flags, FLT def )
+{
+	if( !(flags & SC_OVERRIDE) )
+	{
+		config_entry * cv = sc_search( ctx, tag );
+		if( cv )
+			return cv->numeric.f;
+	}
+
+	//If override is flagged, or, we can't find the variable, ,continue on.
+	if( flags & SC_SETCONFIG )
+	{
+		config_set_float( ctx->temporary_config_values, tag, def );
+		config_set_float( ctx->global_config_values, tag, def );
+	}
+	else if( flags & SC_SET )
+	{
+		config_set_float( ctx->temporary_config_values, tag, def );
+	}
+
+	return def;
+}
+
+uint32_t survive_configi( SurviveContext * ctx, const char *tag, char flags, uint32_t def )
+{
+	if( !(flags & SC_OVERRIDE) )
+	{
+		config_entry * cv = sc_search( ctx, tag );
+		if( cv )
+			return cv->numeric.i;
+	}
+
+	//If override is flagged, or, we can't find the variable, ,continue on.
+	if( flags & SC_SETCONFIG )
+	{
+		config_set_uint32( ctx->temporary_config_values, tag, def );
+		config_set_uint32( ctx->global_config_values, tag, def );
+	}
+	else if( flags & SC_SET )
+	{
+		config_set_uint32( ctx->temporary_config_values, tag, def );
+	}
+
+	return def;
+}
+
+const char * survive_configs( SurviveContext * ctx, const char *tag, char flags, const char *def )
+{
+	if( !(flags & SC_OVERRIDE) )
+	{
+		config_entry * cv = sc_search( ctx, tag );
+		if( cv )
+			return cv->data;
+	}
+
+	//If override is flagged, or, we can't find the variable, ,continue on.
+	if( flags & SC_SETCONFIG )
+	{
+		config_set_str( ctx->temporary_config_values, tag, def );
+		config_set_str( ctx->global_config_values, tag, def );
+	}
+	else if( flags & SC_SET )
+	{
+		config_set_str( ctx->temporary_config_values, tag, def );
+	}
+
+	return def;
+}
+
diff --git a/src/survive_config.h b/src/survive_config.h
index e2686e9..23d80c8 100644
--- a/src/survive_config.h
+++ b/src/survive_config.h
@@ -1,5 +1,7 @@
 // (C) 2017 <>< Joshua Allen, Under MIT/x11 License.
-
+//
+// This header is for handling internal parameter values.  Most accesses should be done through functions like survive_config
+//
 
 #ifndef _SURVIVE_CONFIG_H
 #define _SURVIVE_CONFIG_H
@@ -52,6 +54,7 @@ const FLT config_set_float(config_group *cg, const char *tag, const FLT value);
 const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value);
 const char* config_set_str(config_group *cg, const char *tag, const char* value);
 
+//These functions look for a parameter in a specific group, and then chose the best to return. If the parameter does not exist, default will be written.
 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, FLT* values, const FLT* def, uint8_t count);
 uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def);
diff --git a/src/survive_data.c b/src/survive_data.c
index 0427659..cb6340b 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -6,9 +6,83 @@
 #include <string.h>
 #include <math.h> /* for sqrt */
 
-#define USE_TURVEYBIGUATOR
+//#define USE_TURVEYBIGUATOR
+#define CHARLES_STATIBUATOR
 
-#ifdef USE_TURVEYBIGUATOR
+#if defined(USE_CHARLESSTATIBGUATOR)
+
+/*
+	The operating principle of 
+
+*/
+
+
+
+typedef struct
+{
+	uint32_t next_expected_time;
+} statbiguator_beamtracker;
+
+typedef struct
+{
+	statbiguator_beamtracker * beams;
+	
+	//NUM_LIGHTHOUSES first, then so->nr_locations
+
+} statbiguator_data;
+
+void handle_lightcap2( SurviveObject * so, LightcapElement * le )
+{
+	SurviveContext * ctx = so->ctx;
+
+	if (so->disambiguator_data == NULL)
+	{
+		fprintf(stderr, "Initializing Disambiguator Data\n");
+		so->disambiguator_data = malloc(sizeof(lightcap2_data));
+		memset(so->disambiguator_data, 0, sizeof(lightcap2_data));
+	}
+
+	
+	if( disambiguator_data
+}
+
+/*
+
+		ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh );
+		// first, send out the sync pulse data for the last round (for OOTX decoding
+		{
+			if (lcd->per_sweep.lh_max_pulse_length[0] != 0)
+			{
+				so->ctx->lightproc(
+					so,
+					-1,
+					handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[0]),
+					lcd->per_sweep.lh_max_pulse_length[0],
+					lcd->per_sweep.lh_start_time[0],
+					0,
+					0);
+			}
+			if (lcd->per_sweep.lh_max_pulse_length[1] != 0)
+			{
+				so->ctx->lightproc(
+					so,
+					-2,
+					handle_lightcap2_getAcodeFromSyncPulse(so, lcd->per_sweep.lh_max_pulse_length[1]),
+					lcd->per_sweep.lh_max_pulse_length[1],
+					lcd->per_sweep.lh_start_time[1],
+					0,
+					1);
+			}
+		}
+*/
+
+
+
+
+//	handle_lightcap2(so,le);
+
+
+#elif defined( USE_TURVEYBIGUATOR )
 
 static const float tau_table[33] = { 0, 0, 0, 1.151140982, 1.425, 1.5712213707, 1.656266074, 1.7110275587, 1.7490784054,
 	1.7770229476, 1.798410005, 1.8153056661, 1.8289916275, 1.8403044103, 1.8498129961, 1.8579178211,
@@ -469,6 +543,11 @@ void handle_lightcap2( SurviveObject * so, LightcapElement * le )
 #endif
 
 
+
+
+
+
+
 int32_t decode_acode(uint32_t length, int32_t main_divisor) {
 	//+50 adds a small offset and seems to help always get it right. 
 	//Check the +50 in the future to see how well this works on a variety of hardware.
@@ -513,7 +592,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 {
 	SurviveContext * ctx = so->ctx;
 
-#ifdef USE_TURVEYBIGUATOR
+#if defined (USE_TURVEYBIGUATOR) || defined(USE_CHARLESSTATIBGUATOR)
 	handle_lightcap2(so,le);
 	return;
 
@@ -605,7 +684,10 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 				{
 					so->last_sync_length[1] = 0;
 				}
+
+				//This is triggered on the first full sync pulse.
 				so->last_sync_time[ssn] = le->timestamp;
+				//printf( "A: %d\n", so->last_sync_time[ssn] );	
 				so->last_sync_length[ssn] = le->length;
 			}
 			else if( so->sync_set_number == -1 )
@@ -623,7 +705,9 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 				}
 				else
 				{
+					//This is triggered on the slave base station's sync pulse.
 					so->last_sync_time[ssn] = le->timestamp;
+					//printf( "B: %d\n", so->last_sync_time[ssn] );
 					so->last_sync_length[ssn] = le->length;
 				}
 			}
@@ -635,6 +719,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 			{
 				if( so->last_sync_time[ssn] > le->timestamp )
 				{
+					//printf( "C: %d\n", so->last_sync_time[ssn] );
 					so->last_sync_time[ssn] = le->timestamp;
 					so->last_sync_length[ssn] = le->length;
 				}
@@ -683,7 +768,7 @@ void handle_lightcap( SurviveObject * so, LightcapElement * le )
 		{
 			int whichlh;
 			if( acode < 0 ) whichlh = 1;
-			else whichlh = !(acode>>2);
+			else whichlh = (acode>>2);
 			ctx->lightproc( so, le->sensor_id, acode, offset_from, le->timestamp, le->length, whichlh );
 		}
 	}
diff --git a/src/survive_playback.c b/src/survive_playback.c
index df9fcaa..014542b 100644
--- a/src/survive_playback.c
+++ b/src/survive_playback.c
@@ -203,7 +203,7 @@ static int LoadConfig(SurvivePlaybackData *sv, SurviveObject *so) {
 }
 
 int DriverRegPlayback(SurviveContext *ctx) {
-	const char *playback_file = config_read_str(ctx->global_config_values, "PlaybackFile", "");
+	const char *playback_file = survive_configs(ctx, "playbackfile", SC_SETCONFIG, "");
 
 	if (strlen(playback_file) == 0) {
 		return 0;
@@ -212,8 +212,7 @@ int DriverRegPlayback(SurviveContext *ctx) {
 	SurvivePlaybackData *sp = calloc(1, sizeof(SurvivePlaybackData));
 	sp->ctx = ctx;
 	sp->playback_dir = playback_file;
-	sp->time_factor =
-		config_read_float(ctx->global_config_values, "PlaybackFactor", 1.);
+	sp->time_factor = survive_configf(ctx, "playbackfactor", SC_SETCONFIG, 1.f);
 
 	printf("%s\n", playback_file);
 
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 4411efb..cf068a3 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -339,6 +339,7 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
 {
 	SurviveContext * ctx = sv->ctx;
 #ifdef HIDAPI
+	SV_INFO( "Vive starting in HIDAPI mode." );
 	if( !GlobalRXUSBMutx )
 	{
 		GlobalRXUSBMutx = OGCreateMutex();
@@ -403,6 +404,8 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
 	}
 
 #else
+	SV_INFO( "Vive starting in libusb mode." );
+
 	int r = libusb_init( &sv->usbctx );
 	if( r )
 	{
@@ -495,6 +498,13 @@ int survive_usb_init( SurviveViveData * sv, SurviveObject * hmd, SurviveObject *
 	libusb_free_device_list( devs, 1 );
 #endif
 
+	//Add the drivers - this must happen BEFORE we actually attach interfaces.
+	if( sv->udev[USB_DEV_HMD_IMU_LH]  ) { survive_add_object( ctx, hmd ); }
+	if( sv->udev[USB_DEV_WATCHMAN1]   ) { survive_add_object( ctx, wm0 ); }
+	if( sv->udev[USB_DEV_WATCHMAN2]   ) { survive_add_object( ctx, wm1 ); }
+	if( sv->udev[USB_DEV_TRACKER0]    ) { survive_add_object( ctx, tr0 ); }
+	if( sv->udev[USB_DEV_W_WATCHMAN1] ) { survive_add_object( ctx, ww0 ); }
+
 	if( sv->udev[USB_DEV_HMD] && AttachInterface( sv, hmd, USB_IF_HMD,        sv->udev[USB_DEV_HMD],        0x81, survive_data_cb, "Mainboard" ) ) { return -6; }
 	if( sv->udev[USB_DEV_HMD_IMU_LH] && AttachInterface( sv, hmd, USB_IF_HMD_IMU_LH, sv->udev[USB_DEV_HMD_IMU_LH], 0x81, survive_data_cb, "Lighthouse" ) ) { return -7; }
 	if( sv->udev[USB_DEV_WATCHMAN1] && AttachInterface( sv, wm0, USB_IF_WATCHMAN1,  sv->udev[USB_DEV_WATCHMAN1],  0x81, survive_data_cb, "Watchman 1" ) ) { return -8; }
@@ -532,7 +542,6 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
 {
 	int r;
 	SurviveViveData * sv = drv;
-	printf( "*CALLING %p %p\n", ctx, sv );
 
 	//XXX TODO: Handle haptics, etc.
 	int turnon = magic_code;
@@ -700,6 +709,7 @@ int survive_vive_send_magic(SurviveContext * ctx, void * drv, int magic_code, vo
 int survive_vive_send_haptic(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh, uint16_t pulseLow, uint16_t repeatCount)
 {
 	SurviveViveData *sv = (SurviveViveData*)so->driver;
+	SurviveContext * ctx = so->ctx;
 
 	if (NULL == sv)
 	{
@@ -719,7 +729,7 @@ int survive_vive_send_haptic(SurviveObject * so, uint8_t reserved, uint16_t puls
 	//SV_INFO("UCR: %d", r);
 	if (r != sizeof(vive_controller_haptic_pulse)) 
 	{
-		printf("HAPTIC FAILED **************************\n"); 
+		SV_ERROR("HAPTIC FAILED **************************\n"); 
 		return -1;
 	}
 
@@ -1135,7 +1145,6 @@ SurviveObject *so;
 
 static void handle_watchman( SurviveObject * w, uint8_t * readdata )
 {
-
 	uint8_t startread[29];
 	memcpy( startread, readdata, 29 );
 
@@ -1438,7 +1447,6 @@ void survive_data_cb( SurviveUSBInterface * si )
 		
 	}
 #endif 
-
 	switch( si->which_interface_am_i )
 	{
 	case USB_IF_HMD:
@@ -1673,7 +1681,7 @@ static int LoadConfig( SurviveViveData * sv, SurviveObject * so, int devno, int
 	SurviveContext * ctx = sv->ctx;
 	char * ct0conf = 0;
 	int len = survive_get_config( &ct0conf, sv, devno, iface, extra_magic );
-	printf( "Loading config: %d\n", len );
+	SV_INFO( "Loading config: %d", len );
 
 	if( len < 0 )
 	{
@@ -1712,7 +1720,7 @@ void init_SurviveObject(SurviveObject* so) {
 
 int DriverRegHTCVive( SurviveContext * ctx )
 {
-	const char *playback_dir = config_read_str(ctx->global_config_values, "PlaybackFile", "");
+	const char *playback_dir = survive_configs(ctx, "playbackfile", SC_SETCONFIG, "");
 	if(strlen(playback_dir) != 0) {
 	  SV_INFO("Playback is active; disabling USB driver");
 	  return 0;
@@ -1736,11 +1744,21 @@ int DriverRegHTCVive( SurviveContext * ctx )
 	#endif
 
 	//USB must happen last.
-		if (survive_usb_init(sv, hmd, wm0, wm1, tr0, ww0)) {
+	if (survive_usb_init(sv, hmd, wm0, wm1, tr0, ww0)) {
 			// TODO: Cleanup any libUSB stuff sitting around.
 			goto fail_gracefully;
 	}
 
+	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 {
+	  SV_ERROR("No USB devices detected");
+	}
+
 	//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.");
@@ -1750,22 +1768,6 @@ int DriverRegHTCVive( SurviveContext * ctx )
 	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 ); }
-	if( sv->udev[USB_DEV_WATCHMAN2] ) { survive_add_object( ctx, wm1 ); }
-	if( sv->udev[USB_DEV_TRACKER0]  ) { survive_add_object( ctx, tr0 ); }
-	if( sv->udev[USB_DEV_W_WATCHMAN1]  ) { survive_add_object( ctx, ww0 ); }
-
-	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 4c6bcf3..8c697c1 100644
--- a/test.c
+++ b/test.c
@@ -136,12 +136,12 @@ static void dump_iface( struct SurviveObject * so, const char * prefix )
 
 
 
-int main()
+int main( int argc, char ** argv )
 {
 	int magicon = 0;
 	double Start = OGGetAbsoluteTime();
 
-	ctx = survive_init( 0 );
+	ctx = survive_init( argc, argv );
 
 	survive_install_button_fn(ctx, testprog_button_process);
 	survive_install_raw_pose_fn(ctx, testprog_raw_pose_process);