aboutsummaryrefslogtreecommitdiff
path: root/src/survive_statebased_disambiguator.c
blob: 6b217ee3864f60650173f86dda8548a849c78717 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
//
#include "survive_internal.h"
#include <assert.h>
#include <math.h> /* for sqrt */
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

//#define DEBUG_TB(...) SV_INFO(__VA_ARGS__)
#define DEBUG_TB(...)
/**
 * The lighthouses go in the following order:
 *
 *     Ticks  State
 *         0  ACode 0b1x0 (4) <--- B
 *    20 000  ACode 0b0x0 (0) <--- A/c
 *            LH A X Sweep
 *   400 000  ACode 0b1x1 (5) <--- B
 *   420 000  ACode 0b0x1 (1) <--- A/c
 *            LH A Y SWEEP
 *   800 000  ACode 0b0x0 (0) <--- B
 *   820 000  ACode 0b1x0 (4) <--- A/c
 *            LH B X Sweep
 * 1 200 000  ACode 0b0x1 (1) <--- B
 * 1 220 000  ACode 0b1x1 (5) <--- A/c
 *            LH B Y SWEEP
 * 1 600 000  < REPEAT >
 *
 * NOTE: Obviously you cut the data bit out for this
 *
 * This disambiguator works by finding where in that order it is, and tracking along with it.
 * It is able to maintain this tracking for extended periods of time without further data
 * by knowing the modulo of the start of the cycle and calculating appropriatly although this
 * will run into issues when the timestamp rolls over or we simply drift off in accuracy.
 *
 * Neither case is terminal though; it will just have to find the modulo again which only takes
 * a handful of pulses.
 *
 * The main advantage to this scheme is that its reasonably fast and is able to deal with being
 * close enough to the lighthouse that the lengths are in a valid sync pulse range.
 */

// Every pulse_window seems roughly 20k ticks long. That leaves ~360 to the capture window
#define PULSE_WINDOW 20000
#define CAPTURE_WINDOW 360000

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
};

typedef struct {
	int acode, lh, axis, window, offset;
	bool is_sweep;
} LighthouseStateParameters;

// clang-format off
const LighthouseStateParameters LS_Params[LS_END + 1] = {
	{.acode = -1, .lh = -1, .axis = -1, .window = -1},

	{.acode = 4, .lh = 0, .axis = 0, .window = PULSE_WINDOW,   .offset = 0 * PULSE_WINDOW + 0 * CAPTURE_WINDOW},                // 0
	{.acode = 0, .lh = 1, .axis = 0, .window = PULSE_WINDOW,   .offset = 1 * PULSE_WINDOW + 0 * CAPTURE_WINDOW},                // 20000
	{.acode = 4, .lh = 1, .axis = 0, .window = CAPTURE_WINDOW, .offset = 2 * PULSE_WINDOW + 0 * CAPTURE_WINDOW, .is_sweep = 1}, // 40000

	{.acode = 5, .lh = 0, .axis = 1, .window = PULSE_WINDOW,   .offset = 2 * PULSE_WINDOW + 1 * CAPTURE_WINDOW},                // 400000
	{.acode = 1, .lh = 1, .axis = 1, .window = PULSE_WINDOW,   .offset = 3 * PULSE_WINDOW + 1 * CAPTURE_WINDOW},                // 420000
	{.acode = 5, .lh = 1, .axis = 1, .window = CAPTURE_WINDOW, .offset = 4 * PULSE_WINDOW + 1 * CAPTURE_WINDOW, .is_sweep = 1}, // 440000

	{.acode = 0, .lh = 0, .axis = 0, .window = PULSE_WINDOW,   .offset = 4 * PULSE_WINDOW + 2 * CAPTURE_WINDOW},                // 800000
	{.acode = 4, .lh = 1, .axis = 0, .window = PULSE_WINDOW,   .offset = 5 * PULSE_WINDOW + 2 * CAPTURE_WINDOW},                // 820000
	{.acode = 0, .lh = 0, .axis = 0, .window = CAPTURE_WINDOW, .offset = 6 * PULSE_WINDOW + 2 * CAPTURE_WINDOW, .is_sweep = 1}, // 840000

	{.acode = 1, .lh = 0, .axis = 1, .window = PULSE_WINDOW,   .offset = 6 * PULSE_WINDOW + 3 * CAPTURE_WINDOW},                // 1200000
	{.acode = 5, .lh = 1, .axis = 1, .window = PULSE_WINDOW,   .offset = 7 * PULSE_WINDOW + 3 * CAPTURE_WINDOW},                // 1220000
	{.acode = 1, .lh = 0, .axis = 1, .window = CAPTURE_WINDOW, .offset = 8 * PULSE_WINDOW + 3 * CAPTURE_WINDOW, .is_sweep = 1}, // 1240000

	{.acode = -1, .lh = -1, .axis = -1, .window = -1, .offset = 8 * PULSE_WINDOW + 4 * CAPTURE_WINDOW}                          // 1600000
};
// clang-format on

