Part 2 and 3 working; cost function and gradient