modified pathfinding function. propagated calculation of correct costs throughout function