sOpenvr.cpp

/*
-----------------------------------------------------------------------------
This source file is part of OpenSpace3D
For the latest info, see http://www.openspace3d.com

Copyright (c) 2013 I-maginer

This program is free software; you can redistribute it and/or modify it under
the terms of the GNU Lesser General Public License as published by the Free Software
Foundation; either version 2 of the License, or (at your option) any later
version.

This program is distributed in the hope that it will be useful, but WITHOUT
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License along with
this program; if not, write to the Free Software Foundation, Inc., 59 Temple
Place - Suite 330, Boston, MA 02111-1307, USA, or go to
http://www.gnu.org/copyleft/lesser.txt

-----------------------------------------------------------------------------
*/

#include "sOpenvr.h"

//TODO get player area vr::HmdQuad_t TempPlayArea; vr::VRChaperone()->GetPlayAreaRect(&TempPlayArea); // https://bitbucket.org/mkultra333/openvrtest

extern int Openvr_QUIT_CB;
extern int Openvr_IPD_CB;

//generic tickcount function
int64 getTickCount(void)
{
#if defined WIN32 || defined _WIN32 || defined WINCE
  LARGE_INTEGER counter;
  QueryPerformanceCounter(&counter);
  return (int64)counter.QuadPart;
#elif defined __linux || defined __linux__
  struct timespec tp;
  clock_gettime(CLOCK_MONOTONIC, &tp);
  return (int64)tp.tv_sec * 1000000000 + tp.tv_nsec;
#elif defined __MACH__ && defined __APPLE__
  return (int64)mach_absolute_time();
#else
  struct timeval tv;
  struct timezone tz;
  gettimeofday(&tv, &tz);
  return (int64)tv.tv_sec * 1000000 + tv.tv_usec;
#endif
}

sOpenvr::sOpenvr(const char* inpuPath)
{
  mConnected = false;
  mHMDSystem = NULL;
  mVisible = false;
  mCamera = NULL;
  mCameraWidth = 0;
  mCameraHeight = 0;
  mCameraFrameBufferSize = 0;
  mCameraFrameBuffer = NULL;
  mCameraLastFrameId = 0;
  mCameraEnable = false;

#if USE_VR_INPUT
  mInputPath = inpuPath;
  mActionSet = vr::k_ulInvalidActionSetHandle;
#endif

  mTrackedCameraHandle = INVALID_TRACKED_CAMERA_HANDLE;

  mTexturesBounds[0].uMin = 0;
  mTexturesBounds[0].uMax = 1;
  mTexturesBounds[0].vMin = 1;
  mTexturesBounds[0].vMax = 0;

  mTexturesBounds[1].uMin = 0;
  mTexturesBounds[1].uMax = 1;
  mTexturesBounds[1].vMin = 1;
  mTexturesBounds[1].vMax = 0;

  for (unsigned int i = 0; i < MAX_VR_CONTROLLERS; i++)
    mControllers[i] = 0;

#ifdef USE_VIVE_GESTURE
  mGestureInterface = true;
  UseExternalTransform(false);
  mLastGestureFrameIndex = -1;
#endif
}

sOpenvr::~sOpenvr()
{
  Disconnect();
}

bool sOpenvr::IsConnected()
{
  return mConnected;
}

