Add lib PathFinding for test
Various code improvement.