minor modifs on the C code, added a function for penalizing