/*
* $Id: VRHeroWrapper.h 2016-08-18 $
*
* Description  : VRHero wrapper.
* Author       : Anton�n M�ek, Jozef Kobrtek
*				
* Purpose      :
*     Wrapper for VRHero 5K headset
*/

#pragma once
#ifdef __WXMSW__
#include "hmd64.h"
#endif
#include "openvr.h"
#include "eventhandler.h"

/// -----How to use VRHero (based on RenderGL3 implementation)-----
/// - set its screens in Windows so that "VRHero left" is next to your real monitor(s), place "VRHero right" under the "VRHero left"
/// - in autoexec or scripts, set fullscreen mode through both screens as example here :
///   fullscreenset 0 1 3840 0 2560 2880
///   Where:
///   '0' is window ID, 
///   '1' is fullscreen mode, 
///   '3840' is X offset (in my case, two FullHD screens to the right, 2x1920), 
///   '0' is Y offset (Y starts on top of the screen)
///   '2560' and '2880' is resolution of the merged area of both screens
/// - use wrapper to obtain camera data (projection, rotation)
/// - dont forget to call UpdateEyeMatrix, because IPD may change during runtime and headset has sensor for its change

namespace VRUT
{
	class VRHeroWrapper
	{
	private:
		class VRHeroWrapperDeviceReadThread;

	public:
		VRHeroWrapper(smart_pointer_v2<Scene> scene, NODE_ID cameraID, /*CONTEXTTYPE hglrc,*/ 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), _scale(1)
		{
			_initOk = _initializeVRHero(/*hglrc,*/ scale);

			// tracking
			_vrHeroWrapperDeviceReadThread = new VRHeroWrapperDeviceReadThread(this);
			if (separateTrackingThread)
			{
				if (_vrHeroWrapperDeviceReadThread->Create() == wxTHREAD_NO_ERROR)
					_vrHeroWrapperDeviceReadThread->Run();
			}
			else
			{
				_vrHeroWrapperDeviceReadThread->exit = true;
				_vrHeroWrapperDeviceReadThread->Entry();
			}
		}
		~VRHeroWrapper()
		{
			if (_vrHeroWrapperDeviceReadThread)
				if (_vrHeroWrapperDeviceReadThread->IsRunning())
					_vrHeroWrapperDeviceReadThread->Delete();

			while (vrHeroWrapperDeviceReadThreadIsRunning)
				wxThread::Sleep(1);
#ifdef __WXMSW__
			setHMDIMU(0, false);
			freeHMDDevices();
#endif
		}

		wxCriticalSection openvrLock;
		bool vrHeroWrapperDeviceReadThreadIsRunning;

