Added notes before major overhaul

2 files changed, 173 insertions, 56 deletions

diff --git a/src/survive_tb_disambiguator.c b/src/survive_tb_disambiguator.c
index 5d79429..b4dd0ea 100644
--- a/src/survive_tb_disambiguator.c
+++ b/src/survive_tb_disambiguator.c
@@ -8,6 +8,46 @@
 
 #define NUM_HISTORY 3
 
+/**
+ * The lighthouses go in the following order:
+ *
+ *     Ticks  State
+ *         0  ACode 0b1x0 (4)
+ *    20 000  ACode 0b0x0 (0)
+ *            LH A X Sweep
+ *   400 000  ACode 0b1x1 (5)
+ *   420 000  ACode 0b0x1 (1)
+ *            LH A Y SWEEP
+ *   800 000  ACode 0b0x0 (0)
+ *   820 000  ACode 0b1x0 (4)
+ *            LH B X Sweep
+ * 1 200 000  ACode 0b0x1 (1)
+ * 1 220 000  ACode 0b1x1 (5)
+ *            LH B Y SWEEP
+ * 1 600 000  < REPEAT >
+ *
+ * NOTE: Obviously you cut the data bit out for this
+ */
+
+enum LighthouseState {
+	LS_UNKNOWN = 0
+
+	LS_WaitLHA_ACode4 = 1,
+	LS_WaitLHA_ACode0,
+	LS_SweepAX,
+	LS_WaitLHA_ACode5,
+	LS_WaitLHA_ACode1,
+	LS_SweepAY,
+	LS_WaitLHB_ACode0,
+	LS_WaitLHB_ACode4,
+	LS_SweepBX,
+	LS_WaitLHB_ACode1,
+	LS_WaitLHB_ACode5,
+	LS_SweepBY,
+
+	LS_END
+};
+
 enum LightcapClassification { LCC_UNKNOWN = 0, LCC_SYNC = 1, LCC_SWEEP = 2 };
 
 typedef struct {
@@ -17,12 +57,19 @@ typedef struct {
 } SensorHistory_t;
 
 typedef struct {
-	LightcapElement last_sync;
+	uint64_t last_sync_timestamp;
+	uint64_t last_sync_length;
+	int last_sync_count;
+	bool lastWasSync;
+
+	uint32_t mod_offset;
+	LighthouseState state;
+
 	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);
+	return calloc(1, sizeof(Disambiguator_data_t) + sizeof(SensorHistory_t) * so->sensor_ct);
 }
 
 static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) {
@@ -31,11 +78,67 @@ static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) {
 	return (0xFFFFFFFF - prior) + recent;
 }
 
+static int find_acode(uint32_t pulseLen) {
+	const static int offset = 0;
+	if (pulseLen < 2200 + 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;
+}
+
 #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 bool overlaps(const LightcapElement *a, const LightcapElement *b) {
+	int overlap = 0;
+	if (a->timestamp < b->timestamp && a->length + a->timestamp > b->timestamp)
+		overlap = a->length + a->timestamp - b->timestamp;
+	else if (b->timestamp < a->timestamp && b->length + b->timestamp > a->timestamp)
+		overlap = b->length + b->timestamp - a->timestamp;
+
+	return overlap > a->length / 2;
+}
+
+const int SKIP_BIT = 4;
+const int DATA_BIT = 2;
+const int AXIS_BIT = 1;
+
+LightcapElement get_last_sync(Disambiguator_data_t *d) {
+	if (d->last_sync_count == 0) {
+		return (LightcapElement){0};
+	}
+
+	return (LightcapElement){.timestamp = (d->last_sync_timestamp + d->last_sync_count / 2) / d->last_sync_count,
+							 .length = (d->last_sync_length + d->last_sync_count / 2) / d->last_sync_count,
+							 .sensor_id = -d->last_sync_count};
+}
+
+static uint32_t next_sync_expected(Disambiguator_data_t *d) {
+	int acode = find_acode(get_last_sync(d).length);
+	if (acode & SKIP_BIT)
+		return get_last_sync(d).timestamp + 20000;
+	return get_last_sync(d).timestamp + 399840;
+}
+
 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;
@@ -44,17 +147,24 @@ static enum LightcapClassification classify(Disambiguator_data_t *d, SensorHisto
 		return LCC_SWEEP;
 	}
 
-	uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, d->last_sync.timestamp);
+	uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, get_last_sync(d).timestamp);
 	uint32_t split_time = 399840; // 8.33ms in 48mhz
-	uint32_t jitter_allowance = 4000;
+	uint32_t jitter_allowance = 20000;
 
 	// 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)) {
+	if (get_last_sync(d).length > 0 && abs(timestamp_diff(le->timestamp, next_sync_expected(d))) < jitter_allowance) {
 		return LCC_SYNC;
 	}
 
