From 2063bc30b8c66256bee35cea8139d751aed8a568 Mon Sep 17 00:00:00 2001
From: Jakob Bornecrantz <jakob@collabora.com>
Date: Wed, 8 Apr 2020 12:27:41 +0100
Subject: [PATCH] st/prober: Load tracking config from json and use new
 settings struct

---
 doc/changes/state_trackers/mr.266.2.md       |   1 +
 src/xrt/state_trackers/prober/CMakeLists.txt |   1 +
 src/xrt/state_trackers/prober/meson.build    |   1 +
 src/xrt/state_trackers/prober/p_json.c       | 178 +++++++++++++++++++
 src/xrt/state_trackers/prober/p_prober.c     |   8 +
 src/xrt/state_trackers/prober/p_prober.h     |  18 ++
 src/xrt/state_trackers/prober/p_tracking.c   |  75 +++++++-
 7 files changed, 273 insertions(+), 9 deletions(-)
 create mode 100644 doc/changes/state_trackers/mr.266.2.md
 create mode 100644 src/xrt/state_trackers/prober/p_json.c

diff --git a/doc/changes/state_trackers/mr.266.2.md b/doc/changes/state_trackers/mr.266.2.md
new file mode 100644
index 000000000..5741dee66
--- /dev/null
+++ b/doc/changes/state_trackers/mr.266.2.md
@@ -0,0 +1 @@
+prober: Load tracking config from json and use new settings struct.
diff --git a/src/xrt/state_trackers/prober/CMakeLists.txt b/src/xrt/state_trackers/prober/CMakeLists.txt
index 3e697a725..d1f777e81 100644
--- a/src/xrt/state_trackers/prober/CMakeLists.txt
+++ b/src/xrt/state_trackers/prober/CMakeLists.txt
@@ -6,6 +6,7 @@ set(PROBER_INCLUDES)
 set(PROBER_SOURCE_FILES
 	p_documentation.h
 	p_dump.c
+	p_json.c
 	p_prober.c
 	p_prober.h
 	p_tracking.c
diff --git a/src/xrt/state_trackers/prober/meson.build b/src/xrt/state_trackers/prober/meson.build
index 05657b32d..ea7725850 100644
--- a/src/xrt/state_trackers/prober/meson.build
+++ b/src/xrt/state_trackers/prober/meson.build
@@ -4,6 +4,7 @@
 prober_sources = [
 	'p_documentation.h',
 	'p_dump.c',
+	'p_json.c',
 	'p_prober.c',
 	'p_prober.h',
 	'p_tracking.c',
diff --git a/src/xrt/state_trackers/prober/p_json.c b/src/xrt/state_trackers/prober/p_json.c
new file mode 100644
index 000000000..dbcdf84cb
--- /dev/null
+++ b/src/xrt/state_trackers/prober/p_json.c
@@ -0,0 +1,178 @@
+// Copyright 2019, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Code to manage the settings file.
+ * @author Jakob Bornecrantz <jakob@collabora.com>
+ * @ingroup st_prober
+ */
+
+#include "util/u_file.h"
+#include "util/u_json.h"
+#include "util/u_debug.h"
+#include "p_prober.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/stat.h>
+
+
+char *
+read_content(FILE *file)
+{
+	// Go to the end of the file.
+	fseek(file, 0L, SEEK_END);
+	size_t file_size = ftell(file);
+
+	// Return back to the start of the file.
+	fseek(file, 0L, SEEK_SET);
+
+	char *buffer = (char *)calloc(file_size + 1, sizeof(char));
+	if (buffer == NULL) {
+		return NULL;
+	}
+
+	// Do the actual reading.
+	size_t ret = fread(buffer, sizeof(char), file_size, file);
+	if (ret != file_size) {
+		free(buffer);
+		return NULL;
+	}
+
+	return buffer;
+}
+
+cJSON *
+p_json_open_or_create_main_file(void)
+{
+	FILE *file = u_file_open_file_in_config_dir("config_v0.json", "r");
+	if (file == NULL) {
+		fprintf(stderr, "Could not open the file!\n");
+		return NULL;
+	}
+
+	char *str = read_content(file);
+	fclose(file);
+	if (str == NULL) {
+		fprintf(stderr, "Could not read the contents of the file!\n");
+		return NULL;
+	}
+
+	cJSON *ret = cJSON_Parse(str);
+	if (ret == NULL) {
+		fprintf(stderr, "Failed to parse JSON:\n%s\n#######\n", str);
+		fprintf(stderr, "'%s'\n", cJSON_GetErrorPtr());
+	}
+
+	free(str);
+	return ret;
+}
+
+static cJSON *
+get_obj(cJSON *json, const char *name)
+{
+	cJSON *item = cJSON_GetObjectItemCaseSensitive(json, name);
+	if (item == NULL) {
+		fprintf(stderr, "Failed to find node '%s'!\n", name);
+	}
+	return item;
+}
+
+XRT_MAYBE_UNUSED static bool
+get_obj_bool(cJSON *json, const char *name, bool *out_bool)
+{
+	cJSON *item = get_obj(json, name);
+	if (item == NULL) {
+		return false;
+	}
+
+	if (!u_json_get_bool(item, out_bool)) {
+		fprintf(stderr, "Failed to parse '%s'!\n", name);
+		return false;
+	}
+
+	return true;
+}
+
+static bool
+get_obj_int(cJSON *json, const char *name, int *out_int)
+{
+	cJSON *item = get_obj(json, name);
+	if (item == NULL) {
+		return false;
+	}
+
+	if (!u_json_get_int(item, out_int)) {
+		fprintf(stderr, "Failed to parse '%s'!\n", name);
+		return false;
+	}
+
+	return true;
+}
+
+static bool
+get_obj_str(cJSON *json, const char *name, char *array, size_t array_size)
+{
+	cJSON *item = get_obj(json, name);
+	if (item == NULL) {
+		return false;
+	}
+
+	if (!u_json_get_string_into_array(item, array, array_size)) {
+		fprintf(stderr, "Failed to parse '%s'!\n", name);
+		return false;
+	}
+
+	return true;
+}
+
+bool
+p_json_get_tracking_settings(cJSON *root, struct xrt_settings_tracking *s)
+{
+	if (root == NULL) {
+		return false;
+	}
+
+	cJSON *t = cJSON_GetObjectItemCaseSensitive(root, "tracking");
+	if (t == NULL) {
+		fprintf(stderr, "No tracking node!\n");
+		return false;
+	}
+
+	char tmp[16];
+
+	int ver = -1;
+	bool bad = false;
+
+	bad |= !get_obj_int(t, "version", &ver);
+	if (bad || ver >= 1) {
+		fprintf(stderr, "Missing or unknown version  tag '%i'\n", ver);
+		return false;
+	}
+
+	bad |= !get_obj_str(t, "camera_name", s->camera_name,
+	                    sizeof(s->camera_name));
+	bad |= !get_obj_int(t, "camera_mode", &s->camera_mode);
+	bad |= !get_obj_str(t, "camera_type", tmp, sizeof(tmp));
+	bad |= !get_obj_str(t, "calibration_path", s->calibration_path,
+	                    sizeof(s->calibration_path));
+	if (bad) {
+		return false;
+	}
+
+	if (strcmp(tmp, "regular_mono") == 0) {
+		s->camera_type = XRT_SETTINGS_CAMERA_TYPE_REGULAR_MONO;
+	} else if (strcmp(tmp, "regular_sbs") == 0) {
+		s->camera_type = XRT_SETTINGS_CAMERA_TYPE_REGULAR_SBS;
+	} else if (strcmp(tmp, "ps4") == 0) {
+		s->camera_type = XRT_SETTINGS_CAMERA_TYPE_PS4;
+	} else if (strcmp(tmp, "leap_motion") == 0) {
+		s->camera_type = XRT_SETTINGS_CAMERA_TYPE_LEAP_MOTION;
+	} else {
+		fprintf(stderr, "Unknown camera type '%s'\n", tmp);
+		return false;
+	}
+
+	return true;
+}
diff --git a/src/xrt/state_trackers/prober/p_prober.c b/src/xrt/state_trackers/prober/p_prober.c
index 366dcecc1..51127d531 100644
--- a/src/xrt/state_trackers/prober/p_prober.c
+++ b/src/xrt/state_trackers/prober/p_prober.c
@@ -9,6 +9,7 @@
 
 #include "util/u_var.h"
 #include "util/u_misc.h"
+#include "util/u_json.h"
 #include "util/u_debug.h"
 #include "os/os_hid.h"
 #include "p_prober.h"
@@ -327,6 +328,8 @@ initialize(struct prober *p, struct xrt_prober_entry_lists *lists)
 
 	int ret;
 
+	p->json.root = p_json_open_or_create_main_file();
+
 	ret = collect_entries(p);
 	if (ret != 0) {
 		teardown(p);
@@ -444,6 +447,11 @@ teardown(struct prober *p)
 #ifdef XRT_HAVE_LIBUSB
 	p_libusb_teardown(p);
 #endif
+
+	if (p->json.root != NULL) {
+		cJSON_Delete(p->json.root);
+		p->json.root = NULL;
+	}
 }
 
 
diff --git a/src/xrt/state_trackers/prober/p_prober.h b/src/xrt/state_trackers/prober/p_prober.h
index 2e9660798..237d7904b 100644
--- a/src/xrt/state_trackers/prober/p_prober.h
+++ b/src/xrt/state_trackers/prober/p_prober.h
@@ -13,6 +13,7 @@
 #include "xrt/xrt_config_os.h"
 #include "xrt/xrt_compiler.h"
 #include "xrt/xrt_prober.h"
+#include "xrt/xrt_settings.h"
 
 #ifdef XRT_HAVE_LIBUSB
 #include <libusb-1.0/libusb.h>
@@ -130,6 +131,11 @@ struct prober
 
 	struct xrt_prober_entry_lists *lists;
 
+	struct
+	{
+		cJSON *root;
+	} json;
+
 #ifdef XRT_HAVE_LIBUSB
 	struct
 	{
@@ -167,6 +173,18 @@ struct prober
  *
  */
 
+/*!
+ * Load the JSON config file.
+ */
+cJSON *
+p_json_open_or_create_main_file(void);
+
+/*!
+ * Extract tracking settings from the JSON.
+ */
+bool
+p_json_get_tracking_settings(cJSON *root, struct xrt_settings_tracking *s);
+
 /*!
  * Dump the given device to stdout.
  */
diff --git a/src/xrt/state_trackers/prober/p_tracking.c b/src/xrt/state_trackers/prober/p_tracking.c
index 2f2061aed..10c5ad750 100644
--- a/src/xrt/state_trackers/prober/p_tracking.c
+++ b/src/xrt/state_trackers/prober/p_tracking.c
@@ -40,6 +40,12 @@ struct p_factory
 	// Owning prober.
 	struct prober *p;
 
+	// Has the settings be loaded.
+	bool setting_ok;
+
+	// Settings for this tracking system.
+	struct xrt_settings_tracking settings;
+
 	//! Shared tracking origin.
 	struct xrt_tracking_origin origin;
 
@@ -93,8 +99,7 @@ on_video_device(struct xrt_prober *xp,
 		return;
 	}
 
-	// Hardcoded to PS4 camera.
-	if (strcmp(name, "USB Camera-OV580") != 0) {
+	if (strcmp(name, fact->settings.camera_name) != 0) {
 		return;
 	}
 
@@ -105,6 +110,11 @@ on_video_device(struct xrt_prober *xp,
 static void
 p_factory_ensure_frameserver(struct p_factory *fact)
 {
+	// No settings loaded.
+	if (!fact->setting_ok) {
+		return;
+	}
+
 	// Already created.
 	if (fact->xfs != NULL) {
 		return;
@@ -118,11 +128,19 @@ p_factory_ensure_frameserver(struct p_factory *fact)
 		return;
 	}
 
-	// Now load the calibration data.
-	if (!t_stereo_camera_calibration_load_v1_hack(&fact->data)) {
+	// Open the calibration file.
+	FILE *file = fopen(fact->settings.calibration_path, "rb");
+	if (file == NULL) {
 		return;
 	}
 
+	// Parse the calibration data from the file.
+	if (!t_stereo_camera_calibration_load_v1(file, &fact->data)) {
+		fclose(file);
+		return;
+	}
+	fclose(file);
+
 	struct xrt_frame_sink *xsink = NULL;
 	struct xrt_frame_sink *xsinks[4] = {0};
 	struct xrt_colour_rgb_f32 rgb[2] = {{1.f, 0.f, 0.f}, {1.f, 0.f, 1.f}};
@@ -152,12 +170,34 @@ p_factory_ensure_frameserver(struct p_factory *fact)
 	// Hardcoded quirk sink.
 	struct u_sink_quirk_params qp;
 	U_ZERO(&qp);
-	qp.stereo_sbs = true;
-	qp.ps4_cam = true;
+
+	switch (fact->settings.camera_type) {
+	case XRT_SETTINGS_CAMERA_TYPE_REGULAR_MONO:
+		qp.stereo_sbs = false;
+		qp.ps4_cam = false;
+		qp.leap_motion = false;
+		break;
+	case XRT_SETTINGS_CAMERA_TYPE_REGULAR_SBS:
+		qp.stereo_sbs = true;
+		qp.ps4_cam = false;
+		qp.leap_motion = false;
+		break;
+	case XRT_SETTINGS_CAMERA_TYPE_PS4:
+		qp.stereo_sbs = true;
+		qp.ps4_cam = true;
+		qp.leap_motion = false;
+		break;
+	case XRT_SETTINGS_CAMERA_TYPE_LEAP_MOTION:
+		qp.stereo_sbs = true;
+		qp.ps4_cam = false;
+		qp.leap_motion = true;
+		break;
+	}
+
 	u_sink_quirk_create(&fact->xfctx, xsink, &qp, &xsink);
 
 	// Start the stream now.
-	xrt_fs_stream_start(fact->xfs, xsink, 1);
+	xrt_fs_stream_start(fact->xfs, xsink, fact->settings.camera_mode);
 }
 #endif
 
@@ -173,8 +213,13 @@ p_factory_create_tracked_psmv(struct xrt_tracking_factory *xfact,
                               struct xrt_device *xdev,
                               struct xrt_tracked_psmv **out_xtmv)
 {
-#ifdef XRT_HAVE_OPENCV
 	struct p_factory *fact = p_factory(xfact);
+
+	if (!fact->setting_ok) {
+		return -1;
+	}
+
+#ifdef XRT_HAVE_OPENCV
 	struct xrt_tracked_psmv *xtmv = NULL;
 
 	p_factory_ensure_frameserver(fact);
@@ -201,8 +246,13 @@ p_factory_create_tracked_psvr(struct xrt_tracking_factory *xfact,
                               struct xrt_device *xdev,
                               struct xrt_tracked_psvr **out_xtvr)
 {
-#ifdef XRT_HAVE_OPENCV
 	struct p_factory *fact = p_factory(xfact);
+
+	if (!fact->setting_ok) {
+		return -1;
+	}
+
+#ifdef XRT_HAVE_OPENCV
 	struct xrt_tracked_psvr *xtvr = NULL;
 
 	p_factory_ensure_frameserver(fact);
@@ -254,6 +304,13 @@ p_tracking_init(struct prober *p)
 	// Finally set us as the tracking factory.
 	p->base.tracking = &fact->base;
 
+	fact->setting_ok =
+	    p_json_get_tracking_settings(p->json.root, &fact->settings);
+
+	if (!fact->setting_ok) {
+		fprintf(stderr, "Failed to load settings!\n");
+	}
+
 	return 0;
 }