bool sOpenvr::Connect()
{
  if (mConnected)
    return false;

#ifdef USE_VIVE_GESTURE

  // Add your code here to modify option by changing backend or mode
  mGestureOption.mode = GestureModeSkeleton;

  // Start detection and check result
  GestureFailure result = StartGestureDetection(&mGestureOption);
  if (result != GestureFailureNone)
    mGestureInterface = false;
  else
    mGestureInterface = true;
#endif

  mHMDSystem = VR_Init(&mHMDError, vr::EVRApplicationType::VRApplication_Scene);
  if (mHMDError != vr::VRInitError_None) //Check for errors
  {
    switch (mHMDError)
    {
    default:
      MMechostr(MSKRUNTIME, "OpenVR Error: failed OpenVR VR_Init, Non described error when initializing the OpenVR Render object.\n");
      break;

    case vr::VRInitError_Init_HmdNotFound:
    case vr::VRInitError_Init_HmdNotFoundPresenceFailed:
      MMechostr(MSKRUNTIME, "OpenVR Error: cannot find HMD, Please install SteamVR and launch it, and verify HMD USB and HDMI connection\n");
      break;
    }
    return false;
  }

  if (!mHMDSystem)
  {
    MMechostr(MSKRUNTIME, "OpenVR Error: failed to init VRsystem, Non described error when initializing the OpenVR Render object.\n");
    return false;
  }

  //Check if VRCompositor is present
  if (!vr::VRCompositor())
  {
    MMechostr(MSKRUNTIME, "OpenVR Error: failed to init OpenVR VRCompositor\n");
    return false;
  }

  //Get Driver and Display information
  std::string sdevice;
  int bufflen = mHMDSystem->GetStringTrackedDeviceProperty(vr::k_unTrackedDeviceIndex_Hmd, vr::Prop_TrackingSystemName_String, 0, 0, 0);
  if (bufflen != 0)
  {
    char* buff = new char[bufflen];
    mHMDSystem->GetStringTrackedDeviceProperty(vr::k_unTrackedDeviceIndex_Hmd, vr::Prop_TrackingSystemName_String, buff, bufflen, 0);
    sdevice = buff;
    delete[] buff;
  }

  MMechostr(MSKRUNTIME, "OpenVR HMD name: %s\n", sdevice.c_str());

  std::string sserial;
  bufflen = mHMDSystem->GetStringTrackedDeviceProperty(vr::k_unTrackedDeviceIndex_Hmd, vr::Prop_SerialNumber_String, 0, 0, 0);
  if (bufflen != 0)
  {
    char* buff = new char[bufflen];
    mHMDSystem->GetStringTrackedDeviceProperty(vr::k_unTrackedDeviceIndex_Hmd, vr::Prop_SerialNumber_String, buff, bufflen, 0);
    sserial = buff;
    delete[] buff;
  }

  MMechostr(MSKRUNTIME, "OpenVR HMD serial: %s\n", sserial.c_str());

  // init controllers
  //mControllers[VrControllerSide::Left] = new sOpenvrController(mHMDSystem, VrControllerSide::Left);
  //mControllers[VrControllerSide::Right] = new sOpenvrController(mHMDSystem, VrControllerSide::Right);
  for (unsigned int i = 0; i < MAX_VR_CONTROLLERS; i++)
  {
    mControllers[i] = new sOpenvrController(mHMDSystem, i);
  }

  //check camera
  mCamera = vr::VRTrackedCamera();
  if (!mCamera)
  {
    MMechostr(MSKRUNTIME, "OpenVR No camera found\n");
  }

  bool bHasCamera = false;
  vr::EVRTrackedCameraError nCameraError = mCamera->HasCamera(vr::k_unTrackedDeviceIndex_Hmd, &bHasCamera);
  if (nCameraError != vr::VRTrackedCameraError_None || !bHasCamera)
  {
    MMechostr(MSKRUNTIME, "OpenVR No tracked camera available: %s\n", mCamera->GetCameraErrorNameFromEnum(nCameraError));
    mCamera = NULL;
  }

#if USE_VR_INPUT
  //TODO check file exist or create it / them + check errors
  vr::VRInput()->SetActionManifestPath(mInputPath.c_str());
  vr::VRInput()->GetActionSetHandle("/actions/scol", &mActionSet);
#endif

  mConnected = true;
  return true;
}

std::string sOpenvr::GetHmdName()
{
  if (!mConnected)
    return "";

  std::string sdevice;
  int bufflen = mHMDSystem->GetStringTrackedDeviceProperty(vr::k_unTrackedDeviceIndex_Hmd, vr::Prop_TrackingSystemName_String, 0, 0, 0);
  if (bufflen != 0)
  {
    char* buff = new char[bufflen];
    mHMDSystem->GetStringTrackedDeviceProperty(vr::k_unTrackedDeviceIndex_Hmd, vr::Prop_TrackingSystemName_String, buff, bufflen, 0);
    sdevice = buff;
    delete[] buff;
  }
  return sdevice;
}