enum LighthouseState LighthouseState_findByOffset(int offset) {
	for (int i = 2; i < LS_END + 1; i++) {
		if (LS_Params[i].offset > offset)
			return i - 1;
	}
	assert(false);
	return -1;
}

typedef struct {
	SurviveObject *so;
	/* We keep the last sync time per LH because lightproc expects numbers relative to it */
	uint32_t time_of_last_sync[NUM_LIGHTHOUSES];

	/* Keep running average of sync signals as they come in */
	uint64_t last_sync_timestamp;
	uint64_t last_sync_length;
	int last_sync_count;

	/**  This part of the structure is general use when we know our state */
	enum LighthouseState state;
	uint32_t mod_offset;
	int confidence;

	/** This rest of the structure is dedicated to finding a state when we are unknown */
	int encoded_acodes;

	int stabalize;
	bool lastWasSync;

	LightcapElement sweep_data[];
} Disambiguator_data_t;

static uint32_t timestamp_diff(uint32_t recent, uint32_t prior) {
	if (recent > prior)
		return recent - prior;
	return (0xFFFFFFFF - prior) + recent;
}

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;
}

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;

#define LOWER_SYNC_TIME 2250
#define UPPER_SYNC_TIME 6750

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};
}

enum LightcapClassification { LCC_SWEEP, LCC_SYNC };
static enum LightcapClassification naive_classify(Disambiguator_data_t *d, const LightcapElement *le) {
	bool clearlyNotSync = le->length < LOWER_SYNC_TIME || le->length > UPPER_SYNC_TIME;

	if (clearlyNotSync) {
		return LCC_SWEEP;
	} else {
		return LCC_SYNC;
	}
}

#define ACODE_TIMING(acode)                                                                                            \
	((3000 + ((acode)&1) * 500 + (((acode) >> 1) & 1) * 1000 + (((acode) >> 2) & 1) * 2000) - 250)
#define ACODE(s, d, a) ((s << 2) | (d << 1) | a)
#define SWEEP 0xFF

static uint32_t SolveForMod_Offset(Disambiguator_data_t *d, enum LighthouseState state, const LightcapElement *le) {
	assert(LS_Params[state].is_sweep == 0); // Doesn't work for sweep data
	SurviveContext *ctx = d->so->ctx;
	DEBUG_TB("Solve for mod %d (%u - %u) = %u", state, le->timestamp, LS_Params[state].offset,
			 (le->timestamp - LS_Params[state].offset));

	return (le->timestamp - LS_Params[state].offset);
}

static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElement *le,
									 enum LighthouseState new_state);
static enum LighthouseState CheckEncodedAcode(Disambiguator_data_t *d, uint8_t newByte) {

	// We chain together acodes / sweep indicators to form an int we can just switch on.
	SurviveContext *ctx = d->so->ctx;
	d->encoded_acodes &= 0xFF;
	d->encoded_acodes = (d->encoded_acodes << 8) | newByte;

	LightcapElement lastSync = get_last_sync(d);

	// These combinations are checked for specificaly to allow for the case one lighthouse is either
	//  missing or completely occluded.
	switch (d->encoded_acodes) {
	case (ACODE(0, 1, 0) << 8) | SWEEP:
		d->mod_offset = SolveForMod_Offset(d, LS_SweepAX - 1, &lastSync);

		return (LS_SweepAX + 1);
	case (ACODE(0, 1, 1) << 8) | SWEEP:
		d->mod_offset = SolveForMod_Offset(d, LS_SweepAY - 1, &lastSync);

		return (LS_SweepAY + 1);
	case (SWEEP << 8) | (ACODE(0, 1, 1)):
		d->mod_offset = SolveForMod_Offset(d, LS_WaitLHB_ACode1, &lastSync);

		return (LS_WaitLHB_ACode1 + 1);
	case (SWEEP << 8) | (ACODE(1, 1, 0)):
		d->mod_offset = SolveForMod_Offset(d, LS_WaitLHA_ACode4, &lastSync);

		return (LS_WaitLHA_ACode4 + 1);
	}

