Grid Based Pathfinding Python