import orin2.bcap.BCAPClient;

public class BCAPSlaveMove {
	public static void main(String[] args) {
		try {
			new BCAPSlaveMove("udp:192.168.0.1", 3000, 3);
		} catch(Throwable cause) {
			System.out.println(cause.toString());
		}
	}
	
	private static final int PERIOD    = 100;
	private static final int AMPLITUDE = 10;
	
	private BCAPClient m_client = null;
	
	public BCAPSlaveMove(String strConn, int timeout, int retry) throws Throwable {
		m_client = new BCAPClient(strConn, timeout, retry);
		m_client.Service_Start("WDT=400");
		
		/* Get controller handle */
		int hCtrl = 0;
		try {
			hCtrl = m_client.Controller_Connect("Ctrl", "CaoProv.DENSO.VRC", "localhost", "");
			
			/* Get robot handle */
			int hRob = 0;
			try {
				hRob = m_client.Controller_GetRobot(hCtrl, "Rob", "");
				
				/* Get arm control authority */
				m_client.Robot_Execute(hRob, "Takearm", new int[] { 0, 1 });
				
				/* Motor on */
				m_client.Robot_Execute(hRob, "Motor", true);
				
				/* Move to first pose */
				m_client.Robot_Move(hRob, 1, "@E J1", "");
				
				/* Get current angle */
				double[] dJntInit = (double[])m_client.Robot_Execute(hRob, "CurJnt", "");
				
				/* Copy current angle to temporary */
				double[] dJntTmp = new double[dJntInit.length];
				for(int i = 2; i < dJntInit.length; i++)
					dJntTmp[i] = dJntInit[i];
				
				/* Start slave mode (Mode 2, J Type) */
				m_client.Robot_Execute(hRob, "slvChangeMode", 0x202);
				
				for(int i = 0; i < PERIOD; i++) {
					dJntTmp[0] = dJntInit[0] + i / 10.0;
					dJntTmp[1] = dJntInit[1] + AMPLITUDE * Math.sin(2*Math.PI*i/PERIOD);;
					m_client.Robot_Execute(hRob, "slvMove", dJntTmp);
				}
				
				/* Stop robot */
				m_client.Robot_Execute(hRob, "slvMove", dJntTmp);
				
				/* Stop slave mode */
				m_client.Robot_Execute(hRob, "slvChangeMode", 0);
				
				/* Motor off */
				m_client.Robot_Execute(hRob, "Motor", false);
				
				/* Release arm control authority */
				m_client.Robot_Execute(hRob, "Givearm", null);
			} catch(Throwable cause) {
				System.out.println(cause.toString());
			} finally {
				if(hRob != 0) {
					/* Release robot handle */
					m_client.Robot_Release(hRob);
				}
			}
		} catch(Throwable cause) {
			System.out.println(cause.toString());
		} finally {
			if(hCtrl != 0) {
				/* Disconnect controller */
				m_client.Controller_Disconnect(hCtrl);
			}
		}
		
		m_client.Service_Stop();
		m_client.Release();
	}
}