package application;


import javax.inject.Inject;

import robotiq.gripper.twoFingerF85.RobotiqGripper2F85;

import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
import static com.kuka.roboticsAPI.motionModel.BasicMotions.*;
import com.kuka.roboticsAPI.motionModel.Spline;

import com.kuka.roboticsAPI.controllerModel.Controller;
import com.kuka.roboticsAPI.deviceModel.LBR;
import com.kuka.roboticsAPI.geometricModel.ObjectFrame;


public class Pickandplacegripper extends RoboticsAPIApplication {
	@Inject
	private Controller kukaController;
	private LBR lBR_iiwa_14_R820_1;
	private RobotiqGripper2F85 robotiqGripper; 


	@Override
	public void initialize() {
		kukaController = (Controller) getContext().getControllers().toArray()[0];
		lBR_iiwa_14_R820_1 = (LBR) kukaController.getDevices().toArray()[0];
		robotiqGripper = new RobotiqGripper2F85(kukaController);

	}

	@Override
	public void run() {
		
		robotiqGripper.activate();
		robotiqGripper.waitForInitialization();
		robotiqGripper.fullyOpen();
		//Can be used to Fully open the gripper
		
			lBR_iiwa_14_R820_1.move(ptpHome());
		
		//put your code in from here	
			
		System.out.println("Move To Start Position");
		// Move to Start position
		lBR_iiwa_14_R820_1.move(ptp(getApplicationData().getFrame("/PickAndPlace1/Start")).setBlendingRel(.5).setJointVelocityRel(0.4));
	
		System.out.println("Move To Pickup Position");
		// Move to Pickup position
		lBR_iiwa_14_R820_1.move(ptp(getApplicationData().getFrame("/PickAndPlace1/Pickup/PickupSafe")).setJointVelocityRel(0.4));
		lBR_iiwa_14_R820_1.move(ptp(getApplicationData().getFrame("/PickAndPlace1/Pickup")).setJointVelocityRel(0.25));
		
		System.out.println("Gripper Closing");
		// Grip Box
		robotiqGripper.moveToCM(65, 10, 1);
		robotiqGripper.waitForMoveEnd();

		System.out.println("Move To Place Position");
		// Move to Place position
		lBR_iiwa_14_R820_1.move(ptp(getApplicationData().getFrame("/PickAndPlace1/Place/PlaceSafe")).setJointVelocityRel(0.2));
		lBR_iiwa_14_R820_1.move(ptp(getApplicationData().getFrame("/PickAndPlace1/Place")).setJointVelocityRel(0.2));
		
		System.out.println("Placing Object");
		// Placing object
		robotiqGripper.fullyOpen();
		robotiqGripper.waitForfullyOpen();
		
		System.out.println("Clearing Area");
		//Go to End
		lBR_iiwa_14_R820_1.move(ptp(getApplicationData().getFrame("/PickAndPlace1/End")).setJointVelocityRel(0.2));
		
		System.out.println("Returning to Home");
		lBR_iiwa_14_R820_1.move(ptpHome());
		
		robotiqGripper.deactivate();
		System.out.println("Deactivated");
		
	}
}