void sOpenvr::Disconnect()
{
  // Free controllers
  for (unsigned int i = 0; i < MAX_VR_CONTROLLERS; i++)
    SAFE_DELETE(mControllers[i]);

  if (mCamera)
  {
    mCamera->ReleaseVideoStreamingService(mTrackedCameraHandle);
    mCamera = NULL;
    mCameraEnable = false;
  }

  if (mCameraFrameBuffer)
  {
    delete[] mCameraFrameBuffer;
    mCameraFrameBuffer = NULL;
  }

  if (mConnected)
  {
    //Shutdown SteamVR
    if (mHMDSystem)
      vr::VR_Shutdown();

    mHMDSystem = NULL;
    mConnected = false;
  }

#ifdef USE_VIVE_GESTURE
  if (mGestureInterface)
    StopGestureDetection();
#endif
}

void sOpenvr::ResetHmd()
{
  if (mHMDSystem)
    mHMDSystem->ResetSeatedZeroPose();
}

bool sOpenvr::IsVisible()
{
  return mVisible;
}

inline Matrix4 sOpenvr::getMatrix4FromSteamVRMatrix34(const vr::HmdMatrix34_t& mat)
{
  return Matrix4
  {
    mat.m[0][0], mat.m[0][1], mat.m[0][2], mat.m[0][3],
    mat.m[1][0], mat.m[1][1], mat.m[1][2], mat.m[1][3],
    mat.m[2][0], mat.m[2][1], mat.m[2][2], mat.m[2][3],
    0.0f, 0.0f, 0.0f, 1.0f
  };
}

bool sOpenvr::GetHmdOrientation(Quaternion &quat)
{
  if (!mConnected)
    return false;

  vr::TrackedDevicePose_t hmdPose;
  if ((hmdPose = mTrackedPoses[vr::k_unTrackedDeviceIndex_Hmd]).bPoseIsValid)
  {
    const vr::HmdMatrix34_t mat = hmdPose.mDeviceToAbsoluteTracking;
    Matrix4 cmat(mat.m[0][0], mat.m[0][1], mat.m[0][2], mat.m[0][3],
      mat.m[1][0], mat.m[1][1], mat.m[1][2], mat.m[1][3],
      mat.m[2][0], mat.m[2][1], mat.m[2][2], mat.m[2][3],
      0.0f, 0.0f, 0.0f, 1.0f);

    quat = cmat.extractQuaternion();
    mPreviousHmdQuat = quat;
  }
  else
  {
    quat = mPreviousHmdQuat;
    //return false;
  }
  return true;
}

bool sOpenvr::GetHmdPosition(Vector3 &vec)
{
  if (!mConnected)// || !(mTrackingState.StatusFlags & ovrStatus_PositionTracked))
    return false;

  vr::TrackedDevicePose_t hmdPose;
  if ((hmdPose = mTrackedPoses[vr::k_unTrackedDeviceIndex_Hmd]).bPoseIsValid)
  {
    const vr::HmdMatrix34_t mat = hmdPose.mDeviceToAbsoluteTracking;
    Matrix4 cmat(mat.m[0][0], mat.m[0][1], mat.m[0][2], mat.m[0][3],
      mat.m[1][0], mat.m[1][1], mat.m[1][2], mat.m[1][3],
      mat.m[2][0], mat.m[2][1], mat.m[2][2], mat.m[2][3],
      0.0f, 0.0f, 0.0f, 1.0f);

    vec = cmat.getTrans();
    mPreviousHmdPos = vec;
  }
  else
  {
    vec = mPreviousHmdPos;
    //return false;
  }
  return true;
}

bool sOpenvr::GetProjectionMatrix(vr::EVREye eye, float nearclip, float farclip, Matrix4 &mat)
{
  if (!mHMDSystem)
    return false;

  vr::HmdMatrix44_t vrmat = mHMDSystem->GetProjectionMatrix(eye, nearclip, farclip);
  for (unsigned int x : { 0, 1, 2, 3 })
    for (unsigned int y : { 0, 1, 2, 3 })
      mat[x][y] = vrmat.m[x][y];

  return true;
}

