Merge branch 'jpicht-disambiguate'

10 files changed, 224 insertions, 104 deletions

diff --git a/Makefile b/Makefile
index dbd4869..8279efa 100644
--- a/Makefile
+++ b/Makefile
@@ -1,6 +1,6 @@
 all : lib data_recorder test
 
-CFLAGS:=-Iinclude -fPIC -g -Os -Iredist -flto
+CFLAGS:=-Iinclude -fPIC -g -Os -Iredist -flto -DUSE_OLD_DISAMBIGUATOR
 LDFLAGS:=-lpthread -lusb-1.0 -lz -lX11 -flto
 
 test : test.c lib/libsurvive.so redist/os_generic.o
@@ -12,7 +12,7 @@ data_recorder : data_recorder.c lib/libsurvive.so redist/os_generic.o redist/Dra
 lib:
 	mkdir lib
 
-lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o redist/jsmn.o $(DEBUGSTUFF)
+lib/libsurvive.so : src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/disambiguator.c redist/jsmn.o $(DEBUGSTUFF)
 	gcc -o $@ $^ $(LDFLAGS) -shared
 
 clean :
diff --git a/data_recorder.c b/data_recorder.c
index 913f8e0..4c5627f 100644
--- a/data_recorder.c
+++ b/data_recorder.c
@@ -33,57 +33,51 @@ void HandleMotion( int x, int y, int mask )
 {
 }
 
