Updated simple pathing algorithm