#include <stdio.h>
#include <windows.h>
#include <tchar.h>
#include "USB_Win32_HIDDevice.h"
#include "MagCalibrate.h"
#include "SensorImpl.h"
#include "IMU_CAPI.h"
#include "UsbHid_CAPI.h"
#include "quaternion.h"

#include <setupapi.h>
#include "hidsdi.h"
#include  <iostream>

#include <sddl.h>

#include "PipeHMD.h"

#include<conio.h>
#include <windows.h> 
#include <stdio.h> 
#include <tchar.h>
#include <strsafe.h>
#include "openvr_driver.h"
#include "OVR_CAPI.h"
#include "MagCalibrate.h"

int global_calibration_flag = 0;//0none 1 gyro 2 acc 3 mag
int global_Show = 0;//0 hide 1 gyro 2acc 3mag 4angle 5Hex

std::string sCurrent_Device_str("null");
std::string strGlobalDevicePath("null");
extern "C" int globalpacklen;

extern "C" BOOL device_is_found;
extern "C" PSP_DEVICE_INTERFACE_DETAIL_DATA pData;
extern "C" HANDLE DM_HID_Read_HANDLE;
extern "C" HANDLE DM_HID_Write_HANDLE;
extern float gx0, gy0, gz0;
extern "C" OVERLAPPED usb_read_over_lapped;

int global_pipeserver = 0;
//extern Quaternion Qcorr;
bool global_debug = false;

#include <fstream>

#include <time.h>



FILE *fpdata;

#define BUFSIZE 512
#define COMMAND_DEVICE_IS_OPEN  1
#define COMMAND_GET_LEFT_POSE	2
#define COMMAND_GET_RIGHT_POSE	3
#define COMMAND_GET_LEFT_BAT	10
#define COMMAND_GET_RIGHT_BAT	11

#define COMMAND_SET_RUMBLE	    20


#define LEFT_HAND	0
#define RIGHT_HAND	1




#define BUFSIZE 512



#define COMMAND_DEVICE_IS_OPEN 1
#define COMMAND_GET_DATA 2
#define COMMAND_DEVICE_CALIBRATION 3
#define COMMAND_DEVICE_GET_PURE_DATA 4
#define COMMAND_DEVICE_ENTER_DFU_MODE 11

#define ENTER_DFU_MODE_SUCCESS 1
#define ENTER_DFU_MODE_FAILED  0
#define CALIBRATION_SUCCESS 1
#define CALIBRATION_FAILED  0


#define FEATURE_OFFSET_SET_GYRO 1
#define FEATURE_OFFSET_SET_ACC	2
#define FEATURE_OFFSET_SET_MAG  3
#define FEATURE_OFFSET_SET_LED  4
#define FEATURE_OFFSET_GET_GYRO 0x11
#define FEATURE_OFFSET_GET_ACC	0x12
#define FEATURE_OFFSET_GET_MAG  0x13



#define LED_COLOR_BLACK		 0x00
#define LED_COLOR_WHITE		 0x07
#define LED_COLOR_RED		 0x04
#define LED_COLOR_GREEN		 0x02
#define LED_COLOR_BLUE		 0x01
#define LED_COLOR_CYAN		 0x03
#define LED_COLOR_MAGENTA	 0x06
#define LED_COLOR_YELLOW	 0x05

UINT8 LED_COLOR_LIST[8] = {LED_COLOR_BLACK,LED_COLOR_WHITE,LED_COLOR_RED,LED_COLOR_GREEN,
LED_COLOR_BLUE,LED_COLOR_CYAN,LED_COLOR_MAGENTA,LED_COLOR_YELLOW,};

Quaternion CQGamePad = { 1, 0, 0, 0 };
Quaternion CQHMD = { 1, 0, 0, 0 };






#define FEATURE_OFFSET_SIZE		52//3+1+4*12 

DWORD WINAPI InstanceThread(LPVOID);
VOID GetAnswerToRequest(LPCSTR, LPCSTR, LPDWORD);
BOOL CreatePipeSecurity(PSECURITY_ATTRIBUTES *ppSa)
{
	BOOL fSucceeded = TRUE;
	DWORD dwError = ERROR_SUCCESS;

	PSECURITY_DESCRIPTOR pSd = NULL;
	PSECURITY_ATTRIBUTES pSa = NULL;

	// Define the SDDL for the security descriptor.
	LPCSTR szSDDL = "D:"       // Discretionary ACL
		"(A;OICI;GRGW;;;AU)"   // Allow read/write to authenticated users
		"(A;OICI;GA;;;BA)";    // Allow full control to administrators

	if (!ConvertStringSecurityDescriptorToSecurityDescriptor(szSDDL,
		SDDL_REVISION_1, &pSd, NULL))
	{
		fSucceeded = FALSE;
		dwError = GetLastError();
		goto Cleanup;
	}

	// Allocate the memory of SECURITY_ATTRIBUTES.
	pSa = (PSECURITY_ATTRIBUTES)LocalAlloc(LPTR, sizeof(*pSa));
	if (pSa == NULL)
	{
		fSucceeded = FALSE;
		dwError = GetLastError();
		goto Cleanup;
	}

	pSa->nLength = sizeof(*pSa);
	pSa->lpSecurityDescriptor = pSd;
	pSa->bInheritHandle = FALSE;

	*ppSa = pSa;

Cleanup:
	// Clean up the allocated resources if something is wrong.
	if (!fSucceeded)
	{
		if (pSd)
		{
			LocalFree(pSd);
			pSd = NULL;
		}
		if (pSa)
		{
			LocalFree(pSa);
			pSa = NULL;
		}

		SetLastError(dwError);
	}

	return fSucceeded;
}
void FreePipeSecurity(PSECURITY_ATTRIBUTES pSa)
{
	if (pSa)
	{
		if (pSa->lpSecurityDescriptor)
		{
			LocalFree(pSa->lpSecurityDescriptor);
		}
		LocalFree(pSa);
	}
}
int pipeservermain(VOID)
{
	if (global_pipeserver != 1)
	{
		return 0;
	}
	global_pipeserver = 2;

	BOOL   fConnected = FALSE;
	DWORD  dwThreadId = 0;
	HANDLE hPipe = INVALID_HANDLE_VALUE, hThread = NULL;
	//LPCSTR lpszPipename = TEXT("\\\\.\\pipe\\Dematrix_SteamVR");
	LPCSTR lpszPipename = TEXT("\\\\.\\pipe\\Dematrix_CTL");
	PSECURITY_ATTRIBUTES pSa;
	if (!CreatePipeSecurity(&pSa))
	{
		printf("CreatePipeSecurity pSa FALSE\n");
		FreePipeSecurity(pSa);
		return 0;
	}
	// The main loop creates an instance of the named pipe and 
	// then waits for a client to connect to it. When the client 
	// connects, a thread is created to handle communications 
	// with that client, and this loop is free to wait for the
	// next client connect request. It is an infinite loop.

	for (;;)
	{
		if (global_pipeserver == -1)
		{
			global_pipeserver = 0;
			break;
		}
		//_tprintf(TEXT("\nPipe Server: Main thread awaiting client connection on %s\n"), lpszPipename);
		hPipe = CreateNamedPipe(
			lpszPipename,             // pipe name 
			PIPE_ACCESS_DUPLEX,       // read/write access 
			PIPE_TYPE_MESSAGE |       // message type pipe 
			PIPE_READMODE_MESSAGE |   // message-read mode 
			PIPE_WAIT,                // blocking mode 
			PIPE_UNLIMITED_INSTANCES, // max. instances  
			BUFSIZE,                  // output buffer size 
			BUFSIZE,                  // input buffer size 
			0,                        // client time-out 
			pSa);                    // default security attribute 

		if (hPipe == INVALID_HANDLE_VALUE)
		{
			_tprintf(TEXT("CreateNamedPipe failed, GLE=%d.\n"), GetLastError());
			return -1;
		}

		// Wait for the client to connect; if it succeeds, 
		// the function returns a nonzero value. If the function
		// returns zero, GetLastError returns ERROR_PIPE_CONNECTED. 

		fConnected = ConnectNamedPipe(hPipe, NULL) ?
		TRUE : (GetLastError() == ERROR_PIPE_CONNECTED);

		if (fConnected)
		{
			//printf("Client connected, creating a processing thread.\n");

			// Create a thread for this client. 
			hThread = CreateThread(
				NULL,              // no security attribute 
				0,                 // default stack size 
				InstanceThread,    // thread proc
				(LPVOID)hPipe,    // thread parameter 
				0,                 // not suspended 
				&dwThreadId);      // returns thread ID 

			if (hThread == NULL)
			{
				_tprintf(TEXT("CreateThread failed, GLE=%d.\n"), GetLastError());
				return -1;
			}
			else CloseHandle(hThread);
		}
		else
			// The client could not connect, so close the pipe. 
			CloseHandle(hPipe);
	}
	printf("管道服务器终止....\n");
	return 0;
}

