Pathfinding now works as intended. Need refactoring though.