tracker/steamvr: support choosing device by its serial number

Since the vr handle is accessed on the GUI and pipeline threads
now, had to add implicit locking. This sadly reorganizes most of
the file.

Sadly this refactor likely broke things.

cf. https://github.com/opentrack/opentrack/issues/352#issuecomment-290252520

4 files changed, 296 insertions, 130 deletions

diff --git a/tracker-steamvr/dialog.cpp b/tracker-steamvr/dialog.cpp
deleted file mode 100644
index bbb8866d..00000000
--- a/tracker-steamvr/dialog.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-#include "steamvr.hpp"
-#include "api/plugin-api.hpp"
-
-steamvr_dialog::steamvr_dialog()
-{
-    ui.setupUi(this);
-
-    connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
-    connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
-}
-
-void steamvr_dialog::doOK()
-{
-    s.b->save();
-    close();
-}
-
-void steamvr_dialog::doCancel()
-{
-    close();
-}
-
diff --git a/tracker-steamvr/dialog.ui b/tracker-steamvr/dialog.ui
index 9d3b1e85..12a01816 100644
--- a/tracker-steamvr/dialog.ui
+++ b/tracker-steamvr/dialog.ui
@@ -9,15 +9,15 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>281</width>
-    <height>66</height>
+    <width>394</width>
+    <height>85</height>
    </rect>
   </property>
-  <property name="sizePolicy">
-   <sizepolicy hsizetype="Minimum" vsizetype="Minimum">
-    <horstretch>0</horstretch>
-    <verstretch>0</verstretch>
-   </sizepolicy>
+  <property name="minimumSize">
+   <size>
+    <width>394</width>
+    <height>0</height>
+   </size>
   </property>
   <property name="windowTitle">
    <string>Valve SteamVR</string>
@@ -32,8 +32,40 @@
   <property name="autoFillBackground">
    <bool>false</bool>
   </property>
-  <layout class="QGridLayout" name="gridLayout_2">
-   <item row="1" column="0">
+  <layout class="QVBoxLayout" name="verticalLayout">
+   <item>
+    <widget class="QFrame" name="frame">
+     <property name="frameShape">
+      <enum>QFrame::NoFrame</enum>
+     </property>
+     <layout class="QGridLayout" name="gridLayout">
+      <item row="0" column="0">
+       <widget class="QLabel" name="label">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Preferred" vsizetype="Maximum">
+          <horstretch>3</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
+        <property name="text">
+         <string>Device</string>
+        </property>
+       </widget>
+      </item>
+      <item row="0" column="1">
+       <widget class="QComboBox" name="device">
+        <property name="sizePolicy">
+         <sizepolicy hsizetype="Preferred" vsizetype="Maximum">
+          <horstretch>8</horstretch>
+          <verstretch>0</verstretch>
+         </sizepolicy>
+        </property>
+       </widget>
+      </item>
+     </layout>
+    </widget>
+   </item>
+   <item>
     <widget class="QDialogButtonBox" name="buttonBox">
      <property name="sizePolicy">
       <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
@@ -46,13 +78,6 @@
      </property>
     </widget>
    </item>
-   <item row="0" column="0">
-    <widget class="QLabel" name="label">
-     <property name="text">
-      <string>No settings necessary</string>
-     </property>
-    </widget>
-   </item>
   </layout>
  </widget>
  <resources/>
diff --git a/tracker-steamvr/steamvr.cpp b/tracker-steamvr/steamvr.cpp
index 2d92c5a4..b60d4896 100644
--- a/tracker-steamvr/steamvr.cpp
+++ b/tracker-steamvr/steamvr.cpp
@@ -16,45 +16,161 @@
  */
 
 #include "steamvr.hpp"
+
 #include "api/plugin-api.hpp"
 #include "compat/util.hpp"
 
 #include <cstdlib>
 #include <cmath>
+#include <type_traits>
+#include <algorithm>
 
 #include <QMessageBox>
 #include <QDebug>
 
-void steamvr::vr_deleter()
+QMutex device_list::mtx(QMutex::Recursive);
+
+template<typename F>
+static auto with_vr_lock(F&& fun) -> decltype(fun(vr_t(), error_t()))
 {
-    static std::atomic_flag atexit_done = ATOMIC_FLAG_INIT;
+    QMutexLocker l(&device_list::mtx);
+    error_t e; vr_t v;
+    std::tie(v, e) = device_list::vr_init();
+    return fun(v, e);
+}
 
