#include "stdafx.h"
#include "Project1.h"
#include "Project1Dlg.h"

#include <atlbase.h>
#include "CAO_i.c"

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

/////////////////////////////////////////////////////////////////////////////
// CProject1Dlg Dialog

CProject1Dlg::CProject1Dlg(CWnd* pParent /*=NULL*/)
	: CDialog(CProject1Dlg::IDD, pParent)
{
	//{{AFX_DATA_INIT(CProject1Dlg)
	//}}AFX_DATA_INIT
	m_hIcon = AfxGetApp()->LoadIcon(IDR_MAINFRAME);
}

void CProject1Dlg::DoDataExchange(CDataExchange* pDX)
{
	//{{AFX_DATA_MAP(CProject1Dlg)
	DDX_Control(pDX, IDC_BTN_INIT, m_btnInit);
	DDX_Control(pDX, IDC_BTN_CONNECT, m_btnConnect);
	DDX_Control(pDX, IDC_BTN_DISCONNECT, m_btnDisconnect);
	DDX_Control(pDX, IDC_BTN_MOVE, m_btnMove);
	DDX_Control(pDX, IDC_BTN_APPROACH, m_btnApproach);
	DDX_Control(pDX, IDC_BTN_DEPART, m_btnDepart);
	DDX_Control(pDX, IDC_BTN_DRAW, m_btnDraw);
	DDX_Control(pDX, IDC_BTN_DRIVEA, m_btnDriveA);
	DDX_Control(pDX, IDC_BTN_DRIVE, m_btnDrive);
	DDX_Control(pDX, IDC_BTN_SPEED, m_btnSpeed);
	DDX_Control(pDX, IDC_BTN_TOOL, m_btnTool);
	DDX_Control(pDX, IDC_BTN_WORK, m_btnWork);
	//}}AFX_DATA_MAP
}

BEGIN_MESSAGE_MAP(CProject1Dlg, CDialog)
	//{{AFX_MSG_MAP(CProject1Dlg)
	ON_WM_PAINT()
	ON_WM_QUERYDRAGICON()
	ON_WM_CLOSE()
	ON_BN_CLICKED(IDC_BTN_INIT, OnBtnInit)
	ON_BN_CLICKED(IDC_BTN_APPROACH, OnBtnApproach)
	ON_BN_CLICKED(IDC_BTN_CONNECT, OnBtnConnect)
	ON_BN_CLICKED(IDC_BTN_DEPART, OnBtnDepart)
	ON_BN_CLICKED(IDC_BTN_DISCONNECT, OnBtnDisconnect)
	ON_BN_CLICKED(IDC_BTN_DRAW, OnBtnDraw)
	ON_BN_CLICKED(IDC_BTN_DRIVE, OnBtnDrive)
	ON_BN_CLICKED(IDC_BTN_DRIVEA, OnBtnDrivea)
	ON_BN_CLICKED(IDC_BTN_MOVE, OnBtnMove)
	ON_BN_CLICKED(IDC_BTN_SPEED, OnBtnSpeed)
	ON_BN_CLICKED(IDC_BTN_TOOL, OnBtnTool)
	ON_BN_CLICKED(IDC_BTN_WORK, OnBtnWork)
	//}}AFX_MSG_MAP
END_MESSAGE_MAP()

/////////////////////////////////////////////////////////////////////////////
// CProject1Dlg Message Hundler

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

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

	// Initialize cao objects
	CoInitialize(NULL);
	m_eng	= NULL;
	m_wss	= NULL;
	m_ws	= NULL;
	m_ctrl	= NULL;
	m_robot	= NULL;
	m_var	= NULL;

	return TRUE;
}

void CProject1Dlg::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 CProject1Dlg::OnQueryDragIcon()
{
	return (HCURSOR) m_hIcon;
}

