// JoyStickDlg.cpp : 実装ファイル
//

#include "stdafx.h"
#include "Resource.h"
#include "JoyStick.h"
#include "JoyStickDlg.h"
#include "CAO_i.c"

#ifdef _DEBUG
#define new DEBUG_NEW
#endif

LPCTSTR CJoyStickDlg::m_conAppName = _T("JoyStick");

CComModule _Module;

// CJoyStickDlg ダイアログ
CJoyStickDlg::CJoyStickDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CJoyStickDlg::IDD, pParent)
{
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}

void CJoyStickDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	DDX_Control(pDX, IDC_COMBO_CONNECT, cboConnect);
	DDX_Control(pDX, IDC_BUTTON_CONNECT, btnConnect);
	DDX_Control(pDX, IDC_CHECK_JOYSTICK, chkJoystick);
	DDX_Control(pDX, IDC_STATIC_COLOR, pnlJoystick);
	DDX_Control(pDX, IDC_TEXT_ID, txtJoystick);
	DDX_Control(pDX, IDC_COMBO_INTERPOLATION, cboInterpolation);
	DDX_Control(pDX, IDC_BUTTON_MINUS1, btnMinus[0]);
	DDX_Control(pDX, IDC_BUTTON_MINUS2, btnMinus[1]);
	DDX_Control(pDX, IDC_BUTTON_MINUS3, btnMinus[2]);
	DDX_Control(pDX, IDC_BUTTON_MINUS4, btnMinus[3]);
	DDX_Control(pDX, IDC_BUTTON_MINUS5, btnMinus[4]);
	DDX_Control(pDX, IDC_BUTTON_MINUS6, btnMinus[5]);
	DDX_Control(pDX, IDC_BUTTON_MINUS7, btnMinus[6]);
	DDX_Control(pDX, IDC_BUTTON_MINUS8, btnMinus[7]);
	DDX_Control(pDX, IDC_BUTTON_PLUS1, btnPlus[0]);
	DDX_Control(pDX, IDC_BUTTON_PLUS2, btnPlus[1]);
	DDX_Control(pDX, IDC_BUTTON_PLUS3, btnPlus[2]);
	DDX_Control(pDX, IDC_BUTTON_PLUS4, btnPlus[3]);
	DDX_Control(pDX, IDC_BUTTON_PLUS5, btnPlus[4]);
	DDX_Control(pDX, IDC_BUTTON_PLUS6, btnPlus[5]);
	DDX_Control(pDX, IDC_BUTTON_PLUS7, btnPlus[6]);
	DDX_Control(pDX, IDC_BUTTON_PLUS8, btnPlus[7]);
	DDX_Control(pDX, IDC_LABEL_INCH_VALUE, lblInchValue);
	DDX_Control(pDX, IDC_TEXT_INCH_VALUE, txtInchValue);
	DDX_Control(pDX, IDC_BUTTON_INCH_SET, btnInchSet);
	DDX_Control(pDX, IDC_CHECK_INCHING, chkInching);
	DDX_Control(pDX, IDC_BUTTON_HELP, btnHelp);
}

BEGIN_MESSAGE_MAP(CJoyStickDlg, CDialog)
	ON_WM_PAINT()
	ON_WM_CTLCOLOR()
	ON_WM_QUERYDRAGICON()
	//}}AFX_MSG_MAP
	ON_WM_DESTROY()
	ON_WM_TIMER()
	ON_BN_CLICKED(IDC_BUTTON_CONNECT, &CJoyStickDlg::OnBnClickedButtonConnect)
	ON_BN_CLICKED(IDC_CHECK_JOYSTICK, &CJoyStickDlg::OnBnClickedCheckJoystick)
	ON_CBN_SELCHANGE(IDC_COMBO_INTERPOLATION, &CJoyStickDlg::OnCbnSelchangeComboInterpolation)
	ON_BN_CLICKED(IDC_BUTTON_HELP, &CJoyStickDlg::OnBnClickedButtonHelp)
	ON_BN_CLICKED(IDC_BUTTON_INCH_SET, &CJoyStickDlg::OnBnClickedButtonInchSet)
END_MESSAGE_MAP()


// CJoyStickDlg メッセージ ハンドラ

