Fixed Errors

Ordering of my variables screwed up the final output. Fixed things to
proper order. Tested if the math works.