		bool IsInitialized() const { 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() const { return _m_nRenderWidth; }
		uint32_t GetRenderHeight() const { return _m_nRenderHeight; }
		uint32_t GetFrameRate() const { return _m_nFramerate; }

		Matrix GetHMDlocalMatrix() const { return _HMDlocalMatrix; }
		Matrix GetEyePosLeftMatrix() const { return _m_mat4eyePosLeft; }
		Matrix GetEyePosRightMatrix() const { return _m_mat4eyePosRight; }

		void GetScreenSpaceIpdLeftRight(float& left, float&right) const {
#ifdef __WXMSW__
			getHMDIPDScreenPos(0, left, right);
#endif
		}

		void SetHMDlocalMatrix(const Matrix& m) { _HMDlocalMatrix = m; }

		void ComputeTransformations(const 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) 
		{
			_getHMDMatrixProjections(nearPlane, farPlane, _m_mat4ProjectionLeft, _m_mat4ProjectionRight);			
		}
		void UpdateEyeMatrix() 
		{
			_m_mat4eyePosLeft =  _getHMDMatrixPoseEye(vr::Eye_Left);
			_m_mat4eyePosRight = _getHMDMatrixPoseEye(vr::Eye_Right);
		}

		void GetTrackingData(float &x, float &y, float &z, float &qx, float &qy, float &qz, float &qw) 
		{ 
#ifdef __WXMSW__
			getHMDPositionOrientation(0, x, y, z, qx, qy, qz, qw);
#endif
		}

		void GetMonitorNames(wxString& leftEye, wxString& rightEye)
		{
			char l[32], r[32];
#ifdef __WXMSW__
			getHMDMonitors(0, l, r);
#endif
			leftEye = wxString::From8BitData(l);
			rightEye = wxString::From8BitData(r);
		}

		void SetTrackingEnable(bool trackingEnable) { this->trackingEnable = trackingEnable; }
		void SetCreateGeometries(bool createGeometries) { this->_createGeometries = createGeometries; }
		void SetTrackingBodyID(int trackingBodyID) { this->_trackingBodyID = trackingBodyID; }

		// scale - how many VRUT units (usually millimeters) is one VRHero unit (usually meter)
		void SetScale(float scale) { this->_scale = scale; }
		float GetScale() const { return _scale; }
		int GetTrackingBodyID() const { return _trackingBodyID; }

		struct TrackedDevicePose_ant
		{
			float x, y, z, qx, qy, qz, qw;
			vr::HmdVector3_t vVelocity;
			vr::HmdVector3_t vAngularVelocity;
			vr::ETrackingResult eTrackingResult;
			bool bPoseIsValid;
			bool bDeviceIsConnected;
		};
		typedef void(*pVRg_get_controller_ids)(int*& ids, int& count);
		typedef void(*pVRg_get_cinteract_data)(unsigned int dev_index, TrackedDevicePose_ant& posea, vr::VRControllerState_t& state);
		pVRg_get_controller_ids VRg_get_controller_ids;
		pVRg_get_cinteract_data VRg_get_cinteract_data;

		void ReadTracking()
		{
			if (IsInitialized())
				_vrHeroWrapperDeviceReadThread->ReadTracking();
		}

	private:
		smart_pointer_v2<Scene> _scene;
		NODE_ID _cameraID;

		bool _initOk;
		Matrix _HMDlocalMatrix;

		bool trackingEnable;
		bool _createGeometries;
		int _trackingBodyID;
		float _scale;

		VRHeroWrapperDeviceReadThread *_vrHeroWrapperDeviceReadThread;

		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;
		uint32_t _m_nFramerate;

		bool _initializeVRHero(float scale)
		{
			SetScale(scale);
			smart_pointer<Camera> camera;
			_scene->GetNode(camera, _cameraID);
#ifdef __WXMSW__

			LOG(wxString::Format(wxT("VRHeroWrapper::_initializeVRHero Version=%i"), getVersion()));

			unsigned int err = 0;
			//initHMDRender(0, err);
			initHMDDevice(0, err);
			if (err)
			{
				wxString errStr(wxString::From8BitData(getHMDLastError()));
				if (!errStr.empty() && (errStr != wxT("No error.")))
					LOGERROR(wxT("VRHeroWrapper::_initializeVRHero Error: ") + errStr);

				return false;
			}

			int hmds = getHMDDeviceCount();
			LOG(wxString::Format(wxT("VRHeroWrapper::_initializeVRHero number of devices=%i"), hmds));
			if (hmds == 0)
				return false;
#endif
			wxString leftMonitor, rightMonitor;
			GetMonitorNames(leftMonitor, rightMonitor);
			LOG(wxT("VRHeroWrapper::left monitor: ") + leftMonitor + wxT(", right monitor: ") + rightMonitor);

			//Using internal orientation tracking
			//Works in a rather weird way... (spins)
			//setHMDIMU(0, true); // enable IMU
			//resetHMDIMU(0);

			//Using HTC Tracker
#ifdef __WXMSW__
			setHMDIMU(0, false);
			initHMDPositionTracker(0, 0);
			setHMDPositionCalibration(0);
			//*/
			int framerate, width, height;
			getHMDResolutionFramerate(0, width, height, framerate);
			_m_nRenderWidth = width;
			_m_nRenderHeight = height;
			_m_nFramerate = framerate;

			HINSTANCE trackingDLL = LoadLibrary(L"hmd64t.dll");
			VRg_get_controller_ids = (pVRg_get_controller_ids)GetProcAddress(trackingDLL, "get_controller_ids");
			VRg_get_cinteract_data = (pVRg_get_cinteract_data)GetProcAddress(trackingDLL, "get_cinteract_data");
#endif
			UpdateProjectionMatrix(camera->GetNearPlane(), camera->GetFarPlane());
			UpdateEyeMatrix();

			return true;
		}

		void _getHMDMatrixProjections(const float nearPlane, const float farPlane, Matrix& left, Matrix& right)//eye - false==left
		{
#ifdef __WXMSW__
			float* mats[2];
			getGLProjectionMatrices(0, mats[0], mats[1], nearPlane, farPlane);
			left = Matrix(mats[0]);
			right = Matrix(mats[1]);
#endif
		}

		Matrix _getHMDMatrixPoseEye(const vr::Hmd_Eye nEye)
		{
			float L = 0.0f, R = 0.0f;
#ifdef __WXMSW__
			getHMDIPDinM(0, L, R);
#endif
			if (nEye == vr::Eye_Left)
			{
				return Matrix::Translation(Vector3(-L*_scale, 0, 0));
			}
			else if (nEye == vr::Eye_Right)
			{
				return Matrix::Translation(Vector3(R*_scale, 0, 0));
			}
			return Matrix();
		}

		void _saveToTexture(GLuint texture, wxString file)
		{
			wxImage output;
			output.Create(2560, 1440);
			GLuint tex_data_buffer;

			glPixelStorei(GL_PACK_ALIGNMENT, 1);
			glGenBuffers(1, &tex_data_buffer);
			glBindBuffer(GL_PIXEL_PACK_BUFFER, tex_data_buffer);
			glBufferData(GL_PIXEL_PACK_BUFFER, 2560 * 2440* 3, nullptr, GL_STATIC_COPY);
			glBindTexture(GL_TEXTURE_2D, texture);
			glGetTexImage(GL_TEXTURE_2D, 0, GL_RGB, GL_UNSIGNED_BYTE, nullptr);
			glBindTexture(GL_TEXTURE_2D, 0);
			glGetBufferSubData(GL_PIXEL_PACK_BUFFER, 0, 2560 * 1440 * 3, output.GetData());
			glBindBuffer(GL_PIXEL_PACK_BUFFER, 0);
			glDeleteBuffers(1, &tex_data_buffer);
			tex_data_buffer = 0;
			glPixelStorei(GL_PACK_ALIGNMENT, 4);

			output.SaveFile(file);
		}

		class VRHeroWrapperDeviceReadThread : public wxThread
		{
		private:
			VRHeroWrapper *_vrheroWrapper;

		public:
			bool exit;

			VRHeroWrapperDeviceReadThread(VRHeroWrapper *wrapper) : wxThread(wxTHREAD_DETACHED), _vrheroWrapper(wrapper), exit(false)
			{
				_vrheroWrapper->vrHeroWrapperDeviceReadThreadIsRunning = true;
			}

			void ReadTracking()
			{
				float qx, qy, qz, qw;
				float x, y, z;
				_vrheroWrapper->GetTrackingData(x, y, z, qx, qy, qz, qw);

				//Returned in wrong coordinate system
				//Swapping values...
				//When internal rotation sensing
				//Vector4 axisAngle(-qy, -qz, qx, qw);

				//When HTC tracker enabled
				Vector4 axisAngle(qx, qw, qy, qz);

				axisAngle = axisAngle.QuaternionToAxisAngle();
				Matrix mat(Matrix::RotationAxis(Vector3(axisAngle), axisAngle.w));
				mat = mat * Matrix::Translation(_vrheroWrapper->GetScale()*Vector3(-y, z, x));

				if (_vrheroWrapper->trackingEnable)
					_vrheroWrapper->SetHMDlocalMatrix(mat);

				DeviceData deviceData(_vrheroWrapper->GetTrackingBodyID(), DeviceData::DEVICE_FlyStick2, mat, 0, std::vector<float>());
				KERNEL->GetMessageSink()->DistributeToListeners(GET_EVT_INPUT_TRACKING(deviceData));

				//read controllers
				int* IDs = NULL;
				int controller_count;

				// works after calling initHMDPositionTracker(0, 0); from hmd64.dll
				_vrheroWrapper->VRg_get_controller_ids(IDs, controller_count);

				if (!IDs)
					return;

				// somewhere in update state method
				for (int i = 0; i< controller_count; ++i)
				{
					TrackedDevicePose_ant pose;
					vr::VRControllerState_t state;
					_vrheroWrapper->VRg_get_cinteract_data(IDs[i], pose, state);

					if (pose.bDeviceIsConnected && pose.bPoseIsValid)
					{
						Vector4 axisAngle(pose.qz, pose.qw, pose.qx, pose.qy);
						axisAngle = axisAngle.QuaternionToAxisAngle();
						Matrix mat(Matrix::RotationAxis(Vector3(axisAngle), axisAngle.w));
						mat = mat * Matrix::Translation(_vrheroWrapper->GetScale()*Vector3(-pose.x, pose.y, pose.z));

						int buttons = 0;
						if (state.ulButtonPressed & 2)
							buttons |= 1;
						if (state.ulButtonPressed & 4)
							buttons |= 2;
						if (state.ulButtonPressed & 4294967296)
							buttons |= 4;
						if (state.ulButtonPressed & 8589934592)
							buttons |= 8;

						std::vector<float> controls;
						controls.push_back(state.rAxis[0].x);
						controls.push_back(state.rAxis[0].y);
						DeviceData deviceData(_vrheroWrapper->GetTrackingBodyID() + 1 + i, DeviceData::DEVICE_FlyStick2, mat, buttons, controls);
						KERNEL->GetMessageSink()->DistributeToListeners(GET_EVT_INPUT_TRACKING(deviceData));
					}
				}

			}

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

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