From e8d696e03128242be33eb0734addee645e894635 Mon Sep 17 00:00:00 2001
From: Joshua Allen <axlecrusher@gmail.com>
Date: Mon, 27 Feb 2017 20:52:26 -0500
Subject: basic config can be written to file

---
 calibrate.c           |   3 +
 calibrate_client.c    |   8 ++
 redist/json_helpers.c |  11 +--
 src/survive_cal.c     |   5 ++
 src/survive_config.c  | 230 +++++++++++++++++++++-----------------------------
 src/survive_config.h  |  34 +++++---
 6 files changed, 136 insertions(+), 155 deletions(-)

diff --git a/calibrate.c b/calibrate.c
index 8eedc88..9c01e8a 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -10,6 +10,8 @@
 #include "src/survive_cal.h"
 #include <DrawFunctions.h>
 
+#include "src/survive_config.h"
+
 struct SurviveContext * ctx;
 
 void HandleKey( int keycode, int bDown )
@@ -141,6 +143,7 @@ void * GuiThread( void * v )
 int main()
 {
 	ctx = survive_init( 0 );
+	config_init();
 
 	survive_install_light_fn( ctx,  my_light_process );
 	survive_install_imu_fn( ctx,  my_imu_process );
diff --git a/calibrate_client.c b/calibrate_client.c
index 25b6303..897c9e0 100644
--- a/calibrate_client.c
+++ b/calibrate_client.c
@@ -10,6 +10,8 @@
 #include "src/survive_cal.h"
 #include <DrawFunctions.h>
 
+#include "src/survive_config.h"
+
 struct SurviveContext * ctx;
 
 void HandleKey( int keycode, int bDown )
@@ -152,6 +154,12 @@ int main()
 	CNFGDialogColor = 0x444444;
 	CNFGSetup( "Survive GUI Debug", 640, 480 );
 	OGCreateThread( GuiThread, 0 );
+
+	config_init();
+	config_set_str(&global_config_values, "Hello","World!");
+	const char *s = config_read_str(&global_config_values, "TestStr","This is a test.");
+	printf("%s\n", s);
+	config_save("config.json");
 	
 
 	if( !ctx )
diff --git a/redist/json_helpers.c b/redist/json_helpers.c
index e96e873..74028b2 100644
--- a/redist/json_helpers.c
+++ b/redist/json_helpers.c
@@ -3,6 +3,7 @@
 #define _GNU_SOURCE
 
 #include <stdio.h>
+#include <stdlib.h>
 #include <string.h>
 #include "json_helpers.h"
 
@@ -22,7 +23,7 @@ void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count) {
 		str1=str2;
 		str2=NULL;
 	}
-	asprintf(&str2, "%s]\n", str1,v[i]);
+	asprintf(&str2, "%s]", str1,v[i]);
 	fputs(str2,f);
 	free(str1);
 	free(str2);
@@ -44,20 +45,20 @@ void json_write_double_array(FILE* f, const char* tag, double* v, uint8_t count)
 		str1=str2;
 		str2=NULL;
 	}
-	asprintf(&str2, "%s]\n", str1,v[i]);
+	asprintf(&str2, "%s]", str1,v[i]);
 	fputs(str2,f);
 	free(str1);
 	free(str2);
 }
 
 void json_write_uint32(FILE* f, const char* tag, uint32_t v) {
-	fprintf(f, "\"%s\":\"%d\"\n", tag, v);
+	fprintf(f, "\"%s\":\"%d\"", tag, v);
 }
 
 void json_write_float(FILE* f, const char* tag, float v) {
-	fprintf(f, "\"%s\":\"%f\"\n", tag, v);
+	fprintf(f, "\"%s\":\"%f\"", tag, v);
 }
 
 void json_write_str(FILE* f, const char* tag, const char* v) {
-	fprintf(f, "\"%s\":\"%s\"\n", tag, v);
+	fprintf(f, "\"%s\":\"%s\"", tag, v);
 }
\ No newline at end of file
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 760692c..41d3fb1 100644
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -15,6 +15,8 @@
 #include <sys/stat.h>
 #include <sys/types.h>
 