-	if (d->last_sync.length > 0 && time_diff_last_sync < (split_time - jitter_allowance)) {
+	LightcapElement last_sync = get_last_sync(d);
+	if (get_last_sync(d).length > 0 && overlaps(&last_sync, le)) {
+		return LCC_SYNC;
+	}
+
+	if (le->length > 2000)
+		return LCC_SYNC;
+
+	if (get_last_sync(d).length > 0 && time_diff_last_sync < (split_time - jitter_allowance)) {
 		return LCC_SWEEP;
 	}
 
@@ -62,7 +172,7 @@ static enum LightcapClassification classify(Disambiguator_data_t *d, SensorHisto
 	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) {
+	if (time_diff > split_time) {
 		fprintf(stderr, "Time diff too high %d\n", time_diff);
 		return LCC_UNKNOWN;
 	}
@@ -79,43 +189,22 @@ static enum LightcapClassification update_histories(Disambiguator_data_t *d, con
 	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;
+
+	// Note, this happens if we don't have config yet -- just bail
+	if (so->sensor_ct == 0) {
+		return;
+	}
+
 	if (so->disambiguator_data == NULL) {
-		SV_INFO("Initializing Disambiguator Data");
+		SV_INFO("Initializing Disambiguator Data for TB %d", so->sensor_ct);
 		so->disambiguator_data = Disambiguator_data_t_ctor(so);
 	}
 
@@ -126,18 +215,56 @@ void DisambiguatorTimeBased(SurviveObject *so, const LightcapElement *le) {
 	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);
+	uint32_t time_diff_last_sync = timestamp_diff(le->timestamp, get_last_sync(d).timestamp);
+	if (time_diff_last_sync > (0xFFFFFFFF / 2))
+		time_diff_last_sync = 0xFFFFFFFFF - time_diff_last_sync;
+
+	LightcapElement lastSync = get_last_sync(d);
+	int acode = find_acode(lastSync.length);
+
 	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);
+		LightcapElement lastSync = get_last_sync(d);
+		if (d->lastWasSync == false || overlaps(&lastSync, le) == false) {
+
+			if (lastSync.length) {
+				int acode = find_acode(lastSync.length);
+				SV_INFO("%.03f(%d)\tacode: %d 0x%x a:%d d:%d s:%d (%d)",
+						timestamp_diff(le->timestamp, lastSync.timestamp) / 48000.,
+						timestamp_diff(le->timestamp, lastSync.timestamp), lastSync.length, acode, acode & 1,
+						(bool)(acode & 2), (bool)(acode & 4), ((acode >> 1) & 0x2) | (acode & 1));
+				assert(acode != -1);
+			}
+			d->last_sync_timestamp = le->timestamp;
+			d->last_sync_length = le->length;
+			d->last_sync_count = 1;
+		} else {
+			d->last_sync_timestamp += le->timestamp;
+			d->last_sync_length += le->length;
+			d->last_sync_count++;
+		}
+
+		d->lastWasSync = true;
+
+		lastSync = get_last_sync(d);
+		// SV_INFO("acode building: %u %u %u", lastSync.length, lastSync.timestamp, lastSync.length +
+		// lastSync.timestamp);
+	} else {
+		if (d->lastWasSync) {
+			if (lastSync.length) {
+				int acode = find_acode(lastSync.length);
+				SV_INFO("start acode: %d 0x%x a:%d d:%d s:%d (%d)", lastSync.length, acode, acode & 1,
+						(bool)(acode & 2), (bool)(acode & 4), ((acode >> 1) & 0x2) | (acode & 1));
+				assert(acode != -1);
+			}
+		}
+		d->lastWasSync = false;
 	}
+
+	if (classification == LCC_SWEEP)
+		SV_INFO("%.02fms Classification %d\t%d\t%d\t%u\t%u\t(%.02fms)\ttime since last sync: %.02fms a:%d d:%d s:%d",
+				(double)le->timestamp / 48000, classification, le->sensor_id, le->length, le->timestamp, time_diff,
+				(double)time_diff / 48000.0, (double)time_diff_last_sync / 48000., acode & 1, (bool)(acode & 2),
+				(bool)(acode & 4));
 }
 
 REGISTER_LINKTIME(DisambiguatorTimeBased);
diff --git a/src/survive_vive.c b/src/survive_vive.c
index 91f25af..3c60b2a 100755
--- a/src/survive_vive.c
+++ b/src/survive_vive.c
@@ -1708,16 +1708,6 @@ int survive_vive_close( SurviveContext * ctx, void * driver )
 	return 0;
 }
 
-void init_SurviveObject(SurviveObject* so) {
-	so->acc_scale = NULL;
-	so->acc_bias = NULL;
-	so->gyro_scale = NULL;
-	so->gyro_bias = NULL;
-	so->haptic = NULL;
-	so->PoserData = NULL;
-	so->disambiguator_data = NULL;
-}
-
 int DriverRegHTCVive( SurviveContext * ctx )
 {
 	const char *playback_dir = survive_configs(ctx, "playback", SC_GET, "");