-    if (atexit_done.test_and_set(std::memory_order_seq_cst))
+void device_list::fill_device_specs(QList<device_spec>& list)
+{
+    with_vr_lock([&](vr_t v, error_t)
     {
-        std::atexit(vr::VR_Shutdown);
-    }
+        list.clear();
+        list.reserve(max_devices);
+
+        pose_t device_states[max_devices];
+
+        if (v)
+        {
+            v->GetDeviceToAbsoluteTrackingPose(origin::TrackingUniverseSeated, 0,
+                                               device_states, vr::k_unMaxTrackedDeviceCount);
+
+            static constexpr unsigned bufsiz = vr::k_unTrackingStringSize;
+            char str[bufsiz] {};
+
+            for (unsigned k = 0; k < vr::k_unMaxTrackedDeviceCount; k++)
+            {
+               if (v->GetTrackedDeviceClass(k) == vr::ETrackedDeviceClass::TrackedDeviceClass_Invalid ||
+                   v->GetTrackedDeviceClass(k) == vr::ETrackedDeviceClass::TrackedDeviceClass_TrackingReference)
+                   continue;
+
+               if (!device_states[k].bDeviceIsConnected)
+                   continue;
+
+               unsigned len;
+
+               len = v->GetStringTrackedDeviceProperty(k, vr::ETrackedDeviceProperty::Prop_SerialNumber_String, str, bufsiz);
+               if (!len)
+               {
+                   qDebug() << "steamvr: getting serial number failed for" << k;
+                   continue;
+               }
+
+               device_spec dev;
+
+               dev.serial = str;
+
+               len = v->GetStringTrackedDeviceProperty(k, vr::ETrackedDeviceProperty::Prop_ModelNumber_String, str, bufsiz);
+               if (!len)
+               {
+                   qDebug() << "steamvr: getting model number failed for" << k;
+                   continue;
+               }
+
+               dev.model = str;
+               dev.pose = device_states[k];
+               dev.k = k;
+
+               list.push_back(dev);
+            }
+        }
+    });
+}
+
+device_list::device_list()
+{
+    refresh_device_list();
+}
+
+void device_list::refresh_device_list()
+{
+    fill_device_specs(device_specs);
+}
+
+device_list::maybe_pose device_list::get_pose(int k)
+{
+    if (k < 0 || !(k < max_devices))
+        return maybe_pose(false, pose_t{});
+
+    return with_vr_lock([k](vr_t v, error_t)
+    {
+        static pose_t poses[max_devices] {}; // vr_lock removes reentrancy
+
+        v->GetDeviceToAbsoluteTrackingPose(origin::TrackingUniverseSeated, 0,
+                                           poses, max_devices);
+
+        const pose_t& pose = poses[k];
+
+        if (pose.bPoseIsValid && pose.bDeviceIsConnected)
+            return maybe_pose(true, poses[k]);
+        else
+            once_only(qDebug() << "steamvr:"
+                               << "no valid pose from device" << k
+                               << "valid" << pose.bPoseIsValid
+                               << "connected" << pose.bDeviceIsConnected);
+
+        return maybe_pose(false, pose_t{});
+    });
+}
+
+bool device_list::get_all_poses(pose_t* poses)
+{
+    with_vr_lock([poses](vr_t v, error_t)
+    {
+        if (v)
+        {
+            v->GetDeviceToAbsoluteTrackingPose(origin::TrackingUniverseSeated, 0,
+                                               poses, max_devices);
+        }
+        return v != nullptr;
+    });
+
 }
 
-steamvr::vr_t steamvr::vr_init(error_t& error)
+tt device_list::vr_init()
 {
-    error = error_t::VRInitError_Unknown;
+    static tt t = vr_init_();
+    return t;
+}
 
-    // the background type would surely confuse users
-    vr_t vr = vr::VR_Init(&error, vr::EVRApplicationType::VRApplication_Other);
+tt device_list::vr_init_()
+{
+    error_t error = error_t::VRInitError_Unknown;
+    vr_t v = vr::VR_Init(&error, vr::EVRApplicationType::VRApplication_Other);
 
-    if (vr)
-        vr_deleter();
+    if (v)
+        std::atexit(vr::VR_Shutdown);
+    else
+        once_only(qDebug() << "steamvr: init failure" << error << strerror(error));
 
-    return vr;
+    return tt(v, error);
 }
 
-QString steamvr::strerror(error_t error)
+QString device_list::strerror(error_t err)
 {
-    const char* str(vr::VR_GetVRInitErrorAsSymbol(error));
+    const char* str(vr::VR_GetVRInitErrorAsSymbol(err));
     return QString(str ? str : "No description");
 }
 
-steamvr::steamvr() : vr(nullptr)
+steamvr::steamvr() : device_index(-1)
 {
 }
 
