Basic implementation of A* for routing