D* Routing Algorithm written in Python