#include "stdafx.h"

CComModule _Module;

#include "CAO_i.c"

void Run();

void main()
{
	CoInitialize(0);
	_Module.Init(NULL, ::GetModuleHandle(NULL));

	Run();

	_Module.Term();
	CoUninitialize();
}

void Run()
{
	USES_CONVERSION;
	HRESULT hr = S_OK;
	CComPtr<ICaoEngine> pEng;
	CComPtr<ICaoWorkspaces> pWss;
	CComPtr<ICaoWorkspace> pWs;
	CComPtr<ICaoController> pCtrl;
	CComPtr<ICaoRobot> pArm;
	CComVariant vntPose; // for PoseData type

	// Create CaoEngine object
	hr = CoCreateInstance(CLSID_CaoEngine, NULL, CLSCTX_LOCAL_SERVER, IID_ICaoEngine, (void**)&pEng);
	if (FAILED(hr)) {
		return;
	}

	// Get Workespaces object
	hr = pEng->get_Workspaces(&pWss);
	if (FAILED(hr)) {
		return;
	}

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

	// Create Controller object of DENSO NetwoRC robot controller
	hr = pWs->AddController(CComBSTR(L"Sample"), CComBSTR(L"CaoProv.Dummy"), CComBSTR(L""), CComBSTR(L""), &pCtrl);
	if (FAILED(hr)) {
		return;
	}

	// Create Robot object
	hr = pCtrl->AddRobot(CComBSTR(L"Arm1"), CComBSTR(L""), &pArm);
	if (FAILED(hr)) {
		return;
	}

    // Pick&Place
	//   P11 --- P13
	//    |       |
	//   P12     P14

    //.Speed -1, 100
	hr = pArm->Speed( -1L, 100.0f );
	if (FAILED(hr)) {
		return;
	}

    // .Move 1, "@P P11", "NEXT"
	hr = pArm->Move( 1L, CComVariant(L"@P P11"), CComBSTR(L"NEXT") );
	if (FAILED(hr)) {
		return;
	}
    // .Move 2, "P12", "NEXT"
	hr = pArm->Move( 2L, CComVariant(L"P12"), CComBSTR(L"NEXT") );
	if (FAILED(hr)) {
		return;
	}
    // .Move 2, "@P P11", "NEXT"
	// ------------------------------------------------------------------
	// VT_ARRAY | VT_VARIANT:
	vntPose.Clear();
	SAFEARRAY* psa = ::SafeArrayCreateVector(VT_VARIANT, 0, 3); // <0:Value>,<1:Type>,<2:Pass>
	CComVariant *pvntParam;
	::SafeArrayAccessData(psa, (void**)&pvntParam);
	pvntParam[0] = 11;
	pvntParam[1] = CComBSTR(L"P"); // or = 0L;
	pvntParam[2] = CComBSTR(L"@P"); // or = -1L;
	::SafeArrayUnaccessData(psa);
	vntPose.vt = VT_ARRAY | VT_VARIANT;
	vntPose.parray = psa;
	// ------------------------------------------------------------------
	hr = pArm->Move( 2L, vntPose, CComBSTR(L"NEXT") );
	if (FAILED(hr)) {
		return;
	}
    // .Move 1, "@P P13", "NEXT"
	hr = pArm->Move( 1L, CComVariant(L"@P P13"), CComBSTR(L"NEXT") );
	if (FAILED(hr)) {
		return;
	}
    // .Move 2, "P14", "NEXT"
	hr = pArm->Move( 2L, CComVariant(L"P14"), CComBSTR(L"NEXT") );
	if (FAILED(hr)) {
		return;
	}
    // .Move 2, "@P P13", "NEXT"
	hr = pArm->Move( 2L, CComVariant(L"@P P13"), CComBSTR(L"NEXT") );
	if (FAILED(hr)) {
		return;
	}

	return;
}