@@ -64,97 +180,106 @@ steamvr::~steamvr()
 
 void steamvr::start_tracker(QFrame*)
 {
-    error_t e(error_t::VRInitError_Unknown);
-    vr = vr_init(e);
-
-    if (!vr)
+    with_vr_lock([this](vr_t v, error_t e)
     {
-        QMessageBox::warning(nullptr,
-                             tr("Valve SteamVR init error"), strerror(e),
-                             QMessageBox::Close, QMessageBox::NoButton);
-        return;
-    }
+        if (!v)
+        {
+            QMessageBox::warning(nullptr,
+                                 tr("Valve SteamVR init error"), strerror(e),
+                                 QMessageBox::Close, QMessageBox::NoButton);
+        }
 
-    bool ok = false;
+        const QString serial = s.device_serial;
+        device_list d;
+        const QList<device_spec>& specs = d.devices();
+        const int sz = specs.count();
 
-    for (unsigned k = 0; k < vr::k_unMaxTrackedDeviceCount; k++)
-        if (vr->GetTrackedDeviceClass(k) == vr::ETrackedDeviceClass::TrackedDeviceClass_HMD)
+        if (sz == 0)
         {
-            ok = true;
-            break;
+            QMessageBox::warning(nullptr,
+                                 tr("Valve SteamVR init error"),
+                                 tr("No HMD connected"),
+                                 QMessageBox::Close, QMessageBox::NoButton);
         }
 
-    if (!ok)
-    {
-        QMessageBox::warning(nullptr,
-                             tr("Valve SteamVR init warning"),
-                             tr("No HMD connected"),
-                             QMessageBox::Close, QMessageBox::NoButton);
-        return;
-    }
+        for (const device_spec& spec : specs)
+        {
+            if (serial == "" || serial == spec.serial)
+            {
+                device_index = int(spec.k);
+                break;
+            }
+        }
+    });
 }
 
 void steamvr::data(double* data)
 {
-    if (vr)
+    if (device_index != -1)
     {
-        vr::TrackedDevicePose_t devices[vr::k_unMaxTrackedDeviceCount] = {};
-
-        vr->GetDeviceToAbsoluteTrackingPose(vr::ETrackingUniverseOrigin::TrackingUniverseSeated, 0,
-                                            devices, vr::k_unMaxTrackedDeviceCount);
-
-        for (unsigned k = 0; k < vr::k_unMaxTrackedDeviceCount; k++)
+        pose_t pose; bool ok;
+        std::tie(ok, pose) = device_list::get_pose(device_index);
+        if (ok)
         {
-            if (!devices[k].bPoseIsValid)
-                continue;
-
-            if (vr->GetTrackedDeviceClass(k) != vr::ETrackedDeviceClass::TrackedDeviceClass_HMD)
-                continue;
-
-            const vr::HmdMatrix34_t& result = devices[k].mDeviceToAbsoluteTracking;
+            static constexpr int c = 10;
 
-            int c = 10;
+            const auto& result = pose.mDeviceToAbsoluteTracking;
 
             data[TX] = -result.m[0][3] * c;
             data[TY] = result.m[1][3] * c;
             data[TZ] = result.m[2][3] * c;
 
-            using std::atan2;
-            using std::asin;
-
-            // transformation matrix to euler angles
-            if (result.m[0][0] == 1.0f || result.m[0][0] == -1.0f)
-            {
-                data[Yaw] = -atan2(result.m[0][2], result.m[2][3]);
-                data[Pitch] = 0;
-                data[Roll] = 0;
-            }
-            else
-            {
-                data[Yaw] = -atan2(-result.m[2][0], result.m[0][0]);
-                data[Pitch] = atan2(-result.m[1][2], result.m[1][1]);
-                data[Roll] = atan2(result.m[1][1], result.m[0][1]);
-            }
+            matrix_to_euler(data[Yaw], data[Pitch], data[Roll], result);
 
             static constexpr double r2d = 180 / M_PI;
-
-            data[Yaw] *= r2d;
-            data[Pitch] *= r2d;
-            data[Roll] *= r2d;
-
-            break;
+            data[Yaw] *= r2d; data[Pitch] *= r2d; data[Roll] *= r2d;
         }
     }
 }
 
 bool steamvr::center()
 {
-    if (vr)
-        vr->ResetSeatedZeroPose();
+    with_vr_lock([&](vr_t v, error_t)
+    {
+        if (v)
+            v->ResetSeatedZeroPose();
+    });
     return false;
 }
 
