/*
* $Id: openVRwrapper.h 2016-08-18 $
*
* Description  : One line description of file.
* Author       : Martin Kacerik
*
* Purpose      :
*     Long description of what the file is for.
*/
#ifndef __OPENVRWRAPPER_H__
#define __OPENVRWRAPPER_H__

#pragma once

#include <openvr.h>
#include <map>

namespace VRUT
{
	class OpenVRWrapper
	{
	protected:
		class OpenVRWrapperDeviceReadThread;
		class VRRenderModel;
	public:
		OpenVRWrapper(smart_pointer_v2<Scene> scene, NODE_ID cameraID, const float scale = 1000.0f, bool separateTrackingThread = true, bool trackingEnable = true, bool createGeometries = true, int trackingBodyID = 0)
			: scene(scene), cameraID(cameraID), trackingEnable(trackingEnable), createGeometries(createGeometries), trackingBodyID(trackingBodyID)
		{
			initOk = initializeOpenVR(scale);

			// tracking
			if (initOk)
			{
				openVRWrapperDeviceReadThread = new OpenVRWrapperDeviceReadThread(this);
				if (separateTrackingThread)
				{
					if (openVRWrapperDeviceReadThread->Create() == wxTHREAD_NO_ERROR)
						openVRWrapperDeviceReadThread->Run();
				}
				else
				{
					openVRWrapperDeviceReadThread->exit = true;
					openVRWrapperDeviceReadThread->Entry();
				}
			}
			else
				openVRWrapperDeviceReadThread = nullptr;
		}
		~OpenVRWrapper()
		{
			deinitializeOpenVR();
		}

		wxCriticalSection openvrLock;
		bool openVRWrapperDeviceReadThreadIsRunning;

		bool IsInitialized() { return initOk; }
		Matrix* GetCameraWorldMatrixL() { return &cameraWorldMatrixL; }
		Matrix* GetCameraWorldMatrixR() { return &cameraWorldMatrixR; }
		Matrix* GetCameraProjectionMatrixL() { return &m_mat4ProjectionLeft; }
		Matrix* GetCameraProjectionMatrixR() { return &m_mat4ProjectionRight; }
		Vector3* GetClonedCameraTranslationL() { return &clonedCameraTranslationL; }
		Vector3* GetClonedCameraTranslationR() { return &clonedCameraTranslationR; }
		uint32_t GetRenderWidth() { return m_nRenderWidth; }
		uint32_t GetRenderHeight() { return m_nRenderHeight; }
		vr::IVRSystem* Get_pHMD() { return m_pHMD; }

		Matrix GetHMDlocalMatrix() const { return HMDlocalMatrix; }
		Matrix GetEyePosLeftMatrix() const { return m_mat4eyePosLeft; }
		Matrix GetEyePosRightMatrix() const { return m_mat4eyePosRight; }

		void ComputeTransformations(Matrix cameraWorldMatrix) {
			cameraWorldMatrixL = m_mat4eyePosLeft * HMDlocalMatrix * cameraWorldMatrix;
			cameraWorldMatrixR = m_mat4eyePosRight * HMDlocalMatrix * cameraWorldMatrix;
			clonedCameraTranslationL = cameraWorldMatrixL.ExtractTranslation();
			clonedCameraTranslationR = cameraWorldMatrixR.ExtractTranslation();
		}

		void UpdateProjectionMatrix(const float nearPlane, const float farPlane) {
			m_mat4ProjectionLeft = getHMDMatrixProjectionEye(vr::Eye_Left, nearPlane, farPlane);
			m_mat4ProjectionRight = getHMDMatrixProjectionEye(vr::Eye_Right, nearPlane, farPlane);
		}
		void UpdateEyeMatrix() {
			m_mat4eyePosLeft = getHMDMatrixPoseEye(vr::Eye_Left);
			m_mat4eyePosRight = getHMDMatrixPoseEye(vr::Eye_Right);
		}

