Implementation of A* path finding