Penalty calculation fix

The way I was calculating penalties before was incorrect in a number of
places, I believe I have fixed it now. In addition, everything by soft
constraints 3 and 4 are being calculated pretty efficiently.