-int bufferpts[32*2];
-char buffermts[32*128];
-int buffertimeto[32];
+int bufferpts[32*2*3];
+char buffermts[32*128*3];
+int buffertimeto[32*3];
 
 void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length  )
 {
 	if( acode == -1 ) return;
+//return;
+	int jumpoffset = sensor_id;
+	if( strcmp( so->codename, "WM0" ) == 0 ) jumpoffset += 32;
+	else if( strcmp( so->codename, "WM1" ) == 0 ) jumpoffset += 64;
+
 
 	if( acode == 0 || acode == 2 ) //data = 0
 	{
 		printf( "L X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length );
-		if( strcmp( so->codename, "HMD" ) == 0 )
-		{
-			bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500;
-			buffertimeto[sensor_id] = 0;
-		}
+		bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
 	}
 	if( acode == 1 || acode == 3 ) //data = 1
 	{
 		printf( "L Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length );
-		if( strcmp( so->codename, "HMD" ) == 0 )
-		{
-			bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500;
-			buffertimeto[sensor_id] = 0;
-		}
+		bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
 	}
 
 
 	if( acode == 4 || acode == 6 ) //data = 0
 	{
 		printf( "R X %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length );
-		if( strcmp( so->codename, "HMD" ) == 0 )
-		{
-			bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500;
-			buffertimeto[sensor_id] = 0;
-		}
+		bufferpts[jumpoffset*2+0] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
 	}
 	if( acode == 5 || acode == 7 ) //data = 1
 	{
 		printf( "R Y %s %d %d %d %d %d\n", so->codename, timecode, sensor_id, acode, timeinsweep, length );
-		if( strcmp( so->codename, "HMD" ) == 0 )
-		{
-			bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500;
-			buffertimeto[sensor_id] = 0;
-		}
+		bufferpts[jumpoffset*2+1] = (timeinsweep-100000)/500;
+		buffertimeto[jumpoffset] = 0;
 	}
 
 }
 
 void my_imu_process( struct SurviveObject * so, int16_t * accelgyro, uint32_t timecode, int id )
 {
+return;
 	//if( so->codename[0] == 'H' )
 	if( 1 )
 	{
@@ -105,9 +99,9 @@ void * GuiThread( void * v )
 		CNFGGetDimensions( &screenx, &screeny );
 
 		int i;
-		for( i = 0; i < 32; i++ )
+		for( i = 0; i < 32*3; i++ )
 		{
-			if( buffertimeto[i] < 5 )
+			if( buffertimeto[i] < 50 )
 			{
 				uint32_t color = i * 3231349;
 				uint8_t r = color & 0xff;
diff --git a/include/survive.h b/include/survive.h
index f64091f..8ce5404 100644
--- a/include/survive.h
+++ b/include/survive.h
@@ -31,10 +31,14 @@ struct SurviveObject
 
 	//Flood info, for calculating which laser is currently sweeping.
 	int8_t oldcode;
+#ifdef USE_OLD_DISAMBIGUATOR
 	int16_t total_photos;
 	int32_t last_photo_time;
 	int32_t total_photo_time;
 	int32_t total_pulsecode_time;
+#else
+	struct disambiguator * d;
+#endif
 };
 
 typedef void (*text_feedback_fnptr)( struct SurviveContext * ctx, const char * fault );
diff --git a/src/disambiguator.c b/src/disambiguator.c
new file mode 100644
index 0000000..f641834
--- /dev/null
+++ b/src/disambiguator.c
@@ -0,0 +1,93 @@
+// (C) 2016 Julian Picht, MIT/x11 License.
+//
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+
+#include "disambiguator.h"
+#include <stdlib.h>
+#include <string.h>
+
+void disambiguator_init( struct disambiguator * d ) {
+	memset(&(d->times), 0x0, sizeof(d->times));
+	memset(&(d->scores), 0x0, sizeof(d->scores));
+	d->state = D_STATE_UNLOCKED;
+	d->last = 0;
+	d->max_confidence = 0;
+}
+
+inline void disambiguator_discard( struct disambiguator * d, long age );
+
+void disambiguator_discard( struct disambiguator * d, long age )
+{
+	int confidence = 0;
+	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;
+		} else {
+			if (d->scores[i] > confidence) {
+				confidence = d->scores[i];
+			}
+		}
+	}
+	d->max_confidence = confidence;
+}
+
+inline int disambiguator_find_nearest( struct disambiguator * d, long time, int max_diff );
+
+int disambiguator_find_nearest( struct disambiguator * d, long time, int max_diff )
+{
+	int diff = max_diff; // max allowed diff for a match
+	int idx = -1;
+	for (unsigned int i = 0; i < DIS_NUM_VALUES; ++i) {
+		if (d->times[i] == 0) continue;
+
+		int a = abs(d->times[i] - time);
+		if (a < diff) {
+			idx = i;
+			diff = a;
+		}
+	}
+
+	return idx;
+}
+
+pulse_type disambiguator_step( struct disambiguator * d, long time, int length)
+{
+	if (length < 2750) {
+		return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN;
+	}
+	//printf( "%d %d\n", time, length );
+	//printf( "." );
+	//time -= length/2;
+
+	disambiguator_discard(d, time - 10000000);
+	int idx = disambiguator_find_nearest(d, time - 400000, 100);
+	
+	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 {
+		d->scores[idx]++;
+		if (d->scores[idx] >= 30) {
+			d->state = D_STATE_LOCKED;
+		}
+
+		d->times[idx] = time;
+		d->last = time;
+		return d->state == D_STATE_LOCKED ? (
+			d->scores[idx] >= d->max_confidence ? P_SYNC : P_SWEEP
+		) : P_UNKNOWN;
+	}
+
+	return d->state == D_STATE_LOCKED ? P_SWEEP : P_UNKNOWN;
+}
diff --git a/src/disambiguator.h b/src/disambiguator.h
new file mode 100644
index 0000000..0db19de
--- /dev/null
+++ b/src/disambiguator.h
@@ -0,0 +1,38 @@
+// (C) 2016 Julian Picht, MIT/x11 License.
+//
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+#ifndef DISAMBIGUATOR_H
+#define DISAMBIGUATOR_H
+
+#define DIS_NUM_VALUES 8
+
+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;
+
+struct disambiguator {
+	long times[DIS_NUM_VALUES];
+	int scores[DIS_NUM_VALUES];
+	dis_state state;
+	long last;
+	int max_confidence;
+	char code;
+};
+
+struct classified_pulse_ {
+	pulse_type t;
+	int length;
+};
+
+void disambiguator_init( struct disambiguator * d);
+pulse_type disambiguator_step( struct disambiguator * d, long time, int length);
+
+#endif /* DISAMBIGUATOR_H */
diff --git a/src/survive.c b/src/survive.c
index b12c4cc..7ad2ae9 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -8,6 +8,7 @@
 #include <jsmn.h>
 #include <string.h>
 #include <zlib.h>
+#include "disambiguator.h"
 
 static int jsoneq(const char *json, jsmntok_t *tok, const char *s) {
  if (tok->type == JSMN_STRING && (int) strlen(s) == tok->end - tok->start &&
@@ -91,12 +92,12 @@ static int LoadConfig( struct SurviveContext * ctx, struct SurviveObject * so, i
 		int i;
 		int r = jsmn_parse(&p, ct0conf, len, t, sizeof(t)/sizeof(t[0]));	
 		if (r < 0) {
-			SV_ERROR("Failed to parse JSON in HMD configuration: %d\n", r);
-			return 0;
+			SV_INFO("Failed to parse JSON in HMD configuration: %d\n", r);
+			return -1;
 		}
 		if (r < 1 || t[0].type != JSMN_OBJECT) {
-			SV_ERROR("Object expected in HMD configuration\n");
-			return 0;
+			SV_INFO("Object expected in HMD configuration\n");
+			return -2;
 		}
 
 		for (i = 1; i < r; i++) {
@@ -146,12 +147,15 @@ struct SurviveContext * survive_init()
 
 	ctx->headset.ctx = ctx;
 	memcpy( ctx->headset.codename, "HMD", 4 );
+	ctx->headset.d = calloc( 1, sizeof( struct disambiguator ) );
 
 	ctx->watchman[0].ctx = ctx;
 	memcpy( ctx->watchman[0].codename, "WM0", 4 );
+	ctx->watchman[0].d = calloc( 1, sizeof( struct disambiguator ) );
 
 	ctx->watchman[1].ctx = ctx;
 	memcpy( ctx->watchman[1].codename, "WM1", 4 );
+	ctx->watchman[1].d = calloc( 1, sizeof( struct disambiguator ) );
 
 	//USB must happen last.
 	if( r = survive_usb_init( ctx ) )
@@ -162,8 +166,8 @@ struct SurviveContext * survive_init()
 
 	//Next, pull out the config stuff.
 	if( LoadConfig( ctx, &ctx->headset, 1, 0, 0 ) ) goto fail_gracefully;
-	if( LoadConfig( ctx, &ctx->watchman[0], 2, 0, 1 ) ) goto fail_gracefully;
-	if( LoadConfig( ctx, &ctx->watchman[1], 3, 0, 1 ) ) goto fail_gracefully;
+	if( LoadConfig( ctx, &ctx->watchman[0], 2, 0, 1 ) ) { SV_INFO( "Watchman 0 config issue." ); }
+	if( LoadConfig( ctx, &ctx->watchman[1], 3, 0, 1 ) ) { SV_INFO( "Watchman 1 config issue." ); }
 
 /*
 	int i;
diff --git a/src/survive_data.c b/src/survive_data.c
index 660c3e1..6a1656b 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -11,6 +11,7 @@
 //All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
 
 #include "survive_internal.h"
+#include "disambiguator.h"
 #include <stdint.h>
 #include <string.h>
 
@@ -40,7 +41,23 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 
 	if( le->type != 0xfe || le->length < 50 ) return;
 	//le->timestamp += (le->length/2);
-
+#if 0
+	int32_t offset = le->timestamp - so->d->last;
+	switch( disambiguator_step( so->d, le->timestamp, le->length ) ) {
+		default:
+		case P_UNKNOWN:
+			// not currently locked
+		case P_SYNC:
+			ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, offset );
+			so->d->code = ((le->length+125)/250) - 12;
+			break;
+		case P_SWEEP:
+			if (so->d->code & 1) return;
+			ct->lightproc( so, le->sensor_id, so->d->code >> 1, offset, le->timestamp, le->length );
+			break;
+	}
+#endif
+#ifdef USE_OLD_DISAMBIGUATOR
 	if( le->length > 2100 ) //Pulse longer indicates a sync pulse.
 	{
 		int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_photo_time;
@@ -85,6 +102,7 @@ static void handle_lightcap( struct SurviveObject * so, struct LightcapElement *
 	{
 		//Runt pulse.
 	}
+#endif
 }
 
 
diff --git a/src/survive_usb.c b/src/survive_usb.c
index 808236d..cf12280 100644
--- a/src/survive_usb.c
+++ b/src/survive_usb.c
@@ -384,7 +384,7 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno,
 	cfgbuff[0] = 0x10;
 	if( ( ret = hid_get_feature_report_timeout( dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 )
 	{
-		SV_ERROR( "Could not get survive config data for device %d:%d", devno, iface );
+		SV_INFO( "Could not get survive config data for device %d:%d", devno, iface );
 		return -1;
 	}
 
@@ -395,7 +395,7 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno,
 	{
 		if( (ret = hid_get_feature_report_timeout(dev, iface, cfgbuff, sizeof( cfgbuff ) ) ) < 0 )
 		{
-			SV_ERROR( "Could not read config data (after first packet) on device %d:%d (count: %d)\n", devno, iface, count );
+			SV_INFO( "Could not read config data (after first packet) on device %d:%d (count: %d)\n", devno, iface, count );
 			return -2;
 		}
 
@@ -405,13 +405,13 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno,
 
 		if( size > 62 )
 		{
-			SV_ERROR( "Too much data (%d) on packet from config for device %d:%d (count: %d)", size, devno, iface, count );
+			SV_INFO( "Too much data (%d) on packet from config for device %d:%d (count: %d)", size, devno, iface, count );
 			return -3;
 		}
 
 		if( count + size >= sizeof( compressed_data ) )
 		{
-			SV_ERROR( "Configuration length too long %d:%d (count: %d)", devno, iface, count );
+			SV_INFO( "Configuration length too long %d:%d (count: %d)", devno, iface, count );
 			return -4;
 		}
 
@@ -421,7 +421,7 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno,
 
 	if( count == 0 )
 	{
-		SV_ERROR( "Empty configuration for %d:%d", devno, iface );
+		SV_INFO( "Empty configuration for %d:%d", devno, iface );
 		return -5;
 	}
 
@@ -430,7 +430,7 @@ int survive_get_config( char ** config, struct SurviveContext * ctx, int devno,
 	int len = survive_simple_inflate( ctx, compressed_data, count, uncompressed_data, sizeof(uncompressed_data)-1 );
 	if( len <= 0 )
 	{
-		SV_ERROR( "Error: data for config descriptor %d:%d is bad.", devno, iface );
+		SV_INFO( "Error: data for config descriptor %d:%d is bad.", devno, iface );
 		return -5;
 	}
 
diff --git a/tools/disambiguate/Makefile b/tools/disambiguate/Makefile
index dd5a42b..8e8661c 100644
--- a/tools/disambiguate/Makefile
+++ b/tools/disambiguate/Makefile
@@ -1,5 +1,5 @@
-disambiguate: disambiguate.c
-	gcc -g -o $@ $^ -lm
+disambiguate: disambiguate.c ../../src/disambiguator.c
+	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..7e46625 100644
--- a/tools/disambiguate/disambiguate.c
+++ b/tools/disambiguate/disambiguate.c
@@ -1,88 +1,57 @@
+// (C) 2016 Julian Picht, MIT/x11 License.
+//
+//All MIT/x11 Licensed Code in this file may be relicensed freely under the GPL or LGPL licenses.
+
 #include <stdio.h>
 #include <stdlib.h>
 #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;
-		}
-	}
-}
-
-int findNearest(long time) {
-	int diff = 1000; // 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;
-
-		int a = abs(times[i] - time);
-		if (a < diff) {
-			idx = i;
-			diff = a;
-		}
-
-	}
-
-	//printf("DIFF: %d %d\n", diff, idx);
-
-	return idx;
-}
+#include "../../include/disambiguator.h"
 
 int main() {
 
-	FILE * f = fopen( "raw_light_data_from_watchman.csv", "r" );
+	FILE * f = fopen( "raw_light_data_from_watchman.sorted.csv", "r" );
 	if (f == NULL) {
 		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, lastl = 0;
 
+	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;
+		if (lastl > time) {
+			printf("BACKWARDS: %li %li\n", lastl, time);
+		}
+		lastl = 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;
+					int ll = (length+125)/250;
+					printf("SYNC  %s %2d %10li %5d %c%d %10li %d %d\n", controller, sensor, time, length, (cc & 0x1) ? 'k' : 'j', (cc >> 1) & 0x3, time-last, ll & 1, (ll >> 1) - 6);
+					last = time;
 				}
-			}
-		} 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;
-			}
-			times[idx] = time;
+				continue;
+			case P_SWEEP:
+				printf("SWEEP %s %2d %10li %5d\n", controller, sensor, time - last, length);
+				continue;
 		}
 	}
 	fclose(f);