#include "stdafx.h"
#include "Robot.h"
#include "RobotDlg.h"

#ifdef _DEBUG
#define new DEBUG_NEW
#undef THIS_FILE
static char THIS_FILE[] = __FILE__;
#endif

#include "CAO_i.c"

/////////////////////////////////////////////////////////////////////////////

CRobotDlg::CRobotDlg(CWnd* pParent /*=NULL*/)
	: CDialog(CRobotDlg::IDD, pParent)
{
	//{{AFX_DATA_INIT(CRobotDlg)
	//}}AFX_DATA_INIT

	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}

void CRobotDlg::DoDataExchange(CDataExchange* pDX)
{
	CDialog::DoDataExchange(pDX);
	//{{AFX_DATA_MAP(CRobotDlg)
		DDX_Control(pDX, IDC_CMB_PARAM, m_cmbParam);

		DDX_Control(pDX, IDC_CMB_COMP, m_cmbComp);
		DDX_Control(pDX, IDC_CMB_PATH, m_cmbPath);
		DDX_Control(pDX, IDC_CMB_TYPE, m_cmbType);
		DDX_Control(pDX, IDC_CHECK_MOVE_NEXT, m_optMove);
		DDX_Text(pDX, IDC_EDIT_VARINDEX, m_editVarIndex);

		DDX_Control(pDX, IDC_CMB_CHANGE, m_cmbChange);
		DDX_Text(pDX, IDC_EDIT_CHANGE, m_editChange);

		DDX_Text(pDX, IDC_EDIT_SPEED, m_editSpeed);

		DDX_Control(pDX, IDC_CMB_DRVAT, m_cmbDrvat);
		DDX_Control(pDX, IDC_CMB_AXIS, m_cmbAxis);
		DDX_Control(pDX, IDC_CHECK_DRIVE_NEXT, m_optDrive);
		DDX_Text(pDX, IDC_EDIT_DRIVE, m_editDrive);

		DDX_Control(pDX, IDC_CMB_ROTATE, m_cmb_Rotate);
		DDX_Text(pDX, IDC_EDIT_ROTDEG, m_editRotdeg);
		DDX_Text(pDX, IDC_EDIT_ROTATE1, m_editRotate[0]);
		DDX_Text(pDX, IDC_EDIT_ROTATE2, m_editRotate[1]);
		DDX_Text(pDX, IDC_EDIT_ROTATE3, m_editRotate[2]);
		DDX_Text(pDX, IDC_EDIT_ROTATE4, m_editRotate[3]);
		DDX_Control(pDX, IDC_CHECK_ROTATE_NEXT, m_optRotate);
		DDX_Radio(pDX, IDC_OPT_SUF1, m_optSuf);
		DDX_Control(pDX, IDC_CMB_SUF, m_cmbSuf);

		DDX_Control(pDX, IDC_APPROACH_COMP, m_cmbApproachComp);
		DDX_Control(pDX, IDC_APPROACH_TYPE, m_cmbApproachType);
		DDX_Text(pDX, IDC_APPROACH_VARINDEX, m_nApproachIndex);
		DDX_Control(pDX, IDC_APPROACH_PATH, m_cmbApproachPath);
		DDX_Text(pDX, IDC_APPROACH_LENGTH, m_fApproachLength);
		DDX_Control(pDX, IDC_CHECK_APPROACH_NEXT, m_optApproach);

		DDX_Control(pDX, IDC_DEPART_COMP, m_cmbDepartComp);
		DDX_Control(pDX, IDC_DEPART_PATH, m_cmbDepartPath);
		DDX_Text(pDX, IDC_DEPART_LENGTH, m_fDepartLength);
		DDX_Control(pDX, IDC_CHECK_DEPART_NEXT, m_optDepart);

		DDX_Control(pDX, IDC_GO, m_cmdGo);
		DDX_Control(pDX, IDC_CHANGE, m_cmdChange);
		DDX_Control(pDX, IDC_SPEED, m_cmdSpeed);
		DDX_Control(pDX, IDC_DRIVE, m_cmdDrive);
		DDX_Control(pDX, IDC_ROTATE, m_cmdRotate);
		DDX_Control(pDX, IDC_APPROACH, m_cmdApproach);
		DDX_Control(pDX, IDC_DEPART, m_cmdDepart);
		DDX_Control(pDX, IDC_CANCEL, m_cmdCancel);
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CRobotDlg, CDialog)
	//{{AFX_MSG_MAP(CRobotDlg)
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	ON_WM_DESTROY()
	ON_BN_CLICKED(IDC_CONNECT, OnConnect)
	ON_BN_CLICKED(IDC_DISCONNECT, OnDisconnect)
	ON_BN_CLICKED(IDC_GO, OnRobMaster)
	ON_BN_CLICKED(IDC_CHANGE, OnRobMaster)
	ON_BN_CLICKED(IDC_SPEED, OnRobMaster)
	ON_BN_CLICKED(IDC_DRIVE, OnRobMaster)
	ON_BN_CLICKED(IDC_ROTATE, OnRobMaster)
	ON_BN_CLICKED(IDC_APPROACH, OnRobMaster)
	ON_BN_CLICKED(IDC_DEPART, OnRobMaster)
	ON_BN_CLICKED(IDC_CANCEL, OnCmdCancel)
	ON_WM_TIMER()
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////

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

	SetIcon(m_hIcon, TRUE);
	SetIcon(m_hIcon, FALSE);

	HRESULT	hr = S_OK;
	CoInitializeEx(NULL, COINIT_MULTITHREADED);
	
	// Create CaoEngine object
	hr = CoCreateInstance(CLSID_CaoEngine, NULL, CLSCTX_LOCAL_SERVER, IID_ICaoEngine, (void**)&m_pIEng);
	if (FAILED(hr)) {
		return FALSE;
	}

	// Get Workespaces object
	hr = m_pIEng->get_Workspaces(&m_pIWss);
	if (FAILED(hr)) {
		return FALSE;
	}

	// Get Workspaces(0)
	hr = m_pIWss->Item(CComVariant(0L), &m_pIWs);
	if (FAILED(hr)) {
		return FALSE;
	}

	// Get CaoControllers object
	hr = m_pIWs->get_Controllers(&m_pICtrls);
	if (FAILED(hr)) {
		return FALSE;
	}

	// <----- Initialize interfaces
	m_bThreadFlag	= FALSE;
	m_lThreadSwitch = 0;
	m_pThread		= NULL;

	m_cmbParam.SetCurSel(0);
	m_cmbComp.SetCurSel(0);
	m_cmbType.SetCurSel(0);
	m_editVarIndex = 1;

	m_cmbChange.SetCurSel(0);
	m_editChange = 1;

	m_editSpeed  = 80.0f;

	m_cmbAxis.SetCurSel(0);
	m_editDrive  = 30.0f;

	m_editRotate[0] = "1";
	m_editRotate[1] = "2";
	m_editRotate[2] = "3";
	m_editRotate[3] = "0";

	m_editRotdeg    = 30.0;

	m_optSuf        = 0;
	m_cmbSuf.SetCurSel(0);

	m_cmbApproachComp.SetCurSel(0);
	m_cmbApproachType.SetCurSel(0);
	m_nApproachIndex  = 0;
	m_fApproachLength = 0;

	m_cmbDepartComp.SetCurSel(0);
	m_fDepartLength = 0;

	m_cmdGo.EnableWindow(FALSE);
	m_cmdChange.EnableWindow(FALSE);
	m_cmdSpeed.EnableWindow(FALSE);
	m_cmdDrive.EnableWindow(FALSE);
	m_cmdRotate.EnableWindow(FALSE);
	m_cmdApproach.EnableWindow(FALSE);
	m_cmdDepart.EnableWindow(FALSE);
	m_cmdCancel.EnableWindow(FALSE);
	// Initialize interfaces ----->

	// Start watch timer
	SetTimer(ID_TIMER, 1000, NULL);

	UpdateData(FALSE);
	
	return TRUE;
}

