Pathfinding now works + small tweaks