Finished  best path calculating logic