		void SetTrackingEnable(bool trackingEnable) {
			this->trackingEnable = trackingEnable;
		}
		void SetCreateGeometries(bool createGeometries) {
			this->createGeometries = createGeometries;
			if (createGeometries)
			{
				for (uint32_t unTrackedDevice = vr::k_unTrackedDeviceIndex_Hmd + 1; unTrackedDevice < vr::k_unMaxTrackedDeviceCount; ++unTrackedDevice)
					if (m_pHMD->IsTrackedDeviceConnected(unTrackedDevice))
						SetupRenderModelForTrackedDevice(unTrackedDevice, true);
			}
			else
				RemoveGeometries();
		}
		void SetTrackingBodyID(int trackingBodyID) {
			this->trackingBodyID = trackingBodyID;
		}
		// scale - how many VRUT units (usually millimeters) is one OpenVR unit (usually meter)
		void SetScale(float scale) {
			this->scale = scale;
			//LOG(wxString::Format(wxT("OpenVRWrapper::SetScale: New scale: %f"), scale));
		}

		void SubmitToHMD(GLuint leftEye, GLuint rightEye) {
			if (!initOk)
				return;
#if __x86_64__ || _WIN64
			unsigned long long tmp8 = leftEye;
			vr::Texture_t leftEyeTexture = { (void*)tmp8, vr::API_OpenGL, vr::ColorSpace_Gamma };
			tmp8 = rightEye;
			vr::Texture_t rightEyeTexture = { (void*)tmp8, vr::API_OpenGL, vr::ColorSpace_Gamma };
#else
			vr::Texture_t leftEyeTexture = { (void*)leftEye, vr::API_OpenGL, vr::ColorSpace_Gamma };
			vr::Texture_t rightEyeTexture = { (void*)rightEye, vr::API_OpenGL, vr::ColorSpace_Gamma };
#endif
			int err = vr::VRCompositor()->Submit(vr::Eye_Left, &leftEyeTexture);
			if (err != 0)
				LOG(wxString::Format(wxT("OpenVRWrapper::submitToHMD: vr::VRCompositor()->Submit left error: %i"), err));
			err = vr::VRCompositor()->Submit(vr::Eye_Right, &rightEyeTexture);
			if (err != 0)
				LOG(wxString::Format(wxT("OpenVRWrapper::submitToHMD: vr::VRCompositor()->Submit right error: %i"), err));
		}
		void SubmitToHMDHorizontalSplit(GLuint texture) {
			vr::VRTextureBounds_t left = { 0, 0.5, 1, 1 };
			vr::VRTextureBounds_t right = { 0, 0, 1, 0.5 };

#if __x86_64__ || _WIN64
			unsigned long long tmp8 = texture;
			vr::Texture_t leftEyeTexture = { (void*)tmp8, vr::API_OpenGL, vr::ColorSpace_Gamma };
			vr::Texture_t rightEyeTexture = { (void*)tmp8, vr::API_OpenGL, vr::ColorSpace_Gamma };
#else
			vr::Texture_t leftEyeTexture = { (void*)texture, vr::API_OpenGL, vr::ColorSpace_Gamma };
			vr::Texture_t rightEyeTexture = { (void*)texture, vr::API_OpenGL, vr::ColorSpace_Gamma };
#endif
			int err = vr::VRCompositor()->Submit(vr::Eye_Left, &leftEyeTexture, &left);
			if (err != 0)
				LOG(wxString::Format(wxT("OpenVRWrapper::submitToHMD: vr::VRCompositor()->Submit left error: %i"), err));
			err = vr::VRCompositor()->Submit(vr::Eye_Right, &rightEyeTexture, &right);
			if (err != 0)
				LOG(wxString::Format(wxT("OpenVRWrapper::submitToHMD: vr::VRCompositor()->Submit right error: %i"), err));
		}


		std::string GetTrackedDeviceString(vr::IVRSystem *pHmd, vr::TrackedDeviceIndex_t unDevice, vr::TrackedDeviceProperty prop, vr::TrackedPropertyError *peError = NULL)
		{
			uint32_t unRequiredBufferLen = pHmd->GetStringTrackedDeviceProperty(unDevice, prop, NULL, 0, peError);
			if (unRequiredBufferLen == 0)
				return "";

			char *pchBuffer = new char[unRequiredBufferLen];
			unRequiredBufferLen = pHmd->GetStringTrackedDeviceProperty(unDevice, prop, pchBuffer, unRequiredBufferLen, peError);
			std::string sResult = pchBuffer;
			delete[] pchBuffer;
			return sResult;
		}

