Finish implementing cost function.