From 9d6ca80af365f3bdf642e199426c52e469decec6 Mon Sep 17 00:00:00 2001
From: Moses Turner <moses@collabora.com>
Date: Sun, 11 Sep 2022 23:26:39 -0500
Subject: [PATCH] xrt: Add SimulaVR driver

---
 CMakeLists.txt                                |   3 +
 src/xrt/drivers/CMakeLists.txt                |  11 +
 src/xrt/drivers/simula/svr_hmd.c              | 297 ++++++++++++++++++
 src/xrt/drivers/simula/svr_interface.h        |  47 +++
 src/xrt/targets/common/CMakeLists.txt         |   8 +
 .../targets/common/target_builder_interface.h |  17 +-
 .../targets/common/target_builder_simulavr.c  | 264 ++++++++++++++++
 src/xrt/targets/common/target_lists.c         |   4 +
 8 files changed, 649 insertions(+), 2 deletions(-)
 create mode 100644 src/xrt/drivers/simula/svr_hmd.c
 create mode 100644 src/xrt/drivers/simula/svr_interface.h
 create mode 100644 src/xrt/targets/common/target_builder_simulavr.c

diff --git a/CMakeLists.txt b/CMakeLists.txt
index e286d4852..861ac52ad 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -299,6 +299,7 @@ option_with_deps(XRT_BUILD_DRIVER_ULV2 "Enable Ultraleap v2 driver" DEPENDS Leap
 option_with_deps(XRT_BUILD_DRIVER_VF "Build video frame driver (for video file support, uses gstreamer)" DEPENDS XRT_HAVE_GST)
 option_with_deps(XRT_BUILD_DRIVER_VIVE "Enable driver for HTC Vive, Vive Pro, Valve Index, and their controllers" DEPENDS ZLIB_FOUND XRT_HAVE_LINUX)
 option_with_deps(XRT_BUILD_DRIVER_WMR "Enable Windows Mixed Reality driver" DEPENDS "NOT WIN32")
+option_with_deps(XRT_BUILD_DRIVER_SIMULAVR "Enable simula driver" DEPENDS XRT_HAVE_REALSENSE)
 option(XRT_BUILD_DRIVER_SIMULATED "Enable simulated driver" ON)
 
 option(XRT_BUILD_SAMPLES "Enable compiling sample code implementations that will not be linked into any final targets" ON)
@@ -354,6 +355,7 @@ list(
 	"QWERTY"
 	"WMR"
 	"EUROC"
+	"SIMULAVR"
 	)
 
 # Package name needs to be known by the native code itself.
@@ -520,6 +522,7 @@ message(STATUS "#    DRIVER_ULV2:         ${XRT_BUILD_DRIVER_ULV2}")
 message(STATUS "#    DRIVER_VF:           ${XRT_BUILD_DRIVER_VF}")
 message(STATUS "#    DRIVER_VIVE:         ${XRT_BUILD_DRIVER_VIVE}")
 message(STATUS "#    DRIVER_WMR:          ${XRT_BUILD_DRIVER_WMR}")
+message(STATUS "#    DRIVER_SIMULAVR:     ${XRT_BUILD_DRIVER_SIMULAVR}")
 message(STATUS "#####----- Config -----#####")
 
 if(XRT_FEATURE_SERVICE AND NOT XRT_FEATURE_OPENXR)
diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt
index 2115463a3..5e664ebdd 100644
--- a/src/xrt/drivers/CMakeLists.txt
+++ b/src/xrt/drivers/CMakeLists.txt
@@ -373,6 +373,17 @@ if(XRT_BUILD_DRIVER_EUROC)
 	list(APPEND ENABLED_DRIVERS euroc)
 endif()
 
+if(XRT_BUILD_DRIVER_SIMULAVR)
+	add_library(
+		drv_svr STATIC
+		simula/svr_hmd.c
+		simula/svr_interface.h
+		)
+	target_link_libraries(drv_svr PRIVATE xrt-interfaces aux_math xrt-external-cjson)
+	list(APPEND ENABLED_HEADSET_DRIVERS svr)
+endif()
+
+
 if(XRT_BUILD_SAMPLES)
 	# We build the sample driver to make sure it stays valid,
 	# but it never gets linked into a final target.
diff --git a/src/xrt/drivers/simula/svr_hmd.c b/src/xrt/drivers/simula/svr_hmd.c
new file mode 100644
index 000000000..38dfdbb66
--- /dev/null
+++ b/src/xrt/drivers/simula/svr_hmd.c
@@ -0,0 +1,297 @@
+// Copyright 2020, Collabora, Ltd.
+// Copyright 2020, Moses Turner.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief SimulaVR driver code.
+ * @author Moses Turner <moses@collabora.com>
+ * @ingroup drv_svr
+ */
+
+#include "math/m_mathinclude.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+
+#include "svr_interface.h"
+
+#include "xrt/xrt_defines.h"
+#include "xrt/xrt_device.h"
+
+#include "math/m_api.h"
+#include "math/m_space.h"
+#include "math/m_vec2.h"
+
+#include "os/os_time.h"
+#include "os/os_threading.h"
+
+
+#include "util/u_var.h"
+#include "util/u_debug.h"
+#include "util/u_device.h"
+#include "util/u_time.h"
+#include "util/u_json.h"
+#include "util/u_misc.h"
+#include "util/u_logging.h"
+#include "util/u_distortion_mesh.h"
+
+
+DEBUG_GET_ONCE_LOG_OPTION(svr_log, "SIMULA_LOG", U_LOGGING_INFO)
+
+#define SVR_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__)
+#define SVR_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__)
+#define SVR_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__)
+#define SVR_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__)
+#define SVR_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__)
+
+struct svr_hmd
+{
+	struct xrt_device base;
+
+	struct svr_two_displays_distortion distortion;
+
+	enum u_logging_level log_level;
+};
+
+static inline struct svr_hmd *
+svr_hmd(struct xrt_device *xdev)
+{
+	return (struct svr_hmd *)xdev;
+}
+
+static void
+svr_hmd_destroy(struct xrt_device *xdev)
+{
+	struct svr_hmd *ns = svr_hmd(xdev);
+
+	// Remove the variable tracking.
+	u_var_remove_root(ns);
+
+	u_device_free(&ns->base);
+}
+//
+static void
+svr_hmd_update_inputs(struct xrt_device *xdev)
+{}
+
+static void
+svr_hmd_get_tracked_pose(struct xrt_device *xdev,
+                         enum xrt_input_name name,
+                         uint64_t at_timestamp_ns,
+                         struct xrt_space_relation *out_relation)
+{
+	struct svr_hmd *ns = svr_hmd(xdev);
+
+	if (name != XRT_INPUT_GENERIC_HEAD_POSE) {
+		SVR_ERROR(ns, "unknown input name");
+		return;
+	}
+
+
+	out_relation->angular_velocity = (struct xrt_vec3)XRT_VEC3_ZERO;
+	out_relation->linear_velocity = (struct xrt_vec3)XRT_VEC3_ZERO;
+	out_relation->pose =
+	    (struct xrt_pose)XRT_POSE_IDENTITY; // This is so that tracking overrides/multi driver just transforms us by
+	                                        // the tracker + offset from the tracker.
+	out_relation->relation_flags = XRT_SPACE_RELATION_BITMASK_ALL;
+}
+
+#define DEG_TO_RAD(DEG) (DEG * M_PI / 180.)
+
+static void
+svr_hmd_get_view_poses(struct xrt_device *xdev,
+                       const struct xrt_vec3 *default_eye_relation,
+                       uint64_t at_timestamp_ns,
+                       uint32_t view_count,
+                       struct xrt_space_relation *out_head_relation,
+                       struct xrt_fov *out_fovs,
+                       struct xrt_pose *out_poses)
+{
+	//!@todo: default_eye_relation inherits from the env var OXR_DEBUG_IPD_MM / oxr_session.c
+	// probably needs a lot more attention
+
+	u_device_get_view_poses(xdev, default_eye_relation, at_timestamp_ns, view_count, out_head_relation, out_fovs,
+	                        out_poses);
+
+
+
+	//!@todo you may need to invert this - I can't test locally
+	float turn_vals[2] = {5.0, -5.0};
+	for (uint32_t i = 0; i < view_count && i < 2; i++) {
+		struct xrt_vec3 y_up = (struct xrt_vec3)XRT_VEC3_UNIT_Y;
+		math_quat_from_angle_vector(DEG_TO_RAD(turn_vals[i]), &y_up, &out_poses[i].orientation);
+	}
+}
+
+//!@todo: remove hard-coding and move to u_distortion_mesh
+bool
+svr_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result)
+{
+	struct svr_hmd *svr = svr_hmd(xdev);
+
+	struct svr_one_display_distortion *dist = &svr->distortion.views[view];
+
+	struct svr_display_distortion_polynomial_values *distortion_channels[3] = {&dist->red, &dist->green,
+	                                                                           &dist->blue};
+
+
+	// Somewhere at the program (constants definition)
+	/* Display size in mm */
+	// note for people expecting everything to be in meters: no, really, this is millimeters and we don't need a
+	// scaling factor
+	// float _DispDimsX = 51.7752;
+	// float _DispDimsY = 51.7752;
+	/* Half of the horizontal field of view (in radians) fovH/2 */
+	float _FoVh_2 = dist->half_fov;
+	/* Field of view aspect ratio (fovH/fovV), equals to 1 if fovH = fovV */
+	float _aspect = 1.0f;
+
+
+	// Results r/g/b.
+	struct xrt_vec2 tc[3] = {{0, 0}, {0, 0}, {0, 0}};
+
+	// Dear compiler, please vectorize.
+	for (int i = 0; i < 3; i++) {
+
+		// Just before applying the polynomial (maybe before the loop or at the beginning of it)
+		// Denormalization: conversion from uv texture coordinates (origin at bottom left corner) to mm display
+		// coordinates
+		struct xrt_vec2 XoYo = {0, 0}; // Assuming (0,0) at the center of the display: -DispDimsX/2 <= XoYo.x <=
+		                               // DispDimsX/2; -DispDimsY <= XoYo.y <= DispDimsY
+		XoYo.x = dist->display_size_mm.x * (u - 0.5f);
+		XoYo.y = dist->display_size_mm.y * (v - 0.5f);
+
+		struct xrt_vec2 tanH_tanV = {
+		    0, 0}; // Resulting angular coordinates (tan(H), tan(V)) of input image corresponding to the
+		           // coordinates of the input texture whose color will be sampled
+
+		float r2 = m_vec2_dot(XoYo, XoYo);
+		float r = sqrtf(r2);
+
+		// 9 degree polynomial (only odd coefficients)
+		struct svr_display_distortion_polynomial_values *vals = distortion_channels[i];
+		float k1 = vals->k1;
+		float k3 = vals->k3;
+		float k5 = vals->k5;
+		float k7 = vals->k7;
+		float k9 = vals->k9;
+
+		float k = r * (k1 + r2 * (k3 + r2 * (k5 + r2 * (k7 + r2 * k9))));
+
+		// Avoid problems when r = 0
+		if (r > 0) {
+			tanH_tanV.x = (k * XoYo.x) / r;
+			tanH_tanV.y = (k * XoYo.y) / r;
+		} else {
+			tanH_tanV.x = 0;
+			tanH_tanV.y = 0;
+		}
+
+		// Normalization: Transformation from angular coordinates (tan(H), tan(V)) of input image to tc
+		// (normalized coordinates with origin at the bottom left corner)
+		tc[i].x = (tanH_tanV.x + tanf(_FoVh_2)) / (2 * tanf(_FoVh_2));
+		tc[i].y = ((tanH_tanV.y + tanf(_FoVh_2) / _aspect) / (2 * tanf(_FoVh_2))) * _aspect;
+
+		// SVR_TRACE(svr, "Distortion %f %f -> %i %f %f", u, v, i, tc[i].x, tc[i].y);
+	}
+	result->r = tc[0];
+	result->g = tc[1];
+	result->b = tc[2];
+
+	return true;
+}
+
+
+/*
+ *
+ * Create function.
+ *
+ */
+
+struct xrt_device *
+svr_hmd_create(struct svr_two_displays_distortion *distortion)
+{
+	enum u_device_alloc_flags flags =
+	    (enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE);
+	struct svr_hmd *svr = U_DEVICE_ALLOCATE(struct svr_hmd, flags, 1, 0);
+
+	// Slow copy. Could refcount it but who cares, this runs once.
+	svr->distortion = *distortion;
+
+	svr->log_level = debug_get_log_option_svr_log();
+
+
+
+	svr->base.update_inputs = svr_hmd_update_inputs;
+	svr->base.get_tracked_pose = svr_hmd_get_tracked_pose;
+	svr->base.get_view_poses = svr_hmd_get_view_poses;
+	svr->base.destroy = svr_hmd_destroy;
+	svr->base.name = XRT_DEVICE_GENERIC_HMD;
+
+	// Sorta a lie, we have to do this to make the state tracker happy. (Should multi.c override these?)
+	svr->base.orientation_tracking_supported = true;
+	svr->base.position_tracking_supported = true;
+
+	svr->base.device_type = XRT_DEVICE_TYPE_HMD;
+
+	svr->base.hmd->screens[0].nominal_frame_interval_ns = (uint64_t)time_s_to_ns(1.0f / 90.0f);
+
+
+	// Print name.
+	snprintf(svr->base.str, XRT_DEVICE_NAME_LEN, "SimulaVR HMD");
+	snprintf(svr->base.serial, XRT_DEVICE_NAME_LEN, "0001");
+	// Setup input.
+	svr->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE;
+
+	struct u_extents_2d exts;
+
+	// one screen is 2448px wide, but there are two of them.
+	exts.w_pixels = 2448 * 2;
+	// Both screens are 2448px tall
+	exts.h_pixels = 2448;
+
+	u_extents_2d_split_side_by_side(&svr->base, &exts);
+
+	for (int view = 0; view < 2; view++) {
+		svr->base.hmd->distortion.fov[view].angle_left = -svr->distortion.views[view].half_fov;
+		svr->base.hmd->distortion.fov[view].angle_right = svr->distortion.views[view].half_fov;
+		svr->base.hmd->distortion.fov[view].angle_up = svr->distortion.views[view].half_fov;
+		svr->base.hmd->distortion.fov[view].angle_down = -svr->distortion.views[view].half_fov;
+	}
+
+	u_distortion_mesh_set_none(&svr->base);
+	svr->base.hmd->distortion.models = XRT_DISTORTION_MODEL_COMPUTE;
+	svr->base.hmd->distortion.preferred = XRT_DISTORTION_MODEL_COMPUTE;
+	svr->base.compute_distortion = svr_mesh_calc;
+
+	// Setup variable tracker.
+	u_var_add_root(svr, "Simula HMD", true);
+	svr->base.orientation_tracking_supported = true;
+	svr->base.device_type = XRT_DEVICE_TYPE_HMD;
+
+	size_t idx = 0;
+
+	//!@todo these should be true for the final product iirc but possibly not for the demo unit
+	svr->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_ADDITIVE;
+	svr->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE;
+	svr->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_ALPHA_BLEND;
+
+	svr->base.hmd->blend_mode_count = idx;
+
+	uint64_t start;
+	uint64_t end;
+
+	start = os_monotonic_get_ns();
+	u_distortion_mesh_fill_in_compute(&svr->base);
+	end = os_monotonic_get_ns();
+
+	float diff = (end - start);
+	diff /= U_TIME_1MS_IN_NS;
+
+	SVR_DEBUG(svr, "Filling mesh took %f ms", diff);
+
+
+	return &svr->base;
+}
diff --git a/src/xrt/drivers/simula/svr_interface.h b/src/xrt/drivers/simula/svr_interface.h
new file mode 100644
index 000000000..9a8a784ad
--- /dev/null
+++ b/src/xrt/drivers/simula/svr_interface.h
@@ -0,0 +1,47 @@
+// Copyright 2022, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief SimulaVR driver interface.
+ * @author Moses Turner <moses@collabora.com>
+ * @ingroup drv_svr
+ */
+
+#pragma once
+
+#include "xrt/xrt_defines.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct svr_display_distortion_polynomial_values
+{
+	float k1;
+	float k3;
+	float k5;
+	float k7;
+	float k9;
+};
+
+struct svr_one_display_distortion
+{
+	float half_fov;
+	struct xrt_vec2 display_size_mm;
+
+	struct svr_display_distortion_polynomial_values red, green, blue;
+};
+
+struct svr_two_displays_distortion
+{
+	struct svr_one_display_distortion views[2]; // left, right
+};
+
+// Doesn't take possession of *distortion - feel free to free it after.
+struct xrt_device *
+svr_hmd_create(struct svr_two_displays_distortion *distortion);
+
+
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/xrt/targets/common/CMakeLists.txt b/src/xrt/targets/common/CMakeLists.txt
index 6c8bc4b6c..533adb04e 100644
--- a/src/xrt/targets/common/CMakeLists.txt
+++ b/src/xrt/targets/common/CMakeLists.txt
@@ -33,6 +33,10 @@ if(XRT_BUILD_DRIVER_SURVIVE OR XRT_BUILD_DRIVER_VIVE)
 	target_sources(target_lists PRIVATE target_builder_lighthouse.c)
 endif()
 
