Changed cost function and added many TODOs