Added cost function, move functions to correspond util class