		void SetupRenderModelForTrackedDevice(vr::TrackedDeviceIndex_t unTrackedDeviceIndex, bool isActive)
		{
			if (!createGeometries || (unTrackedDeviceIndex >= vr::k_unMaxTrackedDeviceCount))
				return;

			// node exists
			if (m_rTrackedDeviceToRenderModel[unTrackedDeviceIndex] != NODE_ID_NONE)
			{
				// Turn of visibility of tracking systems (cameras)
				if (m_pHMD->GetTrackedDeviceClass(unTrackedDeviceIndex) != vr::TrackedDeviceClass_TrackingReference)
					scene->SetActive(m_rTrackedDeviceToRenderModel[unTrackedDeviceIndex], isActive);
			}
			else
			{
				// try to find a model we've already set up
				std::string sRenderModelName = GetTrackedDeviceString(m_pHMD, unTrackedDeviceIndex, vr::Prop_RenderModelName_String);
				VRRenderModel *pRenderModel = findOrLoadRenderModel(sRenderModelName.c_str());
				if (!pRenderModel)
				{
					std::string sTrackingSystemName = GetTrackedDeviceString(m_pHMD, unTrackedDeviceIndex, vr::Prop_TrackingSystemName_String);
					LOGERROR(wxString::Format(wxT("<OpenVRWrapper::setupRenderModelForTrackedDevice> Unable to load render model for tracked device %d (%s.%s)"), unTrackedDeviceIndex, sTrackingSystemName.c_str(), sRenderModelName.c_str()));
				}
				else
				{
					// add node to scene
					wxString nodeName = sRenderModelName.c_str();
					NODE_ID nodeId = scene->GetNewNodeID();
					GeometryNode *node = new GeometryNode(nodeName);
					node->SetID(nodeId);
					scene->Insert(node, cameraID);
					scene->SetGeometry(nodeId, pRenderModel->GetGeometryID());
					scene->SetMaterial(nodeId, pRenderModel->GetMaterialID());
					// Turn of visibility of tracking systems (cameras)
					if (m_pHMD->GetTrackedDeviceClass(unTrackedDeviceIndex) == vr::TrackedDeviceClass_TrackingReference)
						scene->SetActive(nodeId, false);

					m_rTrackedDeviceToRenderModel[unTrackedDeviceIndex] = nodeId;
				}
			}
		}

		void ReadTracking()
		{
			if (IsInitialized() && Get_pHMD())
				openVRWrapperDeviceReadThread->ReadTracking();
		}

		//TODO: correct solution is to only return the mesh to rendering module and render it in that module
		void DrawHiddenMesh(vr::Hmd_Eye eye)
		{
			vr::HiddenAreaMesh_t hiddenMesh = m_pHMD->GetHiddenAreaMesh(eye);
			glBegin(GL_TRIANGLES);
			for (size_t i = 0; i < hiddenMesh.unTriangleCount; ++i)
			{
				glVertex3f((hiddenMesh.pVertexData[i * 3].v[0]-0.5f)*2, (hiddenMesh.pVertexData[i * 3].v[1] - 0.5f) * 2, -1.0f);
				glVertex3f((hiddenMesh.pVertexData[i * 3 + 1].v[0] - 0.5f) * 2, (hiddenMesh.pVertexData[i * 3 + 1].v[1] - 0.5f) * 2, -1.0f);
				glVertex3f((hiddenMesh.pVertexData[i * 3 + 2].v[0] - 0.5f) * 2, (hiddenMesh.pVertexData[i * 3 + 2].v[1] - 0.5f) * 2, -1.0f);
			}
			glEnd();
		}

	protected:
		smart_pointer_v2<Scene> scene;
		NODE_ID cameraID;

		bool initOk;
		Matrix HMDlocalMatrix;

		bool trackingEnable;
		bool createGeometries;
		int trackingBodyID;
		float scale;
		// models to render
		std::vector<VRRenderModel*> m_vecRenderModels;
		NODE_ID m_rTrackedDeviceToRenderModel[vr::k_unMaxTrackedDeviceCount];

