Fix light disambiguator location, put with the object instead of the system.

5 files changed, 161 insertions, 107 deletions

diff --git a/data_recorder.c b/data_recorder.c
index 24c8bc1..913f8e0 100644
--- a/data_recorder.c
+++ b/data_recorder.c
@@ -44,7 +44,7 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int
 	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, "HED" ) == 0 )
+		if( strcmp( so->codename, "HMD" ) == 0 )
 		{
 			bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500;
 			buffertimeto[sensor_id] = 0;
@@ -53,7 +53,7 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int
 	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, "HED" ) == 0 )
+		if( strcmp( so->codename, "HMD" ) == 0 )
 		{
 			bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500;
 			buffertimeto[sensor_id] = 0;
@@ -64,7 +64,7 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int
 	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, "HED" ) == 0 )
+		if( strcmp( so->codename, "HMD" ) == 0 )
 		{
 			bufferpts[sensor_id*2+0] = (timeinsweep-100000)/500;
 			buffertimeto[sensor_id] = 0;
@@ -73,7 +73,7 @@ void my_light_process( struct SurviveObject * so, int sensor_id, int acode, int
 	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, "HED" ) == 0 )
+		if( strcmp( so->codename, "HMD" ) == 0 )
 		{
 			bufferpts[sensor_id*2+1] = (timeinsweep-100000)/500;
 			buffertimeto[sensor_id] = 0;
diff --git a/include/survive.h b/include/survive.h
index 61a6620..b574711 100644
--- a/include/survive.h
+++ b/include/survive.h
@@ -7,23 +7,36 @@
 
 struct SurviveContext;
 
-//DANGER: This structure may be redefined 
+//DANGER: This structure may be redefined.  Note that it is logically split into 64-bit chunks
+//for optimization on 32- and 64-bit systems.
 
 struct SurviveObject
 {
 	struct SurviveContext * ctx;
-	char    codename[4];
+
+	char    codename[4];  //3 letters, null-terminated.  Currently HMD, WM0, WM1.
 	int16_t buttonmask;
 	int16_t axis1;
+
 	int16_t axis2;
 	int16_t axis3;
 	int8_t  charge;
 	int8_t  charging:1;
 	int8_t  ison:1;
-	int sensors;
+	int8_t  additional_flags:6;
+	int8_t sensors;
+	int8_t nr_locations;
 
-	int nr_locations;
 	SV_FLOAT * sensor_locations;
+
+	SV_FLOAT * sensor_normals;
+
+	//Flood info, for calculating which laser is currently sweeping.
+	int32_t last_photo_time;
+	int32_t total_photo_time;
+	int32_t total_pulsecode_time;
+	int16_t total_photos;
+	int8_t oldcode;
 };
 
 typedef void (*text_feedback_fnptr)( struct SurviveContext * ctx, const char * fault );
diff --git a/src/survive.c b/src/survive.c
index 773f02a..09c9ddb 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -29,6 +29,98 @@ static void survivenote( struct SurviveContext * ctx, const char * fault )
 	fprintf( stderr, "Info: %s\n", fault );
 }
 