+if(XRT_BUILD_DRIVER_SURVIVE)
+	target_sources(target_lists PRIVATE target_builder_simulavr.c)
+endif()
+
 # Drivers
 if(XRT_BUILD_DRIVER_ARDUINO)
 	target_link_libraries(target_lists PRIVATE drv_arduino)
@@ -132,6 +136,10 @@ if(XRT_BUILD_DRIVER_EUROC)
 	target_link_libraries(target_lists PRIVATE drv_euroc)
 endif()
 
+if(XRT_BUILD_DRIVER_SIMULAVR)
+	target_link_libraries(target_lists PRIVATE drv_svr)
+endif()
+
 ####
 # Instance
 #
diff --git a/src/xrt/targets/common/target_builder_interface.h b/src/xrt/targets/common/target_builder_interface.h
index 2d27586b8..58273b5a1 100644
--- a/src/xrt/targets/common/target_builder_interface.h
+++ b/src/xrt/targets/common/target_builder_interface.h
@@ -28,6 +28,10 @@
 #define T_BUILDER_LIGHTHOUSE
 #endif
 
+#if defined(XRT_BUILD_DRIVER_SIMULAVR) || defined(XRT_DOXYGEN)
+#define T_BUILDER_SIMULAVR
+#endif
+
 // Always enabled.
 #define T_BUILDER_LEGACY
 