BOOL CJoyStickDlg::OnInitDialog()
{
	CDialog::OnInitDialog();

	// このダイアログのアイコンを設定します。アプリケーションのメイン ウィンドウがダイアログでない場合、
	//  Framework は、この設定を自動的に行います。
	SetIcon(m_hIcon, TRUE);			// 大きいアイコンの設定
	SetIcon(m_hIcon, FALSE);		// 小さいアイコンの設定

	CoInitialize(NULL);

	// Create CaoEngine
	CoCreateInstance(CLSID_CaoEngine, NULL, CLSCTX_LOCAL_SERVER, IID_ICaoEngine, (void**)&m_eng);

	// Get CaoWorkspaces
	m_eng->get_Workspaces(&m_wss);

	// Get CaoWorkspace
	m_wss->Item(CComVariant(0L), &m_ws);

	// Initialize cao objects as NULL
	m_ctrl = NULL;
	m_rob = NULL;
	m_curAng = NULL;
	m_curPos = NULL;
	m_robBusy = NULL;
	m_joy = NULL;
	m_event = NULL;

	// Set button information
	for(LONG i = 0L; i < 8L; i++){
		btnMinus[i].SetTargetHWND(this);
		btnMinus[i].SetIndex(-1L, i);

		btnPlus[i].SetTargetHWND(this);
		btnPlus[i].SetIndex(1L, i);
	}

	// Set combo boxes
	cboInterpolation.AddString(_T("1 - Joint"));
	cboInterpolation.AddString(_T("2 - X-Y"));
	cboInterpolation.SetCurSel(0);

	// Read ini file
	AppPath(m_tchFileName);
	LoadHistory();

	m_lManualInterporation = cboInterpolation.GetCurSel() + 1L;

	m_lManualType = 0;
	m_lManualJoint = 0;

	m_brGray.CreateSolidBrush(RGB(192, 192, 192));
	m_brRed.CreateSolidBrush(RGB(255, 0, 0));
	m_brCur = &m_brGray;

	ControlEnabled(FALSE);

	return TRUE;  // フォーカスをコントロールに設定した場合を除き、TRUE を返します。
}

// ダイアログに最小化ボタンを追加する場合、アイコンを描画するための
//  下のコードが必要です。ドキュメント/ビュー モデルを使う MFC アプリケーションの場合、
//  これは、Framework によって自動的に設定されます。

void CJoyStickDlg::OnPaint()
{
	if (IsIconic())
	{
		CPaintDC dc(this); // 描画のデバイス コンテキスト

		SendMessage(WM_ICONERASEBKGND, reinterpret_cast<WPARAM>(dc.GetSafeHdc()), 0);

		// クライアントの四角形領域内の中央
		int cxIcon = GetSystemMetrics(SM_CXICON);
		int cyIcon = GetSystemMetrics(SM_CYICON);
		CRect rect;
		GetClientRect(&rect);
		int x = (rect.Width() - cxIcon + 1) / 2;
		int y = (rect.Height() - cyIcon + 1) / 2;

		// アイコンの描画
		dc.DrawIcon(x, y, m_hIcon);
	}
	else
	{
		CDialog::OnPaint();
	}
}

HBRUSH CJoyStickDlg::OnCtlColor(CDC* pDC, CWnd* pWnd, UINT nCtlColor)
{
	HBRUSH hbr = CDialog::OnCtlColor(pDC, pWnd, nCtlColor);

	int ID = pWnd->GetDlgCtrlID();
	switch(ID) {
		case IDC_STATIC_COLOR:
			return *m_brCur;
	}

	return hbr;
}

// ユーザーが最小化したウィンドウをドラッグしているときに表示するカーソルを取得するために、
//  システムがこの関数を呼び出します。
HCURSOR CJoyStickDlg::OnQueryDragIcon()
{
	return static_cast<HCURSOR>(m_hIcon);
}

void CJoyStickDlg::OnDestroy()
{
	SaveHistory();

	Disconnect();

	// Release CaoWorkspace
	if(m_ws != NULL){
		m_ws->Release();
		m_ws = NULL;
	}

	// Release CaoWorkspaces
	if(m_wss != NULL){
		m_wss->Clear();
		m_wss->Release();
		m_wss = NULL;
	}

	// Release CaoEngine
	if(m_eng != NULL){
		m_eng->Release();
		m_eng = NULL;
	}

	CoUninitialize();

	CDialog::OnDestroy();
}

