Stream motion data

#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

#include <ts_api/ts_types.h>

#ifdef _WIN32
#include <Windows.h>
#elif defined(__APPLE__) || defined(__linux__)
#include <dlfcn.h>
#include <unistd.h>
#endif

// Change these to explore other bones/joints.
static const TsBoneIndex TARGET_BONE_INDEX = TsBoneIndex_RightUpperArm;
static const TsBiomechanicalIndex TARGET_BIOMECHANICAL_INDEX =
    TsBiomechanicalIndex_PelvisTilt;

#ifdef __cplusplus
extern "C" {
#endif

#define API_FUNCTION_NAME(name) name##_func_t

#define DECLARE_API_FUNCTION(ret, name, args)                                  \
  typedef ret(*API_FUNCTION_NAME(name)) args;                                  \
  static API_FUNCTION_NAME(name) name

#define LOAD_API_FUNCTION(lib, name)                                           \
  do {                                                                         \
    name = (API_FUNCTION_NAME(name))api_library_get_function(lib, #name);      \
    if (name == NULL)                                                          \
      return false;                                                            \
  } while (0)

typedef void *SharedLibrary;

static void *api_library_get_function(SharedLibrary lib,
                                      const char *function_name) {
#ifdef _WIN32
  return (void *)GetProcAddress((HMODULE)lib, function_name);
#elif defined(__APPLE__) || defined(__linux__)
  return dlsym(lib, function_name);
#endif
}

static void sleep_ms(int ms) {
#ifdef _WIN32
  Sleep(ms);
#elif defined(__APPLE__) || defined(__linux__)
  usleep(ms * 1000);
#endif
}

// device/session
DECLARE_API_FUNCTION(TsStatusCode, ts_initialize, ());
DECLARE_API_FUNCTION(void, ts_uninitialize, ());
DECLARE_API_FUNCTION(TsStatusCode, ts_get_device_list,
                     (TsDevice *, uint32_t *));
DECLARE_API_FUNCTION(TsDeviceHandle *, ts_device_open, (TsDevice *));
DECLARE_API_FUNCTION(const char *, ts_device_get_name, (TsDeviceHandle *));
DECLARE_API_FUNCTION(void, ts_device_close, (TsDeviceHandle *));
DECLARE_API_FUNCTION(const char *, ts_get_status_code_message, (TsStatusCode));

// mocap
DECLARE_API_FUNCTION(TsStatusCode, ts_mocap_set_skeleton_update_callback,
                     (TsDeviceHandle *, TsMocapSkeletonCallback, void *));
DECLARE_API_FUNCTION(TsStatusCode, ts_mocap_set_sensor_skeleton_update_callback,
                     (TsDeviceHandle *, TsMocapSensorSkeletonCallback, void *));
DECLARE_API_FUNCTION(TsStatusCode, ts_mocap_start_streaming,
                     (TsDeviceHandle *));
DECLARE_API_FUNCTION(TsStatusCode, ts_mocap_stop_streaming, (TsDeviceHandle *));
DECLARE_API_FUNCTION(TsStatusCode, ts_mocap_sensor_skeleton_get_bone,
                     (const TsMocapSensorSkeleton, TsBoneIndex,
                      TsMocapSensor *));
DECLARE_API_FUNCTION(TsStatusCode, ts_mocap_skeleton_get_biomechanical_angle,
                     (const TsMocapSkeleton, TsBiomechanicalIndex, float *));

static bool load_api_functions(SharedLibrary lib) {
  // device/session
  LOAD_API_FUNCTION(lib, ts_initialize);
  LOAD_API_FUNCTION(lib, ts_uninitialize);
  LOAD_API_FUNCTION(lib, ts_get_device_list);
  LOAD_API_FUNCTION(lib, ts_device_open);
  LOAD_API_FUNCTION(lib, ts_device_get_name);
  LOAD_API_FUNCTION(lib, ts_device_close);
  LOAD_API_FUNCTION(lib, ts_get_status_code_message);

  // mocap
  LOAD_API_FUNCTION(lib, ts_mocap_set_skeleton_update_callback);
  LOAD_API_FUNCTION(lib, ts_mocap_set_sensor_skeleton_update_callback);
  LOAD_API_FUNCTION(lib, ts_mocap_start_streaming);
  LOAD_API_FUNCTION(lib, ts_mocap_stop_streaming);
  LOAD_API_FUNCTION(lib, ts_mocap_sensor_skeleton_get_bone);
  LOAD_API_FUNCTION(lib, ts_mocap_skeleton_get_biomechanical_angle);
  return true;
}

static bool check_code(TsStatusCode code) {
  if (code != 0) {
    printf("Error: %s\n", ts_get_status_code_message(code));
    return false;
  }
  return true;
}

// Raw IMU data for TARGET_BONE_INDEX, called once per sensor update.
static void sensor_skeleton_callback(TsDeviceHandle *dev,
                                     TsMocapSensorSkeleton skeleton,
                                     void *user_data) {
  (void)dev;
  (void)user_data;

  TsMocapSensor sensor;
  if (check_code(ts_mocap_sensor_skeleton_get_bone(skeleton, TARGET_BONE_INDEX,
                                                   &sensor))) {
    printf("Gyro: (%.2f, %.2f, %.2f)\n", sensor.gyro.x, sensor.gyro.y,
           sensor.gyro.z);
  }
}

// Processed joint angle for TARGET_BIOMECHANICAL_INDEX, called once per
// skeleton update.
static void skeleton_callback(TsDeviceHandle *dev, TsMocapSkeleton skeleton,
                              void *user_data) {
  (void)dev;
  (void)user_data;

  float angle = 0;
  if (check_code(ts_mocap_skeleton_get_biomechanical_angle(
          skeleton, TARGET_BIOMECHANICAL_INDEX, &angle))) {
    printf("Biomechanical angle: %.2f\n", angle);
  }
}

static void run(TsDeviceHandle *dev) {
  check_code(ts_mocap_set_sensor_skeleton_update_callback(
      dev, sensor_skeleton_callback, NULL));
  check_code(
      ts_mocap_set_skeleton_update_callback(dev, skeleton_callback, NULL));

  if (check_code(ts_mocap_start_streaming(dev))) {
    printf("Press any key to end\n");
    (void)getchar();

    ts_mocap_stop_streaming(dev);
  }
}

#ifdef __cplusplus
}
#endif

int main() {
  const char *api_path = getenv("TESLASUIT_API_LIB_PATH");
  if (api_path == NULL) {
    printf("Error: environment variable TESLASUIT_API_LIB_PATH not set\n");
    return 0;
  }

#ifdef _WIN32
  SharedLibrary lib = (SharedLibrary)LoadLibraryEx(
      api_path, NULL, LOAD_WITH_ALTERED_SEARCH_PATH);
#elif defined(__APPLE__) || defined(__linux__)
  SharedLibrary lib = (SharedLibrary)dlopen(api_path, RTLD_NOW | RTLD_LOCAL);
#endif
  if (lib == NULL) {
    printf("Error: library was not found\n");
    return 0;
  }

  if (!load_api_functions(lib)) {
    printf("Error: failed to load library functions\n");
    goto unload;
  }

  if (!check_code(ts_initialize())) {
    printf("Error: failed to initialize API\n");
    // Nothing to uninitialize: it never successfully initialized.
    goto unload;
  }

  TsDevice device;
  uint32_t device_count = 0;
  while (device_count == 0) {
    printf("Waiting for a device...\n");
    device_count = 1;
    ts_get_device_list(&device, &device_count);
    sleep_ms(100);
  }

  TsDeviceHandle *dev = ts_device_open(&device);
  if (dev == NULL) {
    printf("Error: failed to get device\n");
    goto deinitialize;
  }
  printf("Using device: %s\n", ts_device_get_name(dev));

  run(dev);

  ts_device_close(dev);

deinitialize:
  ts_uninitialize();

unload:
#ifdef _WIN32
  FreeLibrary((HMODULE)lib);
#elif defined(__APPLE__) || defined(__linux__)
  dlclose(lib);
#endif
  return 0;
}

Waiting for a device...
Using device: Teslasuit XR-5
Press any key to end
Gyro: (0.12, -0.03, 0.45)
Biomechanical angle: 15.00
...