Implemented cost function