+#include "survive_config.h"
+
 #define PTS_BEFORE_COMMON 32
 #define NEEDED_COMMON_POINTS 10
 #define NEEDED_TIMES_OF_COMMON 5
@@ -49,6 +51,9 @@ void ootx_packet_clbk_d(ootx_decoder_context *ct, ootx_packet* packet)
 	b->fcalgibmag[0] = v6.fcal_0_gibmag;
 	b->fcalgibmag[1] = v6.fcal_1_gibmag;
 	b->OOTXSet = 1;
+
+	config_set_lighthouse(b,id);
+	config_save("config.json");
 }
 
 int survive_cal_get_status( struct SurviveContext * ctx, char * description, int description_length )
diff --git a/src/survive_config.c b/src/survive_config.c
index 6afe3d1..dfbd0e2 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -5,104 +5,65 @@
 #include <json_helpers.h>
 
 #define MAX_CONFIG_ENTRIES 100
+#define MAX_LIGHTHOUSES 2
 
-config_val config_values[MAX_CONFIG_ENTRIES];
-static uint16_t used_entries = 0;
 
-static FILE *config_file = NULL;
-const FLT* config_set_float_a(const char *tag, const FLT* values, uint8_t count);
-
-void config_init() {
-	uint16_t i = 0;
-	for (i=0;i<MAX_CONFIG_ENTRIES;++i) {
-		config_values[i].data = NULL;
-		config_values[i].tag = NULL;
-		config_values[i].type = CONFIG_UNKNOWN;
-		config_values[i].elements = 0;
-	}
-
-	used_entries = 0;
-}
-
-void write_float(char* tag, FLT x) {
-	fprintf(config_file, "\"%s\":\"%f\"\n", tag, x);
-}
 