DWORD WINAPI InstanceThread(LPVOID lpvParam)
// This routine is a thread processing function to read from and reply to a client
// via the open pipe connection passed from the main loop. Note this allows
// the main loop to continue executing, potentially creating more threads of
// of this procedure to run concurrently, depending on the number of incoming
// client connections.
{
	HANDLE hHeap = GetProcessHeap();
	CHAR* pchRequest = (CHAR*)HeapAlloc(hHeap, 0, BUFSIZE*sizeof(CHAR));
	CHAR* pchReply = (CHAR*)HeapAlloc(hHeap, 0, BUFSIZE*sizeof(CHAR));

	DWORD cbBytesRead = 0, cbReplyBytes = 0, cbWritten = 0;
	BOOL fSuccess = FALSE;
	HANDLE hPipe = NULL;

	// Do some extra error checking since the app will keep running even if this
	// thread fails.

	if (lpvParam == NULL)
	{
		// 		printf("\nERROR - Pipe Server Failure:\n");
		// 		printf("   InstanceThread got an unexpected NULL value in lpvParam.\n");
		// 		printf("   InstanceThread exitting.\n");
		if (pchReply != NULL) HeapFree(hHeap, 0, pchReply);
		if (pchRequest != NULL) HeapFree(hHeap, 0, pchRequest);
		return (DWORD)-1;
	}

	if (pchRequest == NULL)
	{
		// 		printf("\nERROR - Pipe Server Failure:\n");
		// 		printf("   InstanceThread got an unexpected NULL heap allocation.\n");
		// 		printf("   InstanceThread exitting.\n");
		if (pchReply != NULL) HeapFree(hHeap, 0, pchReply);
		return (DWORD)-1;
	}

	if (pchReply == NULL)
	{
		// 		printf("\nERROR - Pipe Server Failure:\n");
		// 		printf("   InstanceThread got an unexpected NULL heap allocation.\n");
		// 		printf("   InstanceThread exitting.\n");
		if (pchRequest != NULL) HeapFree(hHeap, 0, pchRequest);
		return (DWORD)-1;
	}

	// Print verbose messages. In production code, this should be for debugging only.
	//printf("InstanceThread created, receiving and processing messages.\n");

	// The thread's parameter is a handle to a pipe object instance. 

	hPipe = (HANDLE)lpvParam;

	// Loop until done reading
	while (1)
	{
		// Read client requests from the pipe. This simplistic code only allows messages
		// up to BUFSIZE characters in length.
		fSuccess = ReadFile(
			hPipe,        // handle to pipe 
			pchRequest,    // buffer to receive data 
			BUFSIZE*sizeof(TCHAR), // size of buffer 
			&cbBytesRead, // number of bytes read 
			NULL);        // not overlapped I/O 

		if (!fSuccess || cbBytesRead == 0)
		{
			if (GetLastError() == ERROR_BROKEN_PIPE)
			{
				//_tprintf(TEXT("InstanceThread: client disconnected.\n"), GetLastError());
			}
			else
			{
				_tprintf(TEXT("InstanceThread ReadFile failed, GLE=%d.\n"), GetLastError());
			}
			break;
		}

		// Process the incoming message.
		GetAnswerToRequest(pchRequest, pchReply, &cbReplyBytes);

		// Write the reply to the pipe. 
		fSuccess = WriteFile(
			hPipe,        // handle to pipe 
			pchReply,     // buffer to write from 
			cbReplyBytes, // number of bytes to write 
			&cbWritten,   // number of bytes written 
			NULL);        // not overlapped I/O 

		if (!fSuccess || cbReplyBytes != cbWritten)
		{
			_tprintf(TEXT("InstanceThread WriteFile failed, GLE=%d.\n"), GetLastError());
			break;
		}
	}

	// Flush the pipe to allow the client to read the pipe's contents 
	// before disconnecting. Then disconnect the pipe, and close the 
	// handle to this pipe instance. 

	FlushFileBuffers(hPipe);
	DisconnectNamedPipe(hPipe);
	CloseHandle(hPipe);

	HeapFree(hHeap, 0, pchRequest);
	HeapFree(hHeap, 0, pchReply);

	//printf("InstanceThread exitting.\n");
	return 1;
}

void ProcessHMDData(void *read_buffer, void* write_buffer, LPDWORD pchBytes){
	unsigned char* writeBuffer = (unsigned char*)write_buffer;
	BOOL status = FALSE;
	unsigned int *int_value;
	int_value = (unsigned int *)read_buffer;

	memset(writeBuffer, 0, 512);
	switch (int_value[0]){
	case COMMAND_DEVICE_IS_OPEN:{
		BOOL *result = (BOOL*)&writeBuffer[0];
		result[0] = TRUE;
		if (!device_is_found)
		{
			result[0] = FALSE;
		}
		*pchBytes = sizeof(BOOL);
		break;
	}
	case COMMAND_GET_DATA:{
		ovrSensorState *result = (ovrSensorState*)&writeBuffer[0];
		Quaternion q = GetPoseQuat();
		result[0].Recorded.Pose.Orientation.w = q.w;
		result[0].Recorded.Pose.Orientation.x = q.x;
		result[0].Recorded.Pose.Orientation.y = q.y;
		result[0].Recorded.Pose.Orientation.z = q.z;
		*pchBytes = sizeof(ovrSensorState);
		break;
	}
	case COMMAND_DEVICE_CALIBRATION:{
		BOOL *result = (BOOL*)&writeBuffer[0];
		result[0] = TRUE;
		if (!device_is_found)
		{
			result[0] = FALSE;
		}
		*pchBytes = sizeof(BOOL);
		break;
	}
									// 	case COMMAND_DEVICE_GET_PURE_DATA:{
									// 		float *result = (float*)&writeBuffer[0];
									// 		result[0] = usbDEV.fBatteryLevel[LEFT_HAND];
									// 		if (!device_is_found)
									// 		{
									// 			result[0] = 0.0f;
									// 		}
									// 		*pchBytes = sizeof(float);
									// 		break;
									// 	}
	case COMMAND_DEVICE_ENTER_DFU_MODE:{
		BOOL *result = (BOOL*)&writeBuffer[0];
		result[0] = TRUE;
		if (!device_is_found)
		{
			result[0] = FALSE;
		}
		*pchBytes = sizeof(BOOL);
		break;
	}
	}
}
extern UINT8 global_key[4];