		bool initializeOpenVR(float scale) {
			memset(m_rTrackedDeviceToRenderModel, NODE_ID_NONE, sizeof(m_rTrackedDeviceToRenderModel));
			SetScale(scale);
			smart_pointer<Camera> camera;
			scene->GetNode(camera, cameraID);

			// Loading the SteamVR Runtime
			vr::EVRInitError eError = vr::VRInitError_None;
			m_pHMD = vr::VR_Init(&eError, vr::VRApplication_Scene);

			if (eError != vr::VRInitError_None)
			{
				m_pHMD = NULL;
				LOGERROR(wxT("<OpenVRWrapper::initializeOpenVR> Unable to init VR runtime: ") + wxString::From8BitData(vr::VR_GetVRInitErrorAsEnglishDescription(eError)));
				return false;
			}

			m_pRenderModels = (vr::IVRRenderModels *)vr::VR_GetGenericInterface(vr::IVRRenderModels_Version, &eError);
			if (!m_pRenderModels)
			{
				m_pHMD = NULL;
				vr::VR_Shutdown();
				LOGERROR(wxT("<OpenVRWrapper::initializeOpenVR> Unable to get render model interface: ") + wxString::From8BitData(vr::VR_GetVRInitErrorAsEnglishDescription(eError)));
				return false;
			}

			std::string m_strDriver = "No Driver";
			std::string m_strDisplay = "No Display";

			m_strDriver = GetTrackedDeviceString(m_pHMD, vr::k_unTrackedDeviceIndex_Hmd, vr::Prop_TrackingSystemName_String);
			m_strDisplay = GetTrackedDeviceString(m_pHMD, vr::k_unTrackedDeviceIndex_Hmd, vr::Prop_SerialNumber_String);
			m_pHMD->GetRecommendedRenderTargetSize(&m_nRenderWidth, &m_nRenderHeight);

			// init render models
			for (uint32_t unTrackedDevice = vr::k_unTrackedDeviceIndex_Hmd + 1; unTrackedDevice < vr::k_unMaxTrackedDeviceCount; ++unTrackedDevice)
			{
				if (m_pHMD->IsTrackedDeviceConnected(unTrackedDevice))
					SetupRenderModelForTrackedDevice(unTrackedDevice, true);
			}

			// init compositor
			if (!vr::VRCompositor())
			{
				m_pHMD = NULL;
				vr::VR_Shutdown();
				LOGERROR(wxT("<OpenVRWrapper::initializeOpenVR> Compositor initialization failed."));
				return false;
			}

			UpdateProjectionMatrix(camera->GetNearPlane(), camera->GetFarPlane());
			UpdateEyeMatrix();
			return true;
		}

		void deinitializeOpenVR()
		{
			if (!initOk)
				return;
			initOk = false;

			if (openVRWrapperDeviceReadThread)
				openVRWrapperDeviceReadThread->exit = true;
			while (openVRWrapperDeviceReadThreadIsRunning)
				wxThread::Sleep(1);

			m_pHMD = NULL;
			vr::VR_Shutdown();

			// remove models from scene
			RemoveGeometries();
		}

		void RemoveGeometries()
		{
			// remove models from scene
			for (unsigned int i = 0; i < vr::k_unMaxTrackedDeviceCount; ++i)
				if (m_rTrackedDeviceToRenderModel[i] != NODE_ID_NONE)
				{
					scene->Remove(m_rTrackedDeviceToRenderModel[i]);
					m_rTrackedDeviceToRenderModel[i] = NODE_ID_NONE;
				}
		}


		vr::IVRSystem *m_pHMD;
		vr::IVRRenderModels *m_pRenderModels;

		Matrix getHMDMatrixProjectionEye(vr::Hmd_Eye nEye, const float nearPlane, const float farPlane)
		{
			if (!m_pHMD)
				return Matrix();

			vr::HmdMatrix44_t mat = m_pHMD->GetProjectionMatrix(nEye, nearPlane, farPlane, vr::API_OpenGL);

			return Matrix(
				mat.m[0][0], mat.m[1][0], mat.m[2][0], mat.m[3][0],
				mat.m[0][1], mat.m[1][1], mat.m[2][1], mat.m[3][1],
				mat.m[0][2], mat.m[1][2], mat.m[2][2], mat.m[3][2],
				mat.m[0][3], mat.m[1][3], mat.m[2][3], mat.m[3][3]
				);
		}

		Matrix getHMDMatrixPoseEye(vr::Hmd_Eye nEye)
		{
			if (!m_pHMD)
				return Matrix();

			vr::HmdMatrix34_t matEye = m_pHMD->GetEyeToHeadTransform(nEye);
			Matrix matrixObj(
				matEye.m[0][0], matEye.m[1][0], matEye.m[2][0], 0.0,
				matEye.m[0][1], matEye.m[1][1], matEye.m[2][1], 0.0,
				matEye.m[0][2], matEye.m[1][2], matEye.m[2][2], 0.0,
				//matEye.m[0][3], matEye.m[1][3], matEye.m[2][3], 1.0f
				scale * matEye.m[0][3], scale * matEye.m[1][3], scale * matEye.m[2][3], 1.0f //convert to millimeters
				);
			//return matrixObj.Inverse();
			return matrixObj;
		}

