Python Path Finding Algorithm