	return LS_UNKNOWN;
}
static enum LighthouseState EndSweep(Disambiguator_data_t *d, const LightcapElement *le) {
	return CheckEncodedAcode(d, SWEEP);
}
static enum LighthouseState EndSync(Disambiguator_data_t *d, const LightcapElement *le) {
	LightcapElement lastSync = get_last_sync(d);
	int acode = find_acode(lastSync.length) > 0;
	if (acode > 0) {
		return CheckEncodedAcode(d, (acode | DATA_BIT));
	} else {
		// If we can't resolve an acode, just reset
		d->encoded_acodes = 0;
	}
	return LS_UNKNOWN;
}

static enum LighthouseState AttemptFindState(Disambiguator_data_t *d, const LightcapElement *le) {
	enum LightcapClassification classification = naive_classify(d, le);

	if (classification == LCC_SYNC) {
		LightcapElement lastSync = get_last_sync(d);

		// Handle the case that this is a new SYNC coming in
		if (d->lastWasSync == false || overlaps(&lastSync, le) == false) {

			if (d->lastWasSync && timestamp_diff(lastSync.timestamp, le->timestamp) > 30000) {
				// Missed a sweep window; clear encoded values.
				d->encoded_acodes = 0;
			}

			// Now that the previous two states are in, check to see if they tell us where we are
			enum LighthouseState new_state = d->lastWasSync ? EndSync(d, le) : EndSweep(d, le);
			if (new_state != LS_UNKNOWN)
				return new_state;

			// Otherwise, just reset the sync registers and do another
			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;
	} else {
		// If this is the start of a new sweep, check to see if the end of the sync solves
		// the state
		if (d->lastWasSync) {
			enum LighthouseState new_state = EndSync(d, le);
			if (new_state != LS_UNKNOWN)
				return new_state;
		}
		d->lastWasSync = false;
	}

	return LS_UNKNOWN;
}

static enum LighthouseState SetState(Disambiguator_data_t *d, const LightcapElement *le,
									 enum LighthouseState new_state) {

	SurviveContext *ctx = d->so->ctx;
	if (new_state >= LS_END)
		new_state = 1;

	d->encoded_acodes = 0;
	d->state = new_state;

	d->last_sync_timestamp = d->last_sync_length = d->last_sync_count = 0;
	memset(d->sweep_data, 0, sizeof(LightcapElement) * d->so->sensor_ct);

	return new_state;
}

static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le);
static void RunACodeCapture(int target_acode, Disambiguator_data_t *d, const LightcapElement *le) {
	// Just ignore small signals; this has a measurable impact on signal quality
	if (le->length < 100)
		return;

	// We know what state we are in, so we verify that state as opposed to
	// trying to suss out the acode.

	// Calculate what it would be with and without data
	uint32_t time_error_d0 = abs(ACODE_TIMING(target_acode) - le->length);
	uint32_t time_error_d1 = abs(ACODE_TIMING(target_acode | DATA_BIT) - le->length);

	// Take the least of the two erors
	uint32_t error = time_error_d0 > time_error_d1 ? time_error_d1 : time_error_d0;

	// Errors do happen; either reflections or some other noise. Our scheme here is to
	// keep a tally of hits and misses, and if we ever go into the negatives reset
	// the state machine to find the state again.
	if (error > 1250) {
		// Penalize semi-harshly -- if it's ever off track it will take this many syncs
		// to reset
		const int penalty = 3;
		if (d->confidence < penalty) {
			SurviveContext *ctx = d->so->ctx;
			SetState(d, le, LS_UNKNOWN);
			SV_INFO("WARNING: Disambiguator got lost; refinding state for %s", d->so->codename);
		}
		d->confidence -= penalty;
		return;
	}

	if (d->confidence < 100)
		d->confidence++;

	// If its a real timestep, integrate it here and we can take the average later
	d->last_sync_timestamp += le->timestamp;
	d->last_sync_length += le->length;
	d->last_sync_count++;
}

