Fixed calculation error, slight refactor.

Will test tomorrow...