@@ -62,9 +66,18 @@ struct xrt_builder *
 t_builder_legacy_create(void);
 #endif
 
-
+#ifdef T_BUILDER_LIGHTHOUSE
 /*!
- * Builder used as a fallback for drivers not converted to builders yet.
+ * Builder for Lighthouse-tracked devices (vive, index, tundra trackers, etc.)
  */
 struct xrt_builder *
 t_builder_lighthouse_create(void);
+#endif
+
+#ifdef T_BUILDER_SIMULAVR
+/*!
+ * Builder for SimulaVR headsets
+ */
+struct xrt_builder *
+t_builder_simula_create(void);
+#endif
diff --git a/src/xrt/targets/common/target_builder_simulavr.c b/src/xrt/targets/common/target_builder_simulavr.c
new file mode 100644
index 000000000..eee14d09d
--- /dev/null
+++ b/src/xrt/targets/common/target_builder_simulavr.c
@@ -0,0 +1,264 @@
+// Copyright 2022, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Builder for SimulaVR devices
+ * @author Moses Turner <moses@collabora.com>
+ * @ingroup xrt_iface
+ */
+
+#include "multi_wrapper/multi.h"
+#include "realsense/rs_interface.h"
+#include "tracking/t_hand_tracking.h"
+#include "tracking/t_tracking.h"
+
+#include "xrt/xrt_config_drivers.h"
+#include "xrt/xrt_device.h"
+#include "xrt/xrt_prober.h"
+
+#include "util/u_builders.h"
+#include "util/u_config_json.h"
+#include "util/u_debug.h"
+#include "util/u_device.h"
+#include "util/u_sink.h"
+#include "util/u_system_helpers.h"
+#include "util/u_file.h"
+
+#include "target_builder_interface.h"
+
+#include "simula/svr_interface.h"
+#include "v4l2/v4l2_interface.h"
+
+#include "xrt/xrt_frameserver.h"
+#include "xrt/xrt_results.h"
+#include "xrt/xrt_tracking.h"
+
+#include <assert.h>
+
+DEBUG_GET_ONCE_OPTION(simula_config_path, "SIMULA_CONFIG_PATH", NULL)
+DEBUG_GET_ONCE_LOG_OPTION(svr_log, "SIMULA_LOG", U_LOGGING_WARN)
+
+
+#define SVR_TRACE(...) U_LOG_IFL_T(debug_get_log_option_svr_log(), __VA_ARGS__)
+#define SVR_DEBUG(...) U_LOG_IFL_D(debug_get_log_option_svr_log(), __VA_ARGS__)
+#define SVR_INFO(...) U_LOG_IFL_I(debug_get_log_option_svr_log(), __VA_ARGS__)
+#define SVR_WARN(...) U_LOG_IFL_W(debug_get_log_option_svr_log(), __VA_ARGS__)
+#define SVR_ERROR(...) U_LOG_IFL_E(debug_get_log_option_svr_log(), __VA_ARGS__)
+
+static const char *driver_list[] = {
+    "simula",
+};
+
+#define MOVIDIUS_VID 0x03E7
+#define MOVIDIUS_PID 0x2150
+
+#define TM2_VID 0x8087
+#define TM2_PID 0x0B37
+
+struct simula_builder
+{
+	struct xrt_builder base;
+	struct svr_two_displays_distortion display_distortion;
+};
+
+bool
+process_poly_values(const cJSON *values, struct svr_display_distortion_polynomial_values *out_values)
+{
+	bool good = true;
+	good = good && u_json_get_float(u_json_get(values, "k1"), &out_values->k1);
+	good = good && u_json_get_float(u_json_get(values, "k3"), &out_values->k3);
+	good = good && u_json_get_float(u_json_get(values, "k5"), &out_values->k5);
+	good = good && u_json_get_float(u_json_get(values, "k7"), &out_values->k7);
+	good = good && u_json_get_float(u_json_get(values, "k9"), &out_values->k9);
+	return good;
+}
+
+// struct svr_two_displays_distortion
+// process_eye_config(cJSON *config_json)
+// {
+
+// }
+
+static bool
+process_config(const char *config_path, struct svr_two_displays_distortion *out_dist)
+{
+	FILE *file = fopen(config_path, "r");
+	const char *file_content = u_file_read_content(file);
+	int ret = fclose(file);
+	if (ret != 0) {
+		// Apparently I have to handle this. I have no idea how this could happen or if it would be bad, so
+		// let's print and keep going.
+		U_LOG_E("Failed to close file?");
+	}
+
+	cJSON *config_json = cJSON_Parse(file_content);
+
+
+
+	if (config_json == NULL) {
+		const char *error_ptr = cJSON_GetErrorPtr();
+		U_LOG_E("The JSON file at path \"%s\" was unable to parse", config_path);
+		if (error_ptr != NULL) {
+			U_LOG_E("because of an error before %s", error_ptr);
+		}
+		free((void *)file_content);
+		return false;
+	}
+	free((void *)file_content);
+
+	bool good = true;
+
+
+	const cJSON *dd = u_json_get(config_json, "display_distortion");
+
+	if (dd == NULL) {
+		good = false;
+		goto end;
+	}
+
+	// struct svr_two_displays_distortion distortion = {0};
+
+	const char *eye_names[] = {"left_eye", "right_eye"};
+	for (int eye = 0; eye < 2; eye++) {
+		const cJSON *this_eye = u_json_get(dd, eye_names[eye]);
+		if (this_eye == NULL) {
+			good = false;
+			goto end;
+		}
+		// u_json does its own null checking from here on out
+
+		good = good && u_json_get_float(u_json_get(this_eye, "half_fov"), &out_dist->views[eye].half_fov);
+		good = good && u_json_get_float(u_json_get(this_eye, "display_size_mm_x"),
+		                                &out_dist->views[eye].display_size_mm.x);
+		good = good && u_json_get_float(u_json_get(this_eye, "display_size_mm_y"),
+		                                &out_dist->views[eye].display_size_mm.y);
+
+		good = good && process_poly_values(u_json_get(this_eye, "params_red"), &out_dist->views[eye].red);
+		good = good && process_poly_values(u_json_get(this_eye, "params_green"), &out_dist->views[eye].green);
+		good = good && process_poly_values(u_json_get(this_eye, "params_blue"), &out_dist->views[eye].blue);
+	}
+
+end:
+
+
+
+	cJSON_free(config_json);
+
+	return good;
+}
+
+static xrt_result_t
+svr_estimate_system(struct xrt_builder *xb, cJSON *config, struct xrt_prober *xp, struct xrt_builder_estimate *estimate)
+{
+	struct simula_builder *sb = (struct simula_builder *)xb;
+	U_ZERO(estimate);
+
+	const char *config_path = debug_get_option_simula_config_path();
+
+	if (config_path == NULL) {
+		// No failure occurred - the user just didn't ask for Simula
+		return XRT_SUCCESS;
+	}
+
+	bool config_valid = process_config(config_path, &sb->display_distortion);
+
+	if (!config_valid) {
+		U_LOG_E("Failed to parse SimulaVR config");
+		return XRT_SUCCESS;
+	}
+
+	struct xrt_prober_device **xpdevs = NULL;
+	size_t xpdev_count = 0;
+	xrt_result_t xret = XRT_SUCCESS;
+
+	// Lock the device list
+	xret = xrt_prober_lock_list(xp, &xpdevs, &xpdev_count);
+	if (xret != XRT_SUCCESS) {
+		return xret;
+	}
+
+	bool movidius = u_builder_find_prober_device(xpdevs, xpdev_count, MOVIDIUS_VID, MOVIDIUS_PID, XRT_BUS_TYPE_USB);
+	bool tm2 = u_builder_find_prober_device(xpdevs, xpdev_count, TM2_VID, TM2_PID, XRT_BUS_TYPE_USB);
+
+	if (!movidius && !tm2) {
+		U_LOG_E("Simula enabled but couldn't find realsense device!");
+		return XRT_SUCCESS;
+	}
+
+	// I think that ideally we want `movidius` - in that case I think when we grab the device, it reboots to `tm2`
+
+
+	estimate->maybe.head = true;
+	estimate->certain.head = true;
+
+
+	return XRT_SUCCESS;
+}
+
+static xrt_result_t
+svr_open_system(struct xrt_builder *xb, cJSON *config, struct xrt_prober *xp, struct xrt_system_devices **out_xsysd)
+{
+	struct simula_builder *sb = (struct simula_builder *)xb;
+	struct u_system_devices *usysd = u_system_devices_allocate();
+	xrt_result_t result = XRT_SUCCESS;
+
+	if (out_xsysd == NULL || *out_xsysd != NULL) {
+		SVR_ERROR("Invalid output system pointer");
+		result = XRT_ERROR_DEVICE_CREATION_FAILED;
+		goto end;
+	}
+
+
+	// The below is a garbage hack - we should remove the autoprober entirely - but I'm tired af and need to ship it
+
+	struct xrt_device *t265_dev = rs_create_tracked_device_internal_slam(xp);
+
+	struct xrt_device *svr_dev = svr_hmd_create(&sb->display_distortion);
+
+	struct xrt_pose ident = XRT_POSE_IDENTITY;
+
+
+	struct xrt_device *head_device = multi_create_tracking_override(
+	    XRT_TRACKING_OVERRIDE_ATTACHED, svr_dev, t265_dev, XRT_INPUT_GENERIC_TRACKER_POSE, &ident);
+
+	usysd->base.roles.head = head_device;
+	usysd->base.xdevs[0] = usysd->base.roles.head;
+	usysd->base.xdev_count = 1;
+
+
+end:
+	if (result == XRT_SUCCESS) {
+		*out_xsysd = &usysd->base;
+	} else {
+		u_system_devices_destroy(&usysd);
+	}
+
+	return result;
+}
+
+static void
+svr_destroy(struct xrt_builder *xb)
+{
+	free(xb);
+}
+
+/*
+ *
+ * 'Exported' functions.
+ *
+ */
+
+struct xrt_builder *
+t_builder_simula_create(void)
+{
+	struct simula_builder *sb = U_TYPED_CALLOC(struct simula_builder);
+	sb->base.estimate_system = svr_estimate_system;
+	sb->base.open_system = svr_open_system;
+	sb->base.destroy = svr_destroy;
+	sb->base.identifier = "simula";
+	sb->base.name = "SimulaVR headset";
+	sb->base.driver_identifiers = driver_list;
+	sb->base.driver_identifier_count = ARRAY_SIZE(driver_list);
+
+	return &sb->base;
+}
diff --git a/src/xrt/targets/common/target_lists.c b/src/xrt/targets/common/target_lists.c
index 1cac6a272..dd834df4c 100644
--- a/src/xrt/targets/common/target_lists.c
+++ b/src/xrt/targets/common/target_lists.c
@@ -96,6 +96,10 @@ xrt_builder_create_func_t target_builder_list[] = {
     t_builder_rgb_tracking_create,
 #endif // T_BUILDER_RGB_TRACKING
 
+#ifdef T_BUILDER_SIMULAVR
+    t_builder_simula_create,
+#endif
+
 #ifdef T_BUILDER_LIGHTHOUSE
     t_builder_lighthouse_create,
 #endif // T_BUILDER_LIGHTHOUSE