float global_ctlposition[3] = {0,0,0};
void ProcessCtlData(void *read_buffer, void* write_buffer, LPDWORD pchBytes){
	unsigned char* writeBuffer = (unsigned char*)write_buffer;
	BOOL status = FALSE;
	unsigned int *int_value;
	int_value = (unsigned int *)read_buffer;

	memset(writeBuffer, 0, 512);
	switch (int_value[0]){
	case COMMAND_DEVICE_IS_OPEN:{
		BOOL *result = (BOOL*)&writeBuffer[0];
		*result = device_is_found;
		*pchBytes = sizeof(BOOL);
		break;
	}
	case COMMAND_GET_LEFT_POSE:{
		vr::DriverPose_t *result = (vr::DriverPose_t*)&writeBuffer[0];
		//Quaternion q = DM_GetQ();
		result[0].qRotation.w = 1;
		result[0].qRotation.x = 0;
		result[0].qRotation.y = 0;
		result[0].qRotation.z = 0;
		*pchBytes = sizeof(vr::DriverPose_t);
		break;
	}
	case COMMAND_GET_RIGHT_POSE:{
		vr::DriverPose_t *result = (vr::DriverPose_t*)&writeBuffer[0];
		Quaternion q = QuaternionMulti(GetPoseQuat(),CQGamePad);
		result[0].qRotation.w = q.w;
		result[0].qRotation.x = q.x;
		result[0].qRotation.y = q.y;
		result[0].qRotation.z = q.z;
		result[0].vecPosition[0]=global_ctlposition[0];
		result[0].vecPosition[1]=global_ctlposition[1];
		result[0].vecPosition[2]=global_ctlposition[2];
		unsigned char *aAxis = (unsigned char *)&result[0].vecVelocity[3];
		aAxis[0] = global_key[0]<<1;//unsigned char K[4] ;//依次为 8个按键数据(8bit依次△,▽,RB,◎(轴按下), Y,B,A,X ),x轴,y轴,和trig
		//aAxis[1] = usbDEV.K[hand][1];
		//aAxis[2] = usbDEV.K[hand][2];
		aAxis[3] = global_key[3];//我偷懒用这传的按键状态......你说不需要vecVelocity的嘛....
		*pchBytes = sizeof(vr::DriverPose_t);
		break;
	}
	case COMMAND_GET_LEFT_BAT:{
		float *result = (float*)&writeBuffer[0];
		result[0] = 0;
		if (!device_is_found)
		{
			result[0] = 0.0f;
		}
		*pchBytes = sizeof(float);
		break;
	}
	case COMMAND_GET_RIGHT_BAT:{
		float *result = (float*)&writeBuffer[0];
		result[0] = 0.7f;
		if (!device_is_found)
		{
			result[0] = 0.0f;
		}
		*pchBytes = sizeof(float);
		break;
	}
	case COMMAND_SET_RUMBLE:{
		BOOL *result = (BOOL*)&writeBuffer[0];
		*result = true;// TriggerHaptic(*(unsigned int*)a_command.data / 255, 50);
		*pchBytes = sizeof(BOOL);
		break;
	}
	}
}

VOID GetAnswerToRequest(LPCSTR pchRequest,
	LPCSTR pchReply,
	LPDWORD pchBytes)
{
	// This routine is a simple function to print the client request to the console
	// and populate the reply buffer with a default data string. This is where you
	// would put the actual client request processing code that runs in the context
	// of an instance thread. Keep in mind the main thread will continue to wait for
	// and receive other client connections while the instance thread is working.
	ProcessCtlData((void*)pchRequest, (void*)pchReply, pchBytes);
}
FILE* Getfpdata()
{
	return fpdata;
}

Quaternion CorrectQuaterinonY(Quaternion q)
{
	Quaternion qcorr;
	qcorr.w = q.w; qcorr.x = 0.0f;
	qcorr.y = q.y; qcorr.z = 0.0f;
	qcorr = QuaternionInversion(qcorr);
	float mod = QuaternionMod(qcorr);
	qcorr.w = qcorr.w / mod;
	qcorr.x = qcorr.x / mod;
	qcorr.y = qcorr.y / mod;
	qcorr.z = qcorr.z / mod;
	return qcorr;
}
Quaternion CorrectQuaterinon(Quaternion q)
{
	Quaternion qcorr;
	qcorr.w = q.w; qcorr.x = 0.0f;
	qcorr.y = q.y; qcorr.z = 0.0f;
	qcorr = QuaternionInversion(qcorr);
	float mod = QuaternionMod(qcorr);
	qcorr.w = qcorr.w / mod;
	qcorr.x = qcorr.x / mod;
	qcorr.y = qcorr.y / mod;
	qcorr.z = qcorr.z / mod;
	return qcorr;
}

void ShowHelp()
{
	printf("============================HELP===========================\n");
	printf("h - 显示帮助\n");
	printf("c - 检查设备连接\n");
	printf("d - 进入DFU模式\n");
	printf("l - LED 颜色设置\n");
	printf("t - 校准陀螺仪\n");
// 	printf("  - b - 取消校准\n");
// 	printf("  - s - 显示陀螺仪数据\n");
// 	printf("  -   - b - 返回\n");
	printf("p - 启动管道服务器\n");
// 	printf("  - r - 启动管道服务器\n");
// 	printf("  - s - 关闭管道服务器\n");
	printf("g - 进入陀螺仪校准\n");
// 	printf("  - s - 显示陀螺仪数据\n");
// 	printf("  -   - b - 返回\n");
// 	printf("  - c - 校准数据\n");
// 	printf("  - r - 重置为0\n");
// 	printf("  - g - 获取当前陀螺仪计校准参数\n");
// 	printf("  - b - 返回\n");
	printf("m - 进入磁力校准\n");
// 	printf("  - g - 获取当前磁力计校准参数\n");
// 	printf("  - s - 写入默认磁力计参数\n");
// 	printf("  - c - 开始获取磁力计数据并写入data.txt\n");
// 	printf("  -   - f - 数据获取完成 需要输入新的校准参数\n");
// 	printf("  -   - c - 取消校准\n");
// 	printf("  -   - q - 退出校准\n");
	printf("a - 显示欧拉角数据\n");
//	printf("  - b - 返回\n");
	printf("o - 显示原始数据\n");
//	printf("  - b - 返回\n");
	printf("M - 移动虚拟位置\n");
	printf("	c校准方向 b返回 空格归零 \n");
	printf("	ws ad qe按照手柄方向移动虚拟位置  \n");
	printf("	WS AD QE按照头盔方向移动虚拟位置  \n");
	printf("============================帮助===========================\n");
}


