Pathfinding works, but it is not correct