//###################################################################################
//#						                  SpacePoint Fusion Object									          #
//#						          Used To Handle a SpacePoint Fusion device							      #
//#						                          Author : 							                      #
//#						                      Aymeric SUTEAU									                #
//#						                       LISA - ANGERS									                #
//###################################################################################


/*-------------------------------- HEADER INCLUDE ---------------------------------*/
#include "Plugin.h"


/*-------------------------- GLOBAL VARIABLES FOR HID  ----------------------------*/
HIDP_CAPS Capabilities;
PSP_DEVICE_INTERFACE_DETAIL_DATA detailData;
HANDLE DeviceHandle, hEventObject, hDevInfo, hEventObjectWrite, hEventObjectRead;
GUID HidGuid;
OVERLAPPED HIDOverlapped, HIDOverlappedWrite, HIDOverlappedRead;
ULONG Length, Required;
DWORD NumberOfBytesRead, Result;
unsigned char InputReport[21], OutputReport[256];
bool MyDeviceDetected = false;
int nbReadBytes = 0;
std::string MyDevicePathName;
MSG msg;
BOOL bRet;
int iLastMode = 0;


/*-------------------------- CONSTRUCTOR AND DESTRUCTOR ---------------------------*/
Fusion::Fusion() : Thread() 
{
  fAcceleration = new float[3];
  iButton = new int[2];
  iRawAxes = new int[11];

  SetConnected(false);              // Device not connected yet
  SetNbSamples(-1);                 // Number of maximum samples not defined
  SetVID(FUSION_VID);               // Define SpacePoint Fusion VID
  SetPID(FUSION_PID);               // Define SpacePoint Fusion PID
  SetDataType(ORIENTATION_DATA);    // Define data type we want to retrieve (RAW or ORIENTATION)
  iLastStatus = 0;                  // Connection status pending

  // Thread handling
  this->start();
  this->isRunning = true;
  this->bStatus = false;
}

Fusion::~Fusion() 
{  
  // Thread handling
  this->isRunning = false;
  this->bStatus = false;
  this->stop();

  // Delete dynamic arrays
  delete [] fAcceleration;
  delete [] iButton;
  delete [] iRawAxes;

  // Reset connection status to pending
  iLastStatus = 0;
}


/*------------------------------ GETTERS AND SETTERS ------------------------------*/
bool Fusion::GetConnected() 
{
  return this->bConnected;
}

void Fusion::SetConnected(bool status) 
{
  this->bConnected = status;
}

int Fusion::GetNbSamples() 
{
  return this->iNbSamples;
}

void Fusion::SetNbSamples(int nbSamples) 
{
  this->iNbSamples = nbSamples;
}

int Fusion::GetVID() 
{
  return this->iVID;
}

void Fusion::SetVID(int VID) 
{
  this->iVID = VID;
}

int Fusion::GetPID()
{
  return this->iPID;
}

void Fusion::SetPID(int PID) 
{
  this->iPID = PID;
}

int Fusion::GetDataType() 
{
  return this->iDataType;
}

void Fusion::SetDataType(int dataType) 
{
  this->iDataType = dataType;
}


/*--------------------------------- OTHER METHODS ---------------------------------*/
bool Fusion::OpenDevice(int dataType) 
{
  // Set data type we want to retrieve (raw or orientation mode)
  SetDataType(dataType);
  iLastMode = dataType;

  // Try to connect to SpacePoint Fusion device
  SetConnected(FindHID(FUSION_VID, FUSION_PID, GetDataType()));

  // Notify about the initial connection status of SpacePoint Fusion device
  if (!GetConnected())
  {
    PostMessage(HScol, FUSION_DISCONNECTED_CB, (int)this, (LPARAM)NULL);
    return false;
  }
  else 
  {
    // Notifiy Scol window about the initial status of the HID device
    PostMessage(HScol, FUSION_CONNECTED_CB, (int)this, (LPARAM)NULL);
    bStatus = true;
    return true;
  }
  return true;
}

void Fusion::CloseDevice() 
{
  // Update device status
  SetConnected(false);

  // Close handles used for USB HID devices management
  CloseHandles();

  // Destroy window handling device notifications
  DestroyWindow(hwnd);

  // Update connection status for the thread
  bStatus = false;
}

void Fusion::CloseHandles()
{
  if (DeviceHandle != INVALID_HANDLE_VALUE)
    CloseHandle(DeviceHandle);

  if (hEventObject != INVALID_HANDLE_VALUE)
    CloseHandle(hEventObject);

  if (hDevInfo != INVALID_HANDLE_VALUE)
    CloseHandle(hDevInfo);

  if (hEventObjectWrite != INVALID_HANDLE_VALUE)
    CloseHandle(hEventObjectWrite);

  if (hEventObjectRead != INVALID_HANDLE_VALUE)
    CloseHandle(hEventObjectRead);
}

// Interface : 0 = raw data, Interface 1 = Kalman filter output (meaningful orientation data)
bool Fusion::ParseData()
{
  int nbBytes = 0, i = 0, byteIndex = 1;

  // Device opened ?
  if (GetConnected() != 0)
  {
    // Data read ?
    if (nbBytes = ReadDataFromHID() > 0)
    {
      // ------------ "Raw data" mode : retrieve raw data from Fusion device (accelerometer, gyroscope, and magnetometer data)
      if (GetDataType() == 0)
      {
        // Get first raw values
        for (i=0; i<9; i++)
        {
			    iRawAxes[i] = (int)InputReport[byteIndex] + 256*(int)InputReport[byteIndex + 1];
			    byteIndex += 2;
		    }

        // Get buttons values
       	iButton[0] = InputReport[byteIndex] & 1;
		    iButton[1] = (InputReport[byteIndex] >> 1) & 1;

        // Get final raw values
		    iRawAxes[9] = (InputReport[byteIndex] >> 4) & 0xf;
		    byteIndex++;
		    iRawAxes[10] = InputReport[byteIndex];

        // Notify Scol window that new raw data (acceleration and quaternion) has been read
        PostMessage(HScol, FUSION_RAW_DATA_CB, (int)this, (LPARAM)NULL);

        // Eventually notify Scol window if a button has been pressed
        if (iButton[0] == 1 || iButton[1] == 1)
          PostMessage(HScol, FUSION_BUTTON_CB, (int)this, (LPARAM)NULL);
      }

      // ------------ Mode "Orientation data" : retrieve calculated orientation information (acceleration and quaternions)
      else if (GetDataType() == 1)
      {
        // Get first raw values
		    for (i=0; i<7; i++)
        {
          iRawAxes[i] = (int)InputReport[byteIndex] + 256*(int)InputReport[byteIndex+1];
			    byteIndex += 2;
		    }

        // Get buttons values
		    iButton[0] = InputReport[byteIndex]&1;
		    iButton[1] = (InputReport[byteIndex]>>1)&1;

        // Get final raw values
		    iRawAxes[7] = (InputReport[byteIndex] >> 4) & 0xf;

        // 16 bit raw values centered at 32768
		    // acc_scaled = 6 * (acc_received - 32768)/32768;
		    for (i=0; i<3; i++)
			    fAcceleration[i] = 6.0f * (iRawAxes[i] - 32768) / 32768.0f;

		    // 16 bit raw values centered at 32768
		    // q_scaled = 3.0518e - 005 * (qraw - 32768)
        fQuaternion = Quaternion(3.0518e-005f * (iRawAxes[6] - 32768),
                                 3.0518e-005f * (iRawAxes[4] - 32768),
                                 3.0518e-005f * (iRawAxes[5] - 32768),
                                 3.0518e-005f * (iRawAxes[3] - 32768));
        
        // Invert quaternion on the Y axis (initial shift is 180 degrees)
        fQuaternion = fQuaternion * Quaternion(0, 0, 1, 0);

        // Notify Scol window that new orientation data has been read
        PostMessage(HScol, FUSION_ORIENTATION_DATA_CB, (int)this, (LPARAM)NULL);

        // Eventually notify Scol window if a button has been pressed
        if (iButton[0] == 1 || iButton[1] == 1)
          PostMessage(HScol, FUSION_BUTTON_CB, (int)this, (LPARAM)NULL);
      }

      // Data successfully read from device
      return true;
    }
    else
      return false;   // Unable to read data from device
  }
  else
    return false;
}


/*------------------------------- USB HID HANDLING --------------------------------*/
bool Fusion::FindHID(unsigned int vendorID, unsigned int productID, unsigned int interfaceMode)
{
	// Use a series of API calls to find a HID with a specified Vendor IF and Product ID
	HIDD_ATTRIBUTES Attributes;
	SP_DEVICE_INTERFACE_DATA devInfoData;
	bool LastDevice = false;
	int MemberIndex = 0;
	LONG Result;
	Length = 0;
	detailData = NULL;
	DeviceHandle = NULL;

	/*
	API function: HidD_GetHidGuid
	Get the GUID for all system HIDs.
	Returns: the GUID in HidGuid.
	*/
	HidD_GetHidGuid(&HidGuid);
	
	/*
	API function: SetupDiGetClassDevs
	Returns: a handle to a device information set for all installed devices.
	Requires: the GUID returned by GetHidGuid.
	*/
	hDevInfo = SetupDiGetClassDevs(&HidGuid,
		                             NULL,
		                             NULL,
		                             DIGCF_PRESENT|DIGCF_INTERFACEDEVICE);
		
	devInfoData.cbSize = sizeof(devInfoData);

	// Step through the available devices looking for the one we want.
	// Quit on detecting the desired device or checking all available devices without success
	MemberIndex = 0;
	LastDevice = false;
  MyDeviceDetected = false;
	do
	{
		/*
		API function: SetupDiEnumDeviceInterfaces
		On return, MyDeviceInterfaceData contains the handle to a SP_DEVICE_INTERFACE_DATA structure for a detected device.
		Requires:
		  The DeviceInfoSet returned in SetupDiGetClassDevs.
		  The HidGuid returned in GetHidGuid.
		  An index to specify a device.
		*/
		Result = SetupDiEnumDeviceInterfaces(hDevInfo, 
			                                   0, 
			                                   &HidGuid, 
			                                   MemberIndex, 
			                                   &devInfoData);

		if (Result != 0)
		{
			// A device has been detected, so get more information about it
			/*
			API function: SetupDiGetDeviceInterfaceDetail
			Returns: an SP_DEVICE_INTERFACE_DETAIL_DATA structure containing information about a device.
			To retrieve the information, call this function twice:
			  The first time returns the size of the structure in Length.
			  The second time returns a pointer to the data in DeviceInfoSet.
			Requires:
			A DeviceInfoSet returned by SetupDiGetClassDevs
			The SP_DEVICE_INTERFACE_DATA structure returned by SetupDiEnumDeviceInterfaces.
			
			The final parameter is an optional pointer to an SP_DEV_INFO_DATA structure.
			This application doesn't retrieve or use the structure.			
			If retrieving the structure, set MyDeviceInfoData.cbSize = length of MyDeviceInfoData, and pass the structure's address.
			*/
			// Get the Length value
			// The call will return with a "buffer too small" error which can be ignored
			Result = SetupDiGetDeviceInterfaceDetail(hDevInfo, 
				                                       &devInfoData, 
				                                       NULL, 
				                                       0, 
				                                       &Length, 
				                                       NULL);

			// Allocate memory for the hDevInfo structure, using the returned Length
			detailData = (PSP_DEVICE_INTERFACE_DETAIL_DATA) malloc(Length);
			
			// Set cbSize in the detailData structure
			detailData->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA);

			// Call the function again, this time passing it the returned buffer size
			Result = SetupDiGetDeviceInterfaceDetail(hDevInfo, 
				                                       &devInfoData, 
				                                       detailData, 
				                                       Length, 
				                                       &Required, 
				                                       NULL);

			// Open a handle to the device
			// To enable retrieving information about a system mouse or keyboard, don't request Read or Write access for this handle
			/*
			API function: CreateFile
			Returns: a handle that enables reading and writing to the device.
			Requires: The DevicePath in the detailData structure returned by SetupDiGetDeviceInterfaceDetail.
			*/
			DeviceHandle = CreateFile(detailData->DevicePath, 
				                        GENERIC_READ|GENERIC_WRITE,         //0,
				                        FILE_SHARE_READ|FILE_SHARE_WRITE, 
				                        (LPSECURITY_ATTRIBUTES)NULL,
				                        OPEN_EXISTING, 
				                        FILE_FLAG_OVERLAPPED,               //0,
				                        NULL);

			/*
			API function: HidD_GetAttributes
			Requests information from the device.
			Requires: the handle returned by CreateFile.
			Returns: a HIDD_ATTRIBUTES structure containing the Vendor ID, Product ID, and Product Version Number.
			Use this information to decide if the detected device is the one we're looking for.
			*/
			// Set the Size to the number of bytes in the structure
			Attributes.Size = sizeof(Attributes);
			Result = HidD_GetAttributes(DeviceHandle, 
				                          &Attributes);
			
			// Is it the desired device?
			MyDeviceDetected = false;
			
      // Check Vendor ID (must be 'SpacePoint')
			if (Attributes.VendorID == vendorID)
			{
        // Check Product ID (must be 'Fusion')
        if (Attributes.ProductID == productID)
				{
					// Both the Vendor ID and Product ID match, we can get device capabilities to check if interface mode is correct
					MyDeviceDetected = true;

					// Register to receive device notifications
					RegisterForDeviceNotifications();

          // Set device path name for HID events
          MyDevicePathName = detailData->DevicePath;

					// Get the device's capablities
					GetDeviceCapabilities();

          // Check interface mode (0=raw and 1=orientation)
          if (Capabilities.UsagePage != interfaceMode)
          {
            // Interface mode is not correct, so it's not the desired device
            MyDeviceDetected = false;
					  CloseHandle(DeviceHandle);
          }
          else
          {
            // Correct interface mode
            // MMechostr(MSKDEBUG, ">>> Correct interface mode.\n");

            // Register to receive device notifications
					  //RegisterForDeviceNotifications();

            // Send the request command using WriteFile
            hEventObjectWrite = CreateEvent(NULL,
                                            true,
                                            true,
                                            NULL);
            HIDOverlappedWrite.hEvent = hEventObjectWrite;
            HIDOverlappedWrite.Offset = 0;
            HIDOverlappedWrite.OffsetHigh = 0;
            Result = WriteFile(DeviceHandle,
                               &OutputReport[0],
                               Capabilities.OutputReportByteLength,
                               &NumberOfBytesRead,
                               (LPOVERLAPPED)&HIDOverlapped);
          }
				}
				else
        {
					CloseHandle(DeviceHandle);  // Wrong Product ID
        }
      }
			else
      {
				CloseHandle(DeviceHandle);  // Wrong Vendor ID
      }

		  // Free the memory used by the detailData structure (no longer needed)
		  free(detailData);
		}
		else
    {
			//SetupDiEnumDeviceInterfaces returned 0, so there are no more devices to check
      LastDevice = true;
    }

	  //If we haven't found the device yet, and haven't tried every available device, try the next one
	  MemberIndex = MemberIndex + 1;
	}
  while ((LastDevice == false) && (MyDeviceDetected == false));

  // Add log to debug console
	if (MyDeviceDetected == false)
  {
	  MMechostr(MSKDEBUG, "Device not detected\n");
  }
  else
  {
		MMechostr(MSKDEBUG, "Device detected\n");
  }

	// Free the memory reserved for hDevInfo by SetupDiClassDevs
	SetupDiDestroyDeviceInfoList(hDevInfo);

  // Return device detection status
	return MyDeviceDetected;
}

void Fusion::RegisterForDeviceNotifications()
{
	// Request to receive messages when a device is attached or removed
  const char *className = "DevNotifyTest";
  WNDCLASSA wincl = {0};
  wincl.hInstance = GetModuleHandle(0);
  wincl.lpszClassName = className;
  wincl.lpfnWndProc = WinProc;
  if (!RegisterClassA(&wincl))
  {
    DWORD le = GetLastError();
    // MMechostr(MSKDEBUG, "RegisterClassA() failed, last error = %d\n", le);
  }

  // Create the window handling HID devices notifications
  HWND parent = 0;
  hwnd = CreateWindowExA(WS_EX_TOPMOST, className, className, 0, 0, 0, 0, 0, parent, 0, 0, 0);
  if (!hwnd)
  {
    DWORD le = GetLastError();
    MMechostr(MSKDEBUG, "CreateWindowExA() failed, last error = %d\n", le);
  }

  // Add this class instance as window parameter
  SetWindowLongPtr(hwnd, GWLP_USERDATA, (LONG)this);

  // Register for notifications
  DEV_BROADCAST_DEVICEINTERFACE notifyFilter = {0};
  notifyFilter.dbcc_size = sizeof(notifyFilter);
  notifyFilter.dbcc_devicetype = DBT_DEVTYP_DEVICEINTERFACE;
  notifyFilter.dbcc_classguid = HidGuid;
  HDEVNOTIFY hDevNotify = RegisterDeviceNotification(hwnd, &notifyFilter, DEVICE_NOTIFY_WINDOW_HANDLE);   // Notifications sent to window handle
  if (!hDevNotify)
  {
    DWORD le = GetLastError();
    MMechostr(MSKDEBUG, "RegisterDeviceNotification() failed, last error = %d\n", le);
  }
}

void Fusion::GetDeviceCapabilities()
{
	// Get the Capabilities structure for the device
	PHIDP_PREPARSED_DATA	PreparsedData;

	/*
	API function: HidD_GetPreparsedData
	Returns: a pointer to a buffer containing the information about the device's capabilities.
	Requires: A handle returned by CreateFile.
	There's no need to access the buffer directly, but HidP_GetCaps and other API functions require a pointer to the buffer.
	*/
	HidD_GetPreparsedData(DeviceHandle, &PreparsedData);

	/*
	API function: HidP_GetCaps
	Learn the device's capabilities.
	For standard devices such as joysticks, you can find out the specific capabilities of the device.
	For a custom device, the software will probably know what the device is capable of, and the call only verifies the information.
	Requires: the pointer to the buffer returned by HidD_GetPreparsedData.
	Returns: a Capabilities structure containing the information.
	*/
	HidP_GetCaps(PreparsedData, &Capabilities);

	// No need for PreparsedData any more, so free the memory it's using
	HidD_FreePreparsedData(PreparsedData);
}

int Fusion::ReadDataFromHID()
{
  // Send the request command using ReadFile
  hEventObjectRead = CreateEvent(NULL,
                                 true,
                                 true,
                                 NULL);
  HIDOverlappedRead.hEvent = hEventObjectRead;
  HIDOverlappedRead.Offset = 0;
  HIDOverlappedRead.OffsetHigh = 0;
  Result = ReadFile(DeviceHandle,
                    &InputReport[0],
                    Capabilities.InputReportByteLength,
                    &NumberOfBytesRead,
                    (LPOVERLAPPED)&HIDOverlappedRead);
  Result = WaitForSingleObject(hEventObjectRead, 600);
  DWORD dw = GetLastError();
  return dw;
}

LRESULT CALLBACK WinProc(HWND hwnd, UINT msg, WPARAM wParam, LPARAM lParam)
{
  // Only process changes related to hardware configuration
  if (msg == WM_DEVICECHANGE)
  {
    // Get fusion device object
    Fusion* fusionObj = (Fusion*)GetWindowLong(hwnd, GWL_USERDATA);
    if (fusionObj)
    {
	    switch(wParam)
	    {
		    case DBT_DEVICEARRIVAL:
          // Check if the device is SpacePoint Fusion
          if (DeviceNameMatch(lParam) && fusionObj->iLastStatus != 1)
          {
            // Re-connect the SpacePoint Fusion device
            MMechostr(MSKDEBUG, "WinProc --> Data type [%d]\n", fusionObj->GetDataType());
            if (MyDeviceDetected == false)
              MyDeviceDetected = fusionObj->FindHID(FUSION_VID, FUSION_PID, iLastMode);    //fusionObj->GetDataType());     // iLastMode works !!!

            // Send a notification to Scol handle
            PostMessage(HScol, FUSION_CONNECTED_CB, (int)fusionObj, (LPARAM)NULL);
            fusionObj->iLastStatus = 1;
          }
			    break;
  	
		    case DBT_DEVICEREMOVECOMPLETE:
          // Check if the device is SpacePoint Fusion
          if (DeviceNameMatch(lParam) && fusionObj->iLastStatus != 2)
          {
            // Disconnect the SpacePoint Fusion device
            MyDeviceDetected = false;

            // Send a notification to Scol handle
            PostMessage(HScol, FUSION_DISCONNECTED_CB, (int)fusionObj, (LPARAM)NULL);
            fusionObj->iLastStatus = 2;
          }
          break;
  	
		    default:
			    break;
      }
    }
  }
  return 1;
}


bool DeviceNameMatch(LPARAM lParam)
{
	PDEV_BROADCAST_HDR lpdb = (PDEV_BROADCAST_HDR)lParam;
	if (lpdb->dbch_devicetype == DBT_DEVTYP_DEVICEINTERFACE) 
	{	
		PDEV_BROADCAST_DEVICEINTERFACE lpdbi = (PDEV_BROADCAST_DEVICEINTERFACE)lParam;
    std::string DeviceNameString;
		DeviceNameString = lpdbi->dbcc_name;

    // Format both strings before making comparison
    std::transform(MyDevicePathName.begin(), MyDevicePathName.end(), MyDevicePathName.begin(), (int(*)(int)) std::toupper);
    std::transform(DeviceNameString.begin(), DeviceNameString.end(), DeviceNameString.begin(), (int(*)(int)) std::toupper);

    if (DeviceNameString.compare(MyDevicePathName) == 0)
    {
      return true;    // The name matches
    }
    else
    {
      return false;   // It's a different device
    }
  }
	else
		return false;
}

/*-------------------------------- THREAD HANDLING --------------------------------*/
void Fusion::run() 
{
  try
  {
    while(isRunning)
    {
      // Parse data only if device has been opened
      if (bStatus)
      {
        // Parse orientation or raw data
        ParseData();

        // Process USB HID notifications
        bRet = PeekMessage(&msg, hwnd, 0, 0, PM_REMOVE);    // Non blocking procedure to get messages
        if (bRet)
        {
          TranslateMessage(&msg);   // Translate and dispatch messages to the WinProc
          DispatchMessage(&msg);
        }
      }
    }

    // Release connection to the SpacePoint Fusion device
    CloseDevice();
  }
  catch (ThreadException ex) 
  {
    MMechostr(MSKDEBUG, "error thread : %s\n", ex.getMessage().c_str());
  }
}