#include "stdafx.h"
#include "robot_api_c++.h"
#include <iostream>
#include <algorithm>
#include <windows.h>
using namespace std;
using namespace RobotAPI;
int _tmain(int argc, _TCHAR* argv[])
{
	cout << "Program start" << endl;
	const unsigned int connect_count = 10;
	CRobotAPI* p[connect_count];
	for (unsigned int i = 0; i < connect_count; i++) {
		p[i] = new CRobotAPI(i);
	}
	int res1 = p[1]->ConnectRobot("192.168.0.101");
	int res2 = p[2]->ConnectRobot("192.168.0.102");
	int res3 = p[3]->ConnectRobot("192.168.0.103");
	const bool enable = true;

	//使能或禁止外部API控制
	int res = p[1]->EnableApiControl(enable);
	if(res) {
		std::cout << "EanbleApiControl fail!" << std::endl;
	}

	//销毁不用的机器人连接
	delete p[3];

	//获取两个全局变量
	std::cout << "max_connection_count: " << max_connection_count << std::endl;
	std::cout << "curr_connection_count: " << curr_connection_count << std::endl;
	std::cout << res1 << ", " << res2 << ", " << res3 << ", " << std::endl;

	//获取其实位置坐标和轴角度信息
	RobotPos pos;
	p[1]->GetPos(pos);
	std::cout<<"Start pos:"<<std::endl;
	std::cout<<"x:"<<pos.x<<",y:"<<pos.y<<",z:"<<pos.z<<",a:"<<pos.a<<",b:"<<pos.b<<",c:"<<pos.c<<std::endl;

	//查询是否有告警，如果有告警，则清除
	bool alarm_state = false;
	p[1]->GetAlarmState(alarm_state);
	if(alarm_state) {
		unsigned int list[500] = {0};
		p[1]->GetAlarmList(list, 500);
		std::cout<<"Alarm list:"<<std::endl;
		for(unsigned int i = 0; i < 500; i++) {
			if (list[i] != 0) {
				std::cout<<list[i]<<std::endl;
			}
		}
		p[1]->ClearAlarm();
	}
	Sleep(1000);
	p[1]->GetAlarmState(alarm_state);

	if(alarm_state) {
		std::cout << "alarm_state error" << std::endl;
		//如果告警没有被清除...
	}
	

	//设置机器人控制模式为手动低速
	std::cout << "HasControlPermission result2: " << (int)p[1]->HasControlPermission() << std::endl;
	res = p[1]->SetControlMode(ROBOT_MODE_MANUAL);
		if(res != ERROR_OK) {
			std::cout << "HasControlPermission result3: " << (int)p[1]->HasControlPermission() << std::endl;
			std::cout << res << std::endl;
			std::cout << "SetControlMode fail!" << std::endl;
			return 0;
	}
	

	Sleep(500);

	//上电
	res = p[1]->PowerOn();
	if(res != ERROR_OK) {
		std::cout << "PowerOn fail!" << std::endl;
		return 0;
	}

	//获取其实位置坐标和轴角度信息
	p[1]->GetPos(pos);
	std::cout<<"Start pos:"<<std::endl;
	std::cout<<"x:"<<pos.x<<",y:"<<pos.y<<",z:"<<pos.z<<",a:"<<pos.a<<",b:"<<pos.b<<",c:"<<pos.c<<std::endl;
	RobotJoint j;
	p[1]->GetJoint(j);
	std::cout<<"joint:"<<j.j[0]<<","<<j.j[1]<<","<<j.j[2]<<","<<j.j[3]<<","<<j.j[4]<<","<<j.j[5]<<","<<j.j[6]<<","<<j.j[7]<<","<<j.j[8]<<","<<j.j[9]<<","<<j.j[10]<<","<<j.j[11]<<std::endl;

	//回零点
	res = p[1]->Move2Home();
	if(res != ERROR_OK) {
		std::cout << "Move2Home fail!" << std::endl;
		return 0;
	}

	Sleep(100);
	bool running = true;
	while(running) {
		ProgramState state;
		Sleep(1000);
		try{
			p[1]->GetProgramState(state);
			cout << "state: " << state << endl;
			if(state != PROG_STATE_RUNNING){
				running = false;
			}
		}catch(int e){
		}
	}

	//MOVEJ移动到某个角度
	RobotJoint robot_joint;
	robot_joint.j[0] = 10;
	robot_joint.j[1] = 20;
	robot_joint.j[2] = 30;
	res = p[1]->Move2Joint(robot_joint,100);
	if(res != ERROR_OK) {
		std::cout << "Move2Joint fail!" << std::endl;
		return 0;
	}

	Sleep(100);
	running = true;
	while(running) {
		ProgramState state;
		Sleep(1000);
		p[1]->GetProgramState(state);
		if(state != PROG_STATE_RUNNING){
			running = false;
		}
	}
	//获取其实位置坐标和轴角度信息
	p[1]->GetPos(pos);
	std::cout<<"Movej pos:"<<std::endl;
	std::cout<<"x:"<<pos.x<<",y:"<<pos.y<<",z:"<<pos.z<<",a:"<<pos.a<<",b:"<<pos.b<<",c:"<<pos.c<<std::endl;
	p[1]->GetJoint(j);
	std::cout<<"joint:"<<j.j[0]<<","<<j.j[1]<<","<<j.j[2]<<","<<j.j[3]<<","<<j.j[4]<<","<<j.j[5]<<","<<j.j[6]<<","<<j.j[7]<<","<<j.j[8]<<","<<j.j[9]<<","<<j.j[10]<<","<<j.j[11]<<std::endl;
	
	//PTP移动到某个位姿
	RobotPos robot_pos(440,70,1000,0,0,0);
	res = p[1]->Move2Pos(robot_pos,0,0,100);
	if(res != ERROR_OK) {
		std::cout << "Move2Pos!" << std::endl;
		return 0;
	}

	Sleep(100);
	running = true;
	while(running) {
		ProgramState state;
		Sleep(1000);
		p[1]->GetProgramState(state);
		if(state != PROG_STATE_RUNNING){
			running = false;
		}
	}
	//获取其实位置坐标和轴角度信息
	p[1]->GetPos(pos);
	std::cout<<"PTP pos:"<<std::endl;
	std::cout<<"x:"<<pos.x<<",y:"<<pos.y<<",z:"<<pos.z<<",a:"<<pos.a<<",b:"<<pos.b<<",c:"<<pos.c<<std::endl;
	p[1]->GetJoint(j);
	std::cout<<"joint:"<<j.j[0]<<","<<j.j[1]<<","<<j.j[2]<<","<<j.j[3]<<","<<j.j[4]<<","<<j.j[5]<<","<<j.j[6]<<","<<j.j[7]<<","<<j.j[8]<<","<<j.j[9]<<","<<j.j[10]<<","<<j.j[11]<<std::endl;
	
	//LINE移动到某个位姿
	robot_pos.x = 441;
	robot_pos.y = 71;
	robot_pos.z = 1001;
	res = p[1]->Line2Pos(robot_pos,0,0,100,100);
	if(res != ERROR_OK) {
		std::cout << "Line2Pos fail!" << std::endl;
		return 0;
	}

	Sleep(100);
	running = true;
	while(running) {
		ProgramState state;
		Sleep(1000);
		p[1]->GetProgramState(state);
		//cout << "state: " << state << endl;
		if(state != PROG_STATE_RUNNING){
			running = false;
		}
	}
	//获取其实位置坐标和轴角度信息
	p[1]->GetPos(pos);
	std::cout<<"LINE pos:"<<std::endl;
	std::cout<<"x:"<<pos.x<<",y:"<<pos.y<<",z:"<<pos.z<<",a:"<<pos.a<<",b:"<<pos.b<<",c:"<<pos.c<<std::endl;
	p[1]->GetJoint(j);
	std::cout<<"joint:"<<j.j[0]<<","<<j.j[1]<<","<<j.j[2]<<","<<j.j[3]<<","<<j.j[4]<<","<<j.j[5]<<","<<j.j[6]<<","<<j.j[7]<<","<<j.j[8]<<","<<j.j[9]<<","<<j.j[10]<<","<<j.j[11]<<std::endl;

	//CIRCLE移动到某个位姿
	robot_pos.x = 451;
	robot_pos.y = 81;
	robot_pos.z = 1001;
	RobotPos aux = robot_pos;
	aux.x = 400;
	res = p[1]->Circle2Pos(robot_pos,aux,180,0,0,100,100);
	if(res != ERROR_OK) {
		return 0;
	}

	Sleep(100);
	running = true;
	while(running) {
		ProgramState state;
		Sleep(1000);
		try{
			p[1]->GetProgramState(state);
			//cout << "state: " << state << endl;
			if(state != PROG_STATE_RUNNING){
				running = false;
			}
		}catch(int e){
		}
	}
	//获取其实位置坐标和轴角度信息
	p[1]->GetPos(pos);
	std::cout<<"CIRCLE pos:"<<std::endl;
	std::cout<<"x:"<<pos.x<<",y:"<<pos.y<<",z:"<<pos.z<<",a:"<<pos.a<<",b:"<<pos.b<<",c:"<<pos.c<<std::endl;
	p[1]->GetJoint(j);
	std::cout<<"joint:"<<j.j[0]<<","<<j.j[1]<<","<<j.j[2]<<","<<j.j[3]<<","<<j.j[4]<<","<<j.j[5]<<","<<j.j[6]<<","
		<<j.j[7]<<","<<j.j[8]<<","<<j.j[9]<<","<<j.j[10]<<","<<j.j[11]<<std::endl;

	//加载ARL程序并运行
	p[1]->SendProgram("d:\\test_arl");
	p[1]->LoadProgram("test_arl\\1.arl");
	p[1]->StartProgram();
	//下电
	res = p[1]->PowerOff();
	if(res != ERROR_OK) {
		std::cout << "PowerOff fail!" << std::endl;
		return 0;
	}
	//断开机器人连接
	cout << "Program over!" << endl;
	return 0;
}

