[TEST] Adding tests for lib/pathfind