void CJoyStickDlg::OnTimer(UINT_PTR nIDEvent)
{
	HRESULT hr = S_OK;

	CComVariant pVal;
	m_robBusy->get_Value(&pVal);

	// If robot status is free, then move robot
	if(pVal.boolVal == VARIANT_FALSE){
		// Get current cboManualInterporation value
		m_lManualInterporation = cboInterpolation.GetCurSel() + 1L;

		DOUBLE dDev;
		TCHAR tchGetVal[10];
		lblInchValue.GetWindowText(tchGetVal, 10);

		dDev = _tstof(tchGetVal);

		LONG rgIndices;
		CComVariant vntPose, vntVarType, vntPath, vntPoseData;

		vntPoseData.vt = VT_VARIANT | VT_ARRAY;
		vntPoseData.parray = SafeArrayCreateVector(VT_VARIANT, 0L, 3L);

		if(m_lManualInterporation != 2){
			if(m_lManualJoint < 8L){
				m_fJnt[m_lManualJoint] = m_fJnt[m_lManualJoint] + (float) (m_lManualType * dDev);

				// Set destination angle
				{
					vntPose.vt = VT_R4 | VT_ARRAY;
					rgIndices = 0L;
					vntPose.parray = SafeArrayCreateVector(VT_R4, 0L, 8L);

					for(LONG i = 0L; i < 8L; i++){
						SafeArrayPutElement(vntPose.parray, &i, (void*)&m_fJnt[i]);
					}

					SafeArrayPutElement(vntPoseData.parray, &rgIndices, (void*)&vntPose);
				}

				// Set variable type
				{
					vntVarType.vt = VT_BSTR;
					rgIndices = 1L;
					vntVarType.bstrVal = SysAllocString(OLESTR("J"));
					SafeArrayPutElement(vntPoseData.parray, &rgIndices, (void*)&vntVarType);
				}

				// Set path
				{
					vntPath.vt = VT_BSTR;
					rgIndices = 2L;
					vntPath.bstrVal = SysAllocString(OLESTR("@P"));
					SafeArrayPutElement(vntPoseData.parray, &rgIndices, (void*)&vntPath);
				}
				
				hr = m_rob->Move(m_lManualInterporation, vntPoseData, CComBSTR(OLESTR("")));
			}
		}else{
			if(m_lManualJoint < 7L){
				m_fPos[m_lManualJoint] = m_fPos[m_lManualJoint] + (float) (m_lManualType * dDev);

				// Set destination position
				{
					vntPose.vt = VT_R4 | VT_ARRAY;
					rgIndices = 0L;
					vntPose.parray = SafeArrayCreateVector(VT_R4, 0L, 7L);

					for(LONG i = 0L; i < 7L; i++){
						SafeArrayPutElement(vntPose.parray, &i, (void*)&m_fPos[i]);
					}

					SafeArrayPutElement(vntPoseData.parray, &rgIndices, (void*)&vntPose);
				}

				// Set variable type
				{
					vntVarType.vt = VT_BSTR;
					rgIndices = 1L;
					vntVarType.bstrVal = SysAllocString(OLESTR("P"));
					SafeArrayPutElement(vntPoseData.parray, &rgIndices, (void*)&vntVarType);
				}

				// Set path
				{
					vntPath.vt = VT_BSTR;
					rgIndices = 2L;
					vntPath.bstrVal = SysAllocString(OLESTR("@P"));
					SafeArrayPutElement(vntPoseData.parray, &rgIndices, (void*)&vntPath);
				}

				hr = m_rob->Move(m_lManualInterporation, vntPoseData, CComBSTR(_T("")));
			}
		}

		if(FAILED(hr)){
			KillTimer(1);

			m_lManualType = 0L;
			m_lManualJoint = 0L;

			InitCurPos();

			ShowErrorNumber(hr);
		}
	}

	CDialog::OnTimer(nIDEvent);
}

void CJoyStickDlg::OnBnClickedButtonConnect()
{
	if(m_ctrl != NULL) {
		Disconnect();
	} else {
		Connect();
	}
}