+static int ParsePoints( struct SurviveContext * ctx, char * ct0conf, SV_FLOAT ** floats_out, jsmntok_t * t, int i )
+{
+	int k;
+	int pts = t[i+1].size;
+	jsmntok_t * tk;
+
+	ctx->headset.nr_locations = 0;
+	*floats_out = malloc( sizeof( **floats_out ) * 32 * 3 );
+
+	for( k = 0; k < pts; k++ )
+	{
+		tk = &t[i+2+k*4];
+
+		float vals[3];
+		int m;
+		for( m = 0; m < 3; m++ )
+		{
+			char ctt[128];
+
+			tk++;
+			int elemlen = tk->end - tk->start;
+
+			if( tk->type != 4 || elemlen > sizeof( ctt )-1 )
+			{
+				SV_ERROR( "Parse error in JSON\n" );
+				return 1;
+			}
+
+			memcpy( ctt, ct0conf + tk->start, elemlen );
+			ctt[elemlen] = 0;
+			float f = atof( ctt );
+			int id = ctx->headset.nr_locations*3+m;
+			(*floats_out)[id] = f;
+		}
+		ctx->headset.nr_locations++;
+	}
+	return 0;
+}
+
+static int LoadConfig( struct SurviveContext * ctx, struct SurviveObject * so, int devno, int iface )
+{
+	char * ct0conf = 0;
+	int len = survive_get_config( &ct0conf, ctx, devno, iface );
+	if( len > 0 )
+	{
+
+		//From JSMN example.
+		jsmn_parser p;
+		jsmntok_t t[4096];
+		jsmn_init(&p);
+		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;
+		}
+		if (r < 1 || t[0].type != JSMN_OBJECT) {
+			SV_ERROR("Object expected in HMD configuration\n");
+			return 0;
+		}
+
+		for (i = 1; i < r; i++) {
+			jsmntok_t * tk = &t[i];
+
+			char ctxo[100];
+			int ilen = tk->end - tk->start;
+			if( ilen > 99 ) ilen = 99;
+			memcpy(ctxo, ct0conf + tk->start, ilen);
+			ctxo[ilen] = 0;
+//				printf( "%d / %d / %d / %d %s %d\n", tk->type, tk->start, tk->end, tk->size, ctxo, jsoneq(ct0conf, &t[i], "modelPoints") );
+//				printf( "%.*s\n", ilen, ct0conf + tk->start );
+			if (jsoneq(ct0conf, tk, "modelPoints") == 0) {
+				if( ParsePoints( ctx,  ct0conf, &ctx->headset.sensor_locations, t, i  ) )
+				{
+					break;
+				}
+			}
+			if (jsoneq(ct0conf, tk, "modelNormals") == 0) {
+				if( ParsePoints( ctx,  ct0conf, &ctx->headset.sensor_normals, t, i  ) )
+				{
+					break;
+				}
+			}
+		}
+	}
+	else
+	{
+		//TODO: Cleanup any remaining USB stuff.
+		return 1;
+	}
+	return 0;
+}
 
 struct SurviveContext * survive_init()
 {
@@ -43,103 +135,55 @@ struct SurviveContext * survive_init()
 
 	ctx->headset.sensors = 32;
 	ctx->headset.ctx = ctx;
-	memcpy( ctx->headset.codename, "HED", 4 );
+	memcpy( ctx->headset.codename, "HMD", 4 );
 
 	ctx->watchman[0].sensors = 16;
 	ctx->watchman[0].ctx = ctx;
-	memcpy( ctx->watchman[0].codename, "CT0", 4 );
+	memcpy( ctx->watchman[0].codename, "WM0", 4 );
 
 	ctx->watchman[1].sensors = 16;
 	ctx->watchman[1].ctx = ctx;
-	memcpy( ctx->watchman[1].codename, "CT1", 4 );
+	memcpy( ctx->watchman[1].codename, "WM1", 4 );
 
 	//USB must happen last.
 	if( r = survive_usb_init( ctx ) )
 	{
 		//TODO: Cleanup any libUSB stuff sitting around.
-		return 0;
+		goto fail_gracefully;
 	}
 
 	//Next, pull out the config stuff.
+	if( LoadConfig( ctx, &ctx->headset, 1, 0 ) ) goto fail_gracefully;
+
+/*
+	int i;
+	int locs = ctx->headset.nr_locations;
+	printf( "Locs: %d\n", locs );
+	if (ctx->headset.sensor_locations )
 	{
-		char * ct0conf = 0;
-		int len = survive_get_config( &ct0conf, ctx, 1, 0 );
-		if( len > 0 )
+		printf( "POSITIONS:\n" );
+		for( i = 0; i < locs*3; i+=3 )
 		{
-
-			//From JSMN example.
-			jsmn_parser p;
-			jsmntok_t t[4096];
-			jsmn_init(&p);
-			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;
-			}
-			if (r < 1 || t[0].type != JSMN_OBJECT) {
-				SV_ERROR("Object expected in HMD configuration\n");
-				return 0;
-			}
-
-			for (i = 1; i < r; i++) {
-				jsmntok_t * tk = &t[i];
-
-				char ctxo[100];
-				int ilen = tk->end - tk->start;
-				if( ilen > 99 ) ilen = 99;
-				memcpy(ctxo, ct0conf + tk->start, ilen);
-				ctxo[ilen] = 0;
-//				printf( "%d / %d / %d / %d %s %d\n", tk->type, tk->start, tk->end, tk->size, ctxo, jsoneq(ct0conf, &t[i], "modelPoints") );
-
-				if (jsoneq(ct0conf, &t[i], "modelPoints") == 0) {
-					int k;
-					int pts = t[i+1].size;
-
-					ctx->headset.nr_locations = 0;
-					ctx->headset.sensor_locations = malloc( sizeof( *ctx->headset.sensor_locations) * 32 * 3 );
-
-					for( k = 0; k < pts; k++ )
-					{
-						tk = &t[i+2+k*4];
-				
-						float vals[3];
-						int m;
-						for( m = 0; m < 3; m++ )
-						{
-							char ctt[128];
-
-							tk++;
-							int elemlen = tk->end - tk->start;
-
-							if( tk->type != 4 || elemlen > sizeof( ctt )-1 )
-							{
-								SV_ERROR( "Parse error in JSON\n" );
-								break;
-							}
-
-							memcpy( ctt, ct0conf + tk->start, elemlen );
-							ctt[elemlen] = 0;
-							float f = atof( ctt );
-							//printf( "%f%c", f, (m==2)?'\n':',' );
-							int id = ctx->headset.nr_locations*3+m;
-							ctx->headset.sensor_locations[id] = f;
-						}
-						ctx->headset.nr_locations++;
-					}
-				}
-			}
+			printf( "%f %f %f\n", ctx->headset.sensor_locations[i+0], ctx->headset.sensor_locations[i+1], ctx->headset.sensor_locations[i+2] );
 		}
-		else
+	}
+	if( ctx->headset.sensor_normals )
+	{
+		printf( "NORMALS:\n" );
+		for( i = 0; i < locs*3; i+=3 )
 		{
-			//TODO: Cleanup any remaining USB stuff.
-			return 0;
+			printf( "%f %f %f\n", ctx->headset.sensor_normals[i+0], ctx->headset.sensor_normals[i+1], ctx->headset.sensor_normals[i+2] );
 		}
-
 	}
+*/
+
+	
 
-	//ctx->headset->photos = malloc( ctx->headset->sensors * sizeof(struct SurvivePhoto) );
 	return ctx;
+fail_gracefully:
+	survive_usb_close( ctx );
+	free( ctx );
+	return 0;
 }
 
 void survive_install_info_fn( struct SurviveContext * ctx,  text_feedback_fnptr fbp )