int main()
{
// 	printf("DMGamePad调试工具 2016-12-13 02:11:20\n");
// 	/**
// 	 * 初始化USB_HID
// 	 * 相关初始化变量
// 	 * 启动读取线程
// 	 **/globalpacklen = 64;
// 	USB_HID_Init();
// find_device:
// 	/*查找PID=0x0001，VID为0x2833的USB设备*/
// 	//while (!DM_HIDDeviceEnumerate(0x5750, 0x0483))
// 	while (!DM_HIDDeviceEnumerate(0x0002, 0x2833))
// 	{
// 		printf("找不到0x0001, 0x2833 USB设备!\n");
// 		Sleep(1000);
// 		// 		if (_getche()=='q')
// 		// 		{
// 		// 			return 0;
// 		// 		}	
// 	}
	Init("VID_2833&PID_0002");
	//system("cls");
	ShowHelp();
	DMPipe::PipeHMD p("\\\\.\\Pipe\\Dematrix_SteamVR");

// 	UINT8   MAG_Info[FEATURE_OFFSET_SIZE] = { 0x0b };
// 	UINT8   MAG_Set[FEATURE_OFFSET_SIZE] = { 0x0b};

	UINT8   FEATURE_OFFSET_Get[FEATURE_OFFSET_SIZE] = { 0x0b };
	UINT8   FEATURE_OFFSET_Set[FEATURE_OFFSET_SIZE] = { 0x0b };

	UINT8* offset_get_dev = (UINT8*)(FEATURE_OFFSET_Get + 3);
	float* offset_get_data = (float*)(FEATURE_OFFSET_Get + 4);

	UINT8* offset_set_dev = (UINT8*)(FEATURE_OFFSET_Set + 3);
	float* offset_set_data = (float*)(FEATURE_OFFSET_Set + 4);

	while (1)
	{
		//printf(">>");
		*offset_set_dev = *offset_get_dev = 0;
		memset(offset_get_data, 0, FEATURE_OFFSET_SIZE - 4);
		memset(offset_set_data, 0, FEATURE_OFFSET_SIZE - 4);
		char s;
		printf(">>"); s = _getche(); printf("\n");
		if (s == 'c')//检查设备是否有效
		{
			if (CheckDevice()==0)
			{
				printf("设备已连接...\n");
			}
			else
			{
				printf("设备断开连接...\n");
				device_is_found = FALSE;
				Sleep(1000);
				//goto find_device;
			}
		}
		else if (s == 'h')
		{
			ShowHelp();
		}
		else if (s == 'a')
		{
			global_Show = 4;
			while (global_Show != 0)
			{
				printf(">>"); s = _getche(); printf("\n");
				if (s == 'b')
				{
					global_Show = 0;
				}
			}
		}
		else if (s == 'M')
		{
			int _flag = 1;
			float step = 0.005f;
			//clrscr();
			//global_Show = 4;
			while (_flag!=0)
			{
				printf(">>"); s = _getch(); printf("\n");
				MVector3  tmppos = {0,0,0};
				Quaternion qhmd = { 1, 0, 0, 0 };
				if (s == 'b')
				{
					_flag = 0;
				}
				else if (s == ' ')
				{
					global_ctlposition[0] = 0;
					global_ctlposition[1] = 0;
					global_ctlposition[2] = 0;
				}
				else if (s == 'r')
				{
					CQHMD = {1,0,0,0};
					CQGamePad = { 1, 0, 0, 0 };
				}
				else if (s == 'c')
				{
					
					if (p.IsOpen())
					{
						ovrSensorState ss = p.GetSensorData();
						qhmd = { ss.Recorded.Pose.Orientation.w, 0, ss.Recorded.Pose.Orientation.y, 0 };
						CQHMD = CorrectQuaterinonY(qhmd);
						CQGamePad = QuaternionMulti(CorrectQuaterinonY(GetPoseQuat()), qhmd);
						printf("Success!\n");
					}	
					
				}
				else if ((s == 'w') || (s == 'W'))
				{  
					tmppos = { 0, 0, -1 };//向前 -z方向
				}
				else if ((s == 's') || (s == 'S'))
				{
					tmppos = { 0, 0, 1 };//向后  z方向
				}
				else if ((s == 'a') || (s == 'A'))
				{
					tmppos = { 0, 0, -1 };//向左 -x方向
				}
				else if ((s == 'd') || (s == 'D'))
				{
					tmppos = { 0, 0, 1 };//向右   x方向
				}
				else if ((s == 'q') || (s == 'Q'))
				{
					global_ctlposition[1] -= step;
				}
				else if ((s == 'e') || (s == 'E'))
				{
					global_ctlposition[1] += step;
				}
				Quaternion cq;// = CorrectQuaterinonY(DM_GetQ());
				if ((s == 'W') || (s == 'A') || (s == 'S') || (s == 'D'))
				{
					if (p.IsOpen())
					{
						ovrSensorState ss = p.GetSensorData();
						qhmd = { ss.Recorded.Pose.Orientation.w, ss.Recorded.Pose.Orientation.x, ss.Recorded.Pose.Orientation.y, ss.Recorded.Pose.Orientation.z };
						CQHMD = CorrectQuaterinonY(qhmd);
					}
					cq = CQHMD;
				}
				else if ((s == 'w') || (s == 'a') || (s == 's') || (s == 'd'))
				{
					cq = QuaternionMulti(CorrectQuaterinonY(GetPoseQuat()), CQGamePad);
				}

				if ((s == 'a') || (s == 'd') || (s == 'A') || (s == 'D'))
				{
					cq = QuaternionMulti(cq, CreateQuaternion(-90, { 0, 1, 0 }));
				}
				
				Matrix3 rm3 = QuaternionToMatrix(cq);
				MVector3 posv3 = ComputeRotate(rm3, tmppos);
				global_ctlposition[2] += posv3.v[2] * step;
				global_ctlposition[0] += posv3.v[0] * step;
				printf("POS:%f %f %f\n", global_ctlposition[0], global_ctlposition[1], global_ctlposition[2]);
			}
		}
		else if (s == 'o')//原始数据
		{
			system("cls");
			global_Show = 5;
			while (global_Show != 0)
			{
				printf(">>"); s = _getche(); printf("\n");
				if (s == 'b')
				{
					global_Show = 0;
				}
			}
		}
		else if (s=='l')
		{
			printf("\nLED 颜色设置\n");
			printf("K - BLACK\n");
			printf("W - WHITE\n");
			printf("R - RED\n");
			printf("G - GREEN\n");
			printf("B - BLUE\n");
			printf("C - CYAN\n");
			printf("M - MAGENTA\n");
			printf("Y - YELLOW\n");
			printf("L - LOOP\n");
			printf(">>"); s = _getche(); printf("\n");
			if (s=='L'||s=='l')
			{
				for (int i = 0; i < 8; i++)
				{
					*offset_set_dev = FEATURE_OFFSET_SET_LED;
					FEATURE_OFFSET_Set[4] = LED_COLOR_LIST[i];
					if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
					{
						//printf("%x ", FEATURE_OFFSET_Set[4]);
					}
					Sleep(250);
				}
				*offset_set_dev = FEATURE_OFFSET_SET_LED;
				FEATURE_OFFSET_Set[4] = LED_COLOR_LIST[5];
				if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
				{
					//printf("%x ", FEATURE_OFFSET_Set[4]);
				}
			}
			else
			{
				char colorlist[8]{'K', 'W', 'R', 'G', 'B', 'C', 'M', 'Y'};
				for (int i = 0; i < 8; i++)
				{
					if (colorlist[i] == s || colorlist[i] == (s - 32))//大小写
					{
						*offset_set_dev = FEATURE_OFFSET_SET_LED;
						FEATURE_OFFSET_Set[4] = LED_COLOR_LIST[i];
						if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
						{
							printf("Color Set\n");
							//printf("%x ", FEATURE_OFFSET_Set[4]);
						}
						else
						{
							printf("FAILED\n");
						}
						break;
					}
				}
			}
		}
		else if (s == 'g')
		{
			printf("\n上位机陀螺仪校准\n");
			printf("  - s - 显示陀螺仪数据\n");
			printf("  -   - b - 返回\n");
			printf("  - c - 校准数据\n");
			printf("  - r - 重置为0\n");
			printf("  - g - 获取当前陀螺仪计校准参数\n");
			printf("  - b - 返回\n");
			printf(">>"); s = _getche(); printf("\n");
			if (s == 'c')
			{
				//printf("上位机校准尚未实现\n");
				*offset_set_dev = FEATURE_OFFSET_GET_GYRO;
				HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
				UINT8 GYRO_Cali_Last[FEATURE_OFFSET_SIZE] = { 0x0B };
				GYRO_Cali_Last[3] = FEATURE_OFFSET_GET_GYRO;
				if (HidD_GetFeature(DM_HID_Read_HANDLE, GYRO_Cali_Last, FEATURE_OFFSET_SIZE))
				{
					float gyrodefault[3] = { 0.0f, 0.0f, 0.0f};
					memcpy(offset_set_data, gyrodefault, sizeof(float) * 3);
					*offset_set_dev = FEATURE_OFFSET_SET_GYRO;
					HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
					global_Show = 1;
					global_calibration_flag = 1;
					while (global_calibration_flag == 1)//开始获取gyro数据
					{

					}
					global_Show = 0;
					Sleep(500);
					*offset_set_dev = FEATURE_OFFSET_SET_GYRO;
					float gyronew[3] = { -gx0,-gz0,-gy0 };//逆向坐标转换
					memcpy(offset_set_data, gyronew, sizeof(float) * 3);
					if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
					{
						for (int i = 0; i < FEATURE_OFFSET_SIZE; i++)
						{
							if(global_debug) printf("%2x ", FEATURE_OFFSET_Set[i]);
						}
						printf("\n");
						printf("SetGyroOffset\n");
					}
					memset(offset_get_data, 0, FEATURE_OFFSET_SIZE - 4);
					*offset_set_dev = FEATURE_OFFSET_GET_GYRO;
					HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
					*offset_get_dev = FEATURE_OFFSET_GET_GYRO;
					if (HidD_GetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Get, FEATURE_OFFSET_SIZE))
					{
						//显示数据
						for (int i = 0; i < 3; i++)
						{
							printf("%f ", offset_get_data[i]);
						}
						printf("\n");
					}
				}
			}
			else if (s == 'g')//获取陀螺仪零偏数据
			{
				*offset_set_dev = FEATURE_OFFSET_GET_GYRO;
				HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
				*offset_get_dev = FEATURE_OFFSET_GET_GYRO; 
				if (HidD_GetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Get, FEATURE_OFFSET_SIZE))
				{
					printf("GetGyro\n");
					for (int i = 0; i < FEATURE_OFFSET_SIZE; i++)
					{
						if(global_debug) printf("%2x ", FEATURE_OFFSET_Get[i]);
					}
					printf("\n");
					//显示数据
					//float* f= (float*)(FEATURE_OFFSET_Get + 6);
					for (int i = 0; i < 3; i++)
					{
						printf("%f ",offset_get_data[i]);
					}
					printf("\n");
				}
			}
			else if (s=='r')
			{
				*offset_set_dev = FEATURE_OFFSET_SET_GYRO;
				float gyronew[3] = { 0, 0, 0 };
				memcpy(offset_set_data, gyronew, sizeof(float) * 3);
				if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
				{
					printf("ResetGyroOffset\n");
				}
				for (int i = 0; i < 3; i++)
				{
					printf("%f ", offset_set_data[i]);
				}
				printf("\n");
			}
			else if (s == 's')
			{
				global_Show = 1;
				while (global_Show != 0)
				{
					printf(">>"); s = _getche(); printf("\n");
					if (s == 'b')
					{
						global_Show = 0;
					}
				}
			}
			else if (s == 'b')
			{

			}
		}
		else if (s == 't')
		{
			printf("\n下位机陀螺仪校准\n请静置头显 \n 按任意键开始校准陀螺仪\n");
			printf("  - b - 取消校准\n");
			printf("  - s - 显示陀螺仪数据\n");
			printf("  -   - b - 返回\n");
			printf(">>"); s = _getche(); printf("\n");
			if (s == 'b')
			{
				printf("校准取消\n");
			}
			else if (s == 's')
			{
				global_Show = 1;
				while (global_Show != 0)
				{
					printf(">>"); s = _getche(); printf("\n");
					if (s == 'b')
					{
						global_Show = 0;
					}
				}
			}
			else
			{
				Sleep(1000);
				UINT8 gyrocali[5] = { 0x08, 0x00, 0x00, 0x10, 0x26 };
				if (HidD_SetFeature(DM_HID_Read_HANDLE, gyrocali, 5))
				{
					printf("正在校准陀螺仪...请保持头盔静置!\n");
					Sleep(5000);
					printf("校准陀螺仪成功\n");
				}
				else
				{
					printf("校准陀螺仪失败\n");
				}
			}
		}
		else if (s == 'd')
		{
			printf("enter dfu\n");
			UINT8 dfu[5] = { 0x08, 0x00, 0x00, 0x10, 0x25 };
			HidD_SetFeature(DM_HID_Read_HANDLE, dfu, 5);
		}
		else if (s == 'i')
		{
			printf("chipid\n");
			UINT8 chipid[15] = { 0x0A };
			HidD_GetFeature(DM_HID_Read_HANDLE, chipid, 15);
			for (int i = 0; i < 15; i++)
			{
				if(global_debug) printf("%2x ", chipid[i]);
			}
			printf("\n");
		}
		else if (s == 'm')
		{
			printf("\n磁力校准\n");
			printf("  - l - 计算2data.txt");
			printf("  - r - 重置为默认值\n");
			printf("  - g - 获取当前磁力计校准参数\n");
			printf("  - s - 写入默认磁力计参数\n");
			printf("  - c - 开始获取磁力计数据并写入data.txt\n");
			printf("  -   - f - 数据获取完成 需要输入新的校准参数\n");
			printf("  -   - c - 取消校准\n");
			printf("  -   - q - 退出校准\n");
			printf(">>"); s = _getche(); printf("\n");
			if (s=='l')
			{
				GetMagDataFormFile();
			}
			else if (s == 's')
			{
				printf("setreport\n");
				float magcali[6] = { -2.018673f, -2.303837f, 1.989453f, 4.899964f, 4.839235f, 5.541566f };
				memcpy(offset_set_data, magcali, sizeof(float) * 6);


				*offset_set_dev = FEATURE_OFFSET_SET_MAG;
				for (int i = 0; i < FEATURE_OFFSET_SIZE; i++)
				{
					if(global_debug) printf("%2x ", FEATURE_OFFSET_Set[i]);
				}
				if (HidD_SetFeature(DM_HID_Read_HANDLE,FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
				{
					printf("success\n");
				}
			}
			else if (s == 'r')
			{
				*offset_set_dev = FEATURE_OFFSET_SET_MAG;
				float magdefault[6] = { 0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f };
				memcpy(offset_set_data, magdefault, sizeof(float) * 6);
				if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
				{
					printf("ResetMagOffset\n");
				}
				for (int i = 0; i < 6; i++)
				{
					printf("%f ", offset_set_data[i]);
				}
				printf("\n");
			}
			else if (s == 'g')
			{
// 				printf("GetMag\n");
// 				
// 				for (int i = 0; i < FEATURE_OFFSET_SIZE; i++)
// 				{
// 					if(global_debug) printf("%2x ", FEATURE_OFFSET_Get[i]);
// 				}
// 				printf("\nin float\n");
// 				for (int i = 0; i < 6; i++)
// 				{
// 					printf("%f ", offset_get_data[i]);
// 				}
// 				printf("\n");
				*offset_set_dev = FEATURE_OFFSET_GET_MAG;
				HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
				*offset_get_dev = FEATURE_OFFSET_GET_MAG;
				if (HidD_GetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Get, FEATURE_OFFSET_SIZE))
				{
					for (int i = 0; i < FEATURE_OFFSET_SIZE; i++)
					{
						if(global_debug) printf("%2x ", FEATURE_OFFSET_Get[i]);
					}
					printf("\nin float\n");
					for (int i = 0; i < 6; i++)
					{
						printf("%f ", offset_get_data[i]);
					}
					printf("\n");
				}
			}
			else if (s == 'c')
			{
				global_calibration_flag = 3;
				if (global_calibration_flag == 3)//MagConfig
				{
					*offset_set_dev = FEATURE_OFFSET_GET_MAG;
					HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
					UINT8 MAG_Cali_Last[FEATURE_OFFSET_SIZE] = { 0x0B };
					MAG_Cali_Last[3] = FEATURE_OFFSET_GET_MAG;
					//if (HidD_GetFeature(DM_HID_Read_HANDLE, MAG_Cali_Last, FEATURE_OFFSET_SIZE))
					{	
						*offset_set_dev= FEATURE_OFFSET_SET_MAG;
						float magdefault[6] = { 0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f };
						memcpy(offset_set_data, magdefault, sizeof(float) * 6);
						//if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
						{	
							remove("data.txt");
							fopen_s(&fpdata, "data.txt", "w");//打开一个空文件以进行写入。如果该文件存在，其内容将被销毁。
							
							if (fpdata != NULL)
							{
								global_Show = 3;
								printf("文件初始化成功!\n");
								//Sleep(100);
								
							}
							else
							{
								printf("文件初始化失败!\n");
								global_calibration_flag = 0;
							}
							while (global_calibration_flag == 3)//开始获取Mag数据
							{
								printf(">>"); s = _getche(); printf("\n");
								if (s == 'f' || s == 'c' || s == 'q')//结束/取消/退出
								{
									break;
								}
							}
						}					
						global_Show = 0;
					}
					global_calibration_flag = 0;
					if (fpdata != NULL)
					{
						CancelIo(fpdata);
						fclose(fpdata);
					}

					if (s == 'f')//Finished
					{
						printf("写入文件数据结束,等待计算参数后输入:\n");
						float mag_new_cali[6];
						system("pause");
						MagCaliParam magcalitmp = GetMagDataFormFile();
						mag_new_cali[0] = magcalitmp.xOffset;
						mag_new_cali[1] = magcalitmp.yOffset;
						mag_new_cali[2] = magcalitmp.zOffset;
						mag_new_cali[3] = magcalitmp.xScale;
						mag_new_cali[4] = magcalitmp.yScale;
						mag_new_cali[5] = magcalitmp.zScale;
// 						for (int i = 0; i < 6; i++)
// 						{
// 							printf("输入第%d个参数:", i + 1);
// 							std::cin >> mag_str_tmp;
// 							mag_new_cali[i] = atof(mag_str_tmp);
// 						}
						memcpy(offset_set_data, mag_new_cali, sizeof(float) * 6);
					}
					else
					{
						if (s == 'c')
							printf("校准取消");
						memcpy(FEATURE_OFFSET_Set, MAG_Cali_Last, FEATURE_OFFSET_SIZE);
					}
					printf("\n");
					*offset_set_dev = FEATURE_OFFSET_SET_MAG;
					if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
					{
						if (s == 'c')
							printf("参数写入成功\n");
					}
					//最后读取一遍
					printf("Get\n");
					Sleep(100);
					*offset_set_dev = FEATURE_OFFSET_GET_MAG;
					HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
					*offset_get_dev = FEATURE_OFFSET_GET_MAG;
					if (HidD_GetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Get, FEATURE_OFFSET_SIZE))
					{
						printf("当前校准参数:\n");
						for (int i = 0; i < 6; i++)
						{
							printf("%f ", offset_get_data[i]);
						}
						printf("\n");
					}

				}
			}


		}
		else if (s == 'p')
		{

			printf("管道服务器\n");
			printf("  - r - 启动管道服务器\n");
			printf("  - s - 关闭管道服务器\n");
			printf(">>"); s = _getche(); printf("\n");
			if (s == 'r')
			{
				if (global_pipeserver == 0)
				{
					global_pipeserver = 1;
					printf("启动管道服务器.....\n");
					start_thread(pipeservermain, NULL);
				}
				else
				{
					printf("管道服务器已经运行...\n");
				}
			}
			else if (s == 's')
			{
				if (global_pipeserver == 0)
				{
					printf("管道服务器尚未开启...\n");
				}
				else
				{
					global_pipeserver = -1;
				}
			}

		}
	}

	printf("按任意按键开始校准...\n");
	getchar();
	printf("开始写入数据...\n");

	while (1)
	{

	}



}
//win消息
#include <Dbt.h>
BOOL bIsRunning = TRUE;
//transform
#include <algorithm>
#pragma region HID
HDEVINFO device_info_set;