bool sOpenvr::GetStereoTextureSize(unsigned int &w, unsigned int &h)
{
  if (!mHMDSystem)
    return false;

  mHMDSystem->GetRecommendedRenderTargetSize(&w, &h);

  // scale a bit for better quality
  w = (unsigned int)((float)w * 1.25f);
  h = (unsigned int)((float)h * 1.25f);
  /*
  if (UseSSAA)
  {
  if (AALevel / 2 > 0)
  {
  w *= AALevel / 2;
  h *= AALevel / 2;
  }
  }*/

  return true;
}

float sOpenvr::GetStereoConfigAspect()
{
  return 0.0f;
}

float sOpenvr::GetStereoConfigFovY()
{
  return 1.919862f;
}

float sOpenvr::GetStereoIPD()
{
  if (mHMDSystem)
  {
    return mHMDSystem->GetFloatTrackedDeviceProperty(vr::k_unTrackedDeviceIndex_Hmd, vr::Prop_UserIpdMeters_Float);
  }
  else
    return 0.064f;
}

bool sOpenvr::Update()
{
  if (mHMDSystem)
  {
    vr::VRCompositor()->WaitGetPoses(mTrackedPoses, vr::k_unMaxTrackedDeviceCount, 0, 0);

    //Here the controllers are handled
    ProcessEvents();

    //mHMDSystem can be closed in ProcessEvents
    if (mHMDSystem)
      ProcessTrackedDevices();

    return true;
  }

  return false;
}

void sOpenvr::UpdateTextures(void* leftTexture, void* rightTexture, vr::ETextureType type)
{
  if (mHMDSystem)
  {
    mTextures[0] = { leftTexture, type, vr::ColorSpace_Gamma };
    mTextures[1] = { rightTexture, type, vr::ColorSpace_Gamma };

    if (type == vr::TextureType_DirectX)
    {
      mTexturesBounds[0].uMin = 0;
      mTexturesBounds[0].uMax = 1;
      mTexturesBounds[0].vMin = 0;
      mTexturesBounds[0].vMax = 1;

      mTexturesBounds[1].uMin = 0;
      mTexturesBounds[1].uMax = 1;
      mTexturesBounds[1].vMin = 0;
      mTexturesBounds[1].vMax = 1;
    }
    else
    {
      mTexturesBounds[0].uMin = 0;
      mTexturesBounds[0].uMax = 1;
      mTexturesBounds[0].vMin = 1;
      mTexturesBounds[0].vMax = 0;

      mTexturesBounds[1].uMin = 0;
      mTexturesBounds[1].uMax = 1;
      mTexturesBounds[1].vMin = 1;
      mTexturesBounds[1].vMax = 0;
    }
    vr::VRCompositor()->Submit(vr::Eye_Left, &mTextures[0], &mTexturesBounds[0]);
    vr::VRCompositor()->Submit(vr::Eye_Right, &mTextures[1], &mTexturesBounds[1]);
  }
}

void sOpenvr::ProcessEvents()
{
  vr::VREvent_t event;
  //Pump the events, and for each event, switch on it type
  while (mHMDSystem && mHMDSystem->PollNextEvent(&event, sizeof event)) switch (event.eventType)
  {
    //Handle quiting the app from Steam
    case vr::VREvent_DriverRequestedQuit:
    case vr::VREvent_Quit:
      vr::VRSystem()->AcknowledgeQuit_Exiting();
      Disconnect();
      OBJpostEvent(Openvr_QUIT_CB, SCOL_PTR this, 0);
      break;

      //Handle user IPD adjustment
    case vr::VREvent_IpdChanged:
      OBJpostEvent(Openvr_IPD_CB, SCOL_PTR this, 0);
      break;
    default: break;
  }
}

