Completion of base pathfinder, still needs recursive function to build path