A* fully implemented, albeit performs somewhat slowly