finished up pathfinding

still need to check everything (small programming bugs)
and check if it works