Filling out cost functions in cost.h