A2 - Finished costFunction & gradient