Added Penalty Functions.
Cleaned the code of the functions