Fixed AddBlocks (again) and simplified expression.

Herpy derpy, >> 2 is not divide by 2, and I am not sure what I was thinking with the double shift.