Submit working... A* search implementation.
Fixes #2