Improvements in cost calculation

[francosansone]

There is room for improvement in cost calculation, which is the most expensive thing that this algorithm does. The goal is to avoid computing things more than once.