void sOpenvr::ProcessTrackedDevices()
{
  if (mTrackedPoses[vr::k_unTrackedDeviceIndex_Hmd].bPoseIsValid)
    mVisible = true;
  else
    mVisible = false;

#if USE_VR_INPUT
  vr::VRActiveActionSet_t actionSet = { 0 };
  actionSet.ulActionSet = mActionSet;
  vr::VRInput()->UpdateActionState(&actionSet, sizeof(actionSet), 1);
#endif

  unsigned int id = 0;
  for (unsigned int trackedDevice = vr::k_unTrackedDeviceIndex_Hmd + 1; trackedDevice < vr::k_unMaxTrackedDeviceCount; trackedDevice++)
  {
    //If the device is not connected, pass.
    //If the device is not recognized as a controller, pass
    //If we don't have a valid pose of the controller, pass
    if (!mHMDSystem || (mHMDSystem->GetTrackedDeviceClass(trackedDevice) != vr::TrackedDeviceClass_Controller && mHMDSystem->GetTrackedDeviceClass(trackedDevice) != vr::TrackedDeviceClass_GenericTracker))
      continue;

    //From here we know that "trackedDevice" is the device index of a valid controller that has been tracked by the system
    //Extract basic information of the device, detect if they are left/right hands
    VrControllerSide side = VrControllerSide::Undefined;
    switch (mHMDSystem->GetControllerRoleForTrackedDeviceIndex(trackedDevice))
    {
    case vr::ETrackedControllerRole::TrackedControllerRole_LeftHand:
      side = VrControllerSide::Left;
      break;

    case vr::ETrackedControllerRole::TrackedControllerRole_RightHand:
      side = VrControllerSide::Right;
      break;

    case vr::ETrackedControllerRole::TrackedControllerRole_Invalid:
    default:
      break;
    }

    if (side != VrControllerSide::Undefined)
      mControllers[side]->Update(mTrackedPoses, trackedDevice);
    else if (((id + 2) < MAX_VR_CONTROLLERS) && (mControllers[id + 2] != 0))
    {
      mControllers[id + 2]->Update(mTrackedPoses, trackedDevice);
      id++;
    }
  }

#ifdef USE_VIVE_GESTURE
  if (mGestureInterface)
  {
    const GestureResult* points = NULL;

    int frameIndex = -1;
    int size = GetGestureResult(&points, &frameIndex);

    if ((frameIndex >= 0) && (frameIndex != mLastGestureFrameIndex))
    {
      if (size <= 0)
      {
        mControllers[8]->SetState(false);
        mControllers[9]->SetState(false);
        return;
      }

      mLastGestureFrameIndex = frameIndex;
      for (int i = 0; i < size; i++)
      {
        unsigned int id = (points[i].isLeft) ? 8 : 9;
        mControllers[id]->UpdateGesture(points[i], id);
      }
      //std::cout << "Hand " << i << " is " << points[i].isLeft ? "left" : "right" << " hand" << std::endl;
    }
  }
#endif
}

bool sOpenvr::HaveCamera()
{
  return (mCamera) ? true : false;
}

bool sOpenvr::IsCameraEnable()
{
  return mCameraEnable;
}

bool sOpenvr::StartCamera()
{
  uint32_t nCameraFrameBufferSize = 0;
  if (mCamera->GetCameraFrameSize(vr::k_unTrackedDeviceIndex_Hmd, vr::VRTrackedCameraFrameType_Undistorted, &mCameraWidth, &mCameraHeight, &nCameraFrameBufferSize) != vr::VRTrackedCameraError_None)
  {
    MMechostr(MSKRUNTIME, "OpenVR Start camera failed\n");
    return false;
  }

  if (nCameraFrameBufferSize && nCameraFrameBufferSize != mCameraFrameBufferSize)
  {
    if (mCameraFrameBufferSize)
      delete[] mCameraFrameBuffer;

    mCameraFrameBufferSize = nCameraFrameBufferSize;
    mCameraFrameBuffer = new uint8_t[mCameraFrameBufferSize];
    memset(mCameraFrameBuffer, 0, mCameraFrameBufferSize);
  }

  mCameraLastFrameId = 0;

  mCamera->AcquireVideoStreamingService(vr::k_unTrackedDeviceIndex_Hmd, &mTrackedCameraHandle);
  if (mTrackedCameraHandle == INVALID_TRACKED_CAMERA_HANDLE)
  {
    MMechostr(MSKRUNTIME, "OpenVR Acquire camera failed\n");
    return false;
  }

  mCameraEnable = true;
  return true;
}

