Slight cleanup, was intended to reduce code but gave the same amount with better functionality. :)
Fixing issue #221 and some bugs in the previous commit.