		VRRenderModel *findOrLoadRenderModel(const char *pchRenderModelName)
		{
			VRRenderModel *pRenderModel = NULL;
			for (auto &renderModel : m_vecRenderModels)
			{
				if (!_strnicmp(renderModel->GetName().c_str(), pchRenderModelName, strlen(pchRenderModelName)))
				{
					pRenderModel = renderModel;
					//LOG(wxString::Format(wxT("<OpenVRWrapper::findOrLoadRenderModel> %s render model loaded."), pchRenderModelName));
					break;
				}
			}

			// load the model if we didn't find one
			if (!pRenderModel)
			{
				vr::RenderModel_t *pModel;
				vr::EVRRenderModelError error;
				while (1)
				{
					error = vr::VRRenderModels()->LoadRenderModel_Async(pchRenderModelName, &pModel);
					if (error != vr::VRRenderModelError_Loading)
						break;

					wxThread::Sleep(1);
				}

				if (error != vr::VRRenderModelError_None)
				{
					LOGERROR(wxString::Format(wxT("<OpenVRWrapper::findOrLoadRenderModel> Unable to load render model %s - %s"), pchRenderModelName, vr::VRRenderModels()->GetRenderModelErrorNameFromEnum(error)));
					return NULL; // move on to the next tracked device
				}

				vr::RenderModel_TextureMap_t *pTexture;
				while (1)
				{
					error = vr::VRRenderModels()->LoadTexture_Async(pModel->diffuseTextureId, &pTexture);
					if (error != vr::VRRenderModelError_Loading)
						break;

					wxThread::Sleep(1);
				}

				if (error != vr::VRRenderModelError_None)
				{
					LOGERROR(wxString::Format(wxT("<OpenVRWrapper::findOrLoadRenderModel> Unable to load render texture id:%d for render model %s"), pModel->diffuseTextureId, pchRenderModelName));
					vr::VRRenderModels()->FreeRenderModel(pModel);
					return NULL; // move on to the next tracked device
				}

				pRenderModel = new VRRenderModel(pchRenderModelName, scene);

				if (!pRenderModel->Create(*pModel, *pTexture, scale))
				{
					LOGERROR(wxString::Format(wxT("<OpenVRWrapper::findOrLoadRenderModel> Unable to create GL model from render model %s"), pchRenderModelName));
					delete pRenderModel;
					pRenderModel = NULL;
				}
				else
				{
					m_vecRenderModels.push_back(pRenderModel);
				}
				vr::VRRenderModels()->FreeRenderModel(pModel);
				vr::VRRenderModels()->FreeTexture(pTexture);
				//LOG(wxString::Format(wxT("<OpenVRWrapper::findOrLoadRenderModel> %s render model created."), pchRenderModelName));
			}
			return pRenderModel;
		}

		OpenVRWrapperDeviceReadThread *openVRWrapperDeviceReadThread;

		Matrix m_mat4ProjectionLeft;
		Matrix m_mat4ProjectionRight;

		Matrix m_mat4eyePosLeft;
		Matrix m_mat4eyePosRight;

		Matrix cameraWorldMatrixL;
		Matrix cameraWorldMatrixR;
		Vector3 clonedCameraTranslationL;
		Vector3 clonedCameraTranslationR;

		uint32_t m_nRenderWidth;
		uint32_t m_nRenderHeight;

		// models for VR devices
		class VRRenderModel
		{
		public:
			VRRenderModel(const std::string & sRenderModelName, smart_pointer_v2<Scene> scene)
				: scene(scene), gid(GEOMETRY_ID_NONE), mid(MATERIAL_ID_NONE), m_sModelName(sRenderModelName) {}
			~VRRenderModel() {
				Cleanup();
			}

