updated pathfinding and added test to main