Refactoring to reuse code standard to htc json format

1 files changed, 154 insertions, 0 deletions

diff --git a/src/survive_default_devices.c b/src/survive_default_devices.c
index 5d2cda7..1882002 100644
--- a/src/survive_default_devices.c
+++ b/src/survive_default_devices.c
@@ -1,6 +1,10 @@
+#include <jsmn.h>
 #include <stdlib.h>
 #include <string.h>
 #include "survive_default_devices.h"
+#include <stdio.h>
+
+#include "json_helpers.h"
 
 static SurviveObject* survive_create_device(SurviveContext * ctx,
 					    const char* driver_name,
@@ -44,3 +48,153 @@ SurviveObject* survive_create_tr0(SurviveContext * ctx, const char* driver_name,
 SurviveObject* survive_create_ww0(SurviveContext * ctx, const char* driver_name, void* driver) {
   return survive_create_device(ctx, driver_name, driver, "WW0", 0);
 }
+
+
+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;
+}
+static int ParsePoints( SurviveContext * ctx, SurviveObject * so, char * ct0conf, FLT ** floats_out, jsmntok_t * t, int i )
+{
+	int k;
+	int pts = t[i+1].size;
+	jsmntok_t * tk;
+
+	so->nr_locations = 0;
+	*floats_out = malloc( sizeof( **floats_out ) * 32 * 3 );
+
+	for( k = 0; k < pts; k++ )
+	{
+		tk = &t[i+2+k*4];
+
+		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;
+			FLT f = atof( ctt );
+			int id = so->nr_locations*3+m;
+			(*floats_out)[id] = f;
+		}
+		so->nr_locations++;
+	}
+	return 0;
+}
+
+int survive_load_htc_config_format(char* ct0conf, int len, SurviveObject * so) {
+  if (len == 0)
+    return -1;
+
+  SurviveContext* ctx = so->ctx;
+  //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_INFO("Failed to parse JSON in HMD configuration: %d\n", r);
+    return -1;
+  }
+  if (r < 1 || t[0].type != JSMN_OBJECT) {
+    SV_INFO("Object expected in HMD configuration\n");
+    return -2;
+  }
+
+  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, so, ct0conf, &so->sensor_locations, t, i  ) )
+	{
+	  break;
+	}
+    }
+    if (jsoneq(ct0conf, tk, "modelNormals") == 0) {
+      if( ParsePoints( ctx, so, ct0conf, &so->sensor_normals, t, i  ) )
+	{
+	  break;
+	}
+    }
+
+
+    if (jsoneq(ct0conf, tk, "acc_bias") == 0) {
+      int32_t count = (tk+1)->size;
+      FLT* values = NULL;
+      if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
+	so->acc_bias = values;
+	so->acc_bias[0] *= .125; //XXX Wat?  Observed by CNL.  Biasing by more than this seems to hose things.
+	so->acc_bias[1] *= .125;
+	so->acc_bias[2] *= .125;
+      }
+    }
+    if (jsoneq(ct0conf, tk, "acc_scale") == 0) {
+      int32_t count = (tk+1)->size;
+      FLT* values = NULL;
+      if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
+	so->acc_scale = values;
+      }
+    }
+
+    if (jsoneq(ct0conf, tk, "gyro_bias") == 0) {
+      int32_t count = (tk+1)->size;
+      FLT* values = NULL;
+      if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
+	so->gyro_bias = values;
+      }
+    }
+    if (jsoneq(ct0conf, tk, "gyro_scale") == 0) {
+      int32_t count = (tk+1)->size;
+      FLT* values = NULL;
+      if ( parse_float_array(ct0conf, tk+2, &values, count) >0 ) {
+	so->gyro_scale = values;
+      }
+    }
+  }
+
+
+  char fname[64];
+
+  sprintf( fname, "calinfo/%s_points.csv", so->codename );
+  FILE * f = fopen( fname, "w" );
+  int j;
+  for( j = 0; j < so->nr_locations; j++ )
+    {
+      fprintf( f, "%f %f %f\n", so->sensor_locations[j*3+0], so->sensor_locations[j*3+1], so->sensor_locations[j*3+2] );
+    }
+  fclose( f );
+
+  sprintf( fname, "calinfo/%s_normals.csv", so->codename );
+  f = fopen( fname, "w" );
+  for( j = 0; j < so->nr_locations; j++ )
+    {
+      fprintf( f, "%f %f %f\n", so->sensor_normals[j*3+0], so->sensor_normals[j*3+1], so->sensor_normals[j*3+2] );
+    }
+  fclose( f );
+
+  
+  return 0;
+}