implement ComputeCostAndGrad

TODO: i have no idea how to test this, gradient checks?