void CJoyStickDlg::OnBnClickedCheckJoystick()
{
	if(m_joy != NULL) {
		DisconnectJoystick();
	} else {
		ConnectJoystick();
	}
}

void CJoyStickDlg::OnCbnSelchangeComboInterpolation()
{
	// Get current angle and posture
	if(m_lManualInterporation != cboInterpolation.GetCurSel() + 1L) {
		InitCurPos();
	}
}

void CJoyStickDlg::OnBnClickedButtonInchSet()
{
	DOUBLE dVal, dMinDev = 0.00001;
	TCHAR tchVal[20];

	txtInchValue.GetWindowText(tchVal, 20);

	dVal = _tstof(tchVal);

	// If tchGetVal can not convert to double, show error message
	if(dVal < dMinDev){
		MessageBox(_T("Can not set this value for inching."), _T("Error"), MB_OK);
	}else{
		_stprintf_s(tchVal, 10, _T("%.7g"), dVal);

        // Update lblInchValue
		lblInchValue.SetWindowText(tchVal);
	}
}

void CJoyStickDlg::OnBnClickedButtonHelp()
{
	MessageBox(_T("[JOINT]\n")
		_T("J<n> : Button [<n>] + [<-]/[->]\n")
		_T("[X-Y]\nX:     Button [1] + [<-] / [->]\n")
		_T("Y:     Button [2] + [<-] / [->]\n")
		_T("Z:     Button [3] + [<-] / [->]\n")
		_T("RX:   Button [4] + [<-] / [->]\n")
		_T("RY:   Button [5] + [<-] / [->]\n")
		_T("RZ:   Button [6] + [<-] / [->]"),
		_T("Help"), MB_OK);
}

void CJoyStickDlg::OnMessage(ICaoMessage* pIMsg)
{
	m_lManualInterporation = cboInterpolation.GetCurSel() + 1L;

	LONG lNumber;
	CComVariant pVal;

	pIMsg->get_Number(&lNumber);
	pIMsg->get_Value(&pVal);

	switch(lNumber){
		case 1: // "BUTTONS" '[8]...[2][1] each bit stands for each button
			if(pVal.vt == VT_I4){
				m_lManualType = 0L; // Do not move

				// If pressed "button 10", then change interporation
				if((pVal.lVal & 0x200) != 0){
					if(cboInterpolation.GetCurSel() != 0){
						cboInterpolation.SetCurSel(0);
					}else{
						cboInterpolation.SetCurSel(1);
					}

					m_lManualJoint = 0L;
				}
				else if((pVal.lVal & 0x1) != 0)
				{
					m_lManualJoint = 0L;
				}
				else if((pVal.lVal & 0x2) != 0)
				{
					m_lManualJoint = 1L;
				}
				else if((pVal.lVal & 0x4) != 0)
				{
					m_lManualJoint = 2L;
				}
				else if((pVal.lVal & 0x8) != 0)
				{
					m_lManualJoint = 3L;
				}
				else if((pVal.lVal & 0x10) != 0)
				{
					m_lManualJoint = 4L;
				}
				else if((pVal.lVal & 0x20) != 0)
				{
					m_lManualJoint = 5L;
				}
				else if((pVal.lVal & 0x40) != 0)
				{
					m_lManualJoint = 6L;
				}
				else if((pVal.lVal & 0x80) != 0)
				{
					m_lManualJoint = 7L;
				}
			}

			break;
		case 2:
		case 3: // "@X", "@Y"
			if(pVal.vt == VT_R8){
				// X = -1.0 〜 +1.0
				if((lNumber == 3) && (m_lManualInterporation == 2)){
					pVal.dblVal = -1 * pVal.dblVal;
				}

				{
					TCHAR tchDebug[16];
					_stprintf_s(tchDebug, 16, _T("@X/Y: %.8g"), pVal.dblVal);
					OutputDebugString(tchDebug);
				}

				if(pVal.dblVal == 1.0)
				{
					m_lManualType = 1;
					StartTimer();
				}
				else if(pVal.dblVal == -1.0)
				{
					m_lManualType = -1;
					StartTimer();
				}
				else {
					KillTimer(1);
				}					
			}

			break;
		case 6:
			if(pVal.vt == VT_I4){
				{
					TCHAR tchDebug[16];
					_stprintf_s(tchDebug, 16, _T("@POV: %d"), pVal.lVal);
					OutputDebugString(tchDebug);
				}

				m_lManualType = 0L;

                // arrow key up->right->down->left = 0 -> 9000 -> 18000 -> 27000
                // not pressed = 65535
				if(pVal.lVal != 65535){
					if(m_lManualInterporation == 2){
						if((pVal.lVal == 9000) || (pVal.lVal == 0)){ // right or up
							m_lManualType = 1L;
						}else if((pVal.lVal == 27000) || (pVal.lVal == 18000)){ // left or down
							m_lManualType = -1L;
						}
					}else{
						if((pVal.lVal == 9000) || (pVal.lVal == 18000)){ // right or down
							m_lManualType = 1L;
						}else if((pVal.lVal == 27000) || (pVal.lVal == 0)){ // left or up
							m_lManualType = -1L;
						}
					}

					StartTimer();
				} else {
					KillTimer(1);
				}
			}

			break;
	}
}

