#include "stdafx.h"
#include "HSI.h"
#include "HSI_JoyStick.h"
#include "HSI_Motion.h"

#define SPEED_MIN   1
#define SPEED_MAX   4
#define XBOX_X_ANGLE    (M_PI / 180.0 * 18.0)
#define XBOX_Y_ANGLE    (M_PI / 180.0 * 72.0)
#define XBOX_XY_ANGLE_BEGIN   (M_PI / 180.0 * 20.0)
#define XBOX_XY_ANGLE_END   (M_PI / 180.0 * 70.0)
#define XBOX_STICKS_MIN_VALUE   (-128)
#define XBOX_STICKS_MAX_VALUE   (128)
#define XBOX_STICKS_TRIMMING_MAX_VALUE    (500)
#define XBOX_STICKS_TRIMMING_MIN_VALUE    (-500)
//HSI_Motion *g_pMotion = nullptr;
HSI_Joystick *g_pHSI_JoyStick = nullptr;
extern HSI_Motion *g_pHSI_Motion;

void SetThreadName(DWORD dwThreadID, const char* threadName)
{
#pragma pack(push,8)
	typedef struct tagTHREADNAME_INFO
	{
		DWORD dwType; // Must be 0x1000.
		LPCSTR szName; // Pointer to name (in user addr space).
		DWORD dwThreadID; // Thread ID (-1=caller thread).
		DWORD dwFlags; // Reserved for future use, must be zero.
	} THREADNAME_INFO;
#pragma pack(pop)
	const DWORD MS_VC_EXCEPTION = 0x406D1388;
	THREADNAME_INFO info;
	info.dwType = 0x1000;
	info.szName = threadName;
	info.dwThreadID = dwThreadID;
	info.dwFlags = 0;
#pragma warning(push)
#pragma warning(disable: 6320 6322)
	__try
	{
		RaiseException(MS_VC_EXCEPTION, 0, sizeof(info) / sizeof(ULONG_PTR), (ULONG_PTR*)&info);
	}
	__except (EXCEPTION_EXECUTE_HANDLER)
	{
	}
#pragma warning(pop)
}

void JoyStickThread(LPVOID ptr)
{
  SetThreadName(GetCurrentThreadId(),"JoyStick");
  HSI_Joystick *pJoystick=(HSI_Joystick *)ptr;
  while (true)
  {
    DWORD dwRetWaitSuspend=WaitForSingleObject(pJoystick->m_hSuspendEvent,0);
    if (dwRetWaitSuspend==WAIT_OBJECT_0)
    {
      DWORD dwRetWaitForMultiples=WaitForMultipleObjects(2,pJoystick->handles,false,INFINITE);
      switch (dwRetWaitForMultiples)
      {
      case WAIT_FAILED:
        ResetEvent(pJoystick->m_hSuspendEvent);
        ResetEvent(pJoystick->m_ResumeEvent);
        continue;
        break;
      case WAIT_OBJECT_0+0:
        ResetEvent(pJoystick->m_hSuspendEvent);
        ResetEvent(pJoystick->m_ResumeEvent);
        break;
      case WAIT_OBJECT_0+1:
        ResetEvent(pJoystick->m_hSuspendEvent);
        ResetEvent(pJoystick->m_ResumeEvent);
        SetEvent(pJoystick->m_hExitNotifyEvent);
        ExitThread(0);
        break;
      default:
        break;
      }
    }
    //do something
    pJoystick->UpdateJosstickData();
    DWORD dwRetWaitExit=WaitForSingleObject(pJoystick->m_hExitEvent,0);
    if (dwRetWaitExit==WAIT_OBJECT_0)
    {
      SetEvent(pJoystick->m_hExitNotifyEvent);
      ExitThread(0);
    }
  }
}

HSI_Joystick::HSI_Joystick(LPVOID ptr):
  m_hSuspendEvent(NULL),
  m_ResumeEvent(NULL),
  m_hExitEvent(NULL),
  m_hExitNotifyEvent(NULL),
  m_pXDirectInput(nullptr),
  m_eMoveState(MOVE_STATE_NONE),
  joystickControlState(nullptr)
{
  m_pXDirectInput=new CXDirectXInput();
  HRESULT hr = m_pXDirectInput->InitDirectInput();
  if(FAILED(hr))
  {
    if (m_pXDirectInput)
    {      
      m_pXDirectInput->FreeDirectInput();
      delete m_pXDirectInput;
      m_pXDirectInput = nullptr;
    }
  }
	//g_pMotion = (HSI_Motion*)ptr;
  memset(handles,0,2*sizeof(HANDLE));
  memset(joystickButtonStates,BUTTON_STATE_RELEASE,sizeof(joystickButtonStates));
  joystickSpeed=SPEED_MAX/2;
  joystickControlState=new JoystickControlState();
  memset(joystickControlState,0,sizeof(JoystickControlState));
  laserPenLastState=joystickButtonStates[BUTTON_LASER_PEN].buttonState;

  m_hSuspendEvent=CreateEvent(NULL,TRUE,FALSE,NULL);
  m_ResumeEvent=CreateEvent(NULL,TRUE,FALSE,NULL);
  m_hExitEvent=CreateEvent(NULL,TRUE,FALSE,NULL);
  m_hExitNotifyEvent=CreateEvent(NULL,TRUE,FALSE,NULL);
  handles[0]=m_ResumeEvent;
  handles[1]=m_hExitEvent;
  SuspendJoyStickThread();
  CreateJoyStickThread();
}

