Added path finding functionality to main branch