Initial pass

2 files changed, 145 insertions, 2 deletions

diff --git a/Makefile b/Makefile
index f95ca61..e0c5306 100644
--- a/Makefile
+++ b/Makefile
@@ -2,7 +2,7 @@ all : lib data_recorder test calibrate calibrate_client simple_pose_test
 
 CC?=gcc
 
-CFLAGS:=-Iinclude/libsurvive -fPIC -g -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke  -lcblas -lm #-Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable
+CFLAGS:=-Iinclude/libsurvive -fPIC -g -O0 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke  -lcblas -lm #-Wall -Wno-unused-variable -Wno-switch -Wno-unused-but-set-variable
 CFLAGS_RELEASE:=-Iinclude/libsurvive -fPIC -msse2 -ftree-vectorize -O3 -Iredist -flto -DUSE_DOUBLE -std=gnu99 -rdynamic -llapacke  -lcblas -lm
 
 
@@ -39,7 +39,7 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/minimal_ope
 ifeq ($(UNAME), Darwin)
 REDISTS:=$(REDISTS) redist/hid-osx.c
 endif
-LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.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 src/survive_reproject.o src/poser.o src/epnp/epnp.c src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o
+LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_charlesbiguator.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 src/survive_reproject.o src/poser.o src/epnp/epnp.c src/survive_sensor_activations.o src/survive_turveybiguator.o src/survive_disambiguator.o src/survive_tb_disambiguator.o
 
 #If you want to use HIDAPI on Linux.
 #CFLAGS:=$(CFLAGS) -DHIDAPI
diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c
new file mode 100644
index 0000000..5d79429
--- /dev/null
+++ b/src/survive_tb_disambiguator.c
@@ -0,0 +1,143 @@
+//
+#include "survive_internal.h"
+#include <assert.h>
+#include <math.h> /* for sqrt */
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+
+#define NUM_HISTORY 3
+
+enum LightcapClassification { LCC_UNKNOWN = 0, LCC_SYNC = 1, LCC_SWEEP = 2 };
+
+typedef struct {
+	LightcapElement history[NUM_HISTORY];
+	enum LightcapClassification classifications[NUM_HISTORY];
+	int idx;
+} SensorHistory_t;
+
+typedef struct {
+	LightcapElement last_sync;
+	SensorHistory_t histories[];
+} Disambiguator_data_t;
+
+Disambiguator_data_t *Disambiguator_data_t_ctor(SurviveObject *so) {
+	return calloc(sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct, 1);
+}
+
+static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) {
+	if (recent > prior)
+		return recent - prior;
+	return (0xFFFFFFFF - prior) + recent;
+}
+
+#define LOWER_SYNC_TIME 2250
+#define UPPER_SYNC_TIME 6750
+
+static int circle_buffer_get(int idx, int offset) { return ((idx + offset) + NUM_HISTORY) % NUM_HISTORY; }
+
+static enum LightcapClassification classify(Disambiguator_data_t *d, SensorHistory_t *history,
+											const LightcapElement *le) {
+	bool clearlyNotSync = le->length < LOWER_SYNC_TIME || le->length > UPPER_SYNC_TIME;
+
+	if (clearlyNotSync) {
+		return LCC_SWEEP;
+	}
+
+	uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, d->last_sync.timestamp);
+	uint32_t split_time = 399840; // 8.33ms in 48mhz
+	uint32_t jitter_allowance = 4000;
+
+	// If we are ~8.33ms ahead of the last sync; we are a sync
+	if (d->last_sync.length > 0 && time_diff_last_sync < (split_time + jitter_allowance) &&
+		time_diff_last_sync > (split_time - jitter_allowance)) {
+		return LCC_SYNC;
+	}
+
+	if (d->last_sync.length > 0 && time_diff_last_sync < (split_time - jitter_allowance)) {
+		return LCC_SWEEP;
+	}
+
+	int prevIdx = circle_buffer_get(history->idx, -1);
+	uint32_t time_diff = timestamp_diff(le->timestamp, history->history[prevIdx].timestamp);
+
+	// We don't have recent data; unclear
+	if (time_diff > split_time - jitter_allowance) {
+		fprintf(stderr, "Time diff too high %d\n", time_diff);
+		return LCC_UNKNOWN;
+	}
+
+	switch (history->classifications[prevIdx]) {
+	case LCC_SWEEP:
+		return LCC_SYNC;
+	}
+	fprintf(stderr, "last not sweep\n");
+	return LCC_UNKNOWN;
+}
+
+static enum LightcapClassification update_histories(Disambiguator_data_t *d, const LightcapElement *le) {
+	SensorHistory_t *history = &d->histories[le->sensor_id];
+
+	enum LightcapClassification classification = classify(d, history, le);
+
+	history->classifications[history->idx] = classification;
+	history->history[history->idx] = *le;
+	history->idx = (history->idx + 1) % NUM_HISTORY;
+
+	return classification;
+}
+
+static int find_acode(uint32_t pulseLen) {
+	const static int offset = 50;
+	if (pulseLen < 2500 - offset)
+		return -1;
+
+	if (pulseLen < 3000 + offset)
+		return 0;
+	if (pulseLen < 3500 + offset)
+		return 1;
+	if (pulseLen < 4000 + offset)
+		return 2;
+	if (pulseLen < 4500 + offset)
+		return 3;
+	if (pulseLen < 5000 + offset)
+		return 4;
+	if (pulseLen < 5500 + offset)
+		return 5;
+	if (pulseLen < 6000 + offset)
+		return 6;
+	if (pulseLen < 6500 + offset)
+		return 7;
+
+	return -1;
+}
+
+void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) {
+	SurviveContext *ctx = so->ctx;
+	if (so->disambiguator_data == NULL) {
+		SV_INFO("Initializing Disambiguator Data");
+		so->disambiguator_data = Disambiguator_data_t_ctor(so);
+	}
+
+	Disambiguator_data_t *d = so->disambiguator_data;
+
+	SensorHistory_t *history = &d->histories[le->sensor_id];
+	int prevIdx = circle_buffer_get(history->idx, -1);
+	uint32_t time_diff = timestamp_diff(le->timestamp, history->history[prevIdx].timestamp);
+	enum LightcapClassification classification = update_histories(d, le);
+
+	SV_INFO("Classification %d\t%d\t%d\t%u\t%u(%.02fms)", classification, le->sensor_id, le->length, le->timestamp,
+			time_diff, (double)time_diff / 48000.0);
+	if (classification == LCC_SYNC) {
+		uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, d->last_sync.timestamp);
+		if (time_diff_last_sync > (0xFFFFFFFF / 2))
+			time_diff_last_sync = 0xFFFFFFFFF - time_diff_last_sync;
+		d->last_sync = *le;
+		int acode = find_acode(le->length);
+		SV_INFO("acode: %d 0x%x a:%d d:%d s:%d (%.02fms)", le->length, acode, acode & 1, (bool)(acode & 2),
+				(bool)(acode & 4), (double)time_diff_last_sync / 48000.);
+		assert(acode != -1);
+	}
+}
+
+REGISTER_LINKTIME(DisambiguatorTimeBased);