static void ProcessStateChange(Disambiguator_data_t *d, const LightcapElement *le, enum LighthouseState new_state) {
	SurviveContext *ctx = d->so->ctx;

	// Leaving a sync ...
	if (LS_Params[d->state].is_sweep == 0) {
		if (d->last_sync_count > 0) {
			// Use the average of the captured pulse to adjust where we are modulo against.
			// This lets us handle drift in any of the timing chararacteristics
			LightcapElement lastSync = get_last_sync(d);
			d->mod_offset = SolveForMod_Offset(d, d->state, &lastSync);

			// Figure out if it looks more like it has data or doesn't. We need this for OOX
			int lengthData = ACODE_TIMING(LS_Params[d->state].acode | DATA_BIT);
			int lengthNoData = ACODE_TIMING(LS_Params[d->state].acode);
			bool hasData = abs(lengthData - lastSync.length) < abs(lengthNoData - lastSync.length);
			int acode = LS_Params[d->state].acode;
			if (hasData) {
				acode |= DATA_BIT;
			}
			ctx->lightproc(d->so, -LS_Params[d->state].lh - 1, acode, 0, lastSync.timestamp, lastSync.length,
						   LS_Params[d->state].lh);

			// Store last sync time for sweep calculations
			d->time_of_last_sync[LS_Params[d->state].lh] = lastSync.timestamp;
		}
	} else {
		// Leaving a sweep ...
		size_t avg_length = 0;
		size_t cnt = 0;

		for (int i = 0; i < d->so->sensor_ct; i++) {
			LightcapElement le = d->sweep_data[i];
			// Only care if we actually have data AND we have a time of last sync. We won't have the latter
			// if we synced with the LH at cetain times.
			if (le.length > 0 && d->time_of_last_sync[LS_Params[d->state].lh] > 0) {
				avg_length += le.length;
				cnt++;
			}
		}
		if (cnt > 0) {
			double var = 1.5;
			size_t minl = (1 / var) * (avg_length + cnt / 2) / cnt;
			size_t maxl = var * (avg_length + cnt / 2) / cnt;

			for (int i = 0; i < d->so->sensor_ct; i++) {
				LightcapElement le = d->sweep_data[i];
				// Only care if we actually have data AND we have a time of last sync. We won't have the latter
				// if we synced with the LH at certain times.
				if (le.length > 0 && d->time_of_last_sync[LS_Params[d->state].lh] > 0 && le.length >= minl &&
					le.length <= maxl) {
					int32_t offset_from =
						timestamp_diff(le.timestamp + le.length / 2, d->time_of_last_sync[LS_Params[d->state].lh]);

					// Send the lightburst out.
					if (offset_from > 0)
						d->so->ctx->lightproc(d->so, i, LS_Params[d->state].acode, offset_from, le.timestamp, le.length,
											  LS_Params[d->state].lh);
				}
			}
		}
	}
	SetState(d, le, new_state);
}

static void PropagateState(Disambiguator_data_t *d, const LightcapElement *le) {
	int le_offset = le->timestamp > d->mod_offset
						? (le->timestamp - d->mod_offset + 10000) % LS_Params[LS_END].offset
						: (0xFFFFFFFF - d->mod_offset + le->timestamp + 10000) % LS_Params[LS_END].offset;

	/** Find where this new element fits into our state machine. This can skip states if its been a while since
	 * its been able to process, or if a LH is missing. */
	enum LighthouseState new_state = LighthouseState_findByOffset(le_offset);

	if (d->state != new_state) {
		// This processes the change -- think setting buffers, and sending OOTX / lightproc calls
		ProcessStateChange(d, le, new_state);
	}

	const LighthouseStateParameters *param = &LS_Params[d->state];
	if (param->is_sweep == 0) {
		RunACodeCapture(param->acode, d, le);
	} else if (le->length > d->sweep_data[le->sensor_id].length &&
			   le->length < 7000 /*anything above 10k seems to be bullshit?*/) {
		// Note we only select the highest length one per sweep. Also, we bundle everything up and send it later all at
		// once.
		// so that we can do this filtering. Might not be necessary?
		d->sweep_data[le->sensor_id] = *le;
	}
}

void DisambiguatorStateBased(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) {
		DEBUG_TB("Initializing Disambiguator Data for TB %d", so->sensor_ct);
		Disambiguator_data_t *d = calloc(1, sizeof(Disambiguator_data_t) + sizeof(LightcapElement) * so->sensor_ct);
		d->so = so;
		so->disambiguator_data = d;
	}

	Disambiguator_data_t *d = so->disambiguator_data;
	// It seems like the first few hundred lightcapelements are missing a ton of data; let it stabilize.
	if (d->stabalize < 200) {
		d->stabalize++;
		return;
	}

	if (d->state == LS_UNKNOWN) {
		enum LighthouseState new_state = AttemptFindState(d, le);
		if (new_state != LS_UNKNOWN) {
			d->confidence = 0;

			int le_offset = (le->timestamp - d->mod_offset) % LS_Params[LS_END].offset;
			enum LighthouseState new_state1 = LighthouseState_findByOffset(le_offset);
			SetState(d, le, new_state1);
			DEBUG_TB("Locked onto state %d(%d, %d) at %u", new_state, new_state1, le_offset, d->mod_offset);
		}
	} else {
		PropagateState(d, le);
	}
}

REGISTER_LINKTIME(DisambiguatorStateBased);