Pathfinding implemented on unit. Does not work yet.