-void set_float_a(char* tag, FLT *x, uint8_t count) {
-	uint8_t i = 0;
-	char t[100];
-		printf("set float\n",t,x[i]);
-	for (i=0;i<count;++i) {
-		sprintf(t,"%s%d",tag,i);
-		printf("%s:%f\n",t,x[i]);
-		config_set_float(t,x[i]);
-	}
-}
-/*
-void set_float_a2(char* tag, char **x, uint8_t count) {
-	uint8_t i = 0;
-	char t[100];
+config_group global_config_values;
+config_group lh_config[MAX_LIGHTHOUSES]; //lighthouse configs
 
-	if (*x == NULL) {
-		*x = (float*)malloc(count*sizeof(float));
-	} else {
-		*x = (float*)realloc(*x, count);
-	}
+//static uint16_t used_entries = 0;
 
-	memcpy(x,)
-	strcpy(*x,src);
+static FILE *config_file = NULL;
+const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* values, uint8_t count);
 
+void init_config_group(config_group *cg, uint16_t count) {
+	uint16_t i = 0;
+	cg->config_entries = malloc(count*sizeof(config_entry));
+	cg->used_entries = 0;
+	cg->max_entries = count;
 
 	for (i=0;i<count;++i) {
-		sprintf(t,"%s%d",tag,i);
-		printf("%s:%f\n",t,x[i]);
-		config_set_float(t,x[i]);
+		cg->config_entries[i].data = NULL;
+		cg->config_entries[i].tag = NULL;
+		cg->config_entries[i].type = CONFIG_UNKNOWN;
+		cg->config_entries[i].elements = 0;
 	}
 }
-*/
-void set_uint32(char* tag, uint32_t x) {
-//	fprintf(config_file, "\"%s\":\"%d\"\n", tag, x);
 
+void config_init() {
+	uint16_t i = 0;
+	init_config_group(&global_config_values, MAX_CONFIG_ENTRIES);
+	for(i=0;i<MAX_LIGHTHOUSES;i++) {
+		init_config_group(lh_config+i, 9);
+	}
 }
 
-void config_open(const char* path, const char* mode) {
-	config_file = fopen(path, mode);
+void config_load(const char* path) {
+	config_file = fopen(path, "r");
 }
 
 void config_close() {
 	fclose(config_file);
 }
-/*
-void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length) {
-	uint8_t i = 0;
-
-	for (i=0;i<length; ++i) {
-		write_uint32("id", bsd[i].BaseStationID);
-		write_float_a("position", bsd[i].Position, 3);
-		write_float_a("quaternion", bsd[i].Quaternion, 4);
-		write_float_a("quaternion", bsd[i].Quaternion, 4);
-		write_float_a("fcalphase", bsd[i].fcalphase, 2);
-		write_float_a("fcaltilt", bsd[i].fcaltilt,2);
-		write_float_a("fcalcurve", bsd[i].fcalcurve,2);
-		write_float_a("fcalgibpha", bsd[i].fcalgibpha,2);
-		write_float_a("fcalgibmag", bsd[i].fcalgibmag,2);
-	}
-}
-*/
+
 void config_set_lighthouse(struct BaseStationData* bsd, uint8_t idx) {
-	config_set_uint32("index", idx);
-	config_set_uint32("id", bsd->BaseStationID);
-	config_set_float_a("position", bsd->Position, 3);
-	config_set_float_a("quaternion", bsd->Quaternion, 4);
-	config_set_float_a("fcalphase", bsd->fcalphase, 2);
-	config_set_float_a("fcaltilt", bsd->fcaltilt,2);
-	config_set_float_a("fcalcurve", bsd->fcalcurve,2);
-	config_set_float_a("fcalgibpha", bsd->fcalgibpha,2);
-	config_set_float_a("fcalgibmag", bsd->fcalgibmag,2);
+	config_group *cg = lh_config+idx;
+	config_set_uint32(cg,"index", idx);
+	config_set_uint32(cg,"id", bsd->BaseStationID);
+	config_set_float_a(cg,"position", bsd->Position, 3);
+	config_set_float_a(cg,"quaternion", bsd->Quaternion, 4);
+	config_set_float_a(cg,"fcalphase", bsd->fcalphase, 2);
+	config_set_float_a(cg,"fcaltilt", bsd->fcaltilt,2);
+	config_set_float_a(cg,"fcalcurve", bsd->fcalcurve,2);
+	config_set_float_a(cg,"fcalgibpha", bsd->fcalgibpha,2);
+	config_set_float_a(cg,"fcalgibmag", bsd->fcalgibmag,2);
 }
 
 void sstrcpy(char** dest, const char *src) {
 	uint32_t len = strlen(src)+1;
+	assert(dest!=NULL);
+
 	if (*dest == NULL) {
 		*dest = (char*)malloc(len);
 	} else {
@@ -111,71 +72,50 @@ void sstrcpy(char** dest, const char *src) {
 	strcpy(*dest,src);
 }
 
-config_val* find_config_entry(const char *tag) {
+config_entry* find_config_entry(config_group *cg, const char *tag) {
 	uint16_t i = 0;
-	for (i=0;i<used_entries;++i) {
-		if ( strcmp(config_values[i].tag, tag) == 0 ) {
-			return config_values+i;
+	for (i=0;i < cg->used_entries;++i) {
+		if ( strcmp(cg->config_entries[i].tag, tag) == 0 ) {
+			return cg->config_entries+i;
 		}
 	}
 	return NULL;
 }
 
-const char* config_read_str(const char *tag, const char *value, const char *def_str) {
-	config_val *cv = find_config_entry(tag);
+const char* config_read_str(config_group *cg, const char *tag, const char *def) {
+	config_entry *cv = find_config_entry(cg, tag);
 
 	if (cv != NULL) return cv->data;
 
-	assert(used_entries<MAX_CONFIG_ENTRIES);
-
-	used_entries++;
-	sstrcpy(&(cv->tag), tag);
-	sstrcpy(&(cv->data), def_str);
-	cv->type = CONFIG_STRING;
-
-	return cv->data;
+	return config_set_str(cg,tag,def);
 }
 
-uint32_t config_read_uint32(const char *tag, const uint32_t value, const uint32_t def) {
-	config_val *cv = find_config_entry(tag);
+uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def) {
+	config_entry *cv = find_config_entry(cg, tag);
 
 	if (cv != NULL) return cv->numeric.i;
 
-	assert(used_entries<MAX_CONFIG_ENTRIES);
-
-	used_entries++;
-	sstrcpy(&(cv->tag), tag);
-	cv->numeric.i = def;
-	cv->type = CONFIG_UINT32;
-
-	return cv->numeric.i;
+	return config_set_uint32(cg, tag, def);
 }
 
-FLT config_read_float(const char *tag, const FLT value, const FLT def) {
-	config_val *cv = find_config_entry(tag);
+FLT config_read_float(config_group *cg, const char *tag, const FLT def) {
+	config_entry *cv = find_config_entry(cg, tag);
 
 	if (cv != NULL) return cv->numeric.f;
 
-	assert(used_entries<MAX_CONFIG_ENTRIES);
-
-	used_entries++;
-	sstrcpy(&(cv->tag), tag);
-	cv->numeric.f = def;
-	cv->type = CONFIG_FLOAT;
-
-	return cv->numeric.f;
+	config_set_float(cg, tag, def);
 }
 
-config_val* next_unused_val() {
-	config_val *cv = config_values+used_entries;
-	assert(used_entries<MAX_CONFIG_ENTRIES);
-	used_entries++;
+config_entry* next_unused_entry(config_group *cg) {
+	config_entry *cv = cg->config_entries + cg->used_entries;
+	assert(cg->used_entries < cg->max_entries);
+	cg->used_entries++;
 	return cv;
 }
 
-const char* config_set_str(const char *tag, const char* value) {
-	config_val *cv = find_config_entry(tag);
-	if (cv == NULL) cv = next_unused_val();
+const char* config_set_str(config_group *cg, const char *tag, const char* value) {
+	config_entry *cv = find_config_entry(cg, tag);
+	if (cv == NULL) cv = next_unused_entry(cg);
 
 	sstrcpy(&(cv->tag), tag);
 	sstrcpy(&(cv->data), value);
@@ -184,9 +124,9 @@ const char* config_set_str(const char *tag, const char* value) {
 	return value;
 }
 
-const uint32_t config_set_uint32(const char *tag, const uint32_t value) {
-	config_val *cv = find_config_entry(tag);
-	if (cv == NULL) cv = next_unused_val();
+const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value) {
+	config_entry *cv = find_config_entry(cg, tag);
+	if (cv == NULL) cv = next_unused_entry(cg);
 
 	sstrcpy(&(cv->tag), tag);
 	cv->numeric.i = value;
@@ -195,9 +135,9 @@ const uint32_t config_set_uint32(const char *tag, const uint32_t value) {
 	return value;
 }
 
-const FLT config_set_float(const char *tag, const FLT value) {
-	config_val *cv = find_config_entry(tag);
-	if (cv == NULL) cv = next_unused_val();
+const FLT config_set_float(config_group *cg, const char *tag, const FLT value) {
+	config_entry *cv = find_config_entry(cg, tag);
+	if (cv == NULL) cv = next_unused_entry(cg);
 
 	sstrcpy(&(cv->tag), tag);
 	cv->numeric.f = value;
@@ -206,9 +146,9 @@ const FLT config_set_float(const char *tag, const FLT value) {
 	return value;
 }
 
-const FLT* config_set_float_a(const char *tag, const FLT* values, uint8_t count) {
-	config_val *cv = find_config_entry(tag);
-	if (cv == NULL) cv = next_unused_val();
+const FLT* config_set_float_a(config_group *cg, const char *tag, const FLT* values, uint8_t count) {
+	config_entry *cv = find_config_entry(cg, tag);
+	if (cv == NULL) cv = next_unused_entry(cg);
 
 	sstrcpy(&(cv->tag), tag);
 
@@ -231,23 +171,41 @@ void _json_write_float_array(FILE* f, const char* tag, FLT* v, uint8_t count) {
 	json_write_double_array(f,tag,v,count);
 }
 
-void config_save(const char* path) {
+void write_config_group(FILE* f, config_group *cg, char *tag) {
 	uint16_t i = 0;
 
-	FILE* f = fopen(path, "w");
+	if (tag != NULL) {
+		fprintf(f, "\"%s\":{\n", tag);
+	}
 
-	for (i=0;i<=used_entries;++i) {
-		if (config_values[i].type == CONFIG_FLOAT) {
-			json_write_float(f, config_values[i].tag, config_values[i].numeric.f);
-		} else if (config_values[i].type == CONFIG_UINT32) {
-			json_write_uint32(f, config_values[i].tag, config_values[i].numeric.i);
-		} else if (config_values[i].type == CONFIG_STRING) {
-			json_write_str(f, config_values[i].tag, config_values[i].data);
-		} else if (config_values[i].type == CONFIG_FLOAT_ARRAY) {
-			_json_write_float_array(f, config_values[i].tag, (FLT*)config_values[i].data, config_values[i].elements);
+	for (i=0;i < cg->used_entries;++i) {
+		if (cg->config_entries[i].type == CONFIG_FLOAT) {
+			json_write_float(f, cg->config_entries[i].tag, cg->config_entries[i].numeric.f);
+		} else if (cg->config_entries[i].type == CONFIG_UINT32) {
+			json_write_uint32(f, cg->config_entries[i].tag, cg->config_entries[i].numeric.i);
+		} else if (cg->config_entries[i].type == CONFIG_STRING) {
+			json_write_str(f, cg->config_entries[i].tag, cg->config_entries[i].data);
+		} else if (cg->config_entries[i].type == CONFIG_FLOAT_ARRAY) {
+			_json_write_float_array(f, cg->config_entries[i].tag, (FLT*)cg->config_entries[i].data, cg->config_entries[i].elements);
 		}
+		if ((i+1) < cg->used_entries) fprintf(f,",");
+		fprintf(f,"\n");
 	};
 
+	if (tag != NULL) {
+		fprintf(f,"}\n");
+	}
+}
+
+void config_save(const char* path) {
+	uint16_t i = 0;
+
+	FILE* f = fopen(path, "w");
+
+	write_config_group(f,&global_config_values, NULL);
+	write_config_group(f,lh_config, "lighthouse0");
+	write_config_group(f,lh_config+1, "lighthouse1");
+
 	fclose(f);
 }
 
diff --git a/src/survive_config.h b/src/survive_config.h
index 3e62ec8..14e2fc6 100644
--- a/src/survive_config.h
+++ b/src/survive_config.h
@@ -13,12 +13,8 @@ typedef enum {
 	CONFIG_STRING = 3,
 	CONFIG_FLOAT_ARRAY = 4,
 } cval_type;
-/*
-typedef union {
-		uint32_t i;
-		FLT f;
-	} Numeric;
-*/
+
+
 typedef struct {
 	char *tag;
 	cval_type type;
@@ -28,21 +24,31 @@ typedef struct {
 	} numeric;
 	char *data;
 	uint32_t elements;
-} config_val;
+} config_entry;
+
+typedef struct {
+	config_entry *config_entries;
+	uint16_t	used_entries;
+	uint16_t	max_entries;
+} config_group;
+
+extern config_group global_config_values;
+extern config_group lh_config[2]; //lighthouse configs
+
 
 void config_init();
 void config_open(const char* path, const char* mode);
 void config_close();
-void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length);
+//void config_write_lighthouse(struct BaseStationData* bsd, uint8_t length);
 void config_set_lighthouse(struct BaseStationData* bsd, uint8_t idx);
 
 void config_save(const char* path);
-const FLT config_set_float(const char *tag, const FLT value);
-const uint32_t config_set_uint32(const char *tag, const uint32_t value);
-const char* config_set_str(const char *tag, const char* value);
-FLT config_read_float(const char *tag, const FLT value, const FLT def);
+const FLT config_set_float(config_group *cg, const char *tag, const FLT value);
+const uint32_t config_set_uint32(config_group *cg, const char *tag, const uint32_t value);
+const char* config_set_str(config_group *cg, const char *tag, const char* value);
 
-uint32_t config_read_uint32(const char *tag, const uint32_t value, const uint32_t def);
-const char* config_read_str(const char *tag, const char *value, const char *def_str);
+FLT config_read_float(config_group *cg, const char *tag, const FLT def);
+uint32_t config_read_uint32(config_group *cg, const char *tag, const uint32_t def);
+const char* config_read_str(config_group *cg, const char *tag, const char *def);
 
 #endif
\ No newline at end of file
-- 
cgit v1.2.3