public Plan getPath(TaskSet tasks) {
		// Do backtracking
		node currentNode = this.lastGoalNode;
		ArrayList<node> path = new ArrayList<node>();

		int capacity = this.vehicle.capacity();
		System.out.println("Capacity vehicle: "+capacity);

		while(currentNode != null) {
			currentNode.printState();
			path.add(currentNode);
			currentNode = currentNode.getParent();	
		}

		Collections.reverse(path);
		Plan optimalPlan = Plan.EMPTY;
		System.out.println("Pathlength: "+path.size());

		if(path.size() != 0) {
			City current = path.get(0).getCity();

			optimalPlan = new Plan(current);

			//System.out.println("Startcity: "+current);
			System.out.println("Tasklist: "+ this.taskList);
			for(int i = 1; i < path.size() ; i++) {
				currentNode = path.get(i);
				City nextCity = currentNode.getCity();
				for(City city : current.pathTo(nextCity)) {
					//System.out.println("City: "+city);
					optimalPlan.appendMove(city);
				}

				int stateSize = currentNode.getState().size();
				//System.out.println("--------------------");
				for(int j=0; j< stateSize; j++) {
					actionStates currentNodeAction = (actionStates) currentNode.getState().get(j).get(1);
					actionStates lastNodeAction = (actionStates) path.get(i-1).getState().get(j).get(1);
					if(currentNodeAction != lastNodeAction) {
						Task currentTask = (Task) path.get(i).getState().get(j).get(0);

						System.out.println("CurrentTask: "+currentTask);

						Task currentTaskFromTaskSet = getTaskFromTaskSet(currentTask.id, tasks);

						switch(currentNodeAction) {
						case PICKEDUP:
							optimalPlan.appendPickup(currentTaskFromTaskSet);
							capacity -= currentTaskFromTaskSet.weight;
							break;
						case DELIVERED:
							optimalPlan.appendDelivery(currentTaskFromTaskSet);
							capacity += currentTaskFromTaskSet.weight;
							break;
						default:
							System.out.println("_OO_");
						}
					}
				}
				current = nextCity;
			}
		}
		System.out.println(optimalPlan);
		return optimalPlan;
	}