Fixed the bug and pending add more features to the PathFinding