-void steamvr_dialog::register_tracker(ITracker*) {}
-void steamvr_dialog::unregister_tracker() {}
+void steamvr::matrix_to_euler(double& yaw, double& pitch, double& roll, const vr::HmdMatrix34_t& result)
+{
+    yaw = atan2(result.m[2][0], result.m[0][0]);
+    pitch = atan2(result.m[1][1], result.m[1][2]);
+    roll = atan2(result.m[1][1], result.m[0][1]);
+}
+
+steamvr_dialog::steamvr_dialog()
+{
+    ui.setupUi(this);
+
+    connect(ui.buttonBox, SIGNAL(accepted()), this, SLOT(doOK()));
+    connect(ui.buttonBox, SIGNAL(rejected()), this, SLOT(doCancel()));
+
+    ui.device->clear();
+    ui.device->addItem("");
+
+    device_list list;
+    for (const device_spec& spec : list.devices())
+        ui.device->addItem(spec.serial);
+
+    tie_setting(s.device_serial, ui.device);
+}
+
+void steamvr_dialog::doOK()
+{
+    s.b->save();
+    close();
+}
+
+void steamvr_dialog::doCancel()
+{
+    close();
+}
 
 OPENTRACK_DECLARE_TRACKER(steamvr, steamvr_dialog, steamvr_metadata)
diff --git a/tracker-steamvr/steamvr.hpp b/tracker-steamvr/steamvr.hpp
index 2842fda3..0c6af65a 100644
--- a/tracker-steamvr/steamvr.hpp
+++ b/tracker-steamvr/steamvr.hpp
@@ -8,47 +8,86 @@
 
 #include <openvr.h>
 
-#include <atomic>
 #include <cmath>
 #include <memory>
+#include <tuple>
 
 #include <QString>
 #include <QMutex>
 #include <QMutexLocker>
+#include <QList>
 
 using namespace options;
+using error_t = vr::EVRInitError;
+using vr_t = vr::IVRSystem*;
+
+using tt = std::tuple<vr_t, error_t>;
+using pose_t = vr::TrackedDevicePose_t;
+using origin = vr::ETrackingUniverseOrigin;
 
 struct settings : opts
 {
+    value<QString> device_serial;
     settings() :
-        opts("valve-steamvr")
+        opts("valve-steamvr"),
+        device_serial(b, "serial", "")
     {}
 };
 
+struct device_spec
+{
+    vr::TrackedDevicePose_t pose;
+    QString model, serial;
+    unsigned k;
+};
+
+struct device_list final
+{
+    using maybe_pose = std::tuple<bool, pose_t>;
+
+    device_list();
+    void refresh_device_list();
+    const QList<device_spec>& devices() const & { return device_specs; }
+
+    static OTR_NEVER_INLINE maybe_pose get_pose(int k);
+    static bool get_all_poses(pose_t*poses);
+    static QString strerror(error_t error);
+    static constexpr int max_devices = int(vr::k_unMaxTrackedDeviceCount);
+
+    template<typename F>
+    friend static auto with_vr_lock(F&& fun) -> decltype(fun(vr_t(), error_t()));
+
+private:
+    QList<device_spec> device_specs;
+    static QMutex mtx;
+    static tt vr_init_();
+    static void vr_deleter();
+    static void fill_device_specs(QList<device_spec>& list);
+    static tt vr_init();
+};
+
 class steamvr : public QObject, public ITracker
 {
     Q_OBJECT
+
+    using error_t = vr::EVRInitError;
+    using vr_t = vr::IVRSystem*;
+
 public:
     steamvr();
     ~steamvr() override;
     void start_tracker(QFrame *) override;
     void data(double *data) override;
     bool center() override;
-private:
 
-    using error_t = vr::EVRInitError;
-    using vr_t = vr::IVRSystem*;
-
-    vr_t vr;
+private:
+    void matrix_to_euler(double &yaw, double &pitch, double &roll, const vr::HmdMatrix34_t& result);
 
     settings s;
+    int device_index;
 
     using rmat = euler::rmat;
     using euler_t = euler::euler_t;
-
-    static void vr_deleter();
-    static vr_t vr_init(error_t& error);
-    static QString strerror(error_t error);
 };
 
 class steamvr_dialog : public ITrackerDialog
@@ -57,12 +96,11 @@ class steamvr_dialog : public ITrackerDialog
 public:
     steamvr_dialog();
 
-    void register_tracker(ITracker *) override;
-    void unregister_tracker() override;
 private:
     Ui::dialog ui;
     settings s;
-    vr::IVRSystem* vr;
+    device_list devices;
+
 private slots:
     void doOK();
     void doCancel();