Added cost function and gradient