make change to Best Path algo

Include distance between closest stop and actual end points when
calculating best path

Signed-off-by: Jason Rogena <jasonrogena@gmail.com>