@Override public void plan() { short[] currentPos = this.getPos(); Node parent = null; int parent_to_reach = 0; char parent_heading = 'r'; if (this.route_taken.size() != 0) { parent = this.route_taken.get(this.route_taken.size() - 1); parent_to_reach = parent.getCostToReach(); parent_heading = parent.getHeading(); } ArrayList<Node> successors = this.getSuccessors(parent, currentPos[0], currentPos[1], currentMem); for (Node s : successors) { int to_reach = parent_to_reach + s.turnCost(parent_heading) + 1; s.setCost(to_reach, this.evaluatePosition(s)); } ; Node cheapest = this.getCheapestSuccessor(successors); this.planned_route.add(cheapest); }