HSI_Joystick::~HSI_Joystick()
{
  DestoryJoyStickThread();
  if (m_hSuspendEvent)
  {
    CloseHandle(m_hSuspendEvent);
  }
  if (m_ResumeEvent)
  {
    CloseHandle(m_ResumeEvent);
  }
  if (m_hExitEvent)
  {
    CloseHandle(m_hExitEvent);
  }
  if (m_hExitNotifyEvent)
  {
    CloseHandle(m_hExitNotifyEvent);
  }
  if (m_pXDirectInput)
  {
    delete m_pXDirectInput;
    m_pXDirectInput=nullptr;
  }
  if (joystickControlState)
  {
    delete joystickControlState;
    joystickControlState=nullptr;
  }
}

void HSI_Joystick::UpdateJosstickData()
{
	short X(0), Y(0), Z(0);
  if (m_pXDirectInput==nullptr)
  {
    return;
  }
  HRESULT rstUpdateInputState=m_pXDirectInput->UpdateInputState();
  if(SUCCEEDED(rstUpdateInputState))
  {
    DIJOYSTATE2 state;
    bool succeed=m_pXDirectInput->GetState(state);
    if (!succeed)
    {
      return;
    }
		if (nullptr == g_pHSI_Motion)
		{
			return;
		}
    for (int buttonindex=0;buttonindex<BUTTON_ROTATETABLE_REFLECTOR;buttonindex++)
    {
      if (state.rgbButtons[buttonindex]!=joystickButtonStates[buttonindex].lastValue)
      {
        if (joystickButtonStates[buttonindex].lastValue==0)
        {
          if (buttonindex<BUTTON_SPEED_PLUS)
          {
            if (joystickButtonStates[buttonindex].buttonState==BUTTON_STATE_RELEASE)
            {
              joystickButtonStates[buttonindex].buttonState=BUTTON_STATE_PRESS;
							g_pHSI_Motion->StopJogEx(buttonindex+1);
            }
            else
            {
              joystickButtonStates[buttonindex].buttonState=BUTTON_STATE_RELEASE;
            }      
          }
        }
        joystickButtonStates[buttonindex].lastValue=state.rgbButtons[buttonindex];
      }
    }

		X = abs(state.lX);
		Y = abs(state.lY);
		Z = abs(state.lZ);
		static bool bMovingX(false), bMovingY(false), bMovingZ(false);
		if (BUTTON_STATE_RELEASE == joystickButtonStates[0].buttonState && X>0)
		{
			bMovingX = true;
			g_pHSI_Motion->JoyStick(HSI_MOTION_AXIS_X, state.lX);
		}
		else
		{
			if (bMovingX)
			{
				g_pHSI_Motion->StopJogEx(1);
				bMovingX = false;
			}
		}
		if (BUTTON_STATE_RELEASE == joystickButtonStates[1].buttonState && Y>0)
		{
			bMovingY = true;
			g_pHSI_Motion->JoyStick(HSI_MOTION_AXIS_Y, -state.lY);
		}
		else
		{
			if (bMovingY)
			{
				g_pHSI_Motion->StopJogEx(2);
				bMovingY = false;
			}
		}
		if (BUTTON_STATE_RELEASE == joystickButtonStates[2].buttonState && Z>0)
		{
			bMovingZ = true;
			g_pHSI_Motion->JoyStick(HSI_MOTION_AXIS_Z, -state.lZ);
		}
		else
		{
			if (bMovingZ)
			{
				g_pHSI_Motion->StopJogEx(3);
				bMovingZ = false;
			}
		}

  }
}

void HSI_Joystick::CreateJoyStickThread()
{
  m_hJoyStickThread=CreateThread((LPSECURITY_ATTRIBUTES)NULL,0,(LPTHREAD_START_ROUTINE)JoyStickThread,(LPVOID)this,0,NULL);
}

void HSI_Joystick::SuspendJoyStickThread()
{
  SetEvent(m_hSuspendEvent);
}

void HSI_Joystick::ResumeJoyStickThread()
{
  SetEvent(m_ResumeEvent);
}

void HSI_Joystick::DestoryJoyStickThread()
{
  SetEvent(m_hExitEvent);
  WaitForSingleObject(m_hExitNotifyEvent,INFINITE);
  if (m_hJoyStickThread)
  {
      CloseHandle(m_hJoyStickThread);
  }
}