Made disambiguator configurable

6 files changed, 48 insertions, 45 deletions

diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 747d076..cb144bd 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -170,6 +170,7 @@ struct SurviveContext
 	raw_pose_func rawposeproc;
 	lighthouse_pose_func lighthouseposeproc;
 	htc_config_func configfunction;
+	handle_lightcap_func lightcapfunction;
 
 	struct config_group* global_config_values;
 	struct config_group* lh_config; //lighthouse configs
@@ -286,17 +287,6 @@ void   RegisterDriver(const char * name, void * data);
 int survive_add_object( SurviveContext * ctx, SurviveObject * obj );
 void survive_add_driver( SurviveContext * ctx, void * payload, DeviceDriverCb poll, DeviceDriverCb close, DeviceDriverMagicCb magic );
 
-//For lightcap, etc.  Don't change this structure at all.  Regular vive is dependent on it being exactly as-is.
-//When you write drivers, you can use this to send survive lightcap data.
-typedef struct
-{
-	uint8_t sensor_id;
-	uint16_t length;
-	uint32_t timestamp;
-} 
-LightcapElement;
-
-
 //This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
 void handle_lightcap( SurviveObject * so, LightcapElement * le );
 
diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h
index 3ea6253..160adda 100644
--- a/include/libsurvive/survive_types.h
+++ b/include/libsurvive/survive_types.h
@@ -53,6 +53,16 @@ typedef void (*raw_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose
 typedef void (*lighthouse_pose_func)(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose,
 									 SurvivePose *object_pose);
 
+// For lightcap, etc.  Don't change this structure at all.  Regular vive is dependent on it being exactly as-is.
+// When you write drivers, you can use this to send survive lightcap data.
+typedef struct {
+	uint8_t sensor_id;
+	uint16_t length;
+	uint32_t timestamp;
+} LightcapElement;
+
+typedef void (*handle_lightcap_func)(SurviveObject *so, LightcapElement *le);
+
 typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount);
 
 //Device drivers (prefix your drivers with "DriverReg") i.e.
diff --git a/src/survive.c b/src/survive.c
index 2c9c15f..2f3034b 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -195,6 +195,33 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) {
 	return ctx;
 }
 
