Reduced some code size

* Replaced repeating code with two (temporary template functions)
* This is quite ugly and should be removed but at least it reduces repeating of parts of the code a bit