			bool Create(const vr::RenderModel_t & vrModel, const vr::RenderModel_TextureMap_t & vrDiffuseTexture, const float scale) {
				gid = scene->GetNewGeometryID();
				GeometryTriangles *geometry = new GeometryTriangles(gid);

				uint32_t vertsNum = vrModel.unVertexCount;
				geometry->vertices.resize(vertsNum);
				geometry->normals.resize(vertsNum);
				geometry->texCoords.resize(vertsNum);
				for (uint32_t i = 0; i < vertsNum; i++) {
					vr::RenderModel_Vertex_t vertData = vrModel.rVertexData[i];
						
					geometry->vertices[i]	= Vector3(vertData.vPosition.v)*scale;
					geometry->normals[i]	= Vector3(vertData.vNormal.v);
					geometry->texCoords[i]	= Vector2(vertData.rfTextureCoord);
				}

				uint32_t indsNum = vrModel.unTriangleCount * 3;
				geometry->indices.resize(indsNum);
				for (uint32_t i = 0; i < indsNum; i++)
					geometry->indices[i] = vrModel.rIndexData[i];

				geometry->triDescList.push_back(std::pair<GeometryTriangles::Index, GeometryTriangles::PRIMITIVE_TYPE>(0, GeometryTriangles::TRI_LIST));

				Material *material = new Material(wxString(m_sModelName));
				material->imageName = m_sModelName;
				material->diffuse = Vector4(1, 1, 1, 1);
				
				unsigned pixels = vrDiffuseTexture.unWidth * vrDiffuseTexture.unHeight;
				unsigned char * rgbData = (unsigned char*)malloc(pixels * 3);
				unsigned char * tmpRGB = rgbData;
				unsigned char * aData = (unsigned char*)malloc(pixels);
				unsigned char * tmpA = aData;
				unsigned char * rgbaData = (unsigned char*)vrDiffuseTexture.rubTextureMapData;
				for (unsigned pix = 0; pix < pixels; pix++)
				{
					*tmpRGB++ = *rgbaData++;
					*tmpRGB++ = *rgbaData++;
					*tmpRGB++ = *rgbaData++;
					*tmpA++   = *rgbaData++;
				}

				wxInitAllImageHandlers();
				wxImage *wximage = new wxImage(vrDiffuseTexture.unWidth, vrDiffuseTexture.unHeight, rgbData);
				wximage->SetAlpha(aData);
				
				ImageCommon *imcommon = new ImageCommon(nullptr);
				imcommon->SetImage(wximage);
				delete wximage;

				KERNEL->sceneManager->AddImage(wxString(m_sModelName), imcommon);
				
				gid = scene->AddGeometry(geometry);
				mid = scene->AddMaterial(material);

				return true;
			}
			void Cleanup() {
				if (gid != GEOMETRY_ID_NONE)
					scene->RemoveGeometry(gid);
				if (mid != MATERIAL_ID_NONE)
					scene->RemoveMaterial(mid);
			}
			const std::string & GetName() const { return m_sModelName; }

			const GEOMETRY_ID GetGeometryID() { return gid; }
			const MATERIAL_ID GetMaterialID() { return mid; }

		private:
			smart_pointer_v2<Scene> scene;
			GEOMETRY_ID gid;
			MATERIAL_ID mid;
			std::string m_sModelName;
		};

		class OpenVRWrapperDeviceReadThread : public wxThread
		{
		private:
			OpenVRWrapper *openVRWrapper;
			unsigned int pressedButtons[vr::k_unMaxTrackedDeviceCount];
			std::vector<float> controls[vr::k_unMaxTrackedDeviceCount];
			vr::TrackedDevicePose_t m_rTrackedDevicePose[vr::k_unMaxTrackedDeviceCount];
			Matrix bodyMatrices[vr::k_unMaxTrackedDeviceCount];
			std::map<std::string, int> controllersBodyID;
			int controllersBodyIDCounter;
			bool headBodyIDinitialized;

			Matrix convertSteamVRMatrixToMatrix(const vr::HmdMatrix34_t &matPose)
			{
				Matrix matrixObj(
					matPose.m[0][0], matPose.m[1][0], matPose.m[2][0], 0.0,
					matPose.m[0][1], matPose.m[1][1], matPose.m[2][1], 0.0,
					matPose.m[0][2], matPose.m[1][2], matPose.m[2][2], 0.0,
					//matPose.m[0][3], matPose.m[1][3], matPose.m[2][3], 1.0f
					openVRWrapper->scale * matPose.m[0][3], openVRWrapper->scale * matPose.m[1][3], openVRWrapper->scale * matPose.m[2][3], 1.0f //convert to millimeters
					);
				return matrixObj;
			}