SP_DEVICE_INTERFACE_DATA device_interface_data;
HIDP_CAPS dematrix_device_capabilities;
GUID HidGuid;
BOOL InitPathFromInterfaceData(HDEVINFO hdevInfoSet, SP_DEVICE_INTERFACE_DATA* pidata)
{
	DWORD detailSize = 0;
	BOOL Result;

	Result = SetupDiGetDeviceInterfaceDetail(hdevInfoSet, pidata, NULL, (DWORD)NULL, &detailSize, NULL);

	pData = (PSP_DEVICE_INTERFACE_DETAIL_DATA)malloc(detailSize);
	if (pData == NULL){
		printf("内存不足！\n");
		SetupDiDestroyDeviceInfoList(hdevInfoSet);
		return FALSE;
	}
	pData->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA);

	Result = SetupDiGetDeviceInterfaceDetail(hdevInfoSet, pidata, pData, detailSize, NULL, NULL);
	if (Result == TRUE)
	{
		//printf("Path = %s\n", (char *)(pData->DevicePath));
		return TRUE;
	}
	return FALSE;
}
BOOL DM_HIDDeviceEnumerate(void)
{

	HDEVINFO                  hdevInfoSet;
	SP_DEVICE_INTERFACE_DATA  interfaceData;
	int gamepadcount = 0;

	int deviceIndex = 0;
	interfaceData.cbSize = sizeof(interfaceData);

	/*    先取得GUID   */
	HidD_GetHidGuid(&HidGuid);

	hdevInfoSet = SetupDiGetClassDevs(&HidGuid, NULL, NULL, DIGCF_INTERFACEDEVICE | DIGCF_PRESENT);

	if (hdevInfoSet == INVALID_HANDLE_VALUE) return FALSE;

	for (deviceIndex = 0;
		SetupDiEnumDeviceInterfaces(hdevInfoSet, NULL, &HidGuid, deviceIndex, &interfaceData);
		deviceIndex++)
	{


		if (!InitPathFromInterfaceData(hdevInfoSet, &interfaceData))
			continue;
		//printf("index = %d\n",deviceIndex);
		string pathstr(pData->DevicePath);
		transform(pathstr.begin(), pathstr.end(), pathstr.begin(), toupper);
		std::size_t found = pathstr.find(strGlobalDevicePath);

		if (found != std::string::npos)
		{
			// 			printf("Path = %s\n", pathstr.c_str());
			sCurrent_Device_str = pathstr;
			//printf("sCurrent_Device_str:\n %s\n", sCurrent_Device_str.c_str());
			DM_HID_Read_HANDLE = CreateFileA((char *)(pData->DevicePath), (DWORD)GENERIC_READ | GENERIC_WRITE, FILE_SHARE_READ |
				FILE_SHARE_WRITE, NULL, OPEN_EXISTING,
				FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED, NULL);

			DM_HID_Write_HANDLE = CreateFileA((char *)(pData->DevicePath), GENERIC_WRITE, FILE_SHARE_READ | FILE_SHARE_WRITE,
				(LPSECURITY_ATTRIBUTES)NULL, OPEN_EXISTING, 0, NULL);

			if (DM_HID_Read_HANDLE != INVALID_HANDLE_VALUE)
			{
				device_is_found = TRUE;
				SetEvent(usb_read_over_lapped.hEvent);
				printf("read打开设备成功！\n>>");
			}
			else
			{
				printf("read打开设备失败！\n>>");
			}
			if (DM_HID_Write_HANDLE != INVALID_HANDLE_VALUE)
			{
				printf("write打开设备成功！\n>>");
			}
			else
			{
				printf("write打开设备失败！\n>>");
			}

			break;
		}
		else
			continue;
	}
	SetupDiDestroyDeviceInfoList(hdevInfoSet);
	if (device_is_found == TRUE)return TRUE;
	else
		return FALSE;
}
#pragma endregion
#pragma region WindowsMessage

