Add working code. Both A* and BFS works.