Got pathfinding working