Rearranged code

Now it makes more sense, and might be a little bit more optimised? Also
fixed a couple numbers.