Path finding kinda finished? Algorithm doesn't crash.