Moved costFun to private CostFun class