Implementation of A* path-finding algorithm