Finished cost function + test cases