void CJoyStickDlg::SetManualButton(LONG lManualType, LONG lManualJoint)
{
	m_lManualType = lManualType;
	m_lManualJoint = lManualJoint;
}

void CJoyStickDlg::StartTimer()
{
	if(chkInching.GetCheck() == TRUE){
		OnTimer(1);
	}else{
		SetTimer(1, 10, NULL);
	}
}

VOID CJoyStickDlg::Connect()
{
	HRESULT hr;

	WCHAR strConn[_MAX_PATH];
	cboConnect.GetWindowTextW(strConn, _MAX_PATH);

	hr = m_ws->AddController(CComBSTR(_T("RC8")), CComBSTR(_T("CaoProv.DENSO.RC8")), CComBSTR(_T("")), CComBSTR(strConn), &m_ctrl);

	if(FAILED(hr)){
		ShowErrorNumber(hr);
		Disconnect();

		return;
	}

	m_ctrl->AddRobot(CComBSTR(_T("Rob")), CComBSTR(_T("")), &m_rob);

	LONG rgIndices, lVal;
	CComVariant vntParam, vntReturn;
	vntParam.vt = VT_I4 | VT_ARRAY;
	vntParam.parray = SafeArrayCreateVector(VT_I4, 0L, 2L);

	// Get arm control authority
	{
		rgIndices = 0L; lVal = 0L;
		SafeArrayPutElement(vntParam.parray, &rgIndices, (void*)&lVal);

		rgIndices = 1L; lVal = 1L;
		SafeArrayPutElement(vntParam.parray, &rgIndices, (void*)&lVal);

		hr = m_rob->Execute(CComBSTR(_T("Takearm")), vntParam, &vntReturn);

		if(FAILED(hr)){
			ShowErrorNumber(hr);
			Disconnect();

			return;
		}
	}

	// Motor on
	{
		rgIndices = 0L; lVal = 1L;
		SafeArrayPutElement(vntParam.parray, &rgIndices, (void*)&lVal);

		rgIndices = 1L; lVal = 0L;
		SafeArrayPutElement(vntParam.parray, &rgIndices, (void*)&lVal);

		hr = m_rob->Execute(CComBSTR(_T("Motor")), vntParam, &vntReturn);

		if(FAILED(hr)){
			ShowErrorNumber(hr);
			Disconnect();

			return;
		}
	}

    // Get variable object for watching current angle and posture
	m_rob->AddVariable(CComBSTR(_T("@CURRENT_ANGLE")), CComBSTR(_T("")), &m_curAng);
	m_rob->AddVariable(CComBSTR(_T("@CURRENT_POSITION")), CComBSTR(_T("")), &m_curPos);

    // Get variable object for watching robot status
	m_rob->AddVariable(CComBSTR(_T("@BUSY_STATUS")), CComBSTR(_T("")), &m_robBusy);

	btnConnect.SetWindowText(_T("&Disconnect"));
	ControlEnabled(TRUE);	// Enable components

	AddConnHistory(strConn);

	InitCurPos();   // Get current angle and posture
}