void CRobotDlg::OnDestroy() 
{
	CDialog::OnDestroy();

	// Disconnect form controller
	OnDisconnect();

	// Release controller object
	m_pICtrls.Release();
	// Release workspace object
	m_pIWs.Release();
	// Release workspaces object
	m_pIWss.Release();
	// Release engine object
	m_pIEng.Release();
}

void CRobotDlg::OnPaint() 
{
	if (IsIconic())
	{
		CPaintDC dc(this);

		SendMessage(WM_ICONERASEBKGND, (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();
	}
}

HCURSOR CRobotDlg::OnQueryDragIcon()
{
	return (HCURSOR) m_hIcon;
}

void CRobotDlg::OnConnect() 
{
	CString					strParam;
	CComPtr<ICaoController>	pICtrl;
	CComPtr<ICaoRobot>		pIRobot;
	HRESULT					hr;

	OnDisconnect();

	// Get connection parameter
	UpdateData();
	if(m_cmbParam.GetCurSel() < 0)
		m_cmbParam.GetWindowText(strParam);
	else
		m_cmbParam.GetLBText(m_cmbParam.GetCurSel(), strParam);

	m_bstrParam = CComBSTR("Server=" + strParam);

	// Connect to controller
	hr = m_pICtrls->Add(CComBSTR(L"Sample"), CComBSTR(L"CaoProv.DENSO.RC8"), CComBSTR(L""), m_bstrParam, &pICtrl);
	if(FAILED(hr)) {
		AfxTrace("Error : AddController( %X )\n", hr);
		return;
	}

	// Get robot object
	hr = pICtrl->AddRobot(CComBSTR(L"RobWatch"), CComBSTR(L""), &pIRobot);
	if(FAILED(hr)) {
		AfxTrace("Error : AddRobot( %X )\n", hr);
		return;
	}

	// Get variable object for watching robot status
	hr = pIRobot->AddVariable(CComBSTR(L"@BUSY_STATUS"), CComBSTR(L""), &m_pIBusyStatusVariable);
	if(FAILED(hr)) {
		AfxTrace("Error : AddVariable( %X )\n", hr);
		return;
	}

	// Get variable object for watching current position
	hr = pIRobot->AddVariable(CComBSTR(L"@CURRENT_POSITION"), CComBSTR(L""), &m_pICurrentPositionVariable);
	if(FAILED(hr)) {
		AfxTrace("Error : AddVariable( %X )\n", hr);
		return;
	}

	// Start a thread which executes robot actions (move, approach, depart, etc.)
	m_bThreadFlag = TRUE;
	m_pThread = AfxBeginThread(CRobotDlg::thread_RobMaster, (LPVOID)this);

	ButtonEnable(TRUE);
	m_cmdCancel.EnableWindow(TRUE);
}

void CRobotDlg::OnDisconnect() 
{
	MSG		msg;
	DWORD	dw;
	long	lCtrlCount;
	HRESULT	hr;

	// Stop thread: thread_RobMaster
	m_bThreadFlag = FALSE;

	// Wait until thread_RobMaster stops
	if(m_pThread){
		while(true){
			dw = WaitForSingleObject(m_pThread->m_hThread, 500);
			if(dw == WAIT_TIMEOUT){
				while(PeekMessage(&msg, NULL, NULL, NULL, PM_REMOVE)){
					DispatchMessage(&msg);
				}
			}else{
				break;
			}
		}
		m_pThread = NULL;
	}

	// Get the number of connected controllers
	hr = m_pICtrls->get_Count(&lCtrlCount);
	if(FAILED(hr)){
		AfxTrace("Error : get_Count( %X )\n", hr);
		return;
	}

	// If the connected controller exists, then disconnect from the controller
	if(lCtrlCount > 0){
		m_pIBusyStatusVariable.Release();
		m_pICurrentPositionVariable.Release();
		m_pICtrls->Clear();
	}

	ButtonEnable(FALSE);
	m_cmdCancel.EnableWindow(FALSE);
}

void CRobotDlg::OnRobMaster()
{
	const MSG* CurMsg;

	UpdateData();

	// Get ID of the pressed button
	CurMsg = GetCurrentMessage();
	m_lThreadSwitch = LOWORD(CurMsg->wParam);
}

UINT CRobotDlg::thread_RobMaster(LPVOID pParam)
{
	HRESULT hr;
	CComVariant					vntIndex;
	CComPtr<ICaoController>		pICtrl;
	CComPtr<ICaoRobot>			pIRobot;
	CRobotDlg*					pThis = (CRobotDlg*)pParam;

	// Connect to controller
	hr = pThis->m_pICtrls->Add(CComBSTR(L"Thread"), CComBSTR(L"CaoProv.DENSO.RC8"), CComBSTR(L""), pThis->m_bstrParam, &pICtrl);
	if(FAILED(hr)) {
		AfxTrace("Error : AddController( %X )\n", hr);
		goto ExitProc;
	}

	// Get robot object
	hr = pICtrl->AddRobot(CComBSTR(L"RobAction"), CComBSTR(L""), &pIRobot);
	if(FAILED(hr)) {
		AfxTrace("Error : AddRobot( %X )\n", hr);
		goto ExitProc;
	}

	// Get arm control authority
	hr = pThis->ExecuteTakearm(pIRobot);
	if(FAILED(hr)) {
		AfxTrace("Error : Takearm( %X )\n", hr);
		goto ExitProc;
	}

	while(pThis->m_bThreadFlag){
		if(pThis->m_lThreadSwitch){
			pThis->ButtonEnable(FALSE);

			switch (pThis->m_lThreadSwitch) {
				case IDC_GO:
					pThis->OnGo(pIRobot);
					break;
				case IDC_CHANGE:
					pThis->OnChange(pIRobot);
					break;
				case IDC_SPEED:
					pThis->OnSpeed(pIRobot);
					break;
				case IDC_DRIVE:
					pThis->OnDrive(pIRobot);
					break;
				case IDC_ROTATE:
					pThis->OnRotate(pIRobot);
					break;
				case IDC_APPROACH:
					pThis->OnApproach(pIRobot);
					break;
				case IDC_DEPART:
					pThis->OnDepart(pIRobot);
					break;
			}

			pThis->ButtonEnable(TRUE);
			pThis->m_lThreadSwitch = 0;
		}
	}

ExitProc:
	pThis->ExecuteGivearm(pIRobot);

	if(pICtrl){
		vntIndex.vt = VT_I4;
		pICtrl->get_Index(&vntIndex.lVal);
		pThis->m_pICtrls->Remove(vntIndex);
	}

	return 0;
}

void CRobotDlg::OnGo(ICaoRobot *pIRobot) 
{
	CString				strPath, strType, strVar;
	CComBSTR			bstrPose;
	HRESULT				hr;

	if(pIRobot){
		m_cmbPath.GetWindowText(strPath);
		m_cmbType.GetWindowText(strType);
		strVar.Format("%d", m_editVarIndex);

		hr = pIRobot->Move(m_cmbComp.GetCurSel() + 1, CComVariant(strPath + " " + strType + strVar), (m_optMove.GetCheck() == 1)? CComBSTR(L"NEXT") : CComBSTR(L""));
		if(FAILED(hr)) {
			AfxTrace("Error : Move( %X )\n", hr);
		}
	}

}

void CRobotDlg::OnChange(ICaoRobot *pIRobot) 
{
	CString				strChange, strWork;
	CComBSTR			bstrChange;
	HRESULT				hr;

	if(pIRobot){
		m_cmbChange.GetWindowText(strChange);
		strWork.Format("%d", m_editChange);
		
		hr = pIRobot->Change(CComBSTR(strChange + strWork));
		if(FAILED(hr)) {
			AfxTrace("Error : Accelerate( %X )\n", hr);
		}
	}
}

void CRobotDlg::OnSpeed(ICaoRobot *pIRobot) 
{
	HRESULT	hr;

	if(pIRobot){
		hr = pIRobot->Speed(-1, m_editSpeed);
		if(FAILED(hr)) {
			AfxTrace("Error : Speed( %X )\n", hr);
		}
	}
}

void CRobotDlg::OnDrive(ICaoRobot *pIRobot) 
{
	CString				strOption, strWork;

	SAFEARRAYBOUND		rgb[1];
	SAFEARRAY*			psa;
	CComVariant*		pVarData;

	CComVariant			pData, pResult;
	HRESULT				hr;

	if(pIRobot){
		m_cmbDrvat.GetWindowText(strWork);	strOption = strWork + " ";
		strOption += "(";
		m_cmbAxis.GetWindowText(strWork);	strOption += strWork;
		strWork.Format(",%f", m_editDrive);	strOption += strWork;
		strOption += ")";

		if(m_optDrive.GetCheck() == 1){
			rgb[0].cElements = 2;
			rgb[0].lLbound   = 0;

			if(psa = SafeArrayCreate(VT_VARIANT, 1, rgb)){
				hr = SafeArrayAccessData(psa, (void **)&pVarData);
				if(SUCCEEDED(hr)){
					pVarData[0] = static_cast<LPCTSTR>(strOption);
					pVarData[1] = L"NEXT";

					pData.parray = psa;
					pData.vt     = VT_ARRAY | VT_VARIANT;

					SafeArrayUnaccessData(psa);
				}
			}
		} else {
			pData = CComVariant(static_cast<LPCTSTR>(strOption));
		}

		hr = pIRobot->Execute(CComBSTR(L"DriveEx"), pData, &pResult);
		if(FAILED(hr)) {
			AfxTrace("Error : Execute( %X )\n", hr);
		}
	}
}

void CRobotDlg::OnRotate(ICaoRobot *pIRobot) 
{
	CString				strRotSuf, strOption;
	HRESULT				hr;

	if(pIRobot){
		m_cmb_Rotate.GetWindowText(strOption);
		if(m_optRotate.GetCheck() == 1)
			strOption += ",NEXT";

		switch(m_optSuf){
		case 0:	// Vn1,Vn2,Vn3
			strRotSuf = "V" + m_editRotate[0] + ", V" + m_editRotate[1] + ", V" + m_editRotate[2];
			break;

		case 1: // XY,YZ,ZX ...
			m_cmbSuf.GetWindowText(strRotSuf);
			break;
		}
		hr = pIRobot->Rotate(CComVariant(static_cast<LPCTSTR>(strRotSuf)), m_editRotdeg, CComVariant(static_cast<LPCTSTR>("V" + m_editRotate[3])), CComBSTR(strOption));
		if(FAILED(hr)) {
			AfxTrace("Error : Rotate( %X )\n", hr);
		}
	}
}

void CRobotDlg::OnApproach(ICaoRobot *pIRobot)
{
	SAFEARRAY*			psa;
	SAFEARRAYBOUND		rgb[1];

	CString				strOption;
	CComVariant			*pVarData, pData, pResult;
	HRESULT				hr;

	if(pIRobot){
		rgb[0].cElements = (m_optApproach.GetCheck())? 4 : 3;
		rgb[0].lLbound   = 0;
		if(psa = SafeArrayCreate(VT_VARIANT, 1, rgb)){

			hr = SafeArrayAccessData(psa, (void **)&pVarData);
			if(SUCCEEDED(hr)){
				CString strVariable = _T("");
				CString strPath     = _T("");
				CString strVarIndex, strLength;
				if (m_cmbApproachType.GetCurSel() != CB_ERR) {
					m_cmbApproachType.GetLBText(m_cmbApproachType.GetCurSel(), strVariable);
				}
				if (m_cmbApproachPath.GetCurSel() != CB_ERR) {
					m_cmbApproachPath.GetLBText(m_cmbApproachPath.GetCurSel(), strPath);
				}
				strVarIndex.Format("%d", m_nApproachIndex);
				strLength.Format(" %f", m_fApproachLength);

				// Interpolation method
				pVarData[0] = static_cast<short>(m_cmbApproachComp.GetCurSel() != CB_ERR)? m_cmbApproachComp.GetCurSel() + 1 : 0;
				// Reference position
				pVarData[1] = static_cast<LPCTSTR>(strVariable + strVarIndex);
				// Approach length
				pVarData[2] = static_cast<LPCTSTR>(strPath + strLength);
				if (m_optApproach.GetCheck()) {
					pVarData[3] = L"NEXT";
				}

				pData.parray = psa;
				pData.vt     = VT_ARRAY | VT_VARIANT;

				SafeArrayUnaccessData(psa);
			}
		}
		hr = pIRobot->Execute(CComBSTR(L"Approach"), pData, &pResult);
		if(FAILED(hr)) {
			AfxTrace("Error : Approach( %X )\n", hr);
		}
	}
}

void CRobotDlg::OnDepart(ICaoRobot *pIRobot)
{
	SAFEARRAY*			psa;
	SAFEARRAYBOUND		rgb[1];

	CString				strOption;
	CComVariant			*pVarData, pData, pResult;
	HRESULT				hr;

	if(pIRobot){
		rgb[0].cElements = (m_optDepart.GetCheck())? 3 : 2;
		rgb[0].lLbound   = 0;
		if(psa = SafeArrayCreate(VT_VARIANT, 1, rgb)){

			hr = SafeArrayAccessData(psa, (void **)&pVarData);
			if(SUCCEEDED(hr)){
				CString strPath = _T("");
				CString strLength;
				if (m_cmbDepartPath.GetCurSel() != CB_ERR) {
					m_cmbDepartPath.GetLBText(m_cmbDepartPath.GetCurSel(), strPath);
				}
				strLength.Format(" %f", m_fDepartLength);

				// Interpolation method
				pVarData[0] = static_cast<short>(m_cmbDepartComp.GetCurSel() != CB_ERR)? m_cmbDepartComp.GetCurSel() + 1 : 0;
				// Depart length
				pVarData[1] = static_cast<LPCTSTR>(strPath + strLength);
				if (m_optDepart.GetCheck()) {
					pVarData[2] = L"NEXT";
				}

				pData.parray = psa;
				pData.vt     = VT_ARRAY | VT_VARIANT;

				SafeArrayUnaccessData(psa);
			}
		}

		hr = pIRobot->Execute(CComBSTR(L"Depart"), pData, &pResult);
		if(FAILED(hr)) {
			AfxTrace("Error : Depart( %X )\n", hr);
		}
	}
}

void CRobotDlg::OnCmdCancel() 
{
	CComPtr<ICaoRobot>	pIRobot;

	pIRobot = GetRobotObject();
	if(pIRobot){
		pIRobot->Halt(CComBSTR(L""));
	}
}

CComPtr<ICaoController> CRobotDlg::GetControllerObject()
{
	long					longValue;
	long					i;

	CComPtr<ICaoController>	pICtrl;
	CComVariant				varIndex;
	HRESULT					hr;

	// Get controller object
	if(m_pICtrls){
		hr = m_pICtrls->get_Count(&longValue);
		if(SUCCEEDED(hr)){
			for(i = 0; i < longValue; i++){
				varIndex = CComVariant(i);
				hr = m_pICtrls->Item(varIndex, &pICtrl);
				if(SUCCEEDED(hr))
					break;
			}

			if(i < longValue)
				return pICtrl;
		}
	}

	return NULL;
}

CComPtr<ICaoRobot> CRobotDlg::GetRobotObject()
{
	long					longValue;
	long					i;

	CComPtr<ICaoController>	pICtrl;
	CComPtr<ICaoRobots>		pIRobots;
	CComPtr<ICaoRobot>		pIRobot;
	CComVariant				varIndex;
	HRESULT					hr;

	// Get controller object
	pICtrl = GetControllerObject();
	if(pICtrl){
		hr = pICtrl->get_Robots(&pIRobots);
		if(SUCCEEDED(hr)){
			hr = pIRobots->get_Count(&longValue);
			if(SUCCEEDED(hr)){
				for(i = 0; i < longValue; i++){
					varIndex = CComVariant(i);
					hr = pIRobots->Item(varIndex, &pIRobot);
					if(SUCCEEDED(hr))
						break;
				}
				if(i < longValue)
					return pIRobot;
			}
		}
	}

	return NULL;
}

HRESULT CRobotDlg::ExecuteTakearm(ICaoRobot *pIRobot)
{
	HRESULT				hr = E_FAIL;
	SAFEARRAY*			psa;
	SAFEARRAYBOUND		rgb[1];
	long				*pVarData;
	CComVariant			pData, pResult;

	if(pIRobot){
		rgb[0].cElements = 2;
		rgb[0].lLbound   = 0;
		if(psa = SafeArrayCreate(VT_I4, 1, rgb)){
			hr = SafeArrayAccessData(psa, (void **)&pVarData);
			if(SUCCEEDED(hr)){
				pVarData[0] = 0L;
				pVarData[1] = 1L;

				pData.parray = psa;
				pData.vt     = VT_ARRAY | VT_I4;

				SafeArrayUnaccessData(psa);
			}
		}
		hr = pIRobot->Execute(CComBSTR(L"Takearm"), pData, &pResult);
		if(FAILED(hr)) {
			AfxTrace("Error : Takearm( %X )\n", hr);
		}
	}

	return hr;
}

HRESULT CRobotDlg::ExecuteGivearm(ICaoRobot *pIRobot)
{
	HRESULT				hr = E_FAIL;
	CComVariant			pData, pResult;

	if(pIRobot){
		hr = pIRobot->Execute(CComBSTR(L"Givearm"), pData, &pResult);
		if(FAILED(hr)) {
			AfxTrace("Error : Givearm( %X )\n", hr);
		}
	}

	return hr;
}

CStringArray* CRobotDlg::GetResultDataArray(CComVariant varResult)
{
	long			longlBound, longUBound;

	CString			strWork;
	CString			fmtType;
	CStringArray	*arrayResult;

	SAFEARRAY*		psa;

	HRESULT			hr;
	long			i;

	arrayResult = new CStringArray;

	if(varResult.vt & VT_ARRAY){
		psa = varResult.parray;
		SafeArrayGetLBound(psa, 1, &longlBound);
		SafeArrayGetUBound(psa, 1, &longUBound);

		hr = SafeArrayLock(psa);
		if(SUCCEEDED(hr)){
			for(i = longlBound; i <= longUBound; i++){
				strWork = "";
				switch(varResult.vt & VT_TYPEMASK){
				case VT_I1:
				case VT_I2:
				case VT_I4:
				case VT_UI1:
				case VT_UI2:
				case VT_UI4:
				case VT_INT:
				case VT_UINT:
					long	longResult;
					SafeArrayGetElement(psa, &i, &longResult);
					strWork.Format("%d", longResult);
					break;

				case VT_R4:
					float	fltResult;
					SafeArrayGetElement(psa, &i, &fltResult);
					strWork.Format("%.2f", fltResult);
					break;

				case VT_R8:
					double	dblResult;
					SafeArrayGetElement(psa, &i, &dblResult);
					strWork.Format("%.2f", dblResult);
					break;

				case VT_BSTR:
					BSTR	bstrResult;
					SafeArrayGetElement(psa, &i, &bstrResult);
					strWork.Format("%s", bstrResult);
					break;

				case VT_VARIANT:
					VARIANT	varResult;
					SafeArrayGetElement(psa, &i, &varResult);
					if(varResult.vt == VT_R4)
						strWork.Format("%f", varResult.fltVal);
					break;


				case VT_BOOL:
					BYTE	byteResult;
					SafeArrayGetElement(psa, &i, &byteResult);
					strWork.Format("%d", byteResult);
					break;

				default:
					AfxTrace("Variant Type : ( %d )", varResult.vt & VT_TYPEMASK);
					break;
				}
				arrayResult->Add(strWork);
			}
			SafeArrayUnlock(psa);
		}

	} else {
		switch(varResult.vt & VT_TYPEMASK){
		case VT_EMPTY:
			strWork = "(No Return)";
			arrayResult->Add(strWork);
			break;

		case VT_I1:
		case VT_I2:
		case VT_I4:
			strWork.Format("%d", varResult.iVal);
			arrayResult->Add(strWork);
			break;

		case VT_UI1:
		case VT_UI2:
		case VT_UI4:
			strWork.Format("%d", varResult.uiVal);
			arrayResult->Add(strWork);
			break;

		case VT_I8:
			strWork.Format("%d", varResult.lVal);
			arrayResult->Add(strWork);
			break;

		case VT_UI8:
			strWork.Format("%d", varResult.ulVal);
			arrayResult->Add(strWork);
			break;

		case VT_INT:
			strWork.Format("%d", varResult.intVal);
			arrayResult->Add(strWork);
			break;

		case VT_UINT:
			strWork.Format("%d", varResult.uintVal);
			arrayResult->Add(strWork);
			break;

		case VT_R4:
			strWork.Format("%d", varResult.fltVal);
			arrayResult->Add(strWork);
			break;

		case VT_R8:
			strWork.Format("%d", varResult.dblVal);
			arrayResult->Add(strWork);
			break;

		case VT_BSTR:
			arrayResult->Add(varResult.bstrVal);
			break;

		case VT_BOOL:
			strWork.Format((varResult.boolVal == 0x0000)? "False" : "True");
			arrayResult->Add(strWork);
			break;

		default:
			AfxTrace("Variant Type : ( %d )", varResult.vt & VT_TYPEMASK);
			break;
		}
	}

	return arrayResult;
}

CString CRobotDlg::GetResultData(CComVariant varResult)
{
	CStringArray*	arrayResult = GetResultDataArray(varResult);
	CString			strResult;

	int i;
	for(i = 0; i < arrayResult->GetSize(); i++){
		strResult += arrayResult->GetAt(i);
	}

	delete arrayResult;

	return strResult;
}

void CRobotDlg::ButtonEnable(BOOL bEnable)
{
	m_cmdGo.EnableWindow(bEnable);
	m_cmdChange.EnableWindow(bEnable);
	m_cmdSpeed.EnableWindow(bEnable);
	m_cmdDrive.EnableWindow(bEnable);
	m_cmdRotate.EnableWindow(bEnable);
	m_cmdApproach.EnableWindow(bEnable);
	m_cmdDepart.EnableWindow(bEnable);
}

void CRobotDlg::OnTimer(UINT nIDEvent) 
{
	CStringArray*	CurrentPositionData;
	CString			strCurrentPosition = "";

	CComVariant	varValue;
	HRESULT		hr;
	int			i;

	switch(nIDEvent){
	case ID_TIMER:
		// Moving = True / False
		if(m_pIBusyStatusVariable){
			hr = m_pIBusyStatusVariable->get_Value(&varValue);
			if(FAILED(hr)) {
				AfxTrace("Error : get_Value( %X )\n", hr);
				break;
			}
			((CEdit*)GetDlgItem(IDC_MOVING))->SetWindowText(GetResultData(varValue));
		}

		// Current Position
		if(m_pICurrentPositionVariable){
			hr = m_pICurrentPositionVariable->get_Value(&varValue);
			if(FAILED(hr)) {
				AfxTrace("Error : get_Value( %X )\n", hr);
				break;
			}

			CurrentPositionData = GetResultDataArray(varValue);
			strCurrentPosition = "P(";
			for(i = 0; i < CurrentPositionData->GetSize() - 1; i++)
				strCurrentPosition += CurrentPositionData->GetAt(i) + ",";
			strCurrentPosition += CurrentPositionData->GetAt(i) + ")";
			((CEdit*)GetDlgItem(IDC_POSITION))->SetWindowText(strCurrentPosition);

			delete CurrentPositionData;
		}
		break;

	default:
		break;
	}

	CDialog::OnTimer(nIDEvent);
}