* Get config from headset.

* Make pretty display.
* Improve disambiguation.

1 files changed, 77 insertions, 5 deletions

diff --git a/src/survive.c b/src/survive.c
index 726e237..b5c8012 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -5,9 +5,17 @@
 #include "survive_internal.h"
 #include <stdio.h>
 #include <stdlib.h>
+#include <jsmn.h>
 #include <string.h>
 #include <zlib.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 &&
+			strncmp(json + tok->start, s, tok->end - tok->start) == 0) {
+		return 0;
+	}
+	return -1;
+}
 
 struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, const char * fault ), void(*notefunction)( struct SurviveContext * ctx, const char * note ) )
 {
@@ -32,15 +40,79 @@ struct SurviveContext * survive_init( void(*ff)( struct SurviveContext * ctx, co
 	//USB must happen last.
 	if( r = survive_usb_init( ctx ) )
 	{
+		//TODO: Cleanup any libUSB stuff sitting around.
 		return 0;
 	}
 
-#if 0
+#if 1
 	//Next, pull out the config stuff.
-	char * ct0conf;
-	int len = survive_get_config( &ct0conf, ctx, 0, 0 );
-	printf( "%d\n", len );
-	puts( ct0conf );
+	{
+		char * ct0conf = 0;
+		int len = survive_get_config( &ct0conf, ctx, 1, 0 );
+		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++) {
+				if (jsoneq(ct0conf, &t[i], "modelPoints") == 0) {
+					int k;
+					jsmntok_t * tk = &t[i+1];
+					//printf( "%d / %d / %d / %d\n", tk->type, tk->start, tk->end, tk->size );
+					int pts = tk->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];
+						//printf( "++%d / %d / %d / %d\n", tk->type, tk->start, tk->end, tk->size );
+				
+						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 );
+							int id = ctx->headset.nr_locations*3+m;
+							ctx->headset.sensor_locations[id] = f;
+						}
+						ctx->headset.nr_locations++;
+					}
+				}
+			}
+		}
+		else
+		{
+			//TODO: Cleanup any remaining USB stuff.
+			return 0;
+		}
+
+	}
 #endif
 
 	//ctx->headset->photos = malloc( ctx->headset->sensors * sizeof(struct SurvivePhoto) );