diff --git a/src/survive_data.c b/src/survive_data.c
index 88c9e7d..660c3e1 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -28,41 +28,46 @@ struct LightcapElement
 } __attribute__((packed));
 
 
-
+//This is the disambiguator function, for taking light timing and figuring out place-in-sweep for a given photodiode.
 static void handle_lightcap( struct SurviveObject * so, struct LightcapElement * le )
 {
 	struct SurviveContext * ct = so->ctx;
 
+//	if( so->codename[0] == 'W' )
+//	{
+//		printf( "%s %d %d %d %d\n", so->codename, le->sensor_id, le->type, le->length, le->timestamp );
+//	}
+
 	if( le->type != 0xfe || le->length < 50 ) return;
 	//le->timestamp += (le->length/2);
 
 	if( le->length > 2100 ) //Pulse longer indicates a sync pulse.
 	{
-		int32_t deltat = (uint32_t)le->timestamp - (uint32_t)ct->last_photo_time;
+		int32_t deltat = (uint32_t)le->timestamp - (uint32_t)so->last_photo_time;
 		if( deltat > 2000 || deltat < -2000 )		//New pulse. (may be inverted)
 		{
-			if( le->timestamp - ct->last_photo_time > 80000 )
+			if( le->timestamp - so->last_photo_time > 80000 )
 			{
-				ct->last_photo_time = le->timestamp;
-				ct->total_photo_time = 0;
-				ct->total_photos = 0;
-				ct->total_pulsecode_time = 0;
+				so->last_photo_time = le->timestamp;
+				so->total_photo_time = 0;
+				so->total_photos = 0;
+				so->total_pulsecode_time = 0;
 				ct->lightproc( so, le->sensor_id, -1, 0, le->timestamp, deltat );
 			}
 		}
 		else
 		{
-			ct->total_pulsecode_time += le->length;
-			ct->total_photo_time += deltat;
-			ct->total_photos++;
+			so->total_pulsecode_time += le->length;
+			so->total_photo_time += deltat;
+			so->total_photos++;
 		}
 	}
-	else if( le->length < 1200 && le->length > 40 && ct->total_photos )
+	else if( le->length < 1200 && le->length > 40 && so->total_photos )
 	{
-		int32_t dl = (ct->total_photo_time/ct->total_photos);
-		int32_t tpco = (ct->total_pulsecode_time/ct->total_photos);
+		int32_t dl = (so->total_photo_time/so->total_photos);
+		int32_t tpco = (so->total_pulsecode_time/so->total_photos);
 		//Adding length 
-		int32_t offset_from = le->timestamp - dl - ct->last_photo_time + le->length/2;
+		int32_t offset_from = le->timestamp - dl - so->last_photo_time + le->length/2;
 
 		//Long pulse-code from IR flood.
 		//Make sure it fits nicely into a divisible-by-500 time.
@@ -341,11 +346,11 @@ void survive_data_cb( struct SurviveUSBInterface * si )
 			uint32_t timecode = POP4;
 			uint8_t code = POP1;
 			//printf( "%d ", code );
-			int8_t cd = code - ctx->oldcode;
+			int8_t cd = code - ctx->headset.oldcode;
 
 			if( cd > 0 )
 			{
-				ctx->oldcode = code;
+				ctx->headset.oldcode = code;
 				ctx->imuproc( &ctx->headset, acceldata, timecode, code );
 			}
 		}
diff --git a/src/survive_internal.h b/src/survive_internal.h
index ac378a2..92a2001 100644
--- a/src/survive_internal.h
+++ b/src/survive_internal.h
@@ -75,17 +75,9 @@ struct SurviveContext
 	light_process_func lightproc;
 	imu_process_func imuproc;
 
-	//Flood info, for calculating which laser is currently sweeping.
-	int8_t oldcode;
-	int32_t last_photo_time;
-	short total_photos;
-	int32_t total_photo_time;
-	int32_t total_pulsecode_time;
-
 	//Data Subsystem
 	struct SurviveObject headset;
 	struct SurviveObject watchman[2];
-//	struct SurvivePhoto 
 };