#include <iostream>
#include <Windows.h>
#include <vector>

#include "nowTime.h"
#include "RobotAPI.h"



int main()
{
	std::string str;
	char ip[32] = "192.168.0.160";
	int port = 50502;
	//ֵĬʧ
	int ret = 1;
	//ǷִгɹĬʧ
	bool isSuccess = false;
	/*
	*ִгɹ
	* 0API_Init()ִгɹ
	* 1API_MotorOn()ִгɹ
	* 2API_HomeMove()ִгɹ
	* 3API_MoveLSinglePositionMoveLִгɹ
	* 4API_MoveLMutiPositionMoveLִгɹ
	* 5API_MoveLSinglePositionMoveJִгɹ
	* 6API_MoveLMutiPositionMoveJִгɹ
	* 7API_MoveAbsjSinglePositionִгɹ
	* 8API_MoveAbsjMutiPositionִгɹ
	* 9API_Releaseִгɹ
	*/
	int functionSuccessCount[11] = { 0 };
	//ѭ
	double count = 0;
	//ȴʱ
	double sleepTime = 1000;

	//ֶλ
#pragma region
	rokae_robtarget_pos rob_pos;
	rob_pos.euler_type = 0;
	rob_pos.rob_pos.cogx = 365;
	rob_pos.rob_pos.cogy = 0;
	rob_pos.rob_pos.cogz = 620;
	rob_pos.rob_pos.q1 = 0;
	rob_pos.rob_pos.q2 = 0;
	rob_pos.rob_pos.q3 = 1;
	rob_pos.rob_pos.q4 = 0;
	rob_pos.rob_pos.a = -150;
	rob_pos.rob_pos.b = 0;
	rob_pos.rob_pos.c = 180;
	rob_pos.motion_param.per = 100;
	rob_pos.motion_param.tcp = 7000;
	rob_pos.motion_param.iZone = 200;

	rokae_robtarget_pos murob_pos[6];
	for (int i = 0; i < 6; i++)
	{
		murob_pos[i].euler_type = 3;
		murob_pos[i].rob_pos.q1 = 0;
		murob_pos[i].rob_pos.q2 = 0;
		murob_pos[i].rob_pos.q3 = 1;
		murob_pos[i].rob_pos.q4 = 0;
		murob_pos[i].motion_param.per = 100;
		murob_pos[i].motion_param.iZone = 200;
		murob_pos[i].motion_param.tcp = 800;
	}

	murob_pos[0].rob_pos.cogx = 365;
	murob_pos[0].rob_pos.cogy = -100;
	murob_pos[0].rob_pos.cogz = 672;

	murob_pos[1].rob_pos.cogx = 365;
	murob_pos[1].rob_pos.cogy = 100;
	murob_pos[1].rob_pos.cogz = 672;

	murob_pos[2].rob_pos.cogx = 365;
	murob_pos[2].rob_pos.cogy = 100;
	murob_pos[2].rob_pos.cogz = 602;

	murob_pos[3].rob_pos.cogx = 365;
	murob_pos[3].rob_pos.cogy = -100;
	murob_pos[3].rob_pos.cogz = 602;

	murob_pos[4].rob_pos.cogx = 365;
	murob_pos[4].rob_pos.cogy = -100;
	murob_pos[4].rob_pos.cogz = 552;

	murob_pos[5].rob_pos.cogx = 365;
	murob_pos[5].rob_pos.cogy = 100;
	murob_pos[5].rob_pos.cogz = 552;

	rokae_jnttarget_pos jnt_pos;
	jnt_pos.jnt_pos.j1 = 0;
	jnt_pos.jnt_pos.j2 = 0;
	jnt_pos.jnt_pos.j3 = 0;
	jnt_pos.jnt_pos.j4 = 0;
	jnt_pos.jnt_pos.j5 = 90;
	jnt_pos.jnt_pos.j6 = 0;
	jnt_pos.motion_param.per = 80;
	jnt_pos.motion_param.iZone = 200;
	jnt_pos.motion_param.tcp = 1000;

	rokae_jnttarget_pos mujnt_pos[2];
	for (int i = 0; i < 2; i++)
	{
		mujnt_pos[i].jnt_pos.j1 = 0;
		mujnt_pos[i].jnt_pos.j2 = 0;
		mujnt_pos[i].jnt_pos.j3 = 0;
		mujnt_pos[i].jnt_pos.j4 = 0;
		mujnt_pos[i].jnt_pos.j5 = 90;
		mujnt_pos[i].jnt_pos.j6 = -200 + i * 400;
		mujnt_pos[i].motion_param.per = 80;
		mujnt_pos[i].motion_param.iZone = 200;
		mujnt_pos[i].motion_param.tcp = 1000;
	}



		//std::cout << NowTime() << "	IP";
		//std::cin >> ip;
		std::cout << NowTime() << "	ִAPI_Init()" << std::endl;
		ret = API_Init(ip, port);
		std::cout << NowTime() << "	API_Init = " << ret << std::endl << std::endl;
		while (true)
		{
			std::cout << NowTime() << "	ָ";
			std::cin >> str;
			//ʼ
			if (str == "init")
			{
				//std::cout << NowTime() << "	IP";
				//std::cin >> ip;
				//std::cout << NowTime() << "	˿ڣ";
				//std::cin >> port;
				std::cout << NowTime() << "	ִAPI_Init()" << std::endl;
				ret = API_Init(ip, port);
				std::cout << NowTime() << "	Init = " << ret << std::endl << std::endl;
			}
			//ϵ
			if (str == "motoron")
			{
				std::cout << NowTime() << "	ִAPI_MotorOn()" << std::endl;
				ret = API_MotorOn();
				std::cout << NowTime() << "	MotorOn = " << ret << std::endl << std::endl;
			}
			//Home
			if (str == "movehome")
			{
				std::cout << NowTime() << "	ִAPI_HomeMove()" << std::endl;
				ret = API_HomeMove();
				std::cout << NowTime() << "	MoveHome = " << ret << std::endl << std::endl;
			}
			//MoveL
			if (str == "movel")
			{
				std::cout << NowTime() << "	ִAPI_MoveLSinglePosition()" << std::endl;
				ret = API_MoveLSinglePosition(1, rob_pos);
				std::cout << NowTime() << "	MoveL = " << ret << std::endl << std::endl;
			}
			//MoveL
			if (str == "mumovel")
			{
				std::cout << NowTime() << "	ִAPI_MoveLMutiPosition()" << std::endl;
				ret = API_MoveLMutiPosition(1, murob_pos, 6);
				std::cout << NowTime() << "	muMoveL = " << ret << std::endl << std::endl;
			}
			//MoveJ
			if (str == "movej")
			{
				std::cout << NowTime() << "	ִAPI_MoveLSinglePosition()" << std::endl;
				ret = API_MoveLSinglePosition(0, rob_pos);
				std::cout << NowTime() << "	Movej = " << ret << std::endl << std::endl;
			}
			//MoveJ
			if (str == "mumovej")
			{
				std::cout << NowTime() << "	ִAPI_MoveLMutiPosition()" << std::endl;
				ret = API_MoveLMutiPosition(0, murob_pos, 6);
				std::cout << NowTime() << "	muMovej = " << ret << std::endl << std::endl;
			}
			//MoveAbsj
			if (str == "moveabsj")
			{
				std::cout << NowTime() << "	ִAPI_MoveAbsjSinglePosition()" << std::endl;
				ret = API_MoveAbsjSinglePosition(jnt_pos);
				std::cout << NowTime() << "	MoveAbsj = " << ret << std::endl << std::endl;
			}
			//MoveAbsj
			if (str == "mumoveabsj")
			{
				std::cout << NowTime() << "	ִAPI_MoveAbsjMutiPosition()" << std::endl;
				ret = API_MoveAbsjMutiPosition(mujnt_pos, 2);
				std::cout << NowTime() << "	muMoveAbsj = " << ret << std::endl << std::endl;
			}
			//
			if (str == "clearalarm")
			{
				std::cout << NowTime() << "	ִAPI_SetAlarmStatus()" << std::endl;
				ret = API_SetAlarmStatus(0.0);
				std::cout << NowTime() << "	ClearAlarm = " << ret << std::endl << std::endl;
			}
			//ͣ
			if (str == "emgstop")
			{
				std::cout << NowTime() << "	ִAPI_EmgStop()" << std::endl;
				ret = API_EmgStop();
				std::cout << NowTime() << "	EmgStop = " << ret << std::endl << std::endl;
			}
			//ͷšµ
			if (str == "release")
			{
				std::cout << NowTime() << "	ִAPI_Release()" << std::endl;
				ret = API_Release();
				std::cout << NowTime() << "	Release = " << ret << std::endl << std::endl;
			}
			//ȡ״̬
			if (str == "status")
			{
				char cmsg;
				unsigned int len;
				std::cout << NowTime() << "	ִAPI_GetRobotStatus()" << std::endl;
				ret = API_GetRobotStatus(&cmsg, &len);
				std::cout << "============================================================================================" << std::endl;
				std::cout << "0ʼ״̬	1ֹͣ״̬	2ϵ״̬	3ֹͣ״̬	4ֹͣ	5״̬" << std::endl;
				std::cout << "============================================================================================" << std::endl;
				std::cout << NowTime() << "  GetRobotStatus = " << ret << "	msg = " << cmsg << std::endl << std::endl;
			}
			//ȡһϢ
			if (str == "lastmsg")
			{
				char cmsg;
				unsigned int len;
				std::cout << NowTime() << "	ִrokae_last_msg()" << std::endl;
				ret = rokae_last_msg(&cmsg, &len);
				std::cout << NowTime() << "  ret = " << ret << "	msg = " << cmsg << "len = " << len << std::endl << std::endl;
			}
			//ȡؽλ
			if (str == "position")
			{
				RokaeSDKGetRobotPosition position;
				unsigned int len = 0;
				std::cout << NowTime() << "	ִAPI_GetRobotPosition()" << std::endl;
				ret = API_GetRobotPosition(&position, &len);
				std::cout << NowTime() << "	GetRobotPosition = " << ret << std::endl << std::endl;

				std::cout << "			len = " << len << std::endl;
				std::cout << "			J1 = " << position.jnt_pos.j1 << std::endl;
				std::cout << "			J2 = " << position.jnt_pos.j2 << std::endl;
				std::cout << "			J3 = " << position.jnt_pos.j3 << std::endl;
				std::cout << "			J4 = " << position.jnt_pos.j4 << std::endl;
				std::cout << "			J5 = " << position.jnt_pos.j5 << std::endl;
				std::cout << "			J6 = " << position.jnt_pos.j6 << std::endl << std::endl;

				std::cout << "			X = " << position.rob_pos.cogx << std::endl;
				std::cout << "			Y = " << position.rob_pos.cogy << std::endl;
				std::cout << "			Z = " << position.rob_pos.cogz << std::endl;
				std::cout << "			q1 = " << position.rob_pos.q1 << std::endl;
				std::cout << "			q2 = " << position.rob_pos.q2 << std::endl;
				std::cout << "			q3 = " << position.rob_pos.q3 << std::endl;
				std::cout << "			q4 = " << position.rob_pos.q4 << std::endl;
				std::cout << "			A = " << position.rob_pos.a << std::endl;
				std::cout << "			B = " << position.rob_pos.b << std::endl;
				std::cout << "			C = " << position.rob_pos.c << std::endl << std::endl;
			}
			//do0true
			if (str == "setdotrue")
			{
				RokaeSDKIOValuePOD pod;
				char name[80] = "do0";
				memcpy(pod.pod_name, name, strlen(name) + 1);
				pod.pod_value = 1;
				std::cout << NowTime() << "	ִAPI_WriteOutBit()" << std::endl;
				ret = API_WriteOutBit(pod);
				std::cout << NowTime() << "	WriteOutBit = " << ret << std::endl << std::endl;
			}
			//do0false
			if (str == "setdofalse")
			{
				RokaeSDKIOValuePOD pod;
				char name[4] = "do0";
				memcpy(pod.pod_name, name, strlen(name) + 1);
				pod.pod_value = 0;
				std::cout << NowTime() << "	ִAPI_WriteOutBit()" << std::endl;
				ret = API_WriteOutBit(pod);
				std::cout << NowTime() << "	WriteOutBit = " << ret << std::endl << std::endl;
			}
			//ȡDI
			if (str == "readdi")
			{
				char di_name[80] = "di0";
				unsigned int len = 0;
				RokaeSDKIOValuePOD pod_msg;

				std::cout << NowTime() << "	ִAPI_ReadInBit()" << std::endl;
				int ret = API_ReadInBit(di_name, strlen(di_name), &pod_msg, &len);

				std::cout << NowTime() << "	ReadInBit = " << ret << "	" << di_name << " = " << pod_msg.pod_value << std::endl << std::endl;
			}
			//λƫ
			if (str == "moveloff")
			{
				rokae_robtarget_pos_off rob_pos;
				rob_pos.base_rob_pos.euler_type = 3;
				rob_pos.base_rob_pos.rob_pos.cogx = 365;
				rob_pos.base_rob_pos.rob_pos.cogy = 0;
				rob_pos.base_rob_pos.rob_pos.cogz = 638;
				rob_pos.base_rob_pos.rob_pos.q1 = 0;
				rob_pos.base_rob_pos.rob_pos.q2 = 0;
				rob_pos.base_rob_pos.rob_pos.q3 = 1;
				rob_pos.base_rob_pos.rob_pos.q4 = 0;
				rob_pos.base_rob_pos.motion_param.tcp = 500;
				rob_pos.base_rob_pos.motion_param.per = 100;
				rob_pos.base_rob_pos.motion_param.iZone = 200;

				rob_pos.x_off = -100;
				rob_pos.y_off = 0;
				rob_pos.z_off = 0;
				std::cout << NowTime() << "	ִAPI_MoveLOffSinglePosition()" << std::endl;
				int ret = API_MoveLOffSinglePosition(-1, rob_pos);
				std::cout << NowTime() << "	MoveLOffSinglePosition = " << ret << std::endl << std::endl;
			}
			//λƫ
			if (str == "mumoveloff")
			{
				rokae_robtarget_pos_off rob_pos[2];
				rob_pos[0].base_rob_pos.euler_type = 2;
				rob_pos[0].base_rob_pos.rob_pos.cogx = 365;
				rob_pos[0].base_rob_pos.rob_pos.cogy = 0;
				rob_pos[0].base_rob_pos.rob_pos.cogz = 638;
				rob_pos[0].base_rob_pos.rob_pos.q1 = 0;
				rob_pos[0].base_rob_pos.rob_pos.q2 = 0;
				rob_pos[0].base_rob_pos.rob_pos.q3 = 1;
				rob_pos[0].base_rob_pos.rob_pos.q4 = 0;
				rob_pos[0].base_rob_pos.motion_param.tcp = 500;
				rob_pos[0].base_rob_pos.motion_param.per = 100;
				rob_pos[0].base_rob_pos.motion_param.iZone = 200;
				rob_pos[0].x_off = 100;
				rob_pos[0].y_off = 0;
				rob_pos[0].z_off = 0;

				rob_pos[1].base_rob_pos.euler_type = 2;
				rob_pos[1].base_rob_pos.rob_pos.cogx = 365;
				rob_pos[1].base_rob_pos.rob_pos.cogy = 100;
				rob_pos[1].base_rob_pos.rob_pos.cogz = 638;
				rob_pos[1].base_rob_pos.rob_pos.q1 = 0;
				rob_pos[1].base_rob_pos.rob_pos.q2 = 0;
				rob_pos[1].base_rob_pos.rob_pos.q3 = 1;
				rob_pos[1].base_rob_pos.rob_pos.q4 = 0;
				rob_pos[1].base_rob_pos.motion_param.tcp = 500;
				rob_pos[1].base_rob_pos.motion_param.per = 100;
				rob_pos[1].base_rob_pos.motion_param.iZone = 200;
				rob_pos[1].x_off = 100;
				rob_pos[1].y_off = 0;
				rob_pos[1].z_off = 0;
				std::cout << NowTime() << "	ִAPI_MoveLOffMutiPosition()" << std::endl;
				int ret = API_MoveLOffMutiPosition(1, rob_pos, 2);
				std::cout << NowTime() << "	MoveLOffMutiPosition = " << ret << std::endl << std::endl;
			}
			//PPtoMain
			if (str == "pptomain")
			{
				std::cout << NowTime() << "	ִAPI_PPtomain()" << std::endl;
				int ret = API_PPtomain();
				std::cout << NowTime() << "	pptomain = " << ret << std::endl << std::endl;
			}
			//start
			if (str == "start")
			{
				std::cout << NowTime() << "	ִAPI_StartRun()" << std::endl;
				int ret = API_StartRun();
				std::cout << NowTime() << "	Start = " << ret << std::endl << std::endl;
			}
			//Stop0
			if (str == "stop0")
			{
				std::cout << NowTime() << "	ִAPI_Stop0()" << std::endl;
				int ret = API_Stop0();
				std::cout << NowTime() << "	Stop0 = " << ret << std::endl << std::endl;
			}
			//Stop1
			if (str == "stop1")
			{
				std::cout << NowTime() << "	ִAPI_Stop1()" << std::endl;
				int ret = API_Stop1();
				std::cout << NowTime() << "	Stop1 = " << ret << std::endl << std::endl;
			}
		}
}



