D* Lite implementation initial commit - early stages