import java.util.ArrayList;

import com.aldebaran.proxy.*;

public class Test {
	private static String NAOQI_IP = "127.0.0.1";
	private static int NAOQI_PORT = 9559;

	public static void main(String[] args){
		Test test = new Test();
		
	}
	
	public Test(){
		
		doArmControl("LArm");
	
	}
	
	public void stiffnessOn(ALMotionProxy proxy){
		Variant pNames = new Variant(new String[]{"Body"});
		Variant pStiffnessLists = new Variant(new float[]{1.0f});
		Variant pTimeLists = new Variant(new float[]{1.0f});
		proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists);
	}
	
	public void doArmControl(String effectorName){
		ALMotionProxy motionProxy = new ALMotionProxy(NAOQI_IP,NAOQI_PORT);
		ALRobotPostureProxy postureProxy = new ALRobotPostureProxy(NAOQI_IP,NAOQI_PORT);
		
		//Set NAO in stifness On
		stiffnessOn(motionProxy);
		
		//Send NAO to Pose Init
		postureProxy.goToPosture("StandInit", 0.5f);
		boolean isEnabled = false;
		int space = 2; //motion.FRAME_ROBOT in python sdk is 2.Seesee pynaoqi-sdk/motion.py
		float[] effectorInit = motionProxy.getPosition(effectorName,2,isEnabled); 
		
		//Active LArm Tracking
		isEnabled = true;
		motionProxy.wbEnableEffectorControl(effectorName, isEnabled);
		
		/**
			#Example showing how to set position target for LArm
		    # The 3 coordinates are absolute LArm position in NAO_SPACE
		    # Position in meter in x, y and z axis.

		    # X Axis LArm Position feasible movement = [ +0.00, +0.12] meter
		    # Y Axis LArm Position feasible movement = [ -0.05, +0.10] meter
		    # Y Axis RArm Position feasible movement = [ -0.10, +0.05] meter
		    # Z Axis LArm Position feasible movement = [ -0.10, +0.10] meter
		**/
		
		float coef = 1.0f;
		if(effectorName.equals("LArm")){
			coef = 1.0f;
		}else if(effectorName.equals("RArm")){
			coef = -1.0f;
		}
		
		float[][] targetCoordinateList = new float[][]{
				{0.12f,0.00f*coef,0.00f},
				{0.12f,0.00f*coef,-0.10f},
				{0.12f,0.05f*coef,-0.10f},
				{0.12f,0.05f*coef,0.10f},
				{0.12f,-0.10f*coef,0.10f},
				{0.12f,0.10f*coef,-0.10f},
				{0.12f,0.00f*coef,-0.10f},
				{0.12f,0.00f*coef,0.00f},
				{0.00f,0.00f*coef,0.00f}
		};
		
		/**
		 # wbSetEffectorControl is a non blocking function
		 # time.sleep allow head go to his target
		 # The recommended minimum period between two successives set commands is
		 # 0.2 s.
		**/
		for(int i=0;ilength;i++){
			Variant targetCoordinate = new Variant(new float[]{targetCoordinateList[i][0]+effectorInit[0],
				targetCoordinateList[i][1]+effectorInit[1],targetCoordinateList[i][2]+effectorInit[2]});
			motionProxy.wbSetEffectorControl(effectorName, targetCoordinate);
			System.out.println("target"+i+": X:"+(targetCoordinateList[i][0]+effectorInit[0])+" Y:"
				+(targetCoordinateList[i][1]+effectorInit[1]) +" Z:"+(targetCoordinateList[i][2]+effectorInit[2]));
			try {
				Thread.sleep(1000); //original is 4 sec but it's too slow
			} catch (InterruptedException e) {
				e.printStackTrace();
			}
		}		
		
		//Deactiate Head Tracking
		isEnabled = false;
		motionProxy.wbEnableEffectorControl(effectorName,isEnabled);

	}
	
}