updated find_path and queue.h