Fixed a calculation issue. Also restructured the code to make it a bit cleaner.