@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);
}
