Changed to use the cost path function