HWND                hMessageWindow;
HDEVNOTIFY          hDeviceNotify;

bool MessageCallback(WORD messageType, const std::string& devicePath)
{
	bool rv = true;
	if (messageType == DBT_DEVICEARRIVAL)
	{

		//rv = pNotificationClient->OnMessage(Notifier::DeviceAdded, devicePath);
		if (!device_is_found)
		{
			printf("检测到新设备插入,正在初始化!\n>>");
			DM_HIDDeviceEnumerate();
			rv = device_is_found;
			//TriggerHaptic(1, 50);
			//TriggerHaptic(1, 100);
		}
		else
		{
			printf("检测到插入VID PID 相同的设备,但设备已经初始化!\n>>");
		}
	}
	else if (messageType == DBT_DEVICEREMOVECOMPLETE)
	{
		//pNotificationClient->OnMessage(Notifier::DeviceRemoved, devicePath);
		//printf("%s\n%s\n", devicePath.c_str(), sCurrent_Device_str.c_str());
		if (device_is_found)
		{
			//found = devicePath.find(sCurrent_Device_str);
			if (devicePath.find(sCurrent_Device_str) != std::string::npos)
			{
				ResetAHRS();
				device_is_found = FALSE;
				sCurrent_Device_str = "NULL";
				if (DM_HID_Read_HANDLE != INVALID_HANDLE_VALUE)
				{
					CloseHandle(DM_HID_Read_HANDLE);
					DM_HID_Read_HANDLE = INVALID_HANDLE_VALUE;
				}
				if (DM_HID_Write_HANDLE != INVALID_HANDLE_VALUE)
				{
					CloseHandle(DM_HID_Write_HANDLE);
					DM_HID_Write_HANDLE = INVALID_HANDLE_VALUE;
				}
				printf("设备已拔出!\n>>");
			}
			else
			{
				printf("拔出了具有相同的vid pid但并不是已经打开的设备!\n>>");
			}
		}
	}
	else
	{

	}
	return rv;
}


