Refactor to make code simpler

This code has fewer repetitions and is more readable.

Again, please review & test. This one is just cosmetics, but still, it looks better and should be faster.