Implement cost function (close #11)