If destination isn't reachable, we at least get to the maximum
reachable point with the direct route
Removed commented code
Plot the full path