pulse level debug version

2 files changed, 131 insertions, 46 deletions

diff --git a/tools/disambiguate/Makefile b/tools/disambiguate/Makefile
index dd5a42b..e719469 100644
--- a/tools/disambiguate/Makefile
+++ b/tools/disambiguate/Makefile
@@ -1,5 +1,5 @@
 disambiguate: disambiguate.c
-	gcc -g -o $@ $^ -lm
+	gcc -Wall -Wextra -Werror -g -o $@ $^ -lm
 
 test: disambiguate
 	./disambiguate | head -n 100
diff --git a/tools/disambiguate/disambiguate.c b/tools/disambiguate/disambiguate.c
index 30034f9..581e964 100644
--- a/tools/disambiguate/disambiguate.c
+++ b/tools/disambiguate/disambiguate.c
@@ -3,35 +3,117 @@
 #include <string.h>
 #include <math.h>
 
-long times[64];
-int scores[64];
-
-void discard(long age) {
-	for (int i = 0; i < sizeof(times)/sizeof(times[0]); ++i) {
-		if (times[i] != 0 && times[i] < age) {
-			times[i] = 0;
-			scores[i] = 0;
+#define DIS_NUM_VALUES 64
+
+typedef enum {
+	D_SYNC_J0 = 0,
+	D_SYNC_K0 = 1,
+	D_SYNC_J1 = 2,
+	D_SYNC_K1 = 3,
+	D_SYNC_J2 = 4,
+	D_SYNC_K2 = 5,
+	D_SYNC_J3 = 6,
+	D_SYNC_K3 = 7,
+} sync_pulse;
+
+typedef enum {
+	D_STATE_INVALID = 0,
+	D_STATE_LOCKED = 1,
+	D_STATE_UNLOCKED = -1,
+} dis_state;
+
+typedef enum {
+	P_UNKNOWN = 0,
+	P_SYNC = 1,
+	P_SWEEP = 2,
+} pulse_type;
+
+typedef struct disambiguator_ {
+	long times[DIS_NUM_VALUES];
+	int scores[DIS_NUM_VALUES];
+	dis_state state;
+	long last;
+} disambiguator;
+
+typedef struct classified_pulse_ {
+	pulse_type t;
+	int length;
+} classified_pulse;
+
+void disambiguator_init(disambiguator * d) {
+	memset(&(d->times), 0x0, sizeof(d->times));
+	memset(&(d->scores), 0x0, sizeof(d->times));
+	d->state = D_STATE_UNLOCKED;
+	d->last = 0;
+}
+
+void disambiguator_discard(disambiguator * d, long age) {
+	for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) {
+		if (d->times[i] != 0 && d->times[i] < age) {
+			d->times[i] = 0;
+			d->scores[i] = 0;
 		}
 	}
 }
 
-int findNearest(long time) {
-	int diff = 1000; // max allowed diff for a match
+int disambiguator_find_nearest(disambiguator * d, long time, int max_diff) {
+	int diff = max_diff; // max allowed diff for a match
 	int idx = -1;
-	for (int i = 0; i < sizeof(times)/sizeof(times[0]); ++i) {
-		if (times[i] == 0) continue;
+	for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) {
+		if (d->times[i] == 0) continue;
 
-		int a = abs(times[i] - time);
+		int a = abs(d->times[i] - time);
 		if (a < diff) {
 			idx = i;
 			diff = a;
 		}
+	}
+
+	return idx;
+}
 
+pulse_type disambiguator_step(disambiguator * d, long time, int length) {
+	if (length < 2750) {
+		return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN;
 	}
 
-	//printf("DIFF: %d %d\n", diff, idx);
+	disambiguator_discard(d, time - 10000000);
+	int idx = disambiguator_find_nearest(d, time - 400000, 100);
 
-	return idx;
+	if (time - d->last > 401000) {
+		d->state = D_STATE_UNLOCKED;
+	}
+
+	if (idx == -1) {
+		for (int i = 0; i < DIS_NUM_VALUES; ++i) {
+			if (d->times[i] == 0) {
+				d->times[i] = time;
+				break;
+			}
+		}
+
+		return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN;
+	} else {
+		// double l = length;
+		// char cc = round(l / 500) - 6;
+		// printf("MATCH: %li %d %d (0b%d%d%d)\n", time - d->times[idx], d->scores[idx], length, (cc >> 2) & 0x1, (cc >> 1) & 0x1, cc & 0x1);
+		d->scores[idx]++;
+		if (d->scores[idx] >= 30) {
+			d->state = D_STATE_LOCKED;
+			// printf("MATCH: %li %d\n", time - d->times[idx], scores[idx]);
+			return P_SYNC;
+		}
+		d->times[idx] = time;
+		d->last = time;
+		return d->state == D_STATE_LOCKED ? P_SYNC : P_UNKNOWN;
+	}
+
+	return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN;
+}
+
+sync_pulse length_to_pulse(int length) {
+	double l = length - 3000;
+	return round(l/500);
 }
 
 int main() {
@@ -41,48 +123,51 @@ int main() {
 		fprintf(stderr, "ERROR OPENING INPUT FILE\n");
 		return -1;
 	}
-	memset(&times, 0, sizeof( times ));
-	memset(&scores, 0, sizeof( scores ));
 
-	long offset = 0, last = 0;
+	long last = 0;
+	int num = 0;
+	long data[12];
+	memset(&data, 0x0, sizeof(data));
 
+	disambiguator d;
+	disambiguator_init(&d);
 	for (;;) {
 		char controller[10];
 		int sensor;
 		int unknown;
 		int length;
 		long time;
+
 		if (fscanf(f, "%s %d %d %d %li", controller, &sensor, &unknown, &length, &time) != 5) {
 			break;
 		}
-		/*if (time - last < 10000) {
-			offset += 0x100000000;
-		}*/
-		time += offset;
-		last = time;
-
-		if (length < 2750) continue;
-
-		discard(time - 10000000);
-		int idx = findNearest(time - 800000);
-		if (idx == -1) {
-			for (int i = 0; i < sizeof(times)/sizeof(times[0]); ++i) {
-				if (times[i] == 0) {
-//					printf("ADD: %d %li\n", i, time);
-					times[i] = time;
-					break;
-				}
-			}
-		} else {
-			double l = length;
-			char cc = round(l / 500) - 6;
-			printf("MATCH: %li %d %d (0b%d%d%d)\n", time - times[idx], scores[idx], length, (cc >> 2) & 0x1, (cc >> 1) & 0x1, cc & 0x1);
-			scores[idx]++;
-			if (scores[idx] >= 30) {
-				printf("MATCH: %li %d\n", time - times[idx], scores[idx]);
-				return 0;
+		if (length > 2750) {
+			char cc = length_to_pulse(length);
+			printf("PULSE %d %c%d ", length, cc & 0x1 ? 'j' : 'k', (cc >> 1) & 0x3);
+			for (int i = 0; i < 12; ++i) {
+				if (data[i] != 0)
+					printf("%8li ", (time-data[i]) % 400000);
 			}
-			times[idx] = time;
+			printf("\n");
+			data[num++ % 12] = time;
+		}
+
+		switch (disambiguator_step(&d, time, length)) {
+			default:
+			case P_UNKNOWN:
+				//printf("UNKN  %s %2d %li %d\n", controller, sensor, time - last, length);
+				continue;
+			case P_SYNC:
+				{
+					double l = length;
+					char cc = round(l / 500) - 6;
+					printf("SYNC  %s %2d %10li    %c%d %10li\n", controller, sensor, time, cc & 0x1 ? 'j' : 'k', (cc >> 1) & 0x3, time-last);
+					last = time;
+				}
+				continue;
+			case P_SWEEP:
+				printf("SWEEP %s %2d %10li %5d\n", controller, sensor, time - last, length);
+				continue;
 		}
 	}
 	fclose(f);