			bool deviceChanged[vr::k_unMaxTrackedDeviceCount];

			int getOrSetBodyID(const int deviceIndex, const int forceValue=-1)
			{
				std::string deviceName = openVRWrapper->GetTrackedDeviceString(openVRWrapper->Get_pHMD(), deviceIndex, vr::Prop_SerialNumber_String);
				auto pos = controllersBodyID.find(deviceName);
				if (pos == controllersBodyID.end())
				{
					if (forceValue < 0)
						controllersBodyID.insert(std::pair<std::string, int>(deviceName, controllersBodyIDCounter++));
					else
						controllersBodyID.insert(std::pair<std::string, int>(deviceName, forceValue));
					pos = controllersBodyID.find(deviceName);
				}
				return pos->second;
			}

		public:
			bool exit;

			OpenVRWrapperDeviceReadThread(OpenVRWrapper *openVRWrapper) : wxThread(wxTHREAD_DETACHED), openVRWrapper(openVRWrapper), controllersBodyIDCounter(1), headBodyIDinitialized(false), exit(false)
			{
				openVRWrapper->openVRWrapperDeviceReadThreadIsRunning = true;
				for (size_t i = 0; i < vr::k_unMaxTrackedDeviceCount; ++i)
				{
					pressedButtons[i] = 0;
					deviceChanged[i] = false;
					controls[i].push_back(0);
					controls[i].push_back(0);
				}
			}

