public void move(PPP ppp) throws InvalidMoveError {
Node move_to = this.planned_route.remove(0);
if ((move_to.getX() == this.getX()) && (move_to.getY() == this.getY())) {
this.totalStationairyMoves++;
}
boolean invalid = false;
if (ppp.isOccupied(move_to.getX(), move_to.getY())) {
// Invalid move made!
invalid = true;
this.invalidMoves++;
throw new InvalidMoveError("Invalid move - " + move_to.toString() + " is Occupied\n");
}
if (invalid) {
System.out.println("invalid move beyond throw");
}
move_to.incVisits();
if (this.route_taken.contains(move_to)) {
this.route_taken.get(this.route_taken.indexOf(move_to)).incVisits();
}
this.route_taken.add(move_to);
short turns =
(short) (this.state.getStateValue().getTurn() + move_to.turnCost(this.getHeading()));
short adv = (short) (this.state.getStateValue().getAdvance() + 1);
short moves = (short) (turns + adv);
this.state.setStateValue(turns, adv, moves);
this.state.setAgentState(move_to.getX(), move_to.getY(), move_to.getHeading());
}
@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);
}