New cost function implementation

Not yet tested. Only a first try.