Add a cost function to Edges
Make use of the cost function instead of just the distance in pathfinding