+static void *setup_func_by_name(SurviveContext *ctx, const char *name, const char *configname, const char *configdef) {
+	const char *Preferred = survive_configs(ctx, configname, SC_SETCONFIG, configdef);
+	const char *DriverName = 0;
+	const char *picked = 0;
+	int i = 0;
+	void *func = 0;
+	int prefixLen = strlen(name);
+
+	SV_INFO("Available %s:", name);
+	while ((DriverName = GetDriverNameMatching(name, i++))) {
+		void *p = GetDriver(DriverName);
+
+		bool match = strcmp(DriverName, Preferred) == 0 || strcmp(DriverName + prefixLen, Preferred) == 0;
+		SV_INFO("\t%c%s", match ? '*' : ' ', DriverName + prefixLen);
+		if (!func || match) {
+			func = p;
+			picked = (DriverName + prefixLen);
+		}
+	}
+	if (!func) {
+		SV_ERROR("Error.  Cannot find any valid %s.", name);
+	}
+	SV_INFO("Totals %d %ss.  Using %s.", i - 1, name, picked);
+
+	return func;
+}
+
 int survive_startup(SurviveContext *ctx) {
 	int r = 0;
 	int i = 0;
@@ -206,26 +233,10 @@ int survive_startup(SurviveContext *ctx) {
 	// start the thread to process button data
 	ctx->buttonservicethread = OGCreateThread(button_servicer, ctx);
 
-	const char *DriverName;
+	PoserCB PreferredPoserCB = setup_func_by_name(ctx, "Poser", "defaultposer", "PoserTurveyTori");
+	ctx->lightcapfunction = setup_func_by_name(ctx, "Disambiguator", "disambiguator", "Turvey");
 
-	// 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;
-	SV_INFO("Available posers:");
-	while ((DriverName = GetDriverNameMatching("Poser", i++))) {
-		PoserCB p = GetDriver(DriverName);
-		if (!PreferredPoserCB)
-			PreferredPoserCB = p;
-		int ThisPoser = strcmp(DriverName, PreferredPoser) == 0;
-		SV_INFO("\t%c%s", ThisPoser ? '*' : ' ', DriverName);
-		if (ThisPoser)
-			PreferredPoserCB = p;
-	}
-	SV_INFO("Totals %d posers.  Using selected poser (or first!).", i - 1);
-	if (!PreferredPoserCB) {
-		SV_ERROR("Error.  Cannot find any valid poser.");
-	}
+	const char *DriverName;
 
 	i = 0;
 	while ((DriverName = GetDriverNameMatching("DriverReg", i++))) {
@@ -234,6 +245,7 @@ int survive_startup(SurviveContext *ctx) {
 		r = dd(ctx);
 		SV_INFO("Driver %s reports status %d", DriverName, r);
 	}
+
 	// Apply poser to objects.
 	for (i = 0; i < ctx->objs_ct; i++) {
 		ctx->objs[i]->PoserFn = PreferredPoserCB;
diff --git a/src/survive_charlesbiguator.c b/src/survive_charlesbiguator.c
index 8a4d71a..fbba888 100644
--- a/src/survive_charlesbiguator.c
+++ b/src/survive_charlesbiguator.c
@@ -45,7 +45,7 @@ static void HandleOOTX(SurviveContext *ctx, SurviveObject *so) {
 }
 
 // This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
-void handle_lightcap_charlesbiguator(SurviveObject *so, LightcapElement *le) {
+void DisambiguatorCharles(SurviveObject *so, LightcapElement *le) {
 	SurviveContext *ctx = so->ctx;
 	//	static int32_t last;
 	//	printf( "%d %lu %d %d\n", le->timestamp-last, le->timestamp, le->length, le->sensor_id );
@@ -191,3 +191,5 @@ void handle_lightcap_charlesbiguator(SurviveObject *so, LightcapElement *le) {
 		// Runt pulse, or no sync pulses available.
 	}
 }
+
+REGISTER_LINKTIME(DisambiguatorCharles);
diff --git a/src/survive_disambiguator.c b/src/survive_disambiguator.c
index 1ceaf27..39b23b6 100644
--- a/src/survive_disambiguator.c
+++ b/src/survive_disambiguator.c
@@ -1,14 +1,3 @@
 #include "survive.h"
 
-#define USE_TURVEYBIGUATOR
-
-void handle_lightcap_charlesbiguator(SurviveObject *so, LightcapElement *le);
-void handle_lightcap_turveybiguator(SurviveObject *so, LightcapElement *le);
-
-void handle_lightcap(SurviveObject *so, LightcapElement *le) {
-#ifdef USE_TURVEYBIGUATOR
-	handle_lightcap_turveybiguator(so, le);
-#else
-	handle_lightcap_charlesbiguator(so, le);
-#endif
-}
+void handle_lightcap(SurviveObject *so, LightcapElement *le) { so->ctx->lightcapfunction(so, le); }
diff --git a/src/survive_turveybiguator.c b/src/survive_turveybiguator.c
index bc69938..8dbce46 100644
--- a/src/survive_turveybiguator.c
+++ b/src/survive_turveybiguator.c
@@ -282,7 +282,6 @@ static void handle_lightcap2_sync(SurviveObject *so, LightcapElement *le) {
 	// static unsigned int recent_sync_time = 0;
 	// static unsigned int recent_sync_count = -1;
 	// static unsigned int activeSweepStartTime;
-
 	int acode = handle_lightcap2_getAcodeFromSyncPulse(so, le->length); // acode for this sensor reading
 
 	// Process any sweep data we have
@@ -435,7 +434,7 @@ static void handle_lightcap2_sweep(SurviveObject *so, LightcapElement *le) {
 	}
 }
 
-void handle_lightcap_turveybiguator(SurviveObject *so, LightcapElement *le) {
+void DisambiguatorTurvey(SurviveObject *so, LightcapElement *le) {
 	SurviveContext *ctx = so->ctx;
 
 	if (so->disambiguator_data == NULL) {
@@ -463,3 +462,4 @@ void handle_lightcap_turveybiguator(SurviveObject *so, LightcapElement *le) {
 	handle_lightcap2_sweep(so, le);
 }
 
+REGISTER_LINKTIME(DisambiguatorTurvey);