// Delete CaoEngine
void CProject1Dlg::OnClose() 
{
	OnBtnDisconnect();

	if(m_ws != NULL)
	{
		if(m_wss != NULL)
		{
			m_wss->Clear();
		}

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

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

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

	CoUninitialize();

	CDialog::OnClose();
}

void CProject1Dlg::OnBtnInit() 
{
	// Create Engine
	CoCreateInstance(CLSID_CaoEngine, NULL, CLSCTX_LOCAL_SERVER, IID_ICaoEngine, (void**)&m_eng);
	m_eng->get_Workspaces(&m_wss);
	m_wss->Item(CComVariant(0L), &m_ws);

	GetDlgItem(IDC_BTN_INIT)->EnableWindow(FALSE);
	GetDlgItem(IDC_BTN_CONNECT)->EnableWindow(TRUE);
}

// ---------------------------------------------------------------
// Connect with the Robot
// ---------------------------------------------------------------
void CProject1Dlg::OnBtnConnect() 
{
	HRESULT hr = E_FAIL;

	// Create object and connect with VRC
	if(m_ws != NULL)
	{
		hr = m_ws->AddController(CComBSTR(L"Robot"), CComBSTR(L"caoProv.DENSO.RC8"), CComBSTR(L""), CComBSTR(L"Server=192.168.0.1"), &m_ctrl);
	}

	if(SUCCEEDED(hr))
	{
		hr = m_ctrl->AddRobot(CComBSTR(L"VS"), CComBSTR(L""), &m_robot);
		if(SUCCEEDED(hr))
		{
			// Valiable of status of Machine lock SW
			hr = m_ctrl->AddVariable(CComBSTR(L"@LOCK"), CComBSTR(L""), &m_var);
			if(SUCCEEDED(hr))
			{
				// Turned on the Machine lock
				m_var->put_Value(CComVariant(true));

				// Get armControl authority
				{
					long lElem, lValue[] = {0L, 1L};
					CComVariant vntTakearm, vntReturn;

					vntTakearm.vt = VT_ARRAY | VT_I4;
					vntTakearm.parray = SafeArrayCreateVector(VT_I4, 0L, 2L);

					for(lElem = 0L; lElem < 2L; lElem++)
					{
						SafeArrayPutElement(vntTakearm.parray, &lElem, &lValue[lElem]);
					}

					hr = m_robot->Execute(CComBSTR(L"Takearm"), vntTakearm, &vntReturn);
				}

				/*
				// Start motoriThe motor cannot start when the machine lock is truej
				{
					long lElem, lValue[] = {1L, 0L};
					CComVariant vntMotor, vntReturn;

					vntMotor.vt = VT_ARRAY | VT_I4;
					vntMotor.parray = SafeArrayCreateVector(VT_I4, 0L, 2L);

					for(lElem = 0L; lElem < 2L; lElem++)
					{
						SafeArrayPutElement(vntMotor.parray, &lElem, &lValue[lElem]);
					}

					hr = m_robot->Execute(CComBSTR(L"Motor"), vntMotor, &vntReturn);
				}
				*/

				// Set the external movement speed of the robot
				{
					long lElem;
					float fValue[] = {50.0f, 25.0f, 25.0f};
					CComVariant vntExtSpeed, vntReturn;

					vntExtSpeed.vt = VT_ARRAY | VT_R4;
					vntExtSpeed.parray = SafeArrayCreateVector(VT_R4, 0L, 3L);

					for(lElem = 0L; lElem < 3L; lElem++)
					{
						SafeArrayPutElement(vntExtSpeed.parray, &lElem, (void*)&fValue[lElem]);
					}
					
					hr = m_robot->Execute(CComBSTR(L"ExtSpeed"), vntExtSpeed, &vntReturn);
				}
			}
		}
	}

	if(SUCCEEDED(hr))
	{
		// Enable command buttons
		ButtonController(true);
	}
	else
	{
		ShowErrorMessage(hr);
	}
}

// ---------------------------------------------------------------
// disconnect from the Robot
// ---------------------------------------------------------------
void CProject1Dlg::OnBtnDisconnect() 
{
	// Disconnect from the Robot
	if(m_robot != NULL)
	{
		/*
		// Stop motor
		{
			long lElem, lValue = 0L;
			CComVariant vntMotor, vntReturn;

			vntMotor.vt = VT_ARRAY | VT_I4;
			vntMotor.parray = SafeArrayCreateVector(VT_I4, 0L, 2L);

			for(lElem = 0L; lElem < 2L; lElem++)
			{
				SafeArrayPutElement(vntMotor.parray, &lElem, &lValue);
			}

			m_robot->Execute(CComBSTR(L"Motor"), vntMotor, &vntReturn);
		}
		*/

		// Release arm control authority
		{
			CComVariant vntGivearm, vntReturn;

			m_robot->Execute(CComBSTR(L"Givearm"), vntGivearm, &vntReturn);
		}

		if(m_ctrl != NULL)
		{
			CComVariant vntRobotName;
			CComPtr<ICaoRobots> robots;

			m_ctrl->get_Robots(&robots);
		
			vntRobotName.vt = VT_BSTR;
			m_robot->get_Name(&vntRobotName.bstrVal);

			robots->Remove(vntRobotName);
		}

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

	// Disconnect from the controller
	if(m_ctrl != NULL)
	{
		if(m_ws != NULL)
		{
			CComVariant vntCtrlName;
			CComPtr<ICaoControllers> ctrls;

			m_ws->get_Controllers(&ctrls);

			vntCtrlName.vt = VT_BSTR;
			m_ctrl->get_Name(&vntCtrlName.bstrVal);

			ctrls->Remove(vntCtrlName);
		}

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

	// Disable command buttons
	ButtonController(false);
}

void CProject1Dlg::OnBtnMove() 
{
	if(m_robot != NULL)
	{
		HRESULT hr = E_FAIL;

		/*
		{
			// Move command
			hr = m_robot->Move(1L, CComVariant(CComBSTR(L"P11")), CComBSTR(L""));
			hr = m_robot->Move(1L, CComVariant(CComBSTR(L"P12")), CComBSTR(L""));
		}
		*/

		{
			// Move command
			long lElem;
			double dValue[] = {524, 0, 330, 180, 0, 180, 5};
			CComVariant vntPoseTmp[3], vntPose;
			
			vntPoseTmp[0].vt = VT_ARRAY | VT_R8;
			vntPoseTmp[0].parray = SafeArrayCreateVector(VT_R8, 0L, 7L);
			for(lElem = 0L; lElem < 7L; lElem++)
			{
				SafeArrayPutElement(vntPoseTmp[0].parray, &lElem, (void*)&dValue[lElem]);
			}

			vntPoseTmp[1].vt = VT_BSTR;
			vntPoseTmp[1].bstrVal = SysAllocString(L"P");

			vntPoseTmp[2].vt = VT_BSTR;
			vntPoseTmp[2].bstrVal = SysAllocString(L"@0");

			vntPose.vt = VT_ARRAY | VT_VARIANT;
			vntPose.parray = SafeArrayCreateVector(VT_VARIANT, 0L, 3L);
			for(lElem = 0L; lElem < 3L; lElem++)
			{
				SafeArrayPutElement(vntPose.parray, &lElem, &vntPoseTmp[lElem]);
			}

			hr = m_robot->Move(1L, vntPose, CComBSTR(L""));
			hr = m_robot->Move(1L, CComVariant(CComBSTR(L"@P P(535, -200, 250, 170, -5, 150, 5)")), CComBSTR(L""));
		}
		
		/*
		{
			// Move command
			long lElem;
			double dValue[] = {0, 45, 90, 0, 45, 0, 0, 0};
			CComVariant vntPoseTmp[3], vntPose;
			
			vntPoseTmp[0].vt = VT_ARRAY | VT_R8;
			vntPoseTmp[0].parray = SafeArrayCreateVector(VT_R8, 0L, 8L);
			for(lElem = 0L; lElem < 8L; lElem++)
			{
				SafeArrayPutElement(vntPoseTmp[0].parray, &lElem, (void*)&dValue[lElem]);
			}

			vntPoseTmp[1].vt = VT_BSTR;
			vntPoseTmp[1].bstrVal = SysAllocString(L"J");

			vntPoseTmp[2].vt = VT_BSTR;
			vntPoseTmp[2].bstrVal = SysAllocString(L"@0");

			vntPose.vt = VT_ARRAY | VT_VARIANT;
			vntPose.parray = SafeArrayCreateVector(VT_VARIANT, 0L, 3L);
			for(lElem = 0L; lElem < 3L; lElem++)
			{
				SafeArrayPutElement(vntPose.parray, &lElem, &vntPoseTmp[lElem]);
			}

			hr = m_robot->Move(1L, vntPose, CComBSTR(L""));
			hr = m_robot->Move(1L, CComVariant(CComBSTR("J(-22, 61, 76, 14, 48, -2)")), CComBSTR(L""));
		}
		*/

		if(FAILED(hr))
		{
			ShowErrorMessage(hr);
		}
	}
}

void CProject1Dlg::OnBtnApproach() 
{
	if(m_robot != NULL)
	{
		HRESULT hr = E_FAIL;

		{
			// Approach command
			long lElem;
			CComVariant vntParamTmp[3], vntParam, vntReturn;
			
			vntParamTmp[0].vt = VT_I4;
			vntParamTmp[0].lVal = 2L;

			vntParamTmp[1].vt = VT_BSTR;
			vntParamTmp[1].bstrVal = SysAllocString(L"P11");

			vntParamTmp[2].vt = VT_BSTR;
			vntParamTmp[2].bstrVal = SysAllocString(L"@P 100");

			vntParam.vt = VT_ARRAY | VT_VARIANT;
			vntParam.parray = SafeArrayCreateVector(VT_VARIANT, 0L, 3L);
			for(lElem = 0L; lElem < 3L; lElem++)
			{
				SafeArrayPutElement(vntParam.parray, &lElem, &vntParamTmp[lElem]);
			}

			hr = m_robot->Execute(CComBSTR(L"Approach"), vntParam, &vntReturn);
		}

		/*
		{
			// Approach command
			long lElem;
			CComVariant vntParamTmp[3], vntParam, vntReturn;
			
			vntParamTmp[0].vt = VT_I4;
			vntParamTmp[0].lVal = 2L;

			vntParamTmp[1].vt = VT_BSTR;
			vntParamTmp[1].bstrVal = SysAllocString(L"P(524, 0, 330, 180, 0, 180, 5)");

			vntParamTmp[2].vt = VT_BSTR;
			vntParamTmp[2].bstrVal = SysAllocString(L"@P 100");

			vntParam.vt = VT_ARRAY | VT_VARIANT;
			vntParam.parray = SafeArrayCreateVector(VT_VARIANT, 0L, 3L);
			for(lElem = 0L; lElem < 3L; lElem++)
			{
				SafeArrayPutElement(vntParam.parray, &lElem, &vntParamTmp[lElem]);
			}

			hr = m_robot->Execute(CComBSTR(L"Approach"), vntParam, &vntReturn);
		}
		*/

		if(FAILED(hr))
		{
			ShowErrorMessage(hr);
		}
	}
}

void CProject1Dlg::OnBtnDepart() 
{
	if(m_robot != NULL)
	{
		HRESULT hr = E_FAIL;

		{
			// Depart command
			long lElem;
			CComVariant vntParamTmp[2], vntParam, vntReturn;
			
			vntParamTmp[0].vt = VT_I4;
			vntParamTmp[0].lVal = 2L;

			vntParamTmp[1].vt = VT_BSTR;
			vntParamTmp[1].bstrVal = SysAllocString(L"@P 100");

			vntParam.vt = VT_ARRAY | VT_VARIANT;
			vntParam.parray = SafeArrayCreateVector(VT_VARIANT, 0L, 2L);
			for(lElem = 0L; lElem < 2L; lElem++)
			{
				SafeArrayPutElement(vntParam.parray, &lElem, &vntParamTmp[lElem]);
			}

			hr = m_robot->Execute(CComBSTR(L"Depart"), vntParam, &vntReturn);
		}

		if(FAILED(hr))
		{
			ShowErrorMessage(hr);
		}
	}
}

void CProject1Dlg::OnBtnDraw() 
{
	if(m_robot != NULL)
	{
		HRESULT hr = E_FAIL;

		{
			// Draw command (X=50,Y=50,Z=50)
			long lElem;
			CComVariant vntParamTmp[2], vntParam, vntReturn;
			
			vntParamTmp[0].vt = VT_I4;
			vntParamTmp[0].lVal = 1L;

			vntParamTmp[1].vt = VT_BSTR;
			vntParamTmp[1].bstrVal = SysAllocString(L"V(50,50,50)");

			vntParam.vt = VT_ARRAY | VT_VARIANT;
			vntParam.parray = SafeArrayCreateVector(VT_VARIANT, 0L, 2L);
			for(lElem = 0L; lElem < 2L; lElem++)
			{
				SafeArrayPutElement(vntParam.parray, &lElem, &vntParamTmp[lElem]);
			}

			hr = m_robot->Execute(CComBSTR(L"Draw"), vntParam, &vntReturn);
		}

		if(FAILED(hr))
		{
			ShowErrorMessage(hr);
		}
	}
}

void CProject1Dlg::OnBtnDrivea() 
{
	if(m_robot != NULL)
	{
		HRESULT hr = E_FAIL;

		{
			// DriveAEx command
			CComVariant vntParam, vntReturn;
			
			vntParam.vt = VT_BSTR;
			vntParam.bstrVal = SysAllocString(L"@P (1,10),(2,50),(3,75),(4,14),(5,55),(6,10)");

			hr = m_robot->Execute(CComBSTR(L"DriveAEx"), vntParam, &vntReturn);
		}

		if(FAILED(hr))
		{
			ShowErrorMessage(hr);
		}
	}
}

void CProject1Dlg::OnBtnDrive() 
{
	if(m_robot != NULL)
	{
		HRESULT hr = E_FAIL;

		{
			// DriveEx command
			CComVariant vntParam, vntReturn;
			
			vntParam.vt = VT_BSTR;
			vntParam.bstrVal = SysAllocString(L"@P (1,-7),(2,9),(3,7),(4,15),(5,10),(6,12)");
			//vntParam.bstrVal = SysAllocString(L"@0 (1,-35)");

			hr = m_robot->Execute(CComBSTR(L"DriveEx"), vntParam, &vntReturn);
		}

		if(FAILED(hr))
		{
			ShowErrorMessage(hr);
		}
	}
}

void CProject1Dlg::OnBtnSpeed() 
{
	if(m_robot != NULL)
	{
		HRESULT hr = E_FAIL;

		{
			// Speed command (Internal speed of 75%)
			hr = m_robot->Speed(-1, 75);
		}

		if(FAILED(hr))
		{
			ShowErrorMessage(hr);
		}
	}
}

void CProject1Dlg::OnBtnTool() 
{
	if(m_robot != NULL)
	{
		HRESULT hr = E_FAIL;

		{
			// Change Tool command (Tool1)
			hr = m_robot->Change(CComBSTR(L"Tool1"));
		}

		if(FAILED(hr))
		{
			ShowErrorMessage(hr);
		}
	}
}

void CProject1Dlg::OnBtnWork() 
{
	if(m_robot != NULL)
	{
		HRESULT hr = E_FAIL;

		{
			// Change Work command (Work1)
			hr = m_robot->Change(CComBSTR(L"Work1"));
		}

		if(FAILED(hr))
		{
			ShowErrorMessage(hr);
		}
	}
}

// bFlag = true:  Enable command buttons
// bFlag = false: Disable command buttons
void CProject1Dlg::ButtonController(bool bFlag)
{
	m_btnConnect.EnableWindow(!bFlag);
	m_btnDisconnect.EnableWindow(bFlag);
	m_btnMove.EnableWindow(bFlag);
	m_btnApproach.EnableWindow(bFlag);
	m_btnDepart.EnableWindow(bFlag);
	m_btnDraw.EnableWindow(bFlag);
	m_btnDriveA.EnableWindow(bFlag);
	m_btnDrive.EnableWindow(bFlag);
	m_btnSpeed.EnableWindow(bFlag);
	m_btnTool.EnableWindow(bFlag);
	m_btnWork.EnableWindow(bFlag);
}

int CProject1Dlg::ShowErrorMessage(HRESULT hr)
{
	int iReturn;
	CString strMsg;

	strMsg.Format(_T("%X"), hr);
	iReturn = MessageBox(strMsg, _T("Error"));

	return iReturn;
}