void sOpenvr::StopCamera()
{
  if (mCamera)
    mCamera->ReleaseVideoStreamingService(mTrackedCameraHandle);
  mTrackedCameraHandle = INVALID_TRACKED_CAMERA_HANDLE;
  mCameraEnable = false;
}

void sOpenvr::GetCameraSize(uint32_t &w, uint32_t &h)
{
  w = mCameraWidth;
  h = mCameraHeight;
}

bool sOpenvr::GetCameraFrame(PtrObjBitmap scolBitmap)
{
  if (!mCameraEnable)
    return false;

  // get the frame header only
  vr::CameraVideoStreamFrameHeader_t frameHeader;
  vr::EVRTrackedCameraError nCameraError = mCamera->GetVideoStreamFrameBuffer(mTrackedCameraHandle, vr::VRTrackedCameraFrameType_Undistorted, nullptr, 0, &frameHeader, sizeof(frameHeader));
  if (nCameraError != vr::VRTrackedCameraError_None)
    return false;

  if (frameHeader.nFrameSequence == mCameraLastFrameId)
  {
    // frame hasn't changed yet, nothing to do
    return false;
  }
  
  // Frame has changed, do the more expensive frame buffer copy
  nCameraError = mCamera->GetVideoStreamFrameBuffer(mTrackedCameraHandle, vr::VRTrackedCameraFrameType_Undistorted, mCameraFrameBuffer, mCameraFrameBufferSize, &frameHeader, sizeof(frameHeader));
  if (nCameraError != vr::VRTrackedCameraError_None)
    return false;

  mCameraLastFrameId = frameHeader.nFrameSequence;

  if (scolBitmap == 0)
    return false;

  if (scolBitmap->TailleW != mCameraWidth || scolBitmap->TailleH != mCameraHeight)
    return false;

  //Scol bitmap are 32bits aligned
  int sbpl = mCameraWidth * 4;
  int dbpl = scolBitmap->BPL;

  if (sbpl == dbpl)
    memcpy(scolBitmap->bits, mCameraFrameBuffer, scolBitmap->TailleW * scolBitmap->TailleH * scolBitmap->BytesPP);
  else
  {
    int numthreads = (scolBitmap->TailleH <= 8) ? 1 : boost::thread::hardware_concurrency();
    int rows = scolBitmap->TailleH / numthreads;
    int extra = scolBitmap->TailleH % numthreads;
    int start = 0;
    int end = rows;

    ConversionTools::SConvertBuffer conv(mCameraFrameBuffer, 0, scolBitmap->bits, scolBitmap->TailleW, scolBitmap->TailleH, sbpl, 1, dbpl, scolBitmap->BytesPP);

    boost::thread_group thgroup;
    for (int t = 1; t <= numthreads; t++)
    {
      if (t == numthreads) // last thread does extra rows:
        end += extra;

      thgroup.create_thread(boost::bind(ConversionTools::BuffToBitmap, start, end, conv));

      start = end;
      end = start + rows;
    }

    thgroup.join_all();
  }

  return true;
}

sOpenvrController* sOpenvr::GetController(unsigned int id)
{
  if (id >= MAX_VR_CONTROLLERS)
    return 0;

  return mControllers[id];
}

