Removed ifs from main calculation, faster now