Modify Algorithm, add cost calculation