Merge cost func block

Now we can merge from here, making PR #23 obsolete, ok?