LRESULT CALLBACK WindowsMessageCallback(HWND hwnd,
	UINT message,
	WPARAM wParam,
	LPARAM lParam)
{
	switch (message)
	{
	case WM_CREATE:
	{
		// Setup window user data with device status object pointer.
		LPCREATESTRUCT create_struct = reinterpret_cast<LPCREATESTRUCT>(lParam);
		void *lpCreateParam = create_struct->lpCreateParams;
		//DeviceStatus *pDeviceStatus = reinterpret_cast<DeviceStatus*>(lpCreateParam);
		//printf("create\n");
		bIsRunning = TRUE;
		//SetWindowLongPtr(hwnd, GWLP_USERDATA, reinterpret_cast<LONG_PTR>(pDeviceStatus));
	}
	return 0;	// Return 0 for successfully handled WM_CREATE.

	case WM_DEVICECHANGE:
	{
		WORD loword = LOWORD(wParam);

		if (loword != DBT_DEVICEARRIVAL &&
			loword != DBT_DEVICEREMOVECOMPLETE)
		{
			// Ignore messages other than device arrive and remove complete 
			// (we're not handling intermediate ones).
			return TRUE;	// Grant WM_DEVICECHANGE request.
		}

		DEV_BROADCAST_DEVICEINTERFACE* hdr;
		hdr = (DEV_BROADCAST_DEVICEINTERFACE*)lParam;

		if (hdr->dbcc_devicetype != DBT_DEVTYP_DEVICEINTERFACE)
		{
			// Ignore non interface device messages.
			return TRUE;	// Grant WM_DEVICECHANGE request.
		}

		//LONG_PTR userData = GetWindowLongPtr(hwnd, GWLP_USERDATA);

		//DeviceStatus* pDeviceStatus = (DeviceStatus*)userData;
		string devicePath(hdr->dbcc_name);

		if (/*pDeviceStatus->*/HidGuid == hdr->dbcc_classguid)
		{
			transform(devicePath.begin(), devicePath.end(), devicePath.begin(), toupper);
			std::string str2(strGlobalDevicePath);
			//printf("devicePath:%s\n", devicePath.c_str());
			std::size_t found = devicePath.find(str2);
			if (found != std::string::npos)
			{
				if (!MessageCallback(loword, devicePath))
				{

				}
				break;
			}
		}
	}
	return TRUE;	// Grant WM_DEVICECHANGE request.


	case WM_CLOSE:
	{
		//LONG_PTR userData = GetWindowLongPtr(hwnd, GWLP_USERDATA);
		bIsRunning = FALSE;
		DestroyWindow(hwnd);
	}
	return 0;	// We processed the WM_CLOSE message.

	case WM_DESTROY:
		PostQuitMessage(0);
		return 0;	// We processed the WM_DESTROY message.
	}

	return DefWindowProc(hwnd, message, wParam, lParam);
}


bool WinProcInitialize()
{
	TCHAR special_name[] = TEXT("wnd_class_name_usbDMCali_JustFeng_2017");
	WNDCLASS wndClass;
	wndClass.style = CS_HREDRAW | CS_VREDRAW;
	wndClass.lpfnWndProc = WindowsMessageCallback;
	wndClass.cbClsExtra = 0;
	wndClass.cbWndExtra = 0;
	wndClass.hInstance = 0;
	wndClass.hIcon = LoadIcon(NULL, IDI_APPLICATION);
	wndClass.hCursor = LoadCursor(NULL, IDC_ARROW);
	wndClass.hbrBackground = (HBRUSH)(COLOR_WINDOW + 1);
	wndClass.lpszMenuName = NULL;
	wndClass.lpszClassName = special_name;

	if (!RegisterClass(&wndClass))
	{
		printf("Failed to register window class.");
		return false;
	}

	// We're going to create a 'message-only' window. This will be hidden, can't be enumerated etc.
	// To do this we supply 'HWND_MESSAGE' as the hWndParent.
	// http://msdn.microsoft.com/en-us/library/ms632599%28VS.85%29.aspx#message_only
	hMessageWindow = CreateWindow(special_name,
		special_name,
		WS_OVERLAPPEDWINDOW,
		CW_USEDEFAULT,
		CW_USEDEFAULT,
		CW_USEDEFAULT,
		CW_USEDEFAULT,
		HWND_MESSAGE,
		NULL,
		0,
		NULL);	// Pass this object via the CREATESTRUCT mechanism 
	// so that we can attach it to the window user data.

	if (hMessageWindow == NULL)
	{
		printf("Failed to create window.");
		return false;
	}

	// According to MS, topmost windows receive WM_DEVICECHANGE faster.
	::SetWindowPos(hMessageWindow, HWND_TOPMOST, 0, 0, 0, 0, SWP_NOMOVE | SWP_NOSIZE);
	UpdateWindow(hMessageWindow);


	// Register notification for additional HID messages.
	HidD_GetHidGuid(&HidGuid);

	DEV_BROADCAST_DEVICEINTERFACE notificationFilter;

	ZeroMemory(&notificationFilter, sizeof(notificationFilter));
	notificationFilter.dbcc_size = sizeof(DEV_BROADCAST_DEVICEINTERFACE);
	notificationFilter.dbcc_devicetype = DBT_DEVTYP_DEVICEINTERFACE;
	//notificationFilter.dbcc_classguid = hidguid;

	// We need DEVICE_NOTIFY_ALL_INTERFACE_CLASSES to detect
	// HDMI plug/unplug events.
	hDeviceNotify = RegisterDeviceNotification(
		hMessageWindow,
		&notificationFilter,
		DEVICE_NOTIFY_ALL_INTERFACE_CLASSES | DEVICE_NOTIFY_WINDOW_HANDLE);

	if (hDeviceNotify == NULL)
	{
		printf("Failed to register for device notifications.");
		return false;
	}

	return true;
}

void ProcessMessages()
{
	//printf("MSG\n");
	if (hMessageWindow == NULL)
		printf("Need to call 'Initialize' before first use.");

	MSG msg;

	// Note WM_DEVICECHANGED messages are dispatched but not retrieved by PeekMessage.
	// I think this is because they are pending, non-queued messages.
	while (PeekMessage(&msg, NULL, 0, 0, PM_REMOVE))
	{
		TranslateMessage(&msg);
		DispatchMessage(&msg);
	}
}

#pragma endregion


DWORD Thread_Win_MSG()
{

	if (!WinProcInitialize())
	{
		bIsRunning = FALSE;
	}
	while (bIsRunning)
	{
		Sleep(1);
		ProcessMessages();

	}
	return 0;
}





	/* 控制器初始化函数 */
	__declspec(dllexport) int Init(char* devpath)
	{
		USB_HID_Init();
		strGlobalDevicePath = devpath;//"VID_2833&PID_0002";
		globalpacklen = 64;
		start_thread(Thread_Win_MSG, 0);
		DM_HIDDeviceEnumerate();
		return 1;
	}


	__declspec(dllexport) int CheckDevice(void)
	{
		UINT8   FEATURE_OFFSET_Get[FEATURE_OFFSET_SIZE] = { 0x0b };
		if (HidD_GetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Get, FEATURE_OFFSET_SIZE))
		{
			printf("设备已连接...\n");
		}
		else
		{
			printf("设备断开连接...\n");
			device_is_found = FALSE;
		}
		return device_is_found?1:0;
	}

	__declspec(dllexport) int GyroCali(void)
	{
		if (device_is_found)
		{
			UINT8 gyrocali[5] = { 0x08, 0x00, 0x00, 0x10, 0x26 };
			if (HidD_SetFeature(DM_HID_Read_HANDLE, gyrocali, 5))
			{
				//printf("正在校准陀螺仪...请保持头盔静置!\n");
				//Sleep(1000);
				return 1;
				//printf("校准陀螺仪成功\n");
			}
		}

			return 0;


	}
	__declspec(dllexport) Vector3 GetGyroCali(void)
	{
		Vector3 v;
		v.x = v.y = v.z = -99999.0f;
		if (!device_is_found)
			return v;

		UINT8   FEATURE_OFFSET_Get[FEATURE_OFFSET_SIZE] = { 0x0b };
		UINT8   FEATURE_OFFSET_Set[FEATURE_OFFSET_SIZE] = { 0x0b };

		UINT8* offset_get_dev = (UINT8*)(FEATURE_OFFSET_Get + 3);
		float* offset_get_data = (float*)(FEATURE_OFFSET_Get + 4);

		UINT8* offset_set_dev = (UINT8*)(FEATURE_OFFSET_Set + 3);
		float* offset_set_data = (float*)(FEATURE_OFFSET_Set + 4);


		*offset_set_dev = *offset_get_dev = 0;
		memset(offset_get_data, 0, FEATURE_OFFSET_SIZE - 4);
		memset(offset_set_data, 0, FEATURE_OFFSET_SIZE - 4);

		*offset_set_dev = FEATURE_OFFSET_GET_GYRO;
		HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
		*offset_get_dev = FEATURE_OFFSET_GET_GYRO;
		if (HidD_GetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Get, FEATURE_OFFSET_SIZE))
		{
			v.x = offset_get_data[0];
			v.y = offset_get_data[1];
			v.z = offset_get_data[2];
		}
		return v;
	}

	__declspec(dllexport) MagCaliParam GetMagCali()
	{
		MagCaliParam mcp;
		UINT8   FEATURE_OFFSET_Get[FEATURE_OFFSET_SIZE] = { 0x0b };
		UINT8   FEATURE_OFFSET_Set[FEATURE_OFFSET_SIZE] = { 0x0b };

		UINT8* offset_get_dev = (UINT8*)(FEATURE_OFFSET_Get + 3);
		float* offset_get_data = (float*)(FEATURE_OFFSET_Get + 4);

		UINT8* offset_set_dev = (UINT8*)(FEATURE_OFFSET_Set + 3);
		float* offset_set_data = (float*)(FEATURE_OFFSET_Set + 4);
		*offset_set_dev = FEATURE_OFFSET_GET_MAG;
		HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
		*offset_get_dev = FEATURE_OFFSET_GET_MAG;
		
		if (HidD_GetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Get, FEATURE_OFFSET_SIZE))
		{
			mcp.xOffset = offset_get_data[0];
			mcp.yOffset = offset_get_data[1];
			mcp.zOffset = offset_get_data[2];
			mcp.xScale = offset_get_data[3];
			mcp.yScale = offset_get_data[4];
			mcp.zScale = offset_get_data[5];

			printf("当前校准参数:\n");
			for (int i = 0; i < 6; i++)
			{
				printf("%f ", offset_get_data[i]);
			}
			printf("\n");
		}
		return mcp;
	}
	__declspec(dllexport) int SetMagCali(float x, float y, float z, float x0, float y0, float z0)
	{
		UINT8   FEATURE_OFFSET_Set[FEATURE_OFFSET_SIZE] = { 0x0b };
		UINT8* offset_set_dev = (UINT8*)(FEATURE_OFFSET_Set + 3);
		float* offset_set_data = (float*)(FEATURE_OFFSET_Set + 4);
		*offset_set_dev = FEATURE_OFFSET_SET_MAG;
		float magdefault[6] = { x, y, z, x0, y0, z0 };
		memcpy(offset_set_data, magdefault, sizeof(float) * 6);
		if (HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE))
		{
			printf("SetMagOffset\n");
			return 1;
		}
		return 0;
	}


int Global_MagCali = 0;

__declspec(dllexport) int MagCalibrate(void)
{
	if (global_calibration_flag == 3)
	{
		return 0;
	}
	UINT8   FEATURE_OFFSET_Get[FEATURE_OFFSET_SIZE] = { 0x0b };
	UINT8   FEATURE_OFFSET_Set[FEATURE_OFFSET_SIZE] = { 0x0b };

	UINT8* offset_get_dev = (UINT8*)(FEATURE_OFFSET_Get + 3);
	float* offset_get_data = (float*)(FEATURE_OFFSET_Get + 4);

	UINT8* offset_set_dev = (UINT8*)(FEATURE_OFFSET_Set + 3);
	float* offset_set_data = (float*)(FEATURE_OFFSET_Set + 4);
	DWORD checkhandle;
	global_calibration_flag = 3;
	if (global_calibration_flag == 3)//MagConfig
	{

		*offset_set_dev = FEATURE_OFFSET_GET_MAG;
		HidD_SetFeature(DM_HID_Read_HANDLE, FEATURE_OFFSET_Set, FEATURE_OFFSET_SIZE);
		UINT8 MAG_Cali_Last[FEATURE_OFFSET_SIZE] = { 0x0B };
		MAG_Cali_Last[3] = FEATURE_OFFSET_GET_MAG;
		if (HidD_GetFeature(DM_HID_Read_HANDLE, MAG_Cali_Last, FEATURE_OFFSET_SIZE))
		{
			*offset_set_dev = FEATURE_OFFSET_SET_MAG;
			if (SetMagCali(0.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f))
			{
				return 1;
			}
			global_Show = 0;
		}
		global_calibration_flag = 0;

		return 0;
	}
}

__declspec(dllexport) int FinishMagCali(void)
{
	global_calibration_flag = 0;
	global_Show = 0;
	return 1;
}
__declspec(dllexport) MagCaliParam  CalculateMagParam()
{
	return GetMagDataFormFile();
}

/* 获取左手信息    */
__declspec(dllexport) Vector3 GetPoint(void)
{	
	if (!device_is_found){
		Vector3 v;
		v.x = v.y = v.z = -99999.0f;
		return v;
	}
	return GetMagData();
}

__declspec(dllexport) Vector3 GetEulerianAngle(void)
{
	Vector3 v;
	v.x = v.y = v.z = -99999.0f;
	if (device_is_found)
	{
		v.z = DM_GetYaw();
		v.x = DM_GetPitch();
		v.y = DM_GetRoll();
	}
	return v;
}

__declspec(dllexport) int Correction(void)
{
	return ResetHeading();
}

__declspec(dllexport) Quaternion GetQuaternion(void)
{
	return GetPoseQuat();
}