VOID CJoyStickDlg::Disconnect()
{
	DisconnectJoystick();

	// Release robot object
	if(m_rob != NULL){
		LONG rgIndices, lVal;
		CComVariant vntParam, vntReturn;
		vntParam.vt = VT_I4 | VT_ARRAY;
		vntParam.parray = SafeArrayCreateVector(VT_I4, 0L, 2L);

		// Release robot variable objects
		{
			if(m_curPos != NULL){
				m_curPos->Release();
				m_curPos = NULL;
			}

			if(m_curAng != NULL){
				m_curAng->Release();
				m_curAng = NULL;
			}

			if(m_robBusy != NULL){
				m_robBusy->Release();
				m_robBusy = NULL;
			}
		}

		// Motor off
		{
			rgIndices = 0L; lVal = 0L;
			SafeArrayPutElement(vntParam.parray, &rgIndices, (void*)&lVal);

			rgIndices = 1L; lVal = 0L;
			SafeArrayPutElement(vntParam.parray, &rgIndices, (void*)&lVal);

			m_rob->Execute(CComBSTR(_T("Motor")), vntParam, &vntReturn);
		}

		// Release arm control authority
		{
			m_rob->Execute(CComBSTR(_T("Givearm")), CComVariant(), &vntReturn);
		}

		m_rob->Release();
		m_rob = NULL;
	}

	// Release controller object
	if(m_ctrl != NULL){
		{
			CComVariant vntIndex;
			CComPtr<ICaoControllers> ctrls;

			m_ws->get_Controllers(&ctrls);

			vntIndex.vt = VT_I4;
			m_ctrl->get_Index(&vntIndex.lVal);

			ctrls->Remove(vntIndex);
		}

		m_ctrl->Release();
		m_ctrl = NULL;
	}

	btnConnect.SetWindowText(_T("&Connect"));
	ControlEnabled(FALSE); // Disable components
}

VOID CJoyStickDlg::ConnectJoystick()
{
	HRESULT hr;

	WCHAR strConn[10];
	txtJoystick.GetWindowTextW(strConn, 10);

	CComBSTR bstrConn(_T("ID="));
	bstrConn.Append(strConn);

	hr = m_ws->AddController(CComBSTR(_T("JoyCtrl")), CComBSTR(_T("CaoProv.DENSO.Joystick")), CComBSTR(_T("")), bstrConn, &m_joy);

	if(FAILED(hr)){
		chkJoystick.SetCheck(FALSE);
		ShowErrorNumber(hr);

		return;
	}

	CComObject<CEventSink>::CreateInstance(&m_event);
	m_event->SetTargetHWND(this);
	m_event->AddRef();
	m_event->DispEventAdvise(m_joy, &DIID__ICaoControllerEvents);

	m_brCur = &m_brRed;
	ChangeColor();
}

VOID CJoyStickDlg::DisconnectJoystick()
{
	m_brCur = &m_brGray;
	ChangeColor();

	KillTimer(1);

	if(m_event != NULL){
		m_event->DispEventUnadvise(m_joy, &DIID__ICaoControllerEvents);
		m_event->Release();
		m_event = NULL;
	}

	if(m_joy != NULL){
		{
			CComVariant vntIndex;
			CComPtr<ICaoControllers> ctrls;

			m_ws->get_Controllers(&ctrls);

			vntIndex.vt = VT_I4;
			m_joy->get_Index(&vntIndex.lVal);

			ctrls->Remove(vntIndex);
		}

		m_joy->Release();
		m_joy = NULL;
	}
}

VOID CJoyStickDlg::ChangeColor()
{
	CRect rcColor;
	pnlJoystick.GetWindowRect(rcColor);
	ScreenToClient(rcColor);
	InvalidateRect(rcColor);
}

VOID CJoyStickDlg::ShowErrorNumber(HRESULT hr)
{
	TCHAR tchVal[9];

	_stprintf_s(tchVal, 9, _T("%X"), hr);

	MessageBox(tchVal, _T("Error Number"), MB_OK);
}