			void ReadTracking()
			{
				static unsigned int OpenVRDeviceButtonMapping[] = { 128, 1, 2, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 128, 4, 8, 128 };

				if (!headBodyIDinitialized)
				{
					for (size_t i = 0; i < vr::k_unMaxTrackedDeviceCount; ++i)
					{
						//the bodyID==0 will be always used for HMD, the controllers will start from 1
						if (openVRWrapper->Get_pHMD()->GetTrackedDeviceClass(i) == vr::TrackedDeviceClass_HMD)
							getOrSetBodyID(i, 0);
					}
					headBodyIDinitialized = true;
				}
				// Process SteamVR events
				vr::VREvent_t event;
				while (openVRWrapper->Get_pHMD()->PollNextEvent(&event, sizeof(event)))
				{
					if (event.eventType == vr::VREvent_ButtonPress)
					{
						pressedButtons[event.trackedDeviceIndex] |= OpenVRDeviceButtonMapping[event.data.controller.button];
						deviceChanged[event.trackedDeviceIndex] = true;
						//LOG(wxString::Format(wxT("<OpenVRWrapperDeviceReadThread> vr::VREvent_t type=%lu, trackedDeviceIndex=%lu, button=%lu"), event.eventType, event.trackedDeviceIndex, event.data.controller.button));
					}
					else if (event.eventType == vr::VREvent_ButtonUnpress)
					{
						pressedButtons[event.trackedDeviceIndex] ^= OpenVRDeviceButtonMapping[event.data.controller.button];
						deviceChanged[event.trackedDeviceIndex] = true;
						//LOG(wxString::Format(wxT("<OpenVRWrapperDeviceReadThread> vr::VREvent_t type=%lu, trackedDeviceIndex=%lu, button=%lu"), event.eventType, event.trackedDeviceIndex, event.data.controller.button));
					}
					else if (event.eventType == vr::VREvent_IpdChanged)
					{
						openVRWrapper->UpdateEyeMatrix();
					}
					else if (event.eventType == vr::VREvent_TrackedDeviceActivated)
					{
						openVRWrapper->SetupRenderModelForTrackedDevice(event.trackedDeviceIndex, true);
						//LOG(wxString::Format(wxT("<OpenVRWrapperDeviceReadThread> Device %u attached. Setting up render model."), event.trackedDeviceIndex));
					}
					else if (event.eventType == vr::VREvent_TrackedDeviceDeactivated)
					{
						openVRWrapper->SetupRenderModelForTrackedDevice(event.trackedDeviceIndex, false);
						//LOG(wxString::Format(wxT("<OpenVRWrapperDeviceReadThread> Device %u deactivated."), event.trackedDeviceIndex));
					}
					else if (event.eventType == vr::VREvent_Quit)
					{
						openVRWrapper->deinitializeOpenVR();//dont't quit VRUT, only disconnect from the OpenVR
						return;
					}
					else
						LOG(wxString::Format(wxT("<OpenVRWrapperDeviceReadThread> vr::VREvent_t type=%lu, trackedDeviceIndex=%lu"), event.eventType, event.trackedDeviceIndex));
				}

				//Read transformations
				vr::VRCompositor()->WaitGetPoses(m_rTrackedDevicePose, vr::k_unMaxTrackedDeviceCount, NULL, 0);
				for (vr::TrackedDeviceIndex_t deviceIndex = 0; deviceIndex < vr::k_unMaxTrackedDeviceCount; ++deviceIndex)
				{
					// Process SteamVR controller state
					vr::VRControllerState_t state;
					if (openVRWrapper->Get_pHMD()->GetControllerState(deviceIndex, &state))
						//if (openVRWrapper->Get_pHMD()->GetControllerState(deviceIndex, &state, sizeof(state)))//needed by OpenVR version 1.0.4
					{
						//TODO: process all axes (now only the first one is used)
						if ((controls[deviceIndex][0] != state.rAxis[0].x) || (controls[deviceIndex][1] != state.rAxis[0].y))
						{
							controls[deviceIndex][0] = state.rAxis[0].x;
							controls[deviceIndex][1] = state.rAxis[0].y;
							deviceChanged[deviceIndex] = true;
							//TODO: if (state.ulButtonPressed == 0) perhaps means that device is connected and visible
						}
					}
					//Process transformations
					if (m_rTrackedDevicePose[deviceIndex].bPoseIsValid)
					{
						bodyMatrices[deviceIndex] = convertSteamVRMatrixToMatrix(m_rTrackedDevicePose[deviceIndex].mDeviceToAbsoluteTracking);

						// set transformation matrix for all active tracked devices - model rendering
						if ((openVRWrapper->m_rTrackedDeviceToRenderModel[deviceIndex] != NODE_ID_NONE) && openVRWrapper->trackingEnable)
							openVRWrapper->scene->SetTransformation(openVRWrapper->m_rTrackedDeviceToRenderModel[deviceIndex], bodyMatrices[deviceIndex]);

						switch (openVRWrapper->Get_pHMD()->GetTrackedDeviceClass(deviceIndex))
						{
						case vr::TrackedDeviceClass_Controller:
							//LOG(wxString::Format(wxT("<OpenVRWrapperDeviceReadThread> vr::WaitGetPoses deviceIndex=%lu (Controller), Matrix="), deviceIndex) + TextWriter::WriteToWxString(bodyMatrices[deviceIndex]._m, 16, wxT(", ")));
							deviceChanged[deviceIndex] = true;
							break;
						case vr::TrackedDeviceClass_HMD:
							//LOG(wxString::Format(wxT("<OpenVRWrapperDeviceReadThread> vr::WaitGetPoses deviceIndex=%lu (HMD), Matrix="), deviceIndex) + TextWriter::WriteToWxString(bodyMatrices[deviceIndex]._m, 16, wxT(", ")));
							deviceChanged[deviceIndex] = true;
							if (openVRWrapper->trackingEnable)
								openVRWrapper->HMDlocalMatrix = bodyMatrices[deviceIndex];
							break;
						case vr::TrackedDeviceClass_Invalid:
						case vr::TrackedDeviceClass_Other:
						case vr::TrackedDeviceClass_TrackingReference:
						default:
							break;
						}
					}
					//send events
					if (deviceChanged[deviceIndex])
					{
						deviceChanged[deviceIndex] = false;
						DeviceData deviceData(openVRWrapper->trackingBodyID + getOrSetBodyID(deviceIndex), DeviceData::DEVICE_FlyStick2, bodyMatrices[deviceIndex], pressedButtons[deviceIndex], controls[deviceIndex]);
						KERNEL->GetMessageSink()->DistributeToListeners(GET_EVT_INPUT_TRACKING(deviceData));
						//LOG(deviceData.ToString());
					}
				}
			}

			virtual ExitCode Entry() {
				while (!TestDestroy() && !exit)
				{
					if (openVRWrapper->IsInitialized() && openVRWrapper->Get_pHMD())
					{
						wxThread::Sleep(2);

						wxCriticalSectionLocker lock(openVRWrapper->openvrLock);
						ReadTracking();
					}
					else
						wxThread::Sleep(1000);
				}
				openVRWrapper->openVRWrapperDeviceReadThreadIsRunning = false;
				return 0;
			}
		};
	};
}

#endif //__OPENVRWRAPPER_H__
