Added cost function calculation with gradient and test for this func