VOID CJoyStickDlg::ControlEnabled(BOOL bEnable)
{
	// Interporation
	cboInterpolation.EnableWindow(bEnable);

	// Joystick
	chkJoystick.EnableWindow(bEnable);
	txtJoystick.EnableWindow(bEnable);
	
	// btnMinus, btnPlus
	for(LONG i = 0L; i < 8L; i++) {
		btnMinus[i].EnableWindow(bEnable);
		btnPlus[i].EnableWindow(bEnable);
	}

	// Deviation
	lblInchValue.EnableWindow(bEnable);
	txtInchValue.EnableWindow(bEnable);
	btnInchSet.EnableWindow(bEnable);
	chkInching.EnableWindow(bEnable);
}

VOID CJoyStickDlg::InitCurPos()
{
	LONG rgIndices;
	DOUBLE dTmp;
	CComVariant pVal;

	// Get current angle
	if(m_curAng != NULL){
		m_curAng->get_Value(&pVal);

		for(rgIndices = 0L; rgIndices < 8L; rgIndices++){
			SafeArrayGetElement(pVal.parray, &rgIndices, (void*)&dTmp);
			m_fJnt[rgIndices] = (FLOAT) dTmp;
		}
	}

	// Get current position
	if(m_curPos != NULL){
		m_curPos->get_Value(&pVal);

		for(rgIndices = 0L; rgIndices < 8L; rgIndices++){
			SafeArrayGetElement(pVal.parray, &rgIndices, (void*)&dTmp);
			m_fPos[rgIndices] = (FLOAT) dTmp;
		}
	}
}

VOID CJoyStickDlg::AppPath(LPTSTR lpIniFullPath)
{
	TCHAR tchExePath[_MAX_PATH], tchDrive[_MAX_DRIVE], tchDir[_MAX_DIR];

	GetModuleFileName(NULL, tchExePath, _MAX_PATH);
	_tsplitpath_s(tchExePath, tchDrive, _MAX_DRIVE, tchDir, _MAX_DIR, NULL, 0, NULL, 0);
	_tmakepath_s(lpIniFullPath, _MAX_PATH, tchDrive, tchDir, _T("Connect"), _T(".ini"));
}

VOID CJoyStickDlg::AddConnHistory(LPCTSTR lpConn)
{
	LONG lCurSel;
	lCurSel = cboConnect.FindStringExact(-1, lpConn);

	if (lCurSel == CB_ERR)
	{
		cboConnect.InsertString(0, lpConn);
	}
	else
	{
		cboConnect.DeleteString(lCurSel);
		cboConnect.InsertString(0, lpConn);
	}

	if(cboConnect.GetCount() > 10){
		cboConnect.DeleteString(10);
	}

	cboConnect.SetCurSel(0);
}

VOID CJoyStickDlg::LoadHistory()
{
	TCHAR tchConn[_MAX_PATH];
	TCHAR tchKey[14];
	DWORD dwReturn;

	for(LONG lLoop = 0L; lLoop < 10L; lLoop++)
	{
		_stprintf_s(tchKey, 14, _T("ConnHistory%d"), lLoop + 1);

		dwReturn = GetPrivateProfileString(m_conAppName, tchKey, _T(""), tchConn, _MAX_PATH, m_tchFileName);

		if (dwReturn != 0L)
		{
			cboConnect.AddString(tchConn);
		}
	}

	if (cboConnect.GetCount() == 0)
	{
		cboConnect.AddString(_T("WPJ=*"));
	}

	cboConnect.SetCurSel(0);

	GetPrivateProfileString(m_conAppName, _T("Deviation"), _T("0.1"), tchConn, _MAX_PATH, m_tchFileName);
	lblInchValue.SetWindowText(tchConn);
}

VOID CJoyStickDlg::SaveHistory()
{
	TCHAR tchConn[_MAX_PATH];
	TCHAR tchKey[14];

	for(LONG lLoop = 0L; lLoop < 10L && lLoop < cboConnect.GetCount(); lLoop++)
	{
		_stprintf_s(tchKey, 14, _T("ConnHistory%d"), lLoop + 1);
		cboConnect.GetLBText(lLoop, tchConn);

		WritePrivateProfileString(m_conAppName, tchKey, tchConn, m_tchFileName);
	}

	lblInchValue.GetWindowText(tchConn, _MAX_PATH);
	WritePrivateProfileString(m_conAppName, _T("Deviation"), tchConn, m_tchFileName); 
}