sOpenvrController::sOpenvrController(vr::IVRSystem* hmdSystem, unsigned int id)
{
  mHmdSystem = hmdSystem;
  mDeviceId = 0;
  mVisible = false;
  mSide = (id > 1) ? VrControllerSide::Undefined : (VrControllerSide)id;
  mType = (id > 7) ? HANDS_CONTROLLER : (id > 1) ? TRACKER_CONTROLLER : CLASSIC_CONTROLLER;
  mLastRumble = 0;
  mMatrix = Matrix4::IDENTITY;
  mAxisValues = Vector3::ZERO;
  mVelocity = Vector3::ZERO;
  mAngularVelocity = Vector3::ZERO;

#if USE_VR_INPUT
  mSource = vr::k_ulInvalidInputValueHandle;
  mActionPose = vr::k_ulInvalidActionHandle;
  mActionHaptic = vr::k_ulInvalidActionHandle;
  mPadAxis = vr::k_ulInvalidActionHandle;
  mTriggerAxis = vr::k_ulInvalidActionHandle;

  std::string sSide = "left";
  if (id == 1)
    sSide = "right";
  else if (id  > 1)
    sSide = "tracker" + std::to_string(id - 1);

  vr::VRInput()->GetInputSourceHandle((std::string("/user/hand/") + sSide).c_str(), &mSource);
  vr::VRInput()->GetActionHandle((std::string("/actions/scol/out/") + sSide + "_haptic").c_str(), &mActionHaptic);
  vr::VRInput()->GetActionHandle((std::string("/actions/scol/in/") + sSide + "_pose").c_str(), &mActionPose);

  vr::VRInput()->GetActionHandle((std::string("/actions/scol/in/") + sSide + "_axis0_value").c_str(), &mPadAxis);
  vr::VRInput()->GetActionHandle((std::string("/actions/scol/in/") + sSide + "_axis1_value").c_str(), &mTriggerAxis);

  vr::VRActionHandle_t act = vr::k_ulInvalidActionHandle;
  vr::VRInput()->GetActionHandle((std::string("/actions/scol/in/") + sSide + "_applicationmenu_press").c_str(), &act);
  mActionsToHandle.push_back(act);

  act = vr::k_ulInvalidActionHandle;
  vr::VRInput()->GetActionHandle((std::string("/actions/scol/in/") + sSide + "_grip_press").c_str(), &act);
  mActionsToHandle.push_back(act);

  act = vr::k_ulInvalidActionHandle;
  vr::VRInput()->GetActionHandle((std::string("/actions/scol/in/") + sSide + "_system_press").c_str(), &act);
  mActionsToHandle.push_back(act);

  act = vr::k_ulInvalidActionHandle;
  vr::VRInput()->GetActionHandle((std::string("/actions/scol/in/") + sSide + "_axis0_press").c_str(), &act);
  mActionsToHandle.push_back(act);

  act = vr::k_ulInvalidActionHandle;
  vr::VRInput()->GetActionHandle((std::string("/actions/scol/in/") + sSide + "_axis1_press").c_str(), &act);
  mActionsToHandle.push_back(act);
#endif

  mButtonsToHandle.push_back(vr::k_EButton_ApplicationMenu);
  mButtonsToHandle.push_back(vr::k_EButton_Grip);
  mButtonsToHandle.push_back(vr::k_EButton_A);
  mButtonsToHandle.push_back(vr::k_EButton_Axis0);
  mButtonsToHandle.push_back(vr::k_EButton_Axis1);
  
  mButtonsStates.resize(mButtonsToHandle.size(), false);
}

sOpenvrController::~sOpenvrController()
{
}

void sOpenvrController::Update(vr::TrackedDevicePose_t* trackedPoses, unsigned int deviceId)
{
  mDeviceId = deviceId;

  if (!trackedPoses[mDeviceId].bDeviceIsConnected || !trackedPoses[mDeviceId].bPoseIsValid)
  {
    mVisible = false;
    return;
  }
  else
  {
    mVisible = true;
  }

  //Extract tracking information from the device
  mMatrix = sOpenvr::getMatrix4FromSteamVRMatrix34(trackedPoses[mDeviceId].mDeviceToAbsoluteTracking);

#if USE_VR_INPUT
  vr::InputDigitalActionData_t actionData;
  for (uint8_t i = 0; i < mButtonsToHandle.size(); i++)
  {
    vr::VRInput()->GetDigitalActionData(mActionsToHandle[i], &actionData, sizeof(actionData), vr::k_ulInvalidInputValueHandle);
    mButtonsStates[i] = (actionData.bActive && actionData.bState) ? true : false;
  }
#else
  //This will fill the buttons array
  //Get the state of the controller. The state contains the buttons and triggers data at the last sample
  mHmdSystem->GetControllerState(mDeviceId, &mControllerState, sizeof mControllerState);

  for (uint8_t i = 0; i < mButtonsToHandle.size(); i++)
  {
    mButtonsStates[i] = ((mControllerState.ulButtonPressed & vr::ButtonMaskFromId(mButtonsToHandle[i])) != 0) ? true : false;
  }
#endif

#if USE_VR_INPUT
  vr::InputAnalogActionData_t analogData;
  if (vr::VRInput()->GetAnalogActionData(mPadAxis, &analogData, sizeof(analogData), vr::k_ulInvalidInputValueHandle) == vr::VRInputError_None && analogData.bActive)
  {
    mAxisValues.x = analogData.x;
    mAxisValues.y = analogData.y;
  }

  if (vr::VRInput()->GetAnalogActionData(mTriggerAxis, &analogData, sizeof(analogData), vr::k_ulInvalidInputValueHandle) == vr::VRInputError_None && analogData.bActive)
  {
    mAxisValues.z = analogData.x;
  }
#else
  //Extract axis values
  mAxisValues.x = mControllerState.rAxis[vr::k_EButton_Axis0 - vr::k_EButton_Axis0].x;
  mAxisValues.y = mControllerState.rAxis[vr::k_EButton_Axis0 - vr::k_EButton_Axis0].y;
  mAxisValues.z = mControllerState.rAxis[vr::k_EButton_Axis1 - vr::k_EButton_Axis0].x;
#endif

  float* vel = trackedPoses[mDeviceId].vVelocity.v;
  mVelocity = Vector3(vel[0], vel[1], vel[2]);

  vel = trackedPoses[mDeviceId].vAngularVelocity.v;
  mAngularVelocity = Vector3(vel[0], vel[1], vel[2]);
}

#ifdef USE_VIVE_GESTURE
GestureType sOpenvrController::GetLastGesture()
{
  return mLastGesture;
}

void sOpenvrController::UpdateGesture(GestureResult result, unsigned int deviceId)
{
  mDeviceId = deviceId;
  mVisible = true;

  mLastGesture = result.gesture;

  //Extract tracking information from the device
  mMatrix = Matrix4();
  mMatrix[0][3] = result.points[0];
  mMatrix[1][3] = result.points[1];
  mMatrix[2][3] = result.points[2];
}
#endif // USE_VIVE_GESTURE

bool sOpenvrController::IsVisible()
{
  return mVisible;
}

void sOpenvrController::SetState(bool state)
{
  mVisible = false;
}

void sOpenvrController::SetSide(VrControllerSide side)
{
  mSide = side;
}

VrControllerSide sOpenvrController::GetSide()
{
  return mSide;
}

void sOpenvrController::SetType(ControllerType type)
{
  mType = type;
}

sOpenvrController::ControllerType sOpenvrController::GetType()
{
  return mType;
}

Vector3 sOpenvrController::GetPosition()
{
  return mMatrix.getTrans();
}

Vector3 sOpenvrController::GetVelocity()
{
  return mVelocity;
}

Vector3 sOpenvrController::GetAngularVelocity()
{
  return mAngularVelocity;
}

Quaternion sOpenvrController::GetOrientation()
{
  return mMatrix.extractQuaternion();
}

void sOpenvrController::Rumble(float value)
{
  int64 current = getTickCount();

  //Limit frequency to 1/50 hz
  if (current - mLastRumble > 50)
  {
    mLastRumble = current;
    //Max value of one pulse will be 3500µs
    mHmdSystem->TriggerHapticPulse(mDeviceId, 0, static_cast<unsigned short>(value * 3500));
  }
}

std::vector<bool> sOpenvrController::GetButtonsState()
{
  return mButtonsStates;
}

Vector3 sOpenvrController::GetAxisValues()
{
  return mAxisValues;
}


void ConversionTools::BuffToBitmap(const int start, const int end, const SConvertBuffer &conv)
{
  unsigned int srcByte;
  unsigned int destByte;
  int x, y;

  for (y = start; y < end; y++)
  {
    for (x = 0; x < conv.width; x++)
    {
      srcByte = (x * 4) + (conv.sbpl * y);
      destByte = (x * conv.bpp) + (conv.dbpl * y);

      ((uint8_t*)conv.dstBuff)[destByte] = conv.srcBuff[srcByte + 2];
      ((uint8_t*)conv.dstBuff)[destByte + 1] = conv.srcBuff[srcByte + 1];
      ((uint8_t*)conv.dstBuff)[destByte + 2] = conv.srcBuff[srcByte];
    }
  }
}