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


// Standard libraries
#include <math.h>
#include <iostream>
#include <windows.h>
#include <conio.h>
#include <algorithm>
#include <cctype>

// WDK libraries for USB HID devices
extern "C" 
{
  #include "../hid/hidsdi.h"
  #include <setupapi.h>
  #include <dbt.h>
}


// Threading library
#include "..\lib\ou_thread.h"

// Namespaces
using namespace std;
using namespace openutils;

// SpacePoint Fusion product and vendor IDs
#define FUSION_VID        0x20ff
#define FUSION_PID        0x0100

// SpacePoint Fusion available modes
#define RAW_DATA          0
#define ORIENTATION_DATA  1

// Function prototypes
LRESULT CALLBACK WinProc(HWND, UINT, WPARAM, LPARAM);
bool DeviceNameMatch(LPARAM);

// Class to handle Quaternions
class Quaternion
{
  public:
    Quaternion()
    {
      w = 1.0f;
      x = 0.0f;
      y = 0.0f;
      z = 0.0f;
    }
  
    Quaternion(float ww, float xx, float yy, float zz)
    {
			w = ww;
      x = xx;
      y = yy;
      z = zz;
    }

    //-----------------------------------------------------------------------
    Quaternion Quaternion::operator+ (const Quaternion& rkQ) const
    {
      return Quaternion(w+rkQ.w,x+rkQ.x,y+rkQ.y,z+rkQ.z);
    }
    //-----------------------------------------------------------------------
    Quaternion Quaternion::operator- (const Quaternion& rkQ) const
    {
      return Quaternion(w-rkQ.w,x-rkQ.x,y-rkQ.y,z-rkQ.z);
    }
    //-----------------------------------------------------------------------
    Quaternion Quaternion::operator* (const Quaternion& rkQ) const
    {
      // NOTE:  Multiplication is not generally commutative, so in most cases p*q != q*p
      return Quaternion
      (
        w * rkQ.w - x * rkQ.x - y * rkQ.y - z * rkQ.z,
        w * rkQ.x + x * rkQ.w + y * rkQ.z - z * rkQ.y,
        w * rkQ.y + y * rkQ.w + z * rkQ.x - x * rkQ.z,
        w * rkQ.z + z * rkQ.w + x * rkQ.y - y * rkQ.x
      );
    }

    Quaternion Quaternion::Inverse () const
    {
      float fNorm = w*w+x*x+y*y+z*z;
      if (fNorm > 0.0)
      {
        float fInvNorm = 1.0f/fNorm;
        return Quaternion(w*fInvNorm,-x*fInvNorm,-y*fInvNorm,-z*fInvNorm);
      }
      else
      {
        // return an invalid result to flag the error
        return Quaternion();
      }
    }

  protected:
  private:

  public:
	  float x;
    float y;
    float z;
    float w;
  protected:
  private:
};

// SpacePoint Fusion object
class MyThread;
class Fusion : public Thread 
{
  public:
    // Public attributes
    int* iRawAxes;                // Raw has 9 axes, buttons, PNI byte
    float* fAcceleration;         // Acceleration values (0=X, 1=Y and 2=Z)
    Quaternion fQuaternion;       // Quaternion values
    int* iButton;                 // Button status (0=Left, 1=Right)

    // Constructor and destructor
    Fusion();
    ~Fusion();

    // Getters and setters
    bool GetConnected();
    int GetNbSamples();
    int GetVID();
    int GetPID();
    float* GetAccelerationValues();
    float* GetQuaternionValues();
    int* GetButtonValues();
    int GetDataType();

    void SetConnected(bool status);
    void SetNbSamples(int nbSamples);
    void SetVID(int vendorId);
    void SetPID(int productId);
    void SetAccelerationValues(float* accel);
    void SetQuaternionValues(float* quaternion);
    void SetButtonValues(int* buttons);
    void SetDataType(int dataType);

    // Other methods
    bool OpenDevice(int dataType);
    void CloseDevice();
    void CloseHandles();
    bool ParseData();    // Parse data read from Fusion device into acceleration and quaternion values

    // USB HID methods
    bool FindHID(unsigned int vendorId, unsigned int productId, unsigned int interfaceMode);
    void RegisterForDeviceNotifications();
    void GetDeviceCapabilities();
    int ReadDataFromHID();

    // Thread Handling
    void run();
    bool isRunning;
    HWND hwnd;        // Window handle
    int iLastStatus;  // Connection status

  private:
    // Private attributes
    bool bConnected;
    int iNbSamples;
